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
Split 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 @@ @@ -8,3 +8,5 @@
__pycache__
*.pyc
/venv
/tools/venv

44
main/ugv.cc
Unescape View File

@ -8,16 +8,16 @@ namespace ugv { @@ -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<float>(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<float>(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<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() { @@ -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() { @@ -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() { @@ -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() { @@ -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_);

2
main/ugv.hh
Unescape View File

@ -53,6 +53,8 @@ class UGV { @@ -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();

140
main/ugv_io_mpu.cc
Unescape View File

@ -22,12 +22,14 @@ static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); @@ -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) {} @@ -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() { @@ -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() { @@ -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() { @@ -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<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) |
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);
}
void MPU::TaskEntry(void *arg) {
MPU *mpu = (MPU *)arg;
mpu->DoTask();
vTaskDelete(NULL);
}
}; // namespace io

27
main/ugv_io_mpu.hh
Unescape View File

@ -42,13 +42,9 @@ struct Vec3f { @@ -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 { @@ -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 { @@ -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 { @@ -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

6
tools/config.yml
Unescape View File

@ -1,12 +1,12 @@ @@ -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

Write Preview
Loading…
Cancel
Save