try to fix comms issues

.gitignore

@@ -7,4 +7,6 @@
 /cmake-build*
 
 __pycache__
-*.pyc
+*.pyc
+/venv
+/tools/venv

main/ugv.cc

@@ -8,16 +8,16 @@ namespace ugv {
 
 static const char *TAG = "ugv_main";
 
-constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
+constexpr uint64_t   LOOP_PERIOD_US = 1e6 / 100;
-constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
+constexpr float      LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
-constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
+constexpr float      LOOP_PERIOD_S  = 1 / LOOP_HZ;
 constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000);
 
 constexpr uint64_t NOISE_PERIOD_US = 2000e3;
 constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f;
 
 constexpr float ACCEL_NOISE_THRESHOLD = 0.001;
-constexpr float GYRO_NOISE_THRESHOLD = 0.3;
+constexpr float GYRO_NOISE_THRESHOLD  = 0.3;
 
 void UpdateLocationFromGPS(comms::messages::Location &location,
                            const io::GpsData &        gps_data) {
@@ -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_);
@@ -167,22 +169,22 @@ void UGV::OnTick() {
   bool  is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
 
   io::Vec3f d_accel = inputs_.mpu.accel - last_mpu_.accel;
-  io::Vec3f d_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate;
+  io::Vec3f d_gyro  = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate;
-  last_mpu_ = inputs_.mpu;
+  last_mpu_         = inputs_.mpu;
 
   accel_noise_accum_ += d_accel.norm2() * LOOP_PERIOD_S;
   gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S;
 
   if (time_us >= last_noise_ + NOISE_PERIOD_US) {
     accel_noise_ = accel_noise_accum_ / NOISE_PERIOD_S;
-    gyro_noise_ = gyro_noise_accum_ / NOISE_PERIOD_S;
+    gyro_noise_  = gyro_noise_accum_ / NOISE_PERIOD_S;
     is_still_    = (accel_noise_ < ACCEL_NOISE_THRESHOLD) &&
                 (gyro_noise_ < GYRO_NOISE_THRESHOLD);
     ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f",
              is_still_ ? "still" : "moving", accel_noise_, gyro_noise_);
     accel_noise_accum_ = 0;
-    gyro_noise_accum_ = 0;
+    gyro_noise_accum_  = 0;
-    last_noise_ = time_us;
+    last_noise_        = time_us;
   }
 
   ReadComms();
@@ -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

@@ -22,12 +22,14 @@ static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
 
 static const Vec3f ACCEL_OFFSET = {0., 0., 0.};
 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   = {-7.79683, 3.6735, 32.3868};
-//static const Vec3f MAG_OFFSET   = {-118.902, 18.8173, -39.209};
+// static const Vec3f MAG_OFFSET   = {-118.902, 18.8173, -39.209};
-static const Vec3f MAG_OFFSET   = {-135.994629, 19.117216, -33.0};
+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.0281408, 0., -0.0284409, 0., 0.,
-static const Mat3f MAG_MAT      = {0., -0.0335989, 0., -0.0330167, 0., 0., 0., 0., 0.0335989};
+// 0., 0., 0.0261544};
-static const Vec3f GYRO_OFFSET  = {-4.33655, -2.76826, -0.908427};
+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.};
 
 static const char *TAG = "ugv_io_mpu";
@@ -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
@@ -50,7 +83,7 @@ void MPU::Init() {
   mpu_ = new mpud::MPU(*mpu_bus_);
 
   esp_err_t ret;
-  int tries = 0;
+  int       tries = 0;
   for (; tries < 5; ++tries) {
     mpu_->getInterruptStatus();
     ret = mpu_->testConnection();
@@ -73,7 +106,7 @@ void MPU::Init() {
     break;
   }
   if (tries == 5) {
-    return;
+    return false;
   }
   // Calibrate();
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
@@ -83,51 +116,62 @@ void MPU::Init() {
   xSemaphoreGive(i2c_mutex);
 
   ESP_LOGI(TAG, "MPU initialized");
+  return true;
 }
 
-void MPU::Calibrate() {
+void MPU::DoTask() {
-  mpud::raw_axes_t accel_offset, gyro_offset;
+  ESP_LOGI(TAG, "MPU task start");
-  mpu_->computeOffsets(&accel_offset, &gyro_offset);
+  mpud::raw_axes_t accel_, gyro_, mag_;
-  mpu_->setAccelOffset(accel_offset);
+  esp_err_t        ret;
-  mpu_->setGyroOffset(gyro_offset);
+  MpuData          data;
-  {
+
-    auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G);
+  if (!Initialize()) {
-    auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS);
+    return;
-    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);
+
+  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);
+    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.accel     = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
+    data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
+    data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET);
+    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;
+    data.mag       = MAG_MAT * (data.mag + MAG_OFFSET);
+    xSemaphoreTake(mutex_, pdMS_TO_TICKS(100));
+    last_data_ = data;
+    xSemaphoreGive(mutex_);
   }
 }
 
-void MPU::GetData(MpuData &data) {
+void MPU::TaskEntry(void *arg) {
-  esp_err_t ret;
+  MPU *mpu = (MPU *)arg;
-  xSemaphoreTake(i2c_mutex, portMAX_DELAY);
+  mpu->DoTask();
-  ret = mpu_->motion(&accel_, &gyro_);
+  vTaskDelete(NULL);
-  if (ret != ESP_OK) {
-    ESP_LOGE(TAG, "error reading MPU: %#x", ret);
-  }
-  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);
-  }
-  mpu_->setAuxI2CBypass(false);
-  xSemaphoreGive(i2c_mutex);
-  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.accel     = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
-  data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
-  data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET);
-  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;
-  data.mag   = MAG_MAT * (data.mag + MAG_OFFSET);
 }
 
 };  // namespace io

main/ugv_io_mpu.hh

@@ -42,13 +42,9 @@ struct Vec3f {
     return *this;
   }
 
-  float dot(const Vec3f& v) const {
+  float dot(const Vec3f& v) const { return x * v.x + y * v.y + z * v.z; }
-    return x * v.x + y * v.y + z * v.z;
-  }
 
-  float norm2() const {
+  float norm2() const { return x * x + y * y + z * z; }
-    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_)
+  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_) : rx(rx_), ry(ry_), rz(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 {
+  Mat3f transpose() const { return {xx, yx, zx, xy, yy, zy, xz, yz, zz}; }
-    return {xx, yx, zx, xy, yy, zy, xz, yz, zz};
-  }
 
   Mat3f operator*(const Mat3f& m) const {
     Mat3f mt = m.transpose();
@@ -106,9 +99,15 @@ class MPU {
   void GetData(MpuData& data);
 
  private:
-  mpud::mpu_bus_t* mpu_bus_;
+  mpud::mpu_bus_t*  mpu_bus_;
-  mpud::MPU*       mpu_;
+  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