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

clang-format without silly alignment

master
Alex Mikhalev 6 years ago
parent
f8b7cc5e8b
commit
6965f03068
23 changed files with 354 additions and 354 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      .clang-format
  2. 58
      main/MadgwickAHRS.cpp
  3. 2
      main/MadgwickAHRS.h
  4. 14
      main/Print.cpp
  5. 8
      main/Print.h
  6. 32
      main/U8g2lib.hh
  7. 74
      main/e32_driver.cc
  8. 36
      main/e32_driver.hh
  9. 24
      main/lat_long.cc
  10. 10
      main/pid_controller.cc
  11. 10
      main/pid_controller.hh
  12. 76
      main/u8g2_esp32_hal.c
  13. 72
      main/ugv.cc
  14. 44
      main/ugv.hh
  15. 52
      main/ugv_comms.cc
  16. 16
      main/ugv_comms.hh
  17. 8
      main/ugv_display.cc
  18. 20
      main/ugv_io.cc
  19. 2
      main/ugv_io.hh
  20. 66
      main/ugv_io_gps.cc
  21. 32
      main/ugv_io_gps.hh
  22. 36
      main/ugv_io_mpu.cc
  23. 12
      main/ugv_io_mpu.hh

4
.clang-format
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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);
}; };

Write Preview
Loading…
Cancel
Save