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

clang-format without silly alignment

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-28 17:11:53 -07:00
parent f8b7cc5e8b
commit 6965f03068
23 changed files with 354 additions and 354 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.clang-format
View File

@ -3,8 +3,8 @@ Language: Cpp
# BasedOnStyle: Google # BasedOnStyle: Google
AccessModifierOffset: -1 AccessModifierOffset: -1
AlignAfterOpenBracket: Align AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: true AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: true AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left AlignEscapedNewlines: Left
AlignOperands: true AlignOperands: true
AlignTrailingComments: true AlignTrailingComments: true

58
main/MadgwickAHRS.cpp
View File

@ -36,12 +36,12 @@
// AHRS algorithm update // AHRS algorithm update
Madgwick::Madgwick() { Madgwick::Madgwick() {
beta = betaDef; beta = betaDef;
q0 = 1.0f; q0 = 1.0f;
q1 = 0.0f; q1 = 0.0f;
q2 = 0.0f; q2 = 0.0f;
q3 = 0.0f; q3 = 0.0f;
invSampleFreq = 1.0f / sampleFreqDef; invSampleFreq = 1.0f / sampleFreqDef;
anglesComputed = 0; anglesComputed = 0;
} }
@ -93,22 +93,22 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay,
_2q0my = 2.0f * q0 * my; _2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz; _2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx; _2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0; _2q0 = 2.0f * q0;
_2q1 = 2.0f * q1; _2q1 = 2.0f * q1;
_2q2 = 2.0f * q2; _2q2 = 2.0f * q2;
_2q3 = 2.0f * q3; _2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2; _2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3; _2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0; q0q0 = q0 * q0;
q0q1 = q0 * q1; q0q1 = q0 * q1;
q0q2 = q0 * q2; q0q2 = q0 * q2;
q0q3 = q0 * q3; q0q3 = q0 * q3;
q1q1 = q1 * q1; q1q1 = q1 * q1;
q1q2 = q1 * q2; q1q2 = q1 * q2;
q1q3 = q1 * q3; q1q3 = q1 * q3;
q2q2 = q2 * q2; q2q2 = q2 * q2;
q2q3 = q2 * q3; q2q3 = q2 * q3;
q3q3 = q3 * q3; q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field // Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
@ -233,7 +233,7 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay,
_8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
_8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
s3 * s3); // normalise step magnitude s3 * s3); // normalise step magnitude
s0 *= recipNorm; s0 *= recipNorm;
@ -269,23 +269,23 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay,
float Madgwick::invSqrt(float x) { float Madgwick::invSqrt(float x) {
float halfx = 0.5f * x; float halfx = 0.5f * x;
float y = x; float y = x;
// long i = *(long *)&y; // long i = *(long *)&y;
long i; long i;
memcpy(&i, &y, sizeof(float)); memcpy(&i, &y, sizeof(float));
i = 0x5f3759df - (i >> 1); i = 0x5f3759df - (i >> 1);
// y = *(float *)&i; // y = *(float *)&i;
memcpy(&y, &i, sizeof(float)); memcpy(&y, &i, sizeof(float));
y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y));
return y; return y;
} }
//------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------
void Madgwick::computeAngles() { void Madgwick::computeAngles() {
roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
anglesComputed = 1; anglesComputed = 1;
} }

2
main/MadgwickAHRS.h
View File

@ -34,7 +34,7 @@ class Madgwick {
float roll; float roll;
float pitch; float pitch;
float yaw; float yaw;
char anglesComputed; char anglesComputed;
void computeAngles(); void computeAngles();

14
main/Print.cpp
View File

@ -41,8 +41,8 @@ size_t Print::write(const uint8_t *buffer, size_t size) {
} }
size_t Print::printf(const char *format, ...) { size_t Print::printf(const char *format, ...) {
char loc_buf[64]; char loc_buf[64];
char * temp = loc_buf; char *temp = loc_buf;
va_list arg; va_list arg;
va_list copy; va_list copy;
va_start(arg, format); va_start(arg, format);
@ -90,7 +90,7 @@ size_t Print::print(long n, int base) {
} else if (base == 10) { } else if (base == 10) {
if (n < 0) { if (n < 0) {
int t = print('-'); int t = print('-');
n = -n; n = -n;
return printNumber(n, 10) + t; return printNumber(n, 10) + t;
} }
return printNumber(n, 10); return printNumber(n, 10);
@ -116,7 +116,7 @@ size_t Print::print(struct tm *timeinfo, const char *format) {
if (!f) { if (!f) {
f = "%c"; f = "%c";
} }
char buf[64]; char buf[64];
size_t written = strftime(buf, 64, f, timeinfo); size_t written = strftime(buf, 64, f, timeinfo);
print(buf); print(buf);
return written; return written;
@ -199,7 +199,7 @@ size_t Print::println(struct tm *timeinfo, const char *format) {
// Private Methods ///////////////////////////////////////////////////////////// // Private Methods /////////////////////////////////////////////////////////////
size_t Print::printNumber(unsigned long n, uint8_t base) { size_t Print::printNumber(unsigned long n, uint8_t base) {
char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte. char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
char *str = &buf[sizeof(buf) - 1]; char *str = &buf[sizeof(buf) - 1];
*str = '\0'; *str = '\0';
@ -250,8 +250,8 @@ size_t Print::printFloat(double number, uint8_t digits) {
number += rounding; number += rounding;
// Extract the integer part of the number and print it // Extract the integer part of the number and print it
unsigned long int_part = (unsigned long)number; unsigned long int_part = (unsigned long)number;
double remainder = number - (double)int_part; double remainder = number - (double)int_part;
n += print(int_part); n += print(int_part);
// Print the decimal point, but only if there are digits beyond // Print the decimal point, but only if there are digits beyond

8
main/Print.h
View File

@ -31,7 +31,7 @@
class Print { class Print {
private: private:
int write_error; int write_error;
size_t printNumber(unsigned long, uint8_t); size_t printNumber(unsigned long, uint8_t);
size_t printFloat(double, uint8_t); size_t printFloat(double, uint8_t);
@ -41,18 +41,18 @@ class Print {
public: public:
Print() : write_error(0) {} Print() : write_error(0) {}
virtual ~Print() {} virtual ~Print() {}
int getWriteError() { return write_error; } int getWriteError() { return write_error; }
void clearWriteError() { setWriteError(0); } void clearWriteError() { setWriteError(0); }
virtual size_t write(uint8_t) = 0; virtual size_t write(uint8_t) = 0;
size_t write(const char *str) { size_t write(const char *str) {
if (str == NULL) { if (str == NULL) {
return 0; return 0;
} }
return write((const uint8_t *)str, strlen(str)); return write((const uint8_t *)str, strlen(str));
} }
virtual size_t write(const uint8_t *buffer, size_t size); virtual size_t write(const uint8_t *buffer, size_t size);
size_t write(const char *buffer, size_t size) { size_t write(const char *buffer, size_t size) {
return write((const uint8_t *)buffer, size); return write((const uint8_t *)buffer, size);
} }

32
main/U8g2lib.hh
View File

@ -6,7 +6,7 @@
class U8G2 : public Print { class U8G2 : public Print {
protected: protected:
u8g2_t u8g2; u8g2_t u8g2;
u8x8_char_cb cpp_next_cb; /* the cpp interface has its own decoding function u8x8_char_cb cpp_next_cb; /* the cpp interface has its own decoding function
for the Arduino print command */ for the Arduino print command */
public: public:
@ -20,7 +20,7 @@ class U8G2 : public Print {
u8g2_t *getU8g2(void) { return &u8g2; } u8g2_t *getU8g2(void) { return &u8g2; }
uint32_t getBusClock(void) { return u8g2_GetU8x8(&u8g2)->bus_clock; } uint32_t getBusClock(void) { return u8g2_GetU8x8(&u8g2)->bus_clock; }
void setBusClock(uint32_t clock_speed) { void setBusClock(uint32_t clock_speed) {
u8g2_GetU8x8(&u8g2)->bus_clock = clock_speed; u8g2_GetU8x8(&u8g2)->bus_clock = clock_speed;
} }
@ -32,13 +32,13 @@ class U8G2 : public Print {
/* u8x8 interface */ /* u8x8 interface */
uint8_t getCols(void) { return u8x8_GetCols(u8g2_GetU8x8(&u8g2)); } uint8_t getCols(void) { return u8x8_GetCols(u8g2_GetU8x8(&u8g2)); }
uint8_t getRows(void) { return u8x8_GetRows(u8g2_GetU8x8(&u8g2)); } uint8_t getRows(void) { return u8x8_GetRows(u8g2_GetU8x8(&u8g2)); }
void drawTile(uint8_t x, uint8_t y, uint8_t cnt, uint8_t *tile_ptr) { void drawTile(uint8_t x, uint8_t y, uint8_t cnt, uint8_t *tile_ptr) {
u8x8_DrawTile(u8g2_GetU8x8(&u8g2), x, y, cnt, tile_ptr); u8x8_DrawTile(u8g2_GetU8x8(&u8g2), x, y, cnt, tile_ptr);
} }
#ifdef U8X8_WITH_USER_PTR #ifdef U8X8_WITH_USER_PTR
void *getUserPtr() { return u8g2_GetUserPtr(&u8g2); } void *getUserPtr() { return u8g2_GetUserPtr(&u8g2); }
void setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); } void setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); }
#endif #endif
#ifdef U8X8_USE_PINS #ifdef U8X8_USE_PINS
@ -118,13 +118,13 @@ class U8G2 : public Print {
void sendBuffer(void) { u8g2_SendBuffer(&u8g2); } void sendBuffer(void) { u8g2_SendBuffer(&u8g2); }
void clearBuffer(void) { u8g2_ClearBuffer(&u8g2); } void clearBuffer(void) { u8g2_ClearBuffer(&u8g2); }
void firstPage(void) { u8g2_FirstPage(&u8g2); } void firstPage(void) { u8g2_FirstPage(&u8g2); }
uint8_t nextPage(void) { return u8g2_NextPage(&u8g2); } uint8_t nextPage(void) { return u8g2_NextPage(&u8g2); }
uint8_t *getBufferPtr(void) { return u8g2_GetBufferPtr(&u8g2); } uint8_t *getBufferPtr(void) { return u8g2_GetBufferPtr(&u8g2); }
uint8_t getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); } uint8_t getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); }
uint8_t getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); } uint8_t getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); }
uint8_t getPageCurrTileRow(void) { uint8_t getPageCurrTileRow(void) {
return u8g2_GetBufferCurrTileRow(&u8g2); return u8g2_GetBufferCurrTileRow(&u8g2);
} // obsolete } // obsolete
void setPageCurrTileRow(uint8_t row) { void setPageCurrTileRow(uint8_t row) {
@ -339,11 +339,11 @@ class U8G2 : public Print {
void setColorIndex(uint8_t color_index) { void setColorIndex(uint8_t color_index) {
u8g2_SetDrawColor(&u8g2, color_index); u8g2_SetDrawColor(&u8g2, color_index);
} }
uint8_t getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); } uint8_t getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); }
int8_t getFontAscent(void) { return u8g2_GetAscent(&u8g2); } int8_t getFontAscent(void) { return u8g2_GetAscent(&u8g2); }
int8_t getFontDescent(void) { return u8g2_GetDescent(&u8g2); } int8_t getFontDescent(void) { return u8g2_GetDescent(&u8g2); }
int8_t getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); } int8_t getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); }
int8_t getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); } int8_t getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); }
u8g2_uint_t getHeight() { return u8g2_GetDisplayHeight(&u8g2); } u8g2_uint_t getHeight() { return u8g2_GetDisplayHeight(&u8g2); }
u8g2_uint_t getWidth() { return u8g2_GetDisplayWidth(&u8g2); } u8g2_uint_t getWidth() { return u8g2_GetDisplayWidth(&u8g2); }
}; };
@ -358,9 +358,9 @@ void u8x8_SetPin_HW_I2C(u8x8_t *u8x8, uint8_t reset, uint8_t clock,
class U8G2_SSD1306_128X64_NONAME_F_HW_I2C : public U8G2 { class U8G2_SSD1306_128X64_NONAME_F_HW_I2C : public U8G2 {
public: public:
U8G2_SSD1306_128X64_NONAME_F_HW_I2C(const u8g2_cb_t *rotation, U8G2_SSD1306_128X64_NONAME_F_HW_I2C(const u8g2_cb_t *rotation,
uint8_t reset = U8X8_PIN_NONE, uint8_t reset = U8X8_PIN_NONE,
uint8_t clock = U8X8_PIN_NONE, uint8_t clock = U8X8_PIN_NONE,
uint8_t data = U8X8_PIN_NONE) uint8_t data = U8X8_PIN_NONE)
: U8G2() { : U8G2() {
u8g2_Setup_ssd1306_i2c_128x64_noname_f( u8g2_Setup_ssd1306_i2c_128x64_noname_f(
&u8g2, rotation, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb); &u8g2, rotation, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb);

74
main/e32_driver.cc
View File

@ -5,40 +5,40 @@
namespace ugv { namespace ugv {
namespace e32 { namespace e32 {
static constexpr size_t E32_BUF_SIZE = 1024; static constexpr size_t E32_BUF_SIZE = 1024;
static constexpr size_t E32_UART_RX_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 E32_UART_TX_BUF_SIZE = 1024;
static constexpr size_t PARAMS_LEN = 6; static constexpr size_t PARAMS_LEN = 6;
static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0; static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0;
static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1}; static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1};
static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2; static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2;
static const uint8_t CMD_READ_VERSION[] = {0xC3, 0xC3, 0xC3}; static const uint8_t CMD_READ_VERSION[] = {0xC3, 0xC3, 0xC3};
static const uint8_t CMD_RESET[] = {0xC4, 0xC4, 0xC4}; static const uint8_t CMD_RESET[] = {0xC4, 0xC4, 0xC4};
static const char* TAG = "e32_driver"; static const char* TAG = "e32_driver";
Config::Config() { Config::Config() {
uart_port = UART_NUM_1; uart_port = UART_NUM_1;
uart_parity = UART_PARITY_DISABLE; uart_parity = UART_PARITY_DISABLE;
uart_tx_pin = UART_PIN_NO_CHANGE; uart_tx_pin = UART_PIN_NO_CHANGE;
uart_rx_pin = UART_PIN_NO_CHANGE; uart_rx_pin = UART_PIN_NO_CHANGE;
uart_baud = 9600; uart_baud = 9600;
} }
Params::Params() { Params::Params() {
// These are defaults for the 433T30D // These are defaults for the 433T30D
save_params = true; save_params = true;
address = 0x0000; address = 0x0000;
uart_partity = UART_PARITY_DISABLE; uart_partity = UART_PARITY_DISABLE;
uart_baud = 9600; // bps uart_baud = 9600; // bps
air_data_rate = 2400; // bps air_data_rate = 2400; // bps
comm_channel = 0x17; comm_channel = 0x17;
tx_mode = TxTransparent; tx_mode = TxTransparent;
io_mode = IoPushPull; io_mode = IoPushPull;
wake_up_time = 250; // ms wake_up_time = 250; // ms
fec_enabled = true; fec_enabled = true;
tx_power = 30; tx_power = 30;
} }
E32_Driver::E32_Driver() : initialized_(false), config_(), params_() {} E32_Driver::E32_Driver() : initialized_(false), config_(), params_() {}
@ -48,13 +48,13 @@ E32_Driver::~E32_Driver() { Free(); }
esp_err_t E32_Driver::Init(Config config) { esp_err_t E32_Driver::Init(Config config) {
config_ = config; config_ = config;
uart_config_t uart_config; uart_config_t uart_config;
uart_config.baud_rate = config_.uart_baud; uart_config.baud_rate = config_.uart_baud;
uart_config.data_bits = UART_DATA_8_BITS; uart_config.data_bits = UART_DATA_8_BITS;
uart_config.parity = config_.uart_parity; uart_config.parity = config_.uart_parity;
uart_config.stop_bits = UART_STOP_BITS_1; uart_config.stop_bits = UART_STOP_BITS_1;
uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
uart_config.rx_flow_ctrl_thresh = 122; uart_config.rx_flow_ctrl_thresh = 122;
uart_config.use_ref_tick = false; uart_config.use_ref_tick = false;
esp_err_t ret; esp_err_t ret;
ret = uart_param_config(config_.uart_port, &uart_config); ret = uart_param_config(config_.uart_port, &uart_config);
@ -92,7 +92,7 @@ error:
esp_err_t E32_Driver::Free() { esp_err_t E32_Driver::Free() {
esp_err_t ret; esp_err_t ret;
if (initialized_) { if (initialized_) {
ret = uart_driver_delete(config_.uart_port); ret = uart_driver_delete(config_.uart_port);
initialized_ = false; initialized_ = false;
} else { } else {
ret = ESP_ERR_INVALID_STATE; ret = ESP_ERR_INVALID_STATE;
@ -159,10 +159,10 @@ esp_err_t E32_Driver::ReadParams(Params& params) {
params.comm_channel = param_data[4]; params.comm_channel = param_data[4];
params.tx_mode = (TxMode)(param_data[5] & (1 << 7)); params.tx_mode = (TxMode)(param_data[5] & (1 << 7));
params.io_mode = (IoMode)(param_data[5] & (1 << 6)); params.io_mode = (IoMode)(param_data[5] & (1 << 6));
params.wake_up_time = 250 * (((param_data[5] >> 3) & 0b111) + 1); params.wake_up_time = 250 * (((param_data[5] >> 3) & 0b111) + 1);
params.fec_enabled = (param_data[5] & (1 << 2)) != 0; params.fec_enabled = (param_data[5] & (1 << 2)) != 0;
// assume it is a 30dbm module // assume it is a 30dbm module
switch (param_data[5] & 0b11) { switch (param_data[5] & 0b11) {
@ -178,7 +178,7 @@ esp_err_t E32_Driver::ReadParams(Params& params) {
} }
esp_err_t E32_Driver::WriteParams(const Params& params) { esp_err_t E32_Driver::WriteParams(const Params& params) {
esp_err_t ret; esp_err_t ret;
uint8_t param_data[PARAMS_LEN]; uint8_t param_data[PARAMS_LEN];
param_data[0] = param_data[0] =
params.save_params ? CMD_WRITE_PARAMS_SAVE : CMD_WRITE_PARAMS_NO_SAVE; params.save_params ? CMD_WRITE_PARAMS_SAVE : CMD_WRITE_PARAMS_NO_SAVE;
@ -306,10 +306,10 @@ int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) {
int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) { int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) {
// TODO: more proper way to read a line // TODO: more proper way to read a line
uint8_t byte; uint8_t byte;
TickType_t start_tick = xTaskGetTickCount(); TickType_t start_tick = xTaskGetTickCount();
TickType_t current_tick = start_tick; TickType_t current_tick = start_tick;
int read, total_read = 0; int read, total_read = 0;
while (total_read < data_size) { while (total_read < data_size) {
read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick));
if (read < 1) { if (read < 1) {
@ -326,16 +326,16 @@ int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) {
int E32_Driver::ReadLn(std::string& data, TickType_t ticks_to_wait) { int E32_Driver::ReadLn(std::string& data, TickType_t ticks_to_wait) {
// TODO: more proper way to read a line // TODO: more proper way to read a line
data.clear(); data.clear();
uint8_t byte; uint8_t byte;
TickType_t start_tick = xTaskGetTickCount(); TickType_t start_tick = xTaskGetTickCount();
TickType_t current_tick = start_tick; TickType_t current_tick = start_tick;
int read, total_read = 0; int read, total_read = 0;
while (true) { while (true) {
read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick));
if (read < 1) { if (read < 1) {
return read; return read;
} }
ticks_to_wait += pdMS_TO_TICKS(10); // give it a bit more time... ticks_to_wait += pdMS_TO_TICKS(10); // give it a bit more time...
if (byte == '\n') break; if (byte == '\n') break;
data += (char)byte; data += (char)byte;
total_read += read; total_read += read;

36
main/e32_driver.hh
View File

@ -9,16 +9,16 @@ namespace e32 {
enum TxMode { enum TxMode {
TxTransparent = 0, TxTransparent = 0,
TxFixed = 1, TxFixed = 1,
}; };
enum IoMode { enum IoMode {
IoOpenCollector = 0, IoOpenCollector = 0,
IoPushPull = 1, IoPushPull = 1,
}; };
typedef uint16_t Address; typedef uint16_t Address;
typedef uint8_t Channel; typedef uint8_t Channel;
constexpr Address BroadcastAddress = 0xFFFF; constexpr Address BroadcastAddress = 0xFFFF;
@ -26,28 +26,28 @@ struct Config {
public: public:
Config(); Config();
uart_port_t uart_port; uart_port_t uart_port;
uart_parity_t uart_parity; uart_parity_t uart_parity;
int uart_tx_pin; int uart_tx_pin;
int uart_rx_pin; int uart_rx_pin;
int uart_baud; int uart_baud;
}; };
struct Params { struct Params {
public: public:
Params(); Params();
bool save_params; bool save_params;
Address address; Address address;
uart_parity_t uart_partity; uart_parity_t uart_partity;
int uart_baud; // bps int uart_baud; // bps
int air_data_rate; // bps int air_data_rate; // bps
Channel comm_channel; Channel comm_channel;
TxMode tx_mode; TxMode tx_mode;
IoMode io_mode; IoMode io_mode;
uint16_t wake_up_time; // ms uint16_t wake_up_time; // ms
bool fec_enabled; bool fec_enabled;
uint16_t tx_power; // dBm uint16_t tx_power; // dBm
}; };
class E32_Driver { class E32_Driver {
@ -80,7 +80,7 @@ class E32_Driver {
void Flush(); void Flush();
private: private:
bool initialized_; bool initialized_;
Config config_; Config config_;
Params params_; Params params_;

24
main/lat_long.cc
View File

@ -1,30 +1,30 @@
#include "lat_long.hh" #include "lat_long.hh"
float LatLong::distance_to(const LatLong &target) const { float LatLong::distance_to(const LatLong &target) const {
float lat1 = latitude * RAD_PER_DEG; float lat1 = latitude * RAD_PER_DEG;
float lat2 = target.latitude * RAD_PER_DEG; float lat2 = target.latitude * RAD_PER_DEG;
float long1 = longitude * RAD_PER_DEG; float long1 = longitude * RAD_PER_DEG;
float long2 = target.longitude * RAD_PER_DEG; float long2 = target.longitude * RAD_PER_DEG;
float clat1 = cosf(lat1); float clat1 = cosf(lat1);
float clat2 = cosf(lat2); float clat2 = cosf(lat2);
float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 + float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 +
powf(sinf((lat2 - lat1) / 2.f), 2.f); powf(sinf((lat2 - lat1) / 2.f), 2.f);
float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
return d_over_r * EARTH_RAD; return d_over_r * EARTH_RAD;
} }
float LatLong::bearing_toward(const LatLong &target) const { float LatLong::bearing_toward(const LatLong &target) const {
float dlong = (target.longitude - longitude) * RAD_PER_DEG; float dlong = (target.longitude - longitude) * RAD_PER_DEG;
float sdlong = sinf(dlong); float sdlong = sinf(dlong);
float cdlong = cosf(dlong); float cdlong = cosf(dlong);
float lat1 = latitude * RAD_PER_DEG; float lat1 = latitude * RAD_PER_DEG;
float lat2 = target.latitude * RAD_PER_DEG; float lat2 = target.latitude * RAD_PER_DEG;
float slat1 = sinf(lat1); float slat1 = sinf(lat1);
float clat1 = cosf(lat1); float clat1 = cosf(lat1);
float slat2 = sinf(lat2); float slat2 = sinf(lat2);
float clat2 = cosf(lat2); float clat2 = cosf(lat2);
float num = sdlong * clat2; float num = sdlong * clat2;
float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong);
float course = atan2f(num, denom); float course = atan2f(num, denom);
if (course < 0.0) { if (course < 0.0) {
course += 2 * PI; course += 2 * PI;

10
main/pid_controller.cc
View File

@ -34,11 +34,11 @@ float PIDController::Error() const {
} }
void PIDController::Reset() { void PIDController::Reset() {
enabled_ = false; enabled_ = false;
setpoint_ = 0.; setpoint_ = 0.;
input_ = 0.; input_ = 0.;
output_ = 0.; output_ = 0.;
integral_ = 0.; integral_ = 0.;
last_error_ = NAN; last_error_ = NAN;
} }

10
main/pid_controller.hh
View File

@ -22,16 +22,16 @@ class PIDController {
kd_ = kd; kd_ = kd;
} }
void MaxOutput(float max_output) { max_output_ = max_output; } void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; } float MaxOutput() const { return max_output_; }
void MaxIError(float max_i_error) { max_i_error_ = max_i_error; } void MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
float MaxIError() const { return max_i_error_; } float MaxIError() const { return max_i_error_; }
void Setpoint(float setpoint) { setpoint_ = setpoint; } void Setpoint(float setpoint) { setpoint_ = setpoint; }
float Setpoint() const { return setpoint_; } float Setpoint() const { return setpoint_; }
void Input(float input) { input_ = input; } void Input(float input) { input_ = input; }
float Input() const { return input_; }; float Input() const { return input_; };
float Error() const; float Error() const;
@ -56,7 +56,7 @@ class PIDController {
float max_output_; float max_output_;
float max_i_error_; float max_i_error_;
bool enabled_; bool enabled_;
float setpoint_; float setpoint_;
float input_; float input_;
float output_; float output_;

76
main/u8g2_esp32_hal.c
View File

@ -2,19 +2,19 @@
#include <string.h> #include <string.h>
#include "esp_log.h" #include "esp_log.h"
#include "sdkconfig.h"
#include "i2c_mutex.h" #include "i2c_mutex.h"
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h" #include "freertos/FreeRTOS.h"
#include "freertos/task.h" #include "freertos/task.h"
#include "u8g2_esp32_hal.h" #include "u8g2_esp32_hal.h"
static const char * TAG = "u8g2_hal"; static const char *TAG = "u8g2_hal";
static const unsigned int I2C_TIMEOUT_MS = 100; static const unsigned int I2C_TIMEOUT_MS = 100;
static spi_device_handle_t handle_spi; // SPI handle. static spi_device_handle_t handle_spi; // SPI handle.
static i2c_cmd_handle_t handle_i2c; // I2C handle. static i2c_cmd_handle_t handle_i2c; // I2C handle.
#undef ESP_ERROR_CHECK #undef ESP_ERROR_CHECK
#define ESP_ERROR_CHECK(x) \ #define ESP_ERROR_CHECK(x) \
@ -52,28 +52,28 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
spi_bus_config_t bus_config; spi_bus_config_t bus_config;
memset(&bus_config, 0, sizeof(spi_bus_config_t)); memset(&bus_config, 0, sizeof(spi_bus_config_t));
bus_config.sclk_io_num = u8x8->pins[U8X8_PIN_SPI_CLOCK]; // CLK bus_config.sclk_io_num = u8x8->pins[U8X8_PIN_SPI_CLOCK]; // CLK
bus_config.mosi_io_num = u8x8->pins[U8X8_PIN_SPI_DATA]; // MOSI bus_config.mosi_io_num = u8x8->pins[U8X8_PIN_SPI_DATA]; // MOSI
bus_config.miso_io_num = -1; // MISO bus_config.miso_io_num = -1; // MISO
bus_config.quadwp_io_num = -1; // Not used bus_config.quadwp_io_num = -1; // Not used
bus_config.quadhd_io_num = -1; // Not used bus_config.quadhd_io_num = -1; // Not used
// ESP_LOGI(TAG, "... Initializing bus."); // ESP_LOGI(TAG, "... Initializing bus.");
ESP_ERROR_CHECK(spi_bus_initialize(HSPI_HOST, &bus_config, 1)); ESP_ERROR_CHECK(spi_bus_initialize(HSPI_HOST, &bus_config, 1));
spi_device_interface_config_t dev_config; spi_device_interface_config_t dev_config;
dev_config.address_bits = 0; dev_config.address_bits = 0;
dev_config.command_bits = 0; dev_config.command_bits = 0;
dev_config.dummy_bits = 0; dev_config.dummy_bits = 0;
dev_config.mode = 0; dev_config.mode = 0;
dev_config.duty_cycle_pos = 0; dev_config.duty_cycle_pos = 0;
dev_config.cs_ena_posttrans = 0; dev_config.cs_ena_posttrans = 0;
dev_config.cs_ena_pretrans = 0; dev_config.cs_ena_pretrans = 0;
dev_config.clock_speed_hz = 10000; dev_config.clock_speed_hz = 10000;
dev_config.spics_io_num = u8x8->pins[U8X8_PIN_CS]; dev_config.spics_io_num = u8x8->pins[U8X8_PIN_CS];
dev_config.flags = 0; dev_config.flags = 0;
dev_config.queue_size = 200; dev_config.queue_size = 200;
dev_config.pre_cb = NULL; dev_config.pre_cb = NULL;
dev_config.post_cb = NULL; dev_config.post_cb = NULL;
// ESP_LOGI(TAG, "... Adding device bus."); // ESP_LOGI(TAG, "... Adding device bus.");
ESP_ERROR_CHECK(spi_bus_add_device(HSPI_HOST, &dev_config, &handle_spi)); ESP_ERROR_CHECK(spi_bus_add_device(HSPI_HOST, &dev_config, &handle_spi));
@ -82,10 +82,10 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
case U8X8_MSG_BYTE_SEND: { case U8X8_MSG_BYTE_SEND: {
spi_transaction_t trans_desc; spi_transaction_t trans_desc;
trans_desc.addr = 0; trans_desc.addr = 0;
trans_desc.cmd = 0; trans_desc.cmd = 0;
trans_desc.flags = 0; trans_desc.flags = 0;
trans_desc.length = 8 * arg_int; // Number of bits NOT number of bytes. trans_desc.length = 8 * arg_int; // Number of bits NOT number of bytes.
trans_desc.rxlength = 0; trans_desc.rxlength = 0;
trans_desc.tx_buffer = arg_ptr; trans_desc.tx_buffer = arg_ptr;
trans_desc.rx_buffer = NULL; trans_desc.rx_buffer = NULL;
@ -105,10 +105,10 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
void *arg_ptr) { void *arg_ptr) {
#define TXBUF_SIZE 32 #define TXBUF_SIZE 32
static uint8_t txbuf[TXBUF_SIZE]; static uint8_t txbuf[TXBUF_SIZE];
static uint8_t *txbuf_ptr; static uint8_t *txbuf_ptr;
// ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, // ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg,
// arg_int, arg_ptr); // arg_int, arg_ptr);
switch (msg) { switch (msg) {
case U8X8_MSG_BYTE_SET_DC: { case U8X8_MSG_BYTE_SET_DC: {
if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) { if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) {
@ -126,10 +126,10 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
break; break;
i2c_config_t conf; i2c_config_t conf;
conf.mode = I2C_MODE_MASTER; conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = u8x8->pins[U8X8_PIN_I2C_DATA]; conf.sda_io_num = u8x8->pins[U8X8_PIN_I2C_DATA];
conf.sda_pullup_en = GPIO_PULLUP_ENABLE; conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = u8x8->pins[U8X8_PIN_I2C_CLOCK]; conf.scl_io_num = u8x8->pins[U8X8_PIN_I2C_CLOCK];
conf.scl_pullup_en = GPIO_PULLUP_ENABLE; conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
ESP_LOGV(TAG, "clk_speed %d", I2C_MASTER_FREQ_HZ); ESP_LOGV(TAG, "clk_speed %d", I2C_MASTER_FREQ_HZ);
conf.master.clk_speed = I2C_MASTER_FREQ_HZ; conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
@ -145,7 +145,7 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
case U8X8_MSG_BYTE_SEND: { case U8X8_MSG_BYTE_SEND: {
uint8_t *data_ptr = (uint8_t *)arg_ptr; uint8_t *data_ptr = (uint8_t *)arg_ptr;
size_t data_len = (size_t)arg_int; size_t data_len = (size_t)arg_int;
// ESP_LOGV(TAG, "U8x8_MSG_BYTE_SEND. txbuf len: %d", txbuf_ptr - txbuf); // ESP_LOGV(TAG, "U8x8_MSG_BYTE_SEND. txbuf len: %d", txbuf_ptr - txbuf);
// ESP_LOG_BUFFER_HEXDUMP(TAG, data_ptr, data_len, ESP_LOG_VERBOSE); // ESP_LOG_BUFFER_HEXDUMP(TAG, data_ptr, data_len, ESP_LOG_VERBOSE);
@ -160,7 +160,7 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
case U8X8_MSG_BYTE_START_TRANSFER: { case U8X8_MSG_BYTE_START_TRANSFER: {
uint8_t i2c_address = u8x8_GetI2CAddress(u8x8); uint8_t i2c_address = u8x8_GetI2CAddress(u8x8);
handle_i2c = i2c_cmd_link_create(); handle_i2c = i2c_cmd_link_create();
// ESP_LOGV(TAG, "Start I2C transfer to %02X.", i2c_address >> 1); // ESP_LOGV(TAG, "Start I2C transfer to %02X.", i2c_address >> 1);
ESP_ERROR_CHECK(i2c_master_start(handle_i2c)); ESP_ERROR_CHECK(i2c_master_start(handle_i2c));
ESP_ERROR_CHECK(i2c_master_write_byte( ESP_ERROR_CHECK(i2c_master_write_byte(
@ -208,9 +208,9 @@ uint8_t u8g2_esp32_gpio_and_delay_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
// RESET have been specified then we define those pins as GPIO outputs. // RESET have been specified then we define those pins as GPIO outputs.
case U8X8_MSG_GPIO_AND_DELAY_INIT: { case U8X8_MSG_GPIO_AND_DELAY_INIT: {
uint64_t bitmask = 0; uint64_t bitmask = 0;
uint8_t dc = u8x8->pins[U8X8_PIN_DC]; uint8_t dc = u8x8->pins[U8X8_PIN_DC];
uint8_t reset = u8x8->pins[U8X8_PIN_RESET]; uint8_t reset = u8x8->pins[U8X8_PIN_RESET];
uint8_t cs = u8x8->pins[U8X8_PIN_CS]; uint8_t cs = u8x8->pins[U8X8_PIN_CS];
if (dc != U8X8_PIN_NONE) { if (dc != U8X8_PIN_NONE) {
bitmask = bitmask | (1ull << dc); bitmask = bitmask | (1ull << dc);
} }
@ -228,10 +228,10 @@ uint8_t u8g2_esp32_gpio_and_delay_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
} }
gpio_config_t gpioConfig; gpio_config_t gpioConfig;
gpioConfig.pin_bit_mask = bitmask; gpioConfig.pin_bit_mask = bitmask;
gpioConfig.mode = GPIO_MODE_OUTPUT; gpioConfig.mode = GPIO_MODE_OUTPUT;
gpioConfig.pull_up_en = GPIO_PULLUP_DISABLE; gpioConfig.pull_up_en = GPIO_PULLUP_DISABLE;
gpioConfig.pull_down_en = GPIO_PULLDOWN_ENABLE; gpioConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
gpioConfig.intr_type = GPIO_INTR_DISABLE; gpioConfig.intr_type = GPIO_INTR_DISABLE;
gpio_config(&gpioConfig); gpio_config(&gpioConfig);
break; break;
} }

72
main/ugv.cc
View File

@ -8,19 +8,19 @@ 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) {
location.set_fix_quality(gps_data.fix_quality); location.set_fix_quality(gps_data.fix_quality);
location.set_latitude(gps_data.latitude); location.set_latitude(gps_data.latitude);
location.set_longitude(gps_data.longitude); location.set_longitude(gps_data.longitude);
@ -35,8 +35,8 @@ extern "C" void UGV_TickTimeout(void *arg) {
UGV::UGV() : angle_controller_(LOOP_PERIOD_S) { UGV::UGV() : angle_controller_(LOOP_PERIOD_S) {
SetTarget({34.069022, -118.443067}); SetTarget({34.069022, -118.443067});
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms); display = new DisplayClass(comms);
SetConfig(DefaultConfig()); SetConfig(DefaultConfig());
@ -79,15 +79,15 @@ void UGV::Init() {
display->Init(); display->Init();
esp_timer_create_args_t timer_args; esp_timer_create_args_t timer_args;
timer_args.callback = UGV_TickTimeout; timer_args.callback = UGV_TickTimeout;
timer_args.arg = this; timer_args.arg = this;
timer_args.dispatch_method = ESP_TIMER_TASK; timer_args.dispatch_method = ESP_TIMER_TASK;
timer_args.name = "ugv_main_loop"; timer_args.name = "ugv_main_loop";
esp_timer_create(&timer_args, &this->timer_handle); esp_timer_create(&timer_args, &this->timer_handle);
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_left_ = 0;
last_right_ = 0; last_right_ = 0;
} }
@ -103,7 +103,7 @@ void UGV::UpdateAHRS() {
yaw_ += 360.; yaw_ += 360.;
} }
pitch_ = ahrs_.getPitch(); pitch_ = ahrs_.getPitch();
roll_ = ahrs_.getRoll(); roll_ = ahrs_.getRoll();
} }
void UGV::DoDebugPrint() { void UGV::DoDebugPrint() {
@ -122,7 +122,7 @@ void UGV::ReadComms() {
comms->status.set_pitch_angle(pitch_); comms->status.set_pitch_angle(pitch_);
comms->status.set_roll_angle(roll_); comms->status.set_roll_angle(roll_);
comms->status.set_is_still(is_still_); comms->status.set_is_still(is_still_);
UGV_State comms_ugv_state = comms->status.state(); UGV_State comms_ugv_state = comms->status.state();
TickType_t ticks_since_last_packet = TickType_t ticks_since_last_packet =
(xTaskGetTickCount() - comms->last_packet_tick); (xTaskGetTickCount() - comms->last_packet_tick);
if (ticks_since_last_packet >= WATCHDOG_TICKS && if (ticks_since_last_packet >= WATCHDOG_TICKS &&
@ -160,31 +160,31 @@ void UGV::OnTick() {
UpdateAHRS(); UpdateAHRS();
angle_controller_.Input(yaw_); angle_controller_.Input(yaw_);
float drive_power = 0.; float drive_power = 0.;
outputs_.left_motor = 0.0; outputs_.left_motor = 0.0;
outputs_.right_motor = 0.0; outputs_.right_motor = 0.0;
float pitch = roll_; // TODO: fix quaternion -> YPR float pitch = roll_; // TODO: fix quaternion -> YPR
auto min_flip_pitch = conf_.min_flip_pitch(); auto min_flip_pitch = conf_.min_flip_pitch();
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();
@ -202,10 +202,10 @@ void UGV::OnTick() {
break; break;
} }
angle_controller_.Disable(); angle_controller_.Disable();
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update; TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update;
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID; bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
if (not_old && not_invalid) { if (not_old && not_invalid) {
next_state_ = UGV_State::STATE_TURNING; next_state_ = UGV_State::STATE_TURNING;
} }
@ -213,7 +213,7 @@ void UGV::OnTick() {
} }
case UGV_State::STATE_FLIPPING: { case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable(); angle_controller_.Disable();
outputs_.left_motor = 0.8; outputs_.left_motor = 0.8;
outputs_.right_motor = 0.8; outputs_.right_motor = 0.8;
if (!is_upside_down) { if (!is_upside_down) {
next_state_ = UGV_State::STATE_AQUIRING; next_state_ = UGV_State::STATE_AQUIRING;
@ -232,7 +232,7 @@ void UGV::OnTick() {
} }
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_bearing = current_pos.bearing_toward(target_); float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable(); angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing); angle_controller_.Setpoint(tgt_bearing);
@ -252,7 +252,7 @@ void UGV::OnTick() {
} }
LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_dist = current_pos.distance_to(target_); float tgt_dist = current_pos.distance_to(target_);
if (tgt_dist <= conf_.min_target_dist()) { if (tgt_dist <= conf_.min_target_dist()) {
ESP_LOGI(TAG, "Finished driving to target"); ESP_LOGI(TAG, "Finished driving to target");
@ -268,7 +268,7 @@ void UGV::OnTick() {
} }
case UGV_State::STATE_TEST: case UGV_State::STATE_TEST:
#ifdef BASIC_TEST #ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI); outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI); outputs.right_motor = cosf(time_s * PI);
#else #else
angle_controller_.Enable(); angle_controller_.Enable();
@ -283,8 +283,8 @@ void UGV::OnTick() {
} }
if (angle_controller_.Enabled()) { if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update(); float angle_pwr = angle_controller_.Update();
outputs_.left_motor = drive_power - angle_pwr; outputs_.left_motor = drive_power - angle_pwr;
outputs_.right_motor = drive_power + angle_pwr; outputs_.right_motor = drive_power + angle_pwr;
} }
@ -292,10 +292,10 @@ void UGV::OnTick() {
if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 && if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 &&
inputs_.mpu.gyro_rate.z == 0) { inputs_.mpu.gyro_rate.z == 0) {
outputs_.left_motor = 0; outputs_.left_motor = 0;
outputs_.right_motor = 0; outputs_.right_motor = 0;
} else { } else {
float dleft = outputs_.left_motor - last_left_; float dleft = outputs_.left_motor - last_left_;
float dright = outputs_.right_motor - last_right_; float dright = outputs_.right_motor - last_right_;
if (dleft > MAX_ACCEL) { if (dleft > MAX_ACCEL) {
outputs_.left_motor = last_left_ + MAX_ACCEL; outputs_.left_motor = last_left_ + MAX_ACCEL;
@ -309,7 +309,7 @@ void UGV::OnTick() {
} }
} }
io->WriteOutputs(outputs_); io->WriteOutputs(outputs_);
last_left_ = outputs_.left_motor; last_left_ = outputs_.left_motor;
last_right_ = outputs_.right_motor; last_right_ = outputs_.right_motor;
if (current_state_ != next_state_) { if (current_state_ != next_state_) {

44
main/ugv.hh
View File

@ -24,37 +24,37 @@ using ugv::io::IOClass;
extern "C" void UGV_TickTimeout(void *arg); extern "C" void UGV_TickTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location, void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data); const io::GpsData &gps_data);
class UGV { class UGV {
private: private:
CommsClass * comms; CommsClass *comms;
IOClass * io; IOClass *io;
DisplayClass * display; DisplayClass *display;
esp_timer_handle_t timer_handle; esp_timer_handle_t timer_handle;
LatLong target_; LatLong target_;
config::Config conf_; config::Config conf_;
PIDController angle_controller_; PIDController angle_controller_;
Madgwick ahrs_; Madgwick ahrs_;
io::Inputs inputs_; io::Inputs inputs_;
io::Outputs outputs_; io::Outputs outputs_;
int64_t last_print_; int64_t last_print_;
UGV_State current_state_; UGV_State current_state_;
UGV_State next_state_; UGV_State next_state_;
float yaw_; float yaw_;
float pitch_; float pitch_;
float roll_; float roll_;
io::MpuData last_mpu_; io::MpuData last_mpu_;
int64_t last_noise_; int64_t last_noise_;
float accel_noise_accum_; float accel_noise_accum_;
float gyro_noise_accum_; float gyro_noise_accum_;
float accel_noise_; float accel_noise_;
float gyro_noise_; float gyro_noise_;
bool is_still_; bool is_still_;
float last_left_; float last_left_;
float last_right_; float last_right_;
void UpdateAHRS(); void UpdateAHRS();
void DoDebugPrint(); void DoDebugPrint();

52
main/ugv_comms.cc
View File

@ -13,7 +13,7 @@
namespace ugv { namespace ugv {
namespace comms { namespace comms {
static const char * TAG = "ugv_comms"; static const char *TAG = "ugv_comms";
static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(15 * 1000); static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(15 * 1000);
CommsClass::CommsClass() CommsClass::CommsClass()
@ -39,19 +39,19 @@ void CommsClass::Init() {
status.set_state(messages::UGV_State::STATE_IDLE); status.set_state(messages::UGV_State::STATE_IDLE);
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_config_t lora_config = sx127x_config_default(); sx127x_config_t lora_config = sx127x_config_default();
lora_config.sck_io_num = (gpio_num_t)LORA_SCK; lora_config.sck_io_num = (gpio_num_t)LORA_SCK;
lora_config.miso_io_num = (gpio_num_t)LORA_MISO; lora_config.miso_io_num = (gpio_num_t)LORA_MISO;
lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI; lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI;
lora_config.cs_io_num = (gpio_num_t)LORA_CS; lora_config.cs_io_num = (gpio_num_t)LORA_CS;
lora_config.rst_io_num = (gpio_num_t)LORA_RST; lora_config.rst_io_num = (gpio_num_t)LORA_RST;
lora_config.irq_io_num = (gpio_num_t)LORA_IRQ; lora_config.irq_io_num = (gpio_num_t)LORA_IRQ;
lora_config.frequency = LORA_FREQ; lora_config.frequency = LORA_FREQ;
lora_config.tx_power = 17; lora_config.tx_power = 17;
lora_config.spreading_factor = 12; lora_config.spreading_factor = 12;
lora_config.signal_bandwidth = 10E3; lora_config.signal_bandwidth = 10E3;
lora_config.sync_word = 0x34; lora_config.sync_word = 0x34;
lora_config.crc = SX127X_CRC_ENABLED; lora_config.crc = SX127X_CRC_ENABLED;
ret = sx127x_init(&lora_config, &lora); ret = sx127x_init(&lora_config, &lora);
if (ret != ESP_OK) { if (ret != ESP_OK) {
@ -67,10 +67,10 @@ void CommsClass::Init() {
ESP_LOGI(TAG, "LoRa initialized"); ESP_LOGI(TAG, "LoRa initialized");
#else #else
e32::Config lora_config; e32::Config lora_config;
lora_config.uart_port = LORA_UART; lora_config.uart_port = LORA_UART;
lora_config.uart_rx_pin = LORA_RX; lora_config.uart_rx_pin = LORA_RX;
lora_config.uart_tx_pin = LORA_TX; lora_config.uart_tx_pin = LORA_TX;
ret = lora.Init(lora_config); ret = lora.Init(lora_config);
if (ret != ESP_OK) { if (ret != ESP_OK) {
const char *error_name = esp_err_to_name(ret); const char *error_name = esp_err_to_name(ret);
ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret); ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret);
@ -94,7 +94,7 @@ int32_t CommsClass::ReadRssi() {
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_read_rssi(lora, &rssi); sx127x_read_rssi(lora, &rssi);
#else #else
rssi = 0; rssi = 0;
#endif #endif
return rssi; return rssi;
} }
@ -121,13 +121,13 @@ void CommsClass::RunTask() {
using messages::UGV_Status; using messages::UGV_Status;
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
next_status_send = current_tick; next_status_send = current_tick;
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_rx_packet_t rx_packet; sx127x_rx_packet_t rx_packet;
#else #else
std::string rx_buf; std::string rx_buf;
int rx_len; int rx_len;
#endif #endif
while (true) { while (true) {
@ -188,7 +188,7 @@ void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
xSemaphoreTake(mutex, portMAX_DELAY); xSemaphoreTake(mutex, portMAX_DELAY);
last_packet_rssi = rx_packet->rssi; last_packet_rssi = rx_packet->rssi;
last_packet_snr = rx_packet->snr; last_packet_snr = rx_packet->snr;
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
HandlePacket(rx_packet->data, rx_packet->data_len); HandlePacket(rx_packet->data, rx_packet->data_len);
@ -203,10 +203,10 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data); ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data);
int ret; int ret;
size_t decoded_size = (data_size * 4 + 2) / 3; size_t decoded_size = (data_size * 4 + 2) / 3;
uint8_t *decoded = new uint8_t[decoded_size]; uint8_t *decoded = new uint8_t[decoded_size];
size_t decoded_len; size_t decoded_len;
ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data, ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data,
data_size); data_size);
@ -316,10 +316,10 @@ esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {
ESP_LOGE(TAG, "SendPacket size too long: %d", size); ESP_LOGE(TAG, "SendPacket size too long: %d", size);
return ESP_ERR_INVALID_SIZE; return ESP_ERR_INVALID_SIZE;
} }
size_t encoded_size = ((size + 6) / 3) * 4; size_t encoded_size = ((size + 6) / 3) * 4;
uint8_t *encoded = new uint8_t[encoded_size]; uint8_t *encoded = new uint8_t[encoded_size];
size_t encoded_len; size_t encoded_len;
int ret = int ret =
mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data, size); mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data, size);
if (ret != 0) { if (ret != 0) {
delete[] encoded; delete[] encoded;

16
main/ugv_comms.hh
View File

@ -16,11 +16,11 @@ namespace ugv {
namespace comms { namespace comms {
namespace messages = ugv::messages; namespace messages = ugv::messages;
namespace config = ugv::config; namespace config = ugv::config;
class CommsClass { class CommsClass {
public: public:
static constexpr int MAX_PACKET_LEN = 128; static constexpr int MAX_PACKET_LEN = 128;
static constexpr TickType_t PACKET_RX_TIMEOUT = pdMS_TO_TICKS(200); static constexpr TickType_t PACKET_RX_TIMEOUT = pdMS_TO_TICKS(200);
CommsClass(); CommsClass();
@ -35,14 +35,14 @@ class CommsClass {
public: public:
SemaphoreHandle_t mutex; SemaphoreHandle_t mutex;
TickType_t last_packet_tick; TickType_t last_packet_tick;
int32_t last_packet_rssi; int32_t last_packet_rssi;
int8_t last_packet_snr; int8_t last_packet_snr;
messages::UGV_Status status; messages::UGV_Status status;
messages::DriveHeadingData drive_heading; messages::DriveHeadingData drive_heading;
messages::TargetLocation* new_target; messages::TargetLocation* new_target;
config::Config* new_config; config::Config* new_config;
private: private:
#ifdef COMMS_SX127X #ifdef COMMS_SX127X

8
main/ugv_display.cc
View File

@ -37,7 +37,7 @@ void DisplayClass::Run() {
int32_t lora_rssi; int32_t lora_rssi;
uint8_t lora_lna_gain; uint8_t lora_lna_gain;
int32_t last_packet_rssi; int32_t last_packet_rssi;
int8_t last_packet_snr; int8_t last_packet_snr;
#endif #endif
TickType_t last_packet_tick; TickType_t last_packet_tick;
messages::UGV_State state; messages::UGV_State state;
@ -46,7 +46,7 @@ void DisplayClass::Run() {
while (true) { while (true) {
oled->firstPage(); oled->firstPage();
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
lora_rssi = comms_->ReadRssi(); lora_rssi = comms_->ReadRssi();
lora_lna_gain = comms_->ReadLnaGain(); lora_lna_gain = comms_->ReadLnaGain();
#endif #endif
@ -55,7 +55,7 @@ void DisplayClass::Run() {
state = comms_->status.state(); state = comms_->status.state();
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
last_packet_rssi = comms_->last_packet_rssi; last_packet_rssi = comms_->last_packet_rssi;
last_packet_snr = comms_->last_packet_snr; last_packet_snr = comms_->last_packet_snr;
#endif #endif
comms_->Unlock(); comms_->Unlock();
do { do {
@ -71,7 +71,7 @@ void DisplayClass::Run() {
heap_info.total_free_bytes); heap_info.total_free_bytes);
oled->setCursor(4, 3 * 8); oled->setCursor(4, 3 * 8);
oled->printf("state: %d", (int) state); oled->printf("state: %d", (int)state);
if (last_packet_tick > 0) { if (last_packet_tick > 0) {
double time_since_last_packet = double time_since_last_packet =

20
main/ugv_io.cc
View File

@ -22,11 +22,11 @@ static const char *TAG = "ugv_io";
} \ } \
} }
static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0; static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0;
static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_2; static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_2;
static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_0; static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12; static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12;
static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16; static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16;
IOClass IO; IOClass IO;
@ -51,11 +51,11 @@ void IOClass::InitMotors() {
ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM)); ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM));
mcpwm_config_t mcpwm_config; mcpwm_config_t mcpwm_config;
mcpwm_config.frequency = 20000; // 20khz mcpwm_config.frequency = 20000; // 20khz
mcpwm_config.cmpr_a = 0; mcpwm_config.cmpr_a = 0;
mcpwm_config.cmpr_b = 0; mcpwm_config.cmpr_b = 0;
mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm
mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high
ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config)); ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config));
ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_1, &mcpwm_config)); ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_1, &mcpwm_config));
} }
@ -78,7 +78,7 @@ void IOClass::WriteOutputs(const Outputs &outputs) {
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
ERROR_CHECK(ret); ERROR_CHECK(ret);
bool left_dir, right_dir; bool left_dir, right_dir;
left_dir = outputs.left_motor > 0.f; left_dir = outputs.left_motor > 0.f;
right_dir = outputs.right_motor < 0.f; right_dir = outputs.right_motor < 0.f;
ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir); ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir);

2
main/ugv_io.hh
View File

@ -30,7 +30,7 @@ class IOClass {
private: private:
UART_GPS gps_; UART_GPS gps_;
MPU mpu_; MPU mpu_;
void InitMotors(); void InitMotors();
}; };

66
main/ugv_io_gps.cc
View File

@ -10,13 +10,13 @@
namespace ugv { namespace ugv {
namespace io { namespace io {
static constexpr size_t GPS_BUF_SIZE = 1024; static constexpr size_t GPS_BUF_SIZE = 1024;
static constexpr uart_port_t GPS_UART = UART_NUM_1; static constexpr uart_port_t GPS_UART = UART_NUM_1;
static constexpr int GPS_UART_TX_PIN = 25; static constexpr int GPS_UART_TX_PIN = 25;
static constexpr int GPS_UART_RX_PIN = 26; static constexpr int GPS_UART_RX_PIN = 26;
static constexpr int GPS_UART_BAUD = 9600; static constexpr int GPS_UART_BAUD = 9600;
static constexpr size_t GPS_UART_RX_BUF_SIZE = 1024; static constexpr size_t GPS_UART_RX_BUF_SIZE = 1024;
static constexpr size_t GPS_UART_TX_BUF_SIZE = 0; static constexpr size_t GPS_UART_TX_BUF_SIZE = 0;
const char NMEA_OUTPUT_RMCONLY[] = const char NMEA_OUTPUT_RMCONLY[] =
"$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"; "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
@ -27,8 +27,8 @@ const char NMEA_OUTPUT_ALL[] =
const char NMEA_OUTPUT_OFF[] = const char NMEA_OUTPUT_OFF[] =
"$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"; "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
const char NMEA_UPDATE_1HZ[] = "$PMTK220,1000*1F"; const char NMEA_UPDATE_1HZ[] = "$PMTK220,1000*1F";
const char NMEA_UPDATE_5HZ[] = "$PMTK220,200*2C"; const char NMEA_UPDATE_5HZ[] = "$PMTK220,200*2C";
const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F"; const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F";
const char NMEA_Q_RELEASE[] = "$PMTK605*31"; const char NMEA_Q_RELEASE[] = "$PMTK605*31";
@ -47,16 +47,16 @@ void UART_GPS::Init() {
esp_err_t ret; esp_err_t ret;
mutex_ = xSemaphoreCreateMutex(); mutex_ = xSemaphoreCreateMutex();
data_ = GpsData{}; data_ = GpsData{};
uart_config_t gps_uart_config; uart_config_t gps_uart_config;
gps_uart_config.baud_rate = GPS_UART_BAUD; gps_uart_config.baud_rate = GPS_UART_BAUD;
gps_uart_config.data_bits = UART_DATA_8_BITS; gps_uart_config.data_bits = UART_DATA_8_BITS;
gps_uart_config.parity = UART_PARITY_DISABLE; gps_uart_config.parity = UART_PARITY_DISABLE;
gps_uart_config.stop_bits = UART_STOP_BITS_1; gps_uart_config.stop_bits = UART_STOP_BITS_1;
gps_uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; gps_uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
gps_uart_config.rx_flow_ctrl_thresh = 122; gps_uart_config.rx_flow_ctrl_thresh = 122;
gps_uart_config.use_ref_tick = false; gps_uart_config.use_ref_tick = false;
ret = uart_param_config(GPS_UART, &gps_uart_config); ret = uart_param_config(GPS_UART, &gps_uart_config);
if (ret != ESP_OK) { if (ret != ESP_OK) {
ESP_LOGE(TAG, "uart_param_config: %d", ret); ESP_LOGE(TAG, "uart_param_config: %d", ret);
@ -115,17 +115,17 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
return; return;
case MINMEA_SENTENCE_RMC: { case MINMEA_SENTENCE_RMC: {
minmea_sentence_rmc rmc; minmea_sentence_rmc rmc;
bool parse_success; bool parse_success;
parse_success = minmea_parse_rmc(&rmc, line); parse_success = minmea_parse_rmc(&rmc, line);
if (!parse_success) { if (!parse_success) {
goto invalid; goto invalid;
} }
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data_.valid = rmc.valid; data_.valid = rmc.valid;
data_.latitude = minmea_tocoord(&rmc.latitude); data_.latitude = minmea_tocoord(&rmc.latitude);
data_.longitude = minmea_tocoord(&rmc.longitude); data_.longitude = minmea_tocoord(&rmc.longitude);
data_.speed = minmea_tofloat(&rmc.speed); data_.speed = minmea_tofloat(&rmc.speed);
data_.course = minmea_tofloat(&rmc.course); data_.course = minmea_tofloat(&rmc.course);
data_.last_update = xTaskGetTickCount(); data_.last_update = xTaskGetTickCount();
ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f", ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f",
data_.valid ? "valid" : "invalid", data_.latitude, data_.valid ? "valid" : "invalid", data_.latitude,
@ -135,20 +135,20 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
} }
case MINMEA_SENTENCE_GGA: { case MINMEA_SENTENCE_GGA: {
minmea_sentence_gga gga; minmea_sentence_gga gga;
bool parse_success; bool parse_success;
parse_success = minmea_parse_gga(&gga, line); parse_success = minmea_parse_gga(&gga, line);
if (!parse_success) { if (!parse_success) {
goto invalid; goto invalid;
} }
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data_.fix_quality = (GpsFixQual)gga.fix_quality; data_.fix_quality = (GpsFixQual)gga.fix_quality;
data_.num_satellites = gga.satellites_tracked; data_.num_satellites = gga.satellites_tracked;
data_.latitude = minmea_tocoord(&gga.latitude); data_.latitude = minmea_tocoord(&gga.latitude);
data_.longitude = minmea_tocoord(&gga.longitude); data_.longitude = minmea_tocoord(&gga.longitude);
if (gga.fix_quality != 0 && gga.altitude_units != 'M') { if (gga.fix_quality != 0 && gga.altitude_units != 'M') {
ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units); ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
} }
data_.altitude = minmea_tofloat(&gga.altitude); data_.altitude = minmea_tofloat(&gga.altitude);
data_.last_update = xTaskGetTickCount(); data_.last_update = xTaskGetTickCount();
ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f", ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f",
data_.fix_quality, data_.num_satellites, data_.latitude, data_.fix_quality, data_.num_satellites, data_.latitude,
@ -158,17 +158,17 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
} }
case MINMEA_SENTENCE_GSA: { case MINMEA_SENTENCE_GSA: {
minmea_sentence_gsa gsa; minmea_sentence_gsa gsa;
bool parse_success; bool parse_success;
parse_success = minmea_parse_gsa(&gsa, line); parse_success = minmea_parse_gsa(&gsa, line);
if (!parse_success) { if (!parse_success) {
goto invalid; goto invalid;
} }
xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
data_.pdop = minmea_tofloat(&gsa.pdop); data_.pdop = minmea_tofloat(&gsa.pdop);
data_.hdop = minmea_tofloat(&gsa.hdop); data_.hdop = minmea_tofloat(&gsa.hdop);
data_.vdop = minmea_tofloat(&gsa.vdop); data_.vdop = minmea_tofloat(&gsa.vdop);
ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", data_.pdop, data_.hdop,
data_.pdop, data_.hdop, data_.vdop); data_.vdop);
xSemaphoreGive(mutex_); xSemaphoreGive(mutex_);
break; break;
} }

32
main/ugv_io_gps.hh
View File

@ -9,22 +9,22 @@ namespace ugv {
namespace io { namespace io {
enum GpsFixQual { enum GpsFixQual {
GPS_FIX_INVALID = 0, // invalid gps fix GPS_FIX_INVALID = 0, // invalid gps fix
GPS_FIX_GPS = 1, // GPS fix GPS_FIX_GPS = 1, // GPS fix
GPS_FIX_DGPS = 2, // differential GPS fix GPS_FIX_DGPS = 2, // differential GPS fix
GPS_FIX_PPS = 3, // PPS fix GPS_FIX_PPS = 3, // PPS fix
GPS_FIX_RTK = 4, // Real Time Kinematic fix GPS_FIX_RTK = 4, // Real Time Kinematic fix
GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix
GPS_FIX_ESTIMATED = 6, // Estimated fix GPS_FIX_ESTIMATED = 6, // Estimated fix
GPS_FIX_MANUAL = 7, // Manual fix GPS_FIX_MANUAL = 7, // Manual fix
GPS_FIX_SIMULATED = 8, // Simulated fix GPS_FIX_SIMULATED = 8, // Simulated fix
}; };
struct GpsData { struct GpsData {
TickType_t last_update; TickType_t last_update;
bool valid; bool valid;
GpsFixQual fix_quality; GpsFixQual fix_quality;
uint8_t num_satellites; uint8_t num_satellites;
float latitude; // degrees +/- float latitude; // degrees +/-
float longitude; // degrees +/- float longitude; // degrees +/-
@ -46,17 +46,17 @@ class UART_GPS {
void GetData(GpsData& data); void GetData(GpsData& data);
private: private:
TaskHandle_t task_; TaskHandle_t task_;
SemaphoreHandle_t mutex_; SemaphoreHandle_t mutex_;
QueueHandle_t uart_queue_; QueueHandle_t uart_queue_;
GpsData data_; GpsData data_;
uint8_t* buffer_; uint8_t* buffer_;
esp_err_t WriteCommand(const char* cmd, size_t len); esp_err_t WriteCommand(const char* cmd, size_t len);
void HandleUartPattern(); void HandleUartPattern();
void ProcessLine(const char* line, size_t len); void ProcessLine(const char* line, size_t len);
void DoTask(); void DoTask();
static void TaskEntry(void* arg); static void TaskEntry(void* arg);
}; };

36
main/ugv_io_mpu.cc
View File

@ -14,23 +14,23 @@ constexpr float M_PI = 3.1415926535897932384626433832795;
namespace ugv { namespace ugv {
namespace io { namespace io {
static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5; static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5;
static constexpr gpio_num_t MPU_SCL = GPIO_NUM_4; static constexpr gpio_num_t MPU_SCL = GPIO_NUM_4;
static constexpr mpud::accel_fs_t MPU_ACCEL_FS = mpud::ACCEL_FS_2G; static constexpr mpud::accel_fs_t MPU_ACCEL_FS = mpud::ACCEL_FS_2G;
static constexpr mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS; static constexpr mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS;
static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); 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., // static const Mat3f MAG_MAT = {0., -0.0281408, 0., -0.0284409, 0., 0.,
// 0., 0., 0.0261544}; // 0., 0., 0.0261544};
static const Mat3f MAG_MAT = {0., -0.0335989, 0., -0.0330167, 0., static const Mat3f MAG_MAT = {0., -0.0335989, 0., -0.0330167, 0.,
0., 0., 0., 0.0335989}; 0., 0., 0., 0.0335989};
static const Vec3f GYRO_OFFSET = {-4.33655, -2.76826, -0.908427}; 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";
@ -69,7 +69,7 @@ void MPU::Calibrate() {
void MPU::GetData(MpuData &data) { void MPU::GetData(MpuData &data) {
xSemaphoreTake(mutex_, pdMS_TO_TICKS(10)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(10));
data = last_data_; data = last_data_;
last_data_.gyro_rate = {0, 0, 0}; last_data_.gyro_rate = {0, 0, 0};
xSemaphoreGive(mutex_); xSemaphoreGive(mutex_);
} }
@ -83,7 +83,7 @@ bool MPU::Initialize() {
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();
@ -122,8 +122,8 @@ bool MPU::Initialize() {
void MPU::DoTask() { void MPU::DoTask() {
ESP_LOGI(TAG, "MPU task start"); ESP_LOGI(TAG, "MPU task start");
mpud::raw_axes_t accel_, gyro_, mag_; mpud::raw_axes_t accel_, gyro_, mag_;
esp_err_t ret; esp_err_t ret;
MpuData data; MpuData data;
if (!Initialize()) { if (!Initialize()) {
return; return;
@ -154,14 +154,14 @@ void MPU::DoTask() {
int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) | int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) |
static_cast<int16_t>(compass_data[4]); static_cast<int16_t>(compass_data[4]);
// ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); // ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET); data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET); data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET);
data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; data.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
data.mag = MAG_MAT * (data.mag + MAG_OFFSET); data.mag = MAG_MAT * (data.mag + MAG_OFFSET);
xSemaphoreTake(mutex_, pdMS_TO_TICKS(100)); xSemaphoreTake(mutex_, pdMS_TO_TICKS(100));
last_data_ = data; last_data_ = data;
xSemaphoreGive(mutex_); xSemaphoreGive(mutex_);

12
main/ugv_io_mpu.hh
View File

@ -99,14 +99,14 @@ 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_;
TaskHandle_t task_; TaskHandle_t task_;
SemaphoreHandle_t mutex_; SemaphoreHandle_t mutex_;
MpuData last_data_; MpuData last_data_;
void DoTask(); void DoTask();
bool Initialize(); bool Initialize();
static void TaskEntry(void* arg); static void TaskEntry(void* arg);
}; };