From 703a0715dfeb44d1a24761ace0eb370e630ba626 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Tue, 28 May 2019 16:52:19 -0700 Subject: [PATCH] try to fix comms issues --- .gitignore | 4 +- main/ugv.cc | 44 ++++++++++---- main/ugv.hh | 2 + main/ugv_io_mpu.cc | 140 +++++++++++++++++++++++++++++---------------- main/ugv_io_mpu.hh | 27 +++++---- tools/config.yml | 6 +- 6 files changed, 147 insertions(+), 76 deletions(-) diff --git a/.gitignore b/.gitignore index a3c4f94..5b368f0 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,6 @@ /cmake-build* __pycache__ -*.pyc \ No newline at end of file +*.pyc +/venv +/tools/venv diff --git a/main/ugv.cc b/main/ugv.cc index 83ce731..e347150 100644 --- a/main/ugv.cc +++ b/main/ugv.cc @@ -8,16 +8,16 @@ namespace ugv { static const char *TAG = "ugv_main"; -constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; -constexpr float LOOP_HZ = 1000000.f / static_cast(LOOP_PERIOD_US); -constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ; +constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; +constexpr float LOOP_HZ = 1000000.f / static_cast(LOOP_PERIOD_US); +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(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; - last_mpu_ = inputs_.mpu; + io::Vec3f d_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate; + 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; - last_noise_ = time_us; + gyro_noise_accum_ = 0; + 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_); diff --git a/main/ugv.hh b/main/ugv.hh index 579df50..ed4376f 100644 --- a/main/ugv.hh +++ b/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(); diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index d5daac3..959bf5e 100644 --- a/main/ugv_io_mpu.cc +++ b/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 = {-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 Vec3f GYRO_OFFSET = {-4.33655, -2.76826, -0.908427}; +// 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 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() { - 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; -void MPU::GetData(MpuData &data) { - esp_err_t ret; - xSemaphoreTake(i2c_mutex, portMAX_DELAY); - ret = mpu_->motion(&accel_, &gyro_); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "error reading MPU: %#x", ret); + if (!Initialize()) { + return; } - 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); + + 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(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.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_); } - mpu_->setAuxI2CBypass(false); - xSemaphoreGive(i2c_mutex); - 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.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); +} + +void MPU::TaskEntry(void *arg) { + MPU *mpu = (MPU *)arg; + mpu->DoTask(); + vTaskDelete(NULL); } }; // namespace io diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh index 05fd94c..c0a66d2 100644 --- a/main/ugv_io_mpu.hh +++ b/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(); @@ -106,9 +99,15 @@ class MPU { void GetData(MpuData& data); private: - mpud::mpu_bus_t* mpu_bus_; - mpud::MPU* mpu_; - mpud::raw_axes_t accel_, gyro_, mag_; + mpud::mpu_bus_t* mpu_bus_; + mpud::MPU* mpu_; + TaskHandle_t task_; + SemaphoreHandle_t mutex_; + MpuData last_data_; + + void DoTask(); + bool Initialize(); + static void TaskEntry(void* arg); }; } // namespace io diff --git a/tools/config.yml b/tools/config.yml index 4ee890f..4417b87 100644 --- a/tools/config.yml +++ b/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