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

Add somewhat shitty ahrs algorithm

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-27 17:04:48 -08:00
parent 67a1c98b34
commit 0a6305c52a
10 changed files with 445 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/CMakeLists.txt
View File

@ -11,6 +11,7 @@ set(COMPONENT_SRCS
"ugv_io_mpu.cc"
"u8g2_esp32_hal.c"
"Print.cpp"
"MadgwickAHRS.cpp"
"e32_driver.cc"
)
set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})

287
main/MadgwickAHRS.cpp Normal file
View File

@ -0,0 +1,287 @@
//=============================================================================================
// MadgwickAHRS.c
//=============================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// From the x-io website "Open-source resources available on this website are
// provided under the GNU General Public Licence unless an alternative licence
// is provided in source."
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
//
//=============================================================================================
//-------------------------------------------------------------------------------------------
// Header files
#include "MadgwickAHRS.h"
#include <math.h>
//-------------------------------------------------------------------------------------------
// Definitions
#define sampleFreqDef 512.0f // sample frequency in Hz
#define betaDef 0.1f // 2 * proportional gain
//============================================================================================
// Functions
//-------------------------------------------------------------------------------------------
// AHRS algorithm update
Madgwick::Madgwick() {
beta = betaDef;
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
invSampleFreq = 1.0f / sampleFreqDef;
anglesComputed = 0;
}
void Madgwick::update(float gx, float gy, float gz, float ax, float ay,
float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
// magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
updateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in
// accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
_2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 +
my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 +
_2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
_2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * q3 + _2bz * q1) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
_2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * q2 + _2bz * q0) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * q3 - _4bz * q1) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
(-_4bx * q2 - _2bz * q0) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * q1 + _2bz * q3) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * q0 - _4bz * q2) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
(-_4bx * q3 + _2bz * q1) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * q0 + _2bz * q2) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * invSampleFreq;
q1 += qDot2 * invSampleFreq;
q2 += qDot3 * invSampleFreq;
q3 += qDot4 * invSampleFreq;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
anglesComputed = 0;
}
//-------------------------------------------------------------------------------------------
// IMU algorithm update
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay,
float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
q3q3;
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in
// accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 +
_8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
_8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * invSampleFreq;
q1 += qDot2 * invSampleFreq;
q2 += qDot3 * invSampleFreq;
q3 += qDot4 * invSampleFreq;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
anglesComputed = 0;
}
//-------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float Madgwick::invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
}
//-------------------------------------------------------------------------------------------
void Madgwick::computeAngles() {
roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
anglesComputed = 1;
}

88
main/MadgwickAHRS.h Normal file
View File

@ -0,0 +1,88 @@
//=============================================================================================
// MadgwickAHRS.h
//=============================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// From the x-io website "Open-source resources available on this website are
// provided under the GNU General Public Licence unless an alternative licence
// is provided in source."
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
//
//=============================================================================================
#ifndef MadgwickAHRS_h
#define MadgwickAHRS_h
#include <math.h>
//--------------------------------------------------------------------------------------------
// Variable declaration
class Madgwick {
private:
static float invSqrt(float x);
float beta; // algorithm gain
float q0;
float q1;
float q2;
float q3; // quaternion of sensor frame relative to auxiliary frame
float invSampleFreq;
float roll;
float pitch;
float yaw;
char anglesComputed;
void computeAngles();
//-------------------------------------------------------------------------------------------
// Function declarations
public:
Madgwick(void);
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
void update(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz);
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
// float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 *
// q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
// q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
// - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
float getRoll() {
if (!anglesComputed) computeAngles();
return roll * 57.29578f;
}
float getPitch() {
if (!anglesComputed) computeAngles();
return pitch * 57.29578f;
}
float getYaw() {
if (!anglesComputed) computeAngles();
return yaw * 57.29578f + 180.0f;
}
float getRollRadians() {
if (!anglesComputed) computeAngles();
return roll;
}
float getPitchRadians() {
if (!anglesComputed) computeAngles();
return pitch;
}
float getYawRadians() {
if (!anglesComputed) computeAngles();
return yaw;
}
};
#endif

4
main/Print.cpp
View File

@ -66,7 +66,9 @@ size_t Print::printf(const char *format, ...) {
// size_t Print::print(const String &s) { return write(s.c_str(), s.length()); }
size_t Print::print(const std::string &s) { return write(s.c_str(), s.length()); }
size_t Print::print(const std::string &s) {
return write(s.c_str(), s.length());
}
size_t Print::print(const char str[]) { return write(str); }

2
main/e32_driver.cc
View File

@ -9,7 +9,7 @@ static constexpr size_t E32_BUF_SIZE = 1024;
static constexpr size_t E32_UART_RX_BUF_SIZE = 1024;
static constexpr size_t E32_UART_TX_BUF_SIZE = 1024;
static constexpr size_t PARAMS_LEN = 5;
static constexpr size_t PARAMS_LEN = 6;
static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0;
static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1};
static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2;

9
main/u8g2_esp32_hal.c
View File

@ -107,8 +107,8 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
#define TXBUF_SIZE 32
static uint8_t txbuf[TXBUF_SIZE];
static uint8_t *txbuf_ptr;
// ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg,
// arg_int, arg_ptr);
// ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg,
// arg_int, arg_ptr);
switch (msg) {
case U8X8_MSG_BYTE_SET_DC: {
if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) {
@ -175,9 +175,10 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
i2c_master_write(handle_i2c, txbuf, txbuf_ptr - txbuf, ACK_CHECK_EN));
ESP_ERROR_CHECK(i2c_master_stop(handle_i2c));
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c,
I2C_TIMEOUT_MS / portTICK_RATE_MS));
esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c,
I2C_TIMEOUT_MS / portTICK_RATE_MS);
xSemaphoreGive(i2c_mutex);
ESP_ERROR_CHECK(ret);
i2c_cmd_link_delete(handle_i2c);
break;
}

9
main/ugv_display.cc
View File

@ -11,10 +11,10 @@ static const char* TAG = "ugv_display";
using comms::CommsClass;
DisplayClass::DisplayClass(CommsClass* comms)
: comms_(comms) {}
DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {}
void DisplayClass::Init() {
ESP_LOGD(TAG, "Initializing u8g2 display");
// For Heltec ESP32 LoRa
// oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
// For wemos Lolin ESP32
@ -27,7 +27,9 @@ void DisplayClass::Init() {
8 * 1024, this, 1, &this->task_handle_);
if (xRet != pdTRUE) {
ESP_LOGE(TAG, "error starting display thread");
return;
}
ESP_LOGD(TAG, "Started display thread");
}
void DisplayClass::Run() {
@ -78,7 +80,8 @@ void DisplayClass::Run() {
oled->print("no pkt rx");
}
// oled->setCursor(4, 6 * 8);
// oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor);
// oled->printf("motors: (%f, %f)", outputs.left_motor,
// outputs.right_motor);
} while (oled->nextPage());
vTaskDelay(pdMS_TO_TICKS(1000));
}

88
main/ugv_io_mpu.cc
View File

@ -3,8 +3,8 @@
#include <driver/uart.h>
#include <esp_log.h>
#include "i2c_mutex.h"
#include "MPU.hpp"
#include "i2c_mutex.h"
#include "mpu/math.hpp"
namespace ugv {
@ -23,18 +23,15 @@ Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {}
Vec3f::Vec3f(const mpud::float_axes_t &axes)
: x(axes.x), y(axes.y), z(axes.z) {}
MPU::MPU() : mpu_(nullptr), mpu_data_() {}
MPU::MPU() : mpu_(nullptr) {}
MPU::~MPU() { delete mpu_; }
void MPU::Init() {
mutex_ = xSemaphoreCreateMutex();
mpu_data_ = MpuData{};
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
mpu_bus_ = &i2c0;
// This is shared with the oled, so just use those pins
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
mpu_bus_->begin(MPU_SDA, MPU_SCL);
// mpu_bus_->begin(MPU_SDA, MPU_SCL);
mpu_ = new mpud::MPU(*mpu_bus_);
esp_err_t ret;
@ -60,56 +57,45 @@ void MPU::Init() {
mpu_->compassWriteByte(0x0A, 0x12);
xSemaphoreGive(i2c_mutex);
BaseType_t xRet =
xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_);
if (xRet != pdTRUE) {
ESP_LOGE(TAG, "error creating MPU task");
return;
}
ESP_LOGI(TAG, "MPU initialized and task started");
ESP_LOGI(TAG, "MPU initialized");
}
void MPU::GetData(MpuData &data) {
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data = MpuData(mpu_data_);
xSemaphoreGive(mutex_);
}
void MPU::DoTask() {
esp_err_t ret;
while (true) {
vTaskDelay(pdMS_TO_TICKS(50));
uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ret = mpu_->motion(&accel_, &gyro_);
mpu_->compassReadByte(0x03, &mxl);
mpu_->compassReadByte(0x04, &mxh);
mpu_->compassReadByte(0x05, &myl);
mpu_->compassReadByte(0x06, &myh);
mpu_->compassReadByte(0x07, &mzl);
mpu_->compassReadByte(0x08, &mzh);
mpu_->compassReadByte(0x09, &n);
xSemaphoreGive(i2c_mutex);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU");
continue;
}
int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
mpu_data_.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
mpu_data_.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
mpu_data_.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
mpu_data_.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
xSemaphoreGive(mutex_);
// uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ret = mpu_->motion(&accel_, &gyro_);
uint8_t compass_data[7];
mpu_->setAuxI2CBypass(true);
mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
mpu_->setAuxI2CBypass(false);
// mpu_->compassReadByte(0x03, &mxl);
// mpu_->compassReadByte(0x04, &mxh);
// mpu_->compassReadByte(0x05, &myl);
// mpu_->compassReadByte(0x06, &myh);
// mpu_->compassReadByte(0x07, &mzl);
// mpu_->compassReadByte(0x08, &mzh);
// mpu_->compassReadByte(0x09, &n);
xSemaphoreGive(i2c_mutex);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU");
}
vTaskDelete(NULL);
// int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
// int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
// int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
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.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
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;
}
void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); }
}; // namespace io
}; // namespace ugv

9
main/ugv_io_mpu.hh
View File

@ -1,6 +1,5 @@
#pragma once
#include <freertos/FreeRTOS.h>
#include <stdint.h>
#include <MPU.hpp>
@ -39,14 +38,6 @@ class MPU {
mpud::mpu_bus_t *mpu_bus_;
mpud::MPU * mpu_;
mpud::raw_axes_t accel_, gyro_, mag_;
TaskHandle_t task_;
SemaphoreHandle_t mutex_;
MpuData mpu_data_;
void DoTask();
static void MPU_Task(void *arg);
};
} // namespace io

22
main/ugv_main.cc
View File

@ -1,9 +1,10 @@
#include <esp_log.h>
#include <esp_timer.h>
#include "MadgwickAHRS.h"
#include "i2c_mutex.h"
#include "ugv_comms.hh"
#include "ugv_display.hh"
#include "ugv_io.hh"
#include "i2c_mutex.h"
#include <math.h>
@ -15,7 +16,7 @@ using ugv::io::IOClass;
static const char *TAG = "ugv_main";
extern "C" {
SemaphoreHandle_t i2c_mutex;
SemaphoreHandle_t i2c_mutex;
}
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
@ -31,7 +32,8 @@ struct State {
esp_timer_handle_t timer_handle;
io::Inputs inputs;
io::Outputs outputs;
int64_t last_print;
int64_t last_print;
Madgwick ahrs_;
State() {
comms = new CommsClass();
@ -43,9 +45,12 @@ struct State {
esp_timer_init();
i2c_mutex = xSemaphoreCreateMutex();
ahrs_.begin(1000000.f /
static_cast<float>(LOOP_PERIOD_US)); // rough sample frequency
comms->Init();
io->Init();
display->Init();
io->Init();
esp_timer_create_args_t timer_args;
timer_args.callback = OnTimeout;
@ -65,13 +70,20 @@ struct State {
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
io->WriteOutputs(outputs);
if (time_us >= last_print + 1 * 1000 * 1000) { // 1s
{
io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel,
&m = inputs.mpu.mag;
ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
}
if (time_us >= last_print + 500 * 1000) { // 1s
ESP_LOGI(TAG,
"inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z,
inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y,
inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y,
inputs.mpu.mag.z);
ESP_LOGI(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll());
last_print = time_us;
}
}