Make compass io actually work somewhat

main/ugv_io_mpu.cc

@@ -56,6 +56,8 @@ void MPU::Init() {
   }
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
   mpu_->setGyroFullScale(MPU_GYRO_FS);
+  // force magnetometer into continuous mode
+  mpu_->compassWriteByte(0x0A, 0x12);
   xSemaphoreGive(i2c_mutex);
 
   BaseType_t xRet =
@@ -77,19 +79,31 @@ 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_, &mag_);
+    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<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)mag_.x) * MPU_MAG_TO_FLUX;
-    mpu_data_.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
-    mpu_data_.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+    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_);
   }
   vTaskDelete(NULL);