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

try to fix comms issues

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-28 16:52:19 -07:00
parent e994d4f898
commit 703a0715df
6 changed files with 147 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

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

44
main/ugv.cc
View File

@ -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() {
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_);

2
main/ugv.hh
View File

@ -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
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 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;
if (!Initialize()) {
return;
}
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) {
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);
}
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);
void MPU::TaskEntry(void *arg) {
MPU *mpu = (MPU *)arg;
mpu->DoTask();
vTaskDelete(NULL);
}
}; // namespace io

27
main/ugv_io_mpu.hh
View File

@ -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

6
tools/config.yml
View File

@ -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