try to fix comms issues

.gitignore

@@ -8,3 +8,5 @@
 
 __pycache__
 *.pyc
+/venv
+/tools/venv

main/ugv.cc

@@ -87,6 +87,8 @@ void UGV::Init() {
   esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
   last_print_ = 0;
   last_noise_ = 0;
+  last_left_  = 0;
+  last_right_ = 0;
 }
 
 void UGV::UpdateAHRS() {
@@ -106,7 +108,7 @@ void UGV::UpdateAHRS() {
 
 void UGV::DoDebugPrint() {
   auto &mpu = inputs_.mpu;
-  ESP_LOGV(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
+  ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
            mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
            mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
   ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
@@ -286,7 +288,29 @@ void UGV::OnTick() {
     outputs_.right_motor = drive_power + angle_pwr;
   }
 
+  constexpr float MAX_ACCEL = 8 * LOOP_PERIOD_S;
+
+  if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 &&
+      inputs_.mpu.gyro_rate.z == 0) {
+    outputs_.left_motor  = 0;
+    outputs_.right_motor = 0;
+  } else {
+    float dleft  = outputs_.left_motor - last_left_;
+    float dright = outputs_.right_motor - last_right_;
+    if (dleft > MAX_ACCEL) {
+      outputs_.left_motor = last_left_ + MAX_ACCEL;
+    } else if (dleft < -MAX_ACCEL) {
+      outputs_.left_motor = last_left_ - MAX_ACCEL;
+    }
+    if (dright > MAX_ACCEL) {
+      outputs_.right_motor = last_right_ + MAX_ACCEL;
+    } else if (dright < -MAX_ACCEL) {
+      outputs_.right_motor = last_right_ - MAX_ACCEL;
+    }
+  }
   io->WriteOutputs(outputs_);
+  last_left_  = outputs_.left_motor;
+  last_right_ = outputs_.right_motor;
 
   if (current_state_ != next_state_) {
     ESP_LOGI(TAG, "switching state to %d", next_state_);

main/ugv.hh

@@ -53,6 +53,8 @@ class UGV {
   float       accel_noise_;
   float       gyro_noise_;
   bool        is_still_;
+  float       last_left_;
+  float       last_right_;
 
   void UpdateAHRS();
   void DoDebugPrint();

main/ugv_io_mpu.cc

@@ -25,8 +25,10 @@ static const Mat3f ACCEL_MAT    = {-1., 0., 0., 0., -1., 0., 0., 0., -1.};
 // static const Vec3f MAG_OFFSET   = {-7.79683, 3.6735, 32.3868};
 // static const Vec3f MAG_OFFSET   = {-118.902, 18.8173, -39.209};
 static const Vec3f MAG_OFFSET = {-135.994629, 19.117216, -33.0};
-//static const Mat3f MAG_MAT      = {0., -0.0281408, 0., -0.0284409, 0., 0., 0., 0., 0.0261544};
-static const Mat3f MAG_MAT      = {0., -0.0335989, 0., -0.0330167, 0., 0., 0., 0., 0.0335989};
+// static const Mat3f MAG_MAT      = {0., -0.0281408, 0., -0.0284409, 0., 0.,
+// 0., 0., 0.0261544};
+static const Mat3f MAG_MAT     = {0., -0.0335989, 0., -0.0330167, 0.,
+                              0., 0.,         0., 0.0335989};
 static const Vec3f GYRO_OFFSET = {-4.33655, -2.76826, -0.908427};
 static const Mat3f GYRO_MAT    = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
 
@@ -42,6 +44,37 @@ MPU::MPU() : mpu_(nullptr) {}
 MPU::~MPU() { delete mpu_; }
 
 void MPU::Init() {
+  mutex_ = xSemaphoreCreateMutex();
+
+  BaseType_t xRet = xTaskCreate(MPU::TaskEntry, "ugv_io_mpu", 8 * 1024, this, 3,
+                                &this->task_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error creating MPU task");
+    return;
+  }
+}
+
+void MPU::Calibrate() {
+  mpud::raw_axes_t accel_offset, gyro_offset;
+  mpu_->computeOffsets(&accel_offset, &gyro_offset);
+  mpu_->setAccelOffset(accel_offset);
+  mpu_->setGyroOffset(gyro_offset);
+  {
+    auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G);
+    auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS);
+    ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x,
+             ao.y, ao.z, go.x, go.y, go.z);
+  }
+}
+
+void MPU::GetData(MpuData &data) {
+  xSemaphoreTake(mutex_, pdMS_TO_TICKS(10));
+  data                 = last_data_;
+  last_data_.gyro_rate = {0, 0, 0};
+  xSemaphoreGive(mutex_);
+}
+
+bool MPU::Initialize() {
   xSemaphoreTake(i2c_mutex, portMAX_DELAY);
   mpu_bus_ = &i2c0;
   // This is shared with the oled, so just use those pins
@@ -73,7 +106,7 @@ void MPU::Init() {
     break;
   }
   if (tries == 5) {
-    return;
+    return false;
   }
   // Calibrate();
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
@@ -83,33 +116,34 @@ void MPU::Init() {
   xSemaphoreGive(i2c_mutex);
 
   ESP_LOGI(TAG, "MPU initialized");
+  return true;
 }
 
-void MPU::Calibrate() {
-  mpud::raw_axes_t accel_offset, gyro_offset;
-  mpu_->computeOffsets(&accel_offset, &gyro_offset);
-  mpu_->setAccelOffset(accel_offset);
-  mpu_->setGyroOffset(gyro_offset);
-  {
-    auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G);
-    auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS);
-    ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x,
-             ao.y, ao.z, go.x, go.y, go.z);
-  }
+void MPU::DoTask() {
+  ESP_LOGI(TAG, "MPU task start");
+  mpud::raw_axes_t accel_, gyro_, mag_;
+  esp_err_t        ret;
+  MpuData          data;
+
+  if (!Initialize()) {
+    return;
   }
 
-void MPU::GetData(MpuData &data) {
-  esp_err_t ret;
+  while (true) {
     xSemaphoreTake(i2c_mutex, portMAX_DELAY);
     ret = mpu_->motion(&accel_, &gyro_);
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error reading MPU: %#x", ret);
+      xSemaphoreGive(i2c_mutex);
+      continue;
     }
     uint8_t compass_data[7];
     mpu_->setAuxI2CBypass(true);
     ret = mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
     if (ret != ESP_OK) {
       ESP_LOGE(TAG, "error reading MPU compass: %#x", ret);
+      xSemaphoreGive(i2c_mutex);
+      continue;
     }
     mpu_->setAuxI2CBypass(false);
     xSemaphoreGive(i2c_mutex);
@@ -128,6 +162,16 @@ void MPU::GetData(MpuData &data) {
     data.mag.y     = ((float)my) * MPU_MAG_TO_FLUX;
     data.mag.z     = ((float)mz) * MPU_MAG_TO_FLUX;
     data.mag       = MAG_MAT * (data.mag + MAG_OFFSET);
+    xSemaphoreTake(mutex_, pdMS_TO_TICKS(100));
+    last_data_ = data;
+    xSemaphoreGive(mutex_);
+  }
+}
+
+void MPU::TaskEntry(void *arg) {
+  MPU *mpu = (MPU *)arg;
+  mpu->DoTask();
+  vTaskDelete(NULL);
 }
 
 };  // namespace io

main/ugv_io_mpu.hh

@@ -42,13 +42,9 @@ struct Vec3f {
     return *this;
   }
 
-  float dot(const Vec3f& v) const {
-    return x * v.x + y * v.y + z * v.z;
-  }
+  float dot(const Vec3f& v) const { return x * v.x + y * v.y + z * v.z; }
 
-  float norm2() const {
-    return x * x + y * y + z * z;
-  }
+  float norm2() const { return x * x + y * y + z * z; }
 };
 
 struct Mat3f {
@@ -65,8 +61,7 @@ struct Mat3f {
     float zx, zy, zz;
   };
 
-  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_)
-      : rx(rx_), ry(ry_), rz(rz_) {}
+  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_) : rx(rx_), ry(ry_), rz(rz_) {}
 
   Mat3f(float xx, float xy, float xz, float yx, float yy, float yz, float zx,
         float zy, float zz)
@@ -76,9 +71,7 @@ struct Mat3f {
     return {rx.dot(v), ry.dot(v), rz.dot(v)};
   }
 
-  Mat3f transpose() const {
-    return {xx, yx, zx, xy, yy, zy, xz, yz, zz};
-  }
+  Mat3f transpose() const { return {xx, yx, zx, xy, yy, zy, xz, yz, zz}; }
 
   Mat3f operator*(const Mat3f& m) const {
     Mat3f mt = m.transpose();
@@ -108,7 +101,13 @@ class MPU {
  private:
   mpud::mpu_bus_t*  mpu_bus_;
   mpud::MPU*        mpu_;
-  mpud::raw_axes_t accel_, gyro_, mag_;
+  TaskHandle_t      task_;
+  SemaphoreHandle_t mutex_;
+  MpuData           last_data_;
+
+  void        DoTask();
+  bool        Initialize();
+  static void TaskEntry(void* arg);
 };
 
 }  // namespace io

tools/config.yml

@@ -1,12 +1,12 @@
 REVISION: 1
 angle_pid:
-  kp: 0.10
+  kp: 0.04
   ki: 0.0001
-  kd: 0.4
+  kd: 0.2
   max_output: 0.5
   max_i_error: 15.0
 
-min_target_dist: 10.0
+min_target_dist: 1.0
 min_flip_pitch: 90.0
 drive_power: 0.3
 mag_declination: 11.5