amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

try to fix comms issues

master
Alex Mikhalev 6 years ago
parent
e994d4f898
commit
703a0715df
6 changed files with 147 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .gitignore
  2. 44
      main/ugv.cc
  3. 2
      main/ugv.hh
  4. 140
      main/ugv_io_mpu.cc
  5. 27
      main/ugv_io_mpu.hh
  6. 6
      tools/config.yml

2
.gitignore vendored
Unescape View File

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

44
main/ugv.cc
Unescape View File

@ -8,16 +8,16 @@ namespace ugv {
static const char *TAG = "ugv_main"; 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 TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000);
constexpr uint64_t NOISE_PERIOD_US = 2000e3; constexpr uint64_t NOISE_PERIOD_US = 2000e3;
constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f; constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f;
constexpr float ACCEL_NOISE_THRESHOLD = 0.001; 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, void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) { const io::GpsData & gps_data) {
@ -87,6 +87,8 @@ void UGV::Init() {
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US); esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
last_print_ = 0; last_print_ = 0;
last_noise_ = 0; last_noise_ = 0;
last_left_ = 0;
last_right_ = 0;
} }
void UGV::UpdateAHRS() { void UGV::UpdateAHRS() {
@ -106,7 +108,7 @@ void UGV::UpdateAHRS() {
void UGV::DoDebugPrint() { void UGV::DoDebugPrint() {
auto &mpu = inputs_.mpu; 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.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); 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_); 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); 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_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; accel_noise_accum_ += d_accel.norm2() * LOOP_PERIOD_S;
gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S; gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S;
if (time_us >= last_noise_ + NOISE_PERIOD_US) { if (time_us >= last_noise_ + NOISE_PERIOD_US) {
accel_noise_ = accel_noise_accum_ / NOISE_PERIOD_S; 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) && is_still_ = (accel_noise_ < ACCEL_NOISE_THRESHOLD) &&
(gyro_noise_ < GYRO_NOISE_THRESHOLD); (gyro_noise_ < GYRO_NOISE_THRESHOLD);
ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f", ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f",
is_still_ ? "still" : "moving", accel_noise_, gyro_noise_); is_still_ ? "still" : "moving", accel_noise_, gyro_noise_);
accel_noise_accum_ = 0; accel_noise_accum_ = 0;
gyro_noise_accum_ = 0; gyro_noise_accum_ = 0;
last_noise_ = time_us; last_noise_ = time_us;
} }
ReadComms(); ReadComms();
@ -286,7 +288,29 @@ void UGV::OnTick() {
outputs_.right_motor = drive_power + angle_pwr; 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_); io->WriteOutputs(outputs_);
last_left_ = outputs_.left_motor;
last_right_ = outputs_.right_motor;
if (current_state_ != next_state_) { if (current_state_ != next_state_) {
ESP_LOGI(TAG, "switching state to %d", next_state_); ESP_LOGI(TAG, "switching state to %d", next_state_);

2
main/ugv.hh
Unescape View File

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

140
main/ugv_io_mpu.cc
Unescape View File

@ -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 Vec3f ACCEL_OFFSET = {0., 0., 0.};
static const Mat3f ACCEL_MAT = {-1., 0., 0., 0., -1., 0., 0., 0., -1.}; 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 Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const char *TAG = "ugv_io_mpu"; static const char *TAG = "ugv_io_mpu";
@ -42,6 +44,37 @@ MPU::MPU() : mpu_(nullptr) {}
MPU::~MPU() { delete mpu_; } MPU::~MPU() { delete mpu_; }
void MPU::Init() { 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); xSemaphoreTake(i2c_mutex, portMAX_DELAY);
mpu_bus_ = &i2c0; mpu_bus_ = &i2c0;
// This is shared with the oled, so just use those pins // This is shared with the oled, so just use those pins
@ -50,7 +83,7 @@ void MPU::Init() {
mpu_ = new mpud::MPU(*mpu_bus_); mpu_ = new mpud::MPU(*mpu_bus_);
esp_err_t ret; esp_err_t ret;
int tries = 0; int tries = 0;
for (; tries < 5; ++tries) { for (; tries < 5; ++tries) {
mpu_->getInterruptStatus(); mpu_->getInterruptStatus();
ret = mpu_->testConnection(); ret = mpu_->testConnection();
@ -73,7 +106,7 @@ void MPU::Init() {
break; break;
} }
if (tries == 5) { if (tries == 5) {
return; return false;
} }
// Calibrate(); // Calibrate();
mpu_->setAccelFullScale(MPU_ACCEL_FS); mpu_->setAccelFullScale(MPU_ACCEL_FS);
@ -83,51 +116,62 @@ void MPU::Init() {
xSemaphoreGive(i2c_mutex); xSemaphoreGive(i2c_mutex);
ESP_LOGI(TAG, "MPU initialized"); 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);
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) { if (!Initialize()) {
esp_err_t ret; return;
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ret = mpu_->motion(&accel_, &gyro_);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU: %#x", ret);
} }
uint8_t compass_data[7];
mpu_->setAuxI2CBypass(true); while (true) {
ret = mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data); xSemaphoreTake(i2c_mutex, portMAX_DELAY);
if (ret != ESP_OK) { ret = mpu_->motion(&accel_, &gyro_);
ESP_LOGE(TAG, "error reading MPU compass: %#x", ret); 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_);
} }
mpu_->setAuxI2CBypass(false); }
xSemaphoreGive(i2c_mutex);
int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) | void MPU::TaskEntry(void *arg) {
static_cast<int16_t>(compass_data[0]); MPU *mpu = (MPU *)arg;
int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) | mpu->DoTask();
static_cast<int16_t>(compass_data[2]); vTaskDelete(NULL);
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 }; // namespace io

27
main/ugv_io_mpu.hh
Unescape View File

@ -42,13 +42,9 @@ struct Vec3f {
return *this; 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 { struct Mat3f {
@ -65,8 +61,7 @@ struct Mat3f {
float zx, zy, zz; 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, Mat3f(float xx, float xy, float xz, float yx, float yy, float yz, float zx,
float zy, float zz) float zy, float zz)
@ -76,9 +71,7 @@ struct Mat3f {
return {rx.dot(v), ry.dot(v), rz.dot(v)}; 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 operator*(const Mat3f& m) const {
Mat3f mt = m.transpose(); Mat3f mt = m.transpose();
@ -106,9 +99,15 @@ class MPU {
void GetData(MpuData& data); void GetData(MpuData& data);
private: 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 } // namespace io

6
tools/config.yml
Unescape View File

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

Write Preview
Loading…
Cancel
Save