Add somewhat shitty ahrs algorithm

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})

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 <math.h>
+
+//-------------------------------------------------------------------------------------------
+// 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;
+}

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 <math.h>
+
+//--------------------------------------------------------------------------------------------
+// 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

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); }
 

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;

main/u8g2_esp32_hal.c

@@ -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;
     }

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));
   }

main/ugv_io_mpu.cc

@@ -3,8 +3,8 @@
 #include <driver/uart.h>
 #include <esp_log.h>
 
-#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;
+  //  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);
+  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");
-      continue;
   }
-    int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
-    int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
-    int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(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_);
+  //  int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
+  //  int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
+  //  int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
+  int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) |
+               static_cast<int16_t>(compass_data[0]);
+  int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) |
+               static_cast<int16_t>(compass_data[2]);
+  int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) |
+               static_cast<int16_t>(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;
 }
-  vTaskDelete(NULL);
-}
-
-void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); }
 
 };  // namespace io
 };  // namespace ugv

main/ugv_io_mpu.hh

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <freertos/FreeRTOS.h>
 #include <stdint.h>
 #include <MPU.hpp>
 
@@ -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

main/ugv_main.cc

@@ -1,9 +1,10 @@
 #include <esp_log.h>
 #include <esp_timer.h>
+#include "MadgwickAHRS.h"
+#include "i2c_mutex.h"
 #include "ugv_comms.hh"
 #include "ugv_display.hh"
 #include "ugv_io.hh"
-#include "i2c_mutex.h"
 
 #include <math.h>
 
@@ -32,6 +33,7 @@ struct State {
   io::Inputs         inputs;
   io::Outputs        outputs;
   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<float>(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;
     }
   }