From 0a6305c52abafa86c24df2cfcc7047fd66073ac1 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Sun, 27 Jan 2019 17:04:48 -0800 Subject: [PATCH] Add somewhat shitty ahrs algorithm --- main/CMakeLists.txt | 1 + main/MadgwickAHRS.cpp | 287 ++++++++++++++++++++++++++++++++++++++++++ main/MadgwickAHRS.h | 88 +++++++++++++ main/Print.cpp | 4 +- main/e32_driver.cc | 2 +- main/u8g2_esp32_hal.c | 9 +- main/ugv_display.cc | 9 +- main/ugv_io_mpu.cc | 88 ++++++------- main/ugv_io_mpu.hh | 9 -- main/ugv_main.cc | 22 +++- 10 files changed, 445 insertions(+), 74 deletions(-) create mode 100644 main/MadgwickAHRS.cpp create mode 100644 main/MadgwickAHRS.h diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 40cb55c..b6e1440 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -11,6 +11,7 @@ set(COMPONENT_SRCS "ugv_io_mpu.cc" "u8g2_esp32_hal.c" "Print.cpp" + "MadgwickAHRS.cpp" "e32_driver.cc" ) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) diff --git a/main/MadgwickAHRS.cpp b/main/MadgwickAHRS.cpp new file mode 100644 index 0000000..38a8f8b --- /dev/null +++ b/main/MadgwickAHRS.cpp @@ -0,0 +1,287 @@ +//============================================================================================= +// MadgwickAHRS.c +//============================================================================================= +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// From the x-io website "Open-source resources available on this website are +// provided under the GNU General Public Licence unless an alternative licence +// is provided in source." +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised +// +//============================================================================================= + +//------------------------------------------------------------------------------------------- +// Header files + +#include "MadgwickAHRS.h" +#include + +//------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreqDef 512.0f // sample frequency in Hz +#define betaDef 0.1f // 2 * proportional gain + +//============================================================================================ +// Functions + +//------------------------------------------------------------------------------------------- +// AHRS algorithm update + +Madgwick::Madgwick() { + beta = betaDef; + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + invSampleFreq = 1.0f / sampleFreqDef; + anglesComputed = 0; +} + +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, + float az, float mx, float my, float mz) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in + // magnetometer normalisation) + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + updateIMU(gx, gy, gz, ax, ay, az); + return; + } + + // Convert gyroscope degrees/sec to radians/sec + gx *= 0.0174533f; + gy *= 0.0174533f; + gz *= 0.0174533f; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in + // accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - + _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * q3 + _2bz * q1) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * q2 + _2bz * q0) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * q3 - _4bz * q1) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * q2 - _2bz * q0) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * q1 + _2bz * q3) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * q0 - _4bz * q2) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * q3 + _2bz * q1) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * q0 + _2bz * q2) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +//------------------------------------------------------------------------------------------- +// IMU algorithm update + +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, + q3q3; + + // Convert gyroscope degrees/sec to radians/sec + gx *= 0.0174533f; + gy *= 0.0174533f; + gz *= 0.0174533f; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in + // accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * invSampleFreq; + q1 += qDot2 * invSampleFreq; + q2 += qDot3 * invSampleFreq; + q3 += qDot4 * invSampleFreq; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + anglesComputed = 0; +} + +//------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root + +float Madgwick::invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +} + +//------------------------------------------------------------------------------------------- + +void Madgwick::computeAngles() { + roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); + pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); + yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); + anglesComputed = 1; +} diff --git a/main/MadgwickAHRS.h b/main/MadgwickAHRS.h new file mode 100644 index 0000000..989eca9 --- /dev/null +++ b/main/MadgwickAHRS.h @@ -0,0 +1,88 @@ +//============================================================================================= +// MadgwickAHRS.h +//============================================================================================= +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// From the x-io website "Open-source resources available on this website are +// provided under the GNU General Public Licence unless an alternative licence +// is provided in source." +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +//============================================================================================= +#ifndef MadgwickAHRS_h +#define MadgwickAHRS_h + +#include + +//-------------------------------------------------------------------------------------------- +// Variable declaration +class Madgwick { + private: + static float invSqrt(float x); + + float beta; // algorithm gain + float q0; + float q1; + float q2; + float q3; // quaternion of sensor frame relative to auxiliary frame + float invSampleFreq; + float roll; + float pitch; + float yaw; + char anglesComputed; + + void computeAngles(); + + //------------------------------------------------------------------------------------------- + // Function declarations + public: + Madgwick(void); + + void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } + + void update(float gx, float gy, float gz, float ax, float ay, float az, + float mx, float my, float mz); + + void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); + + // float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * + // q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f * + // q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2 + // - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; + float getRoll() { + if (!anglesComputed) computeAngles(); + return roll * 57.29578f; + } + + float getPitch() { + if (!anglesComputed) computeAngles(); + return pitch * 57.29578f; + } + + float getYaw() { + if (!anglesComputed) computeAngles(); + return yaw * 57.29578f + 180.0f; + } + + float getRollRadians() { + if (!anglesComputed) computeAngles(); + return roll; + } + + float getPitchRadians() { + if (!anglesComputed) computeAngles(); + return pitch; + } + + float getYawRadians() { + if (!anglesComputed) computeAngles(); + return yaw; + } +}; + +#endif diff --git a/main/Print.cpp b/main/Print.cpp index 3e9e59c..6de22a8 100644 --- a/main/Print.cpp +++ b/main/Print.cpp @@ -66,7 +66,9 @@ size_t Print::printf(const char *format, ...) { // size_t Print::print(const String &s) { return write(s.c_str(), s.length()); } -size_t Print::print(const std::string &s) { return write(s.c_str(), s.length()); } +size_t Print::print(const std::string &s) { + return write(s.c_str(), s.length()); +} size_t Print::print(const char str[]) { return write(str); } diff --git a/main/e32_driver.cc b/main/e32_driver.cc index 2cbff17..c1d13b6 100644 --- a/main/e32_driver.cc +++ b/main/e32_driver.cc @@ -9,7 +9,7 @@ static constexpr size_t E32_BUF_SIZE = 1024; static constexpr size_t E32_UART_RX_BUF_SIZE = 1024; static constexpr size_t E32_UART_TX_BUF_SIZE = 1024; -static constexpr size_t PARAMS_LEN = 5; +static constexpr size_t PARAMS_LEN = 6; static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0; static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1}; static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2; diff --git a/main/u8g2_esp32_hal.c b/main/u8g2_esp32_hal.c index 6a640fc..3301e24 100644 --- a/main/u8g2_esp32_hal.c +++ b/main/u8g2_esp32_hal.c @@ -107,8 +107,8 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, #define TXBUF_SIZE 32 static uint8_t txbuf[TXBUF_SIZE]; static uint8_t *txbuf_ptr; - // ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, - // arg_int, arg_ptr); +// ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, +// arg_int, arg_ptr); switch (msg) { case U8X8_MSG_BYTE_SET_DC: { if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) { @@ -175,9 +175,10 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, i2c_master_write(handle_i2c, txbuf, txbuf_ptr - txbuf, ACK_CHECK_EN)); ESP_ERROR_CHECK(i2c_master_stop(handle_i2c)); xSemaphoreTake(i2c_mutex, portMAX_DELAY); - ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c, - I2C_TIMEOUT_MS / portTICK_RATE_MS)); + esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c, + I2C_TIMEOUT_MS / portTICK_RATE_MS); xSemaphoreGive(i2c_mutex); + ESP_ERROR_CHECK(ret); i2c_cmd_link_delete(handle_i2c); break; } diff --git a/main/ugv_display.cc b/main/ugv_display.cc index 15ffd7d..99d9757 100644 --- a/main/ugv_display.cc +++ b/main/ugv_display.cc @@ -11,10 +11,10 @@ static const char* TAG = "ugv_display"; using comms::CommsClass; -DisplayClass::DisplayClass(CommsClass* comms) - : comms_(comms) {} +DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {} void DisplayClass::Init() { + ESP_LOGD(TAG, "Initializing u8g2 display"); // For Heltec ESP32 LoRa // oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4); // For wemos Lolin ESP32 @@ -27,7 +27,9 @@ void DisplayClass::Init() { 8 * 1024, this, 1, &this->task_handle_); if (xRet != pdTRUE) { ESP_LOGE(TAG, "error starting display thread"); + return; } + ESP_LOGD(TAG, "Started display thread"); } void DisplayClass::Run() { @@ -78,7 +80,8 @@ void DisplayClass::Run() { oled->print("no pkt rx"); } // oled->setCursor(4, 6 * 8); - // oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor); + // oled->printf("motors: (%f, %f)", outputs.left_motor, + // outputs.right_motor); } while (oled->nextPage()); vTaskDelay(pdMS_TO_TICKS(1000)); } diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index 098711b..faa5863 100644 --- a/main/ugv_io_mpu.cc +++ b/main/ugv_io_mpu.cc @@ -3,8 +3,8 @@ #include #include -#include "i2c_mutex.h" #include "MPU.hpp" +#include "i2c_mutex.h" #include "mpu/math.hpp" namespace ugv { @@ -23,18 +23,15 @@ Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {} Vec3f::Vec3f(const mpud::float_axes_t &axes) : x(axes.x), y(axes.y), z(axes.z) {} -MPU::MPU() : mpu_(nullptr), mpu_data_() {} +MPU::MPU() : mpu_(nullptr) {} MPU::~MPU() { delete mpu_; } void MPU::Init() { - mutex_ = xSemaphoreCreateMutex(); - mpu_data_ = MpuData{}; - + xSemaphoreTake(i2c_mutex, portMAX_DELAY); mpu_bus_ = &i2c0; // This is shared with the oled, so just use those pins - xSemaphoreTake(i2c_mutex, portMAX_DELAY); - mpu_bus_->begin(MPU_SDA, MPU_SCL); + // mpu_bus_->begin(MPU_SDA, MPU_SCL); mpu_ = new mpud::MPU(*mpu_bus_); esp_err_t ret; @@ -60,56 +57,45 @@ void MPU::Init() { mpu_->compassWriteByte(0x0A, 0x12); xSemaphoreGive(i2c_mutex); - BaseType_t xRet = - xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_); - if (xRet != pdTRUE) { - ESP_LOGE(TAG, "error creating MPU task"); - return; - } - ESP_LOGI(TAG, "MPU initialized and task started"); + ESP_LOGI(TAG, "MPU initialized"); } void MPU::GetData(MpuData &data) { - xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); - data = MpuData(mpu_data_); - xSemaphoreGive(mutex_); -} - -void MPU::DoTask() { esp_err_t ret; - while (true) { - vTaskDelay(pdMS_TO_TICKS(50)); - uint8_t mxh, mxl, myh, myl, mzh, mzl, n; - xSemaphoreTake(i2c_mutex, portMAX_DELAY); - ret = mpu_->motion(&accel_, &gyro_); - mpu_->compassReadByte(0x03, &mxl); - mpu_->compassReadByte(0x04, &mxh); - mpu_->compassReadByte(0x05, &myl); - mpu_->compassReadByte(0x06, &myh); - mpu_->compassReadByte(0x07, &mzl); - mpu_->compassReadByte(0x08, &mzh); - mpu_->compassReadByte(0x09, &n); - xSemaphoreGive(i2c_mutex); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "error reading MPU"); - continue; - } - int16_t mx = (static_cast(mxh) << 8) | static_cast(mxl); - int16_t my = (static_cast(myh) << 8) | static_cast(myl); - int16_t mz = (static_cast(mzh) << 8) | static_cast(mzl); - ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); - xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); - mpu_data_.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); - mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); - mpu_data_.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; - mpu_data_.mag.y = ((float)my) * MPU_MAG_TO_FLUX; - mpu_data_.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; - xSemaphoreGive(mutex_); + // uint8_t mxh, mxl, myh, myl, mzh, mzl, n; + xSemaphoreTake(i2c_mutex, portMAX_DELAY); + ret = mpu_->motion(&accel_, &gyro_); + uint8_t compass_data[7]; + mpu_->setAuxI2CBypass(true); + mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data); + mpu_->setAuxI2CBypass(false); + // mpu_->compassReadByte(0x03, &mxl); + // mpu_->compassReadByte(0x04, &mxh); + // mpu_->compassReadByte(0x05, &myl); + // mpu_->compassReadByte(0x06, &myh); + // mpu_->compassReadByte(0x07, &mzl); + // mpu_->compassReadByte(0x08, &mzh); + // mpu_->compassReadByte(0x09, &n); + xSemaphoreGive(i2c_mutex); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "error reading MPU"); } - vTaskDelete(NULL); + // int16_t mx = (static_cast(mxh) << 8) | static_cast(mxl); + // int16_t my = (static_cast(myh) << 8) | static_cast(myl); + // int16_t mz = (static_cast(mzh) << 8) | static_cast(mzl); + int16_t mx = (static_cast(compass_data[1]) << 8) | + static_cast(compass_data[0]); + int16_t my = (static_cast(compass_data[3]) << 8) | + static_cast(compass_data[2]); + int16_t mz = (static_cast(compass_data[5]) << 8) | + static_cast(compass_data[4]); + // ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); + data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); + data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); + data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; + data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; + data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; } -void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); } - }; // namespace io }; // namespace ugv diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh index 8d35594..b41e621 100644 --- a/main/ugv_io_mpu.hh +++ b/main/ugv_io_mpu.hh @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -39,14 +38,6 @@ class MPU { mpud::mpu_bus_t *mpu_bus_; mpud::MPU * mpu_; mpud::raw_axes_t accel_, gyro_, mag_; - - TaskHandle_t task_; - SemaphoreHandle_t mutex_; - MpuData mpu_data_; - - void DoTask(); - - static void MPU_Task(void *arg); }; } // namespace io diff --git a/main/ugv_main.cc b/main/ugv_main.cc index c033f08..4f0aa2f 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -1,9 +1,10 @@ #include #include +#include "MadgwickAHRS.h" +#include "i2c_mutex.h" #include "ugv_comms.hh" #include "ugv_display.hh" #include "ugv_io.hh" -#include "i2c_mutex.h" #include @@ -15,7 +16,7 @@ using ugv::io::IOClass; static const char *TAG = "ugv_main"; extern "C" { - SemaphoreHandle_t i2c_mutex; +SemaphoreHandle_t i2c_mutex; } constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; @@ -31,7 +32,8 @@ struct State { esp_timer_handle_t timer_handle; io::Inputs inputs; io::Outputs outputs; - int64_t last_print; + int64_t last_print; + Madgwick ahrs_; State() { comms = new CommsClass(); @@ -43,9 +45,12 @@ struct State { esp_timer_init(); i2c_mutex = xSemaphoreCreateMutex(); + ahrs_.begin(1000000.f / + static_cast(LOOP_PERIOD_US)); // rough sample frequency + comms->Init(); - io->Init(); display->Init(); + io->Init(); esp_timer_create_args_t timer_args; timer_args.callback = OnTimeout; @@ -65,13 +70,20 @@ struct State { outputs.left_motor = sinf(time_s * PI); outputs.right_motor = cosf(time_s * PI); io->WriteOutputs(outputs); - if (time_us >= last_print + 1 * 1000 * 1000) { // 1s + { + io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel, + &m = inputs.mpu.mag; + ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z); + } + if (time_us >= last_print + 500 * 1000) { // 1s ESP_LOGI(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z, inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y, inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y, inputs.mpu.mag.z); + ESP_LOGI(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), + ahrs_.getPitch(), ahrs_.getRoll()); last_print = time_us; } }