diff --git a/.clang-format b/.clang-format index 9693228..5ea45c0 100644 --- a/.clang-format +++ b/.clang-format @@ -3,8 +3,8 @@ Language: Cpp # BasedOnStyle: Google AccessModifierOffset: -1 AlignAfterOpenBracket: Align -AlignConsecutiveAssignments: true -AlignConsecutiveDeclarations: true +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false AlignEscapedNewlines: Left AlignOperands: true AlignTrailingComments: true diff --git a/main/MadgwickAHRS.cpp b/main/MadgwickAHRS.cpp index 6b74d27..2aee4f0 100644 --- a/main/MadgwickAHRS.cpp +++ b/main/MadgwickAHRS.cpp @@ -36,12 +36,12 @@ // AHRS algorithm update Madgwick::Madgwick() { - beta = betaDef; - q0 = 1.0f; - q1 = 0.0f; - q2 = 0.0f; - q3 = 0.0f; - invSampleFreq = 1.0f / sampleFreqDef; + beta = betaDef; + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + invSampleFreq = 1.0f / sampleFreqDef; anglesComputed = 0; } @@ -93,22 +93,22 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; _2q1mx = 2.0f * q1 * mx; - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; _2q0q2 = 2.0f * q0 * q2; _2q2q3 = 2.0f * q2 * q3; - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; // Reference direction of Earth's magnetic field hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + @@ -233,7 +233,7 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; @@ -269,23 +269,23 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float Madgwick::invSqrt(float x) { float halfx = 0.5f * x; - float y = x; + float y = x; // long i = *(long *)&y; - long i; + long i; memcpy(&i, &y, sizeof(float)); - i = 0x5f3759df - (i >> 1); + i = 0x5f3759df - (i >> 1); // y = *(float *)&i; 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; } //------------------------------------------------------------------------------------------- void Madgwick::computeAngles() { - roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); - pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); - yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); + roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); + pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); + yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); anglesComputed = 1; } diff --git a/main/MadgwickAHRS.h b/main/MadgwickAHRS.h index 989eca9..7bf7fc4 100644 --- a/main/MadgwickAHRS.h +++ b/main/MadgwickAHRS.h @@ -34,7 +34,7 @@ class Madgwick { float roll; float pitch; float yaw; - char anglesComputed; + char anglesComputed; void computeAngles(); diff --git a/main/Print.cpp b/main/Print.cpp index 6de22a8..d715b0d 100644 --- a/main/Print.cpp +++ b/main/Print.cpp @@ -41,8 +41,8 @@ size_t Print::write(const uint8_t *buffer, size_t size) { } size_t Print::printf(const char *format, ...) { - char loc_buf[64]; - char * temp = loc_buf; + char loc_buf[64]; + char *temp = loc_buf; va_list arg; va_list copy; va_start(arg, format); @@ -90,7 +90,7 @@ size_t Print::print(long n, int base) { } else if (base == 10) { if (n < 0) { int t = print('-'); - n = -n; + n = -n; return printNumber(n, 10) + t; } return printNumber(n, 10); @@ -116,7 +116,7 @@ size_t Print::print(struct tm *timeinfo, const char *format) { if (!f) { f = "%c"; } - char buf[64]; + char buf[64]; size_t written = strftime(buf, 64, f, timeinfo); print(buf); return written; @@ -199,7 +199,7 @@ size_t Print::println(struct tm *timeinfo, const char *format) { // Private Methods ///////////////////////////////////////////////////////////// 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]; *str = '\0'; @@ -250,8 +250,8 @@ size_t Print::printFloat(double number, uint8_t digits) { number += rounding; // Extract the integer part of the number and print it - unsigned long int_part = (unsigned long)number; - double remainder = number - (double)int_part; + unsigned long int_part = (unsigned long)number; + double remainder = number - (double)int_part; n += print(int_part); // Print the decimal point, but only if there are digits beyond diff --git a/main/Print.h b/main/Print.h index aacd532..482ba16 100644 --- a/main/Print.h +++ b/main/Print.h @@ -31,7 +31,7 @@ class Print { private: - int write_error; + int write_error; size_t printNumber(unsigned long, uint8_t); size_t printFloat(double, uint8_t); @@ -41,18 +41,18 @@ class Print { public: Print() : write_error(0) {} virtual ~Print() {} - int getWriteError() { return write_error; } + int getWriteError() { return write_error; } void clearWriteError() { setWriteError(0); } virtual size_t write(uint8_t) = 0; - size_t write(const char *str) { + size_t write(const char *str) { if (str == NULL) { return 0; } return write((const uint8_t *)str, strlen(str)); } 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); } diff --git a/main/U8g2lib.hh b/main/U8g2lib.hh index eadced4..4a4b9b6 100644 --- a/main/U8g2lib.hh +++ b/main/U8g2lib.hh @@ -6,7 +6,7 @@ class U8G2 : public Print { protected: - u8g2_t u8g2; + u8g2_t u8g2; u8x8_char_cb cpp_next_cb; /* the cpp interface has its own decoding function for the Arduino print command */ public: @@ -20,7 +20,7 @@ class U8G2 : public Print { u8g2_t *getU8g2(void) { return &u8g2; } 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; } @@ -32,13 +32,13 @@ class U8G2 : public Print { /* u8x8 interface */ uint8_t getCols(void) { return u8x8_GetCols(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); } #ifdef U8X8_WITH_USER_PTR void *getUserPtr() { return u8g2_GetUserPtr(&u8g2); } - void setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); } + void setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); } #endif #ifdef U8X8_USE_PINS @@ -118,13 +118,13 @@ class U8G2 : public Print { void sendBuffer(void) { u8g2_SendBuffer(&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 *getBufferPtr(void) { return u8g2_GetBufferPtr(&u8g2); } - uint8_t getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); } - uint8_t getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); } - uint8_t getPageCurrTileRow(void) { + uint8_t getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); } + uint8_t getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); } + uint8_t getPageCurrTileRow(void) { return u8g2_GetBufferCurrTileRow(&u8g2); } // obsolete void setPageCurrTileRow(uint8_t row) { @@ -339,11 +339,11 @@ class U8G2 : public Print { void setColorIndex(uint8_t color_index) { u8g2_SetDrawColor(&u8g2, color_index); } - uint8_t getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); } - int8_t getFontAscent(void) { return u8g2_GetAscent(&u8g2); } - int8_t getFontDescent(void) { return u8g2_GetDescent(&u8g2); } - int8_t getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); } - int8_t getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); } + uint8_t getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); } + int8_t getFontAscent(void) { return u8g2_GetAscent(&u8g2); } + int8_t getFontDescent(void) { return u8g2_GetDescent(&u8g2); } + int8_t getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); } + int8_t getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); } u8g2_uint_t getHeight() { return u8g2_GetDisplayHeight(&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 { public: U8G2_SSD1306_128X64_NONAME_F_HW_I2C(const u8g2_cb_t *rotation, - uint8_t reset = U8X8_PIN_NONE, - uint8_t clock = U8X8_PIN_NONE, - uint8_t data = U8X8_PIN_NONE) + uint8_t reset = U8X8_PIN_NONE, + uint8_t clock = U8X8_PIN_NONE, + uint8_t data = U8X8_PIN_NONE) : U8G2() { u8g2_Setup_ssd1306_i2c_128x64_noname_f( &u8g2, rotation, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb); diff --git a/main/e32_driver.cc b/main/e32_driver.cc index bcc5886..d040c4b 100644 --- a/main/e32_driver.cc +++ b/main/e32_driver.cc @@ -5,40 +5,40 @@ namespace ugv { 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_TX_BUF_SIZE = 1024; -static constexpr size_t PARAMS_LEN = 6; -static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0; -static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1}; -static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2; -static const uint8_t CMD_READ_VERSION[] = {0xC3, 0xC3, 0xC3}; -static const uint8_t CMD_RESET[] = {0xC4, 0xC4, 0xC4}; +static constexpr size_t PARAMS_LEN = 6; +static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0; +static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1}; +static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2; +static const uint8_t CMD_READ_VERSION[] = {0xC3, 0xC3, 0xC3}; +static const uint8_t CMD_RESET[] = {0xC4, 0xC4, 0xC4}; static const char* TAG = "e32_driver"; Config::Config() { - uart_port = UART_NUM_1; + uart_port = UART_NUM_1; uart_parity = UART_PARITY_DISABLE; uart_tx_pin = UART_PIN_NO_CHANGE; uart_rx_pin = UART_PIN_NO_CHANGE; - uart_baud = 9600; + uart_baud = 9600; } Params::Params() { // These are defaults for the 433T30D - save_params = true; - address = 0x0000; - uart_partity = UART_PARITY_DISABLE; - uart_baud = 9600; // bps + save_params = true; + address = 0x0000; + uart_partity = UART_PARITY_DISABLE; + uart_baud = 9600; // bps air_data_rate = 2400; // bps - comm_channel = 0x17; - tx_mode = TxTransparent; - io_mode = IoPushPull; - wake_up_time = 250; // ms - fec_enabled = true; - tx_power = 30; + comm_channel = 0x17; + tx_mode = TxTransparent; + io_mode = IoPushPull; + wake_up_time = 250; // ms + fec_enabled = true; + tx_power = 30; } 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) { config_ = config; uart_config_t uart_config; - uart_config.baud_rate = config_.uart_baud; - uart_config.data_bits = UART_DATA_8_BITS; - uart_config.parity = config_.uart_parity; - uart_config.stop_bits = UART_STOP_BITS_1; - uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; + uart_config.baud_rate = config_.uart_baud; + uart_config.data_bits = UART_DATA_8_BITS; + uart_config.parity = config_.uart_parity; + uart_config.stop_bits = UART_STOP_BITS_1; + uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; uart_config.rx_flow_ctrl_thresh = 122; - uart_config.use_ref_tick = false; + uart_config.use_ref_tick = false; esp_err_t ret; ret = uart_param_config(config_.uart_port, &uart_config); @@ -92,7 +92,7 @@ error: esp_err_t E32_Driver::Free() { esp_err_t ret; if (initialized_) { - ret = uart_driver_delete(config_.uart_port); + ret = uart_driver_delete(config_.uart_port); initialized_ = false; } else { ret = ESP_ERR_INVALID_STATE; @@ -159,10 +159,10 @@ esp_err_t E32_Driver::ReadParams(Params& params) { params.comm_channel = param_data[4]; - params.tx_mode = (TxMode)(param_data[5] & (1 << 7)); - params.io_mode = (IoMode)(param_data[5] & (1 << 6)); + params.tx_mode = (TxMode)(param_data[5] & (1 << 7)); + params.io_mode = (IoMode)(param_data[5] & (1 << 6)); 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 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 ret; - uint8_t param_data[PARAMS_LEN]; + uint8_t param_data[PARAMS_LEN]; param_data[0] = 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) { // TODO: more proper way to read a line - uint8_t byte; - TickType_t start_tick = xTaskGetTickCount(); + uint8_t byte; + TickType_t start_tick = xTaskGetTickCount(); TickType_t current_tick = start_tick; - int read, total_read = 0; + int read, total_read = 0; while (total_read < data_size) { read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); 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) { // TODO: more proper way to read a line data.clear(); - uint8_t byte; - TickType_t start_tick = xTaskGetTickCount(); + uint8_t byte; + TickType_t start_tick = xTaskGetTickCount(); TickType_t current_tick = start_tick; - int read, total_read = 0; + int read, total_read = 0; while (true) { read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick)); if (read < 1) { 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; data += (char)byte; total_read += read; diff --git a/main/e32_driver.hh b/main/e32_driver.hh index 66d5385..1765961 100644 --- a/main/e32_driver.hh +++ b/main/e32_driver.hh @@ -9,16 +9,16 @@ namespace e32 { enum TxMode { TxTransparent = 0, - TxFixed = 1, + TxFixed = 1, }; enum IoMode { IoOpenCollector = 0, - IoPushPull = 1, + IoPushPull = 1, }; typedef uint16_t Address; -typedef uint8_t Channel; +typedef uint8_t Channel; constexpr Address BroadcastAddress = 0xFFFF; @@ -26,28 +26,28 @@ struct Config { public: Config(); - uart_port_t uart_port; + uart_port_t uart_port; uart_parity_t uart_parity; - int uart_tx_pin; - int uart_rx_pin; - int uart_baud; + int uart_tx_pin; + int uart_rx_pin; + int uart_baud; }; struct Params { public: Params(); - bool save_params; - Address address; + bool save_params; + Address address; uart_parity_t uart_partity; - int uart_baud; // bps - int air_data_rate; // bps - Channel comm_channel; - TxMode tx_mode; - IoMode io_mode; - uint16_t wake_up_time; // ms - bool fec_enabled; - uint16_t tx_power; // dBm + int uart_baud; // bps + int air_data_rate; // bps + Channel comm_channel; + TxMode tx_mode; + IoMode io_mode; + uint16_t wake_up_time; // ms + bool fec_enabled; + uint16_t tx_power; // dBm }; class E32_Driver { @@ -80,7 +80,7 @@ class E32_Driver { void Flush(); private: - bool initialized_; + bool initialized_; Config config_; Params params_; diff --git a/main/lat_long.cc b/main/lat_long.cc index e835ab3..b570bde 100644 --- a/main/lat_long.cc +++ b/main/lat_long.cc @@ -1,30 +1,30 @@ #include "lat_long.hh" float LatLong::distance_to(const LatLong &target) const { - float lat1 = latitude * RAD_PER_DEG; - float lat2 = target.latitude * RAD_PER_DEG; + float lat1 = latitude * RAD_PER_DEG; + float lat2 = target.latitude * RAD_PER_DEG; float long1 = longitude * RAD_PER_DEG; float long2 = target.longitude * RAD_PER_DEG; float clat1 = cosf(lat1); 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); float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a)); return d_over_r * EARTH_RAD; } 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 cdlong = cosf(dlong); - float lat1 = latitude * RAD_PER_DEG; - float lat2 = target.latitude * RAD_PER_DEG; - float slat1 = sinf(lat1); - float clat1 = cosf(lat1); - float slat2 = sinf(lat2); - float clat2 = cosf(lat2); - float num = sdlong * clat2; - float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); + float lat1 = latitude * RAD_PER_DEG; + float lat2 = target.latitude * RAD_PER_DEG; + float slat1 = sinf(lat1); + float clat1 = cosf(lat1); + float slat2 = sinf(lat2); + float clat2 = cosf(lat2); + float num = sdlong * clat2; + float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong); float course = atan2f(num, denom); if (course < 0.0) { course += 2 * PI; diff --git a/main/pid_controller.cc b/main/pid_controller.cc index 6b459f1..b49fcc7 100644 --- a/main/pid_controller.cc +++ b/main/pid_controller.cc @@ -34,11 +34,11 @@ float PIDController::Error() const { } void PIDController::Reset() { - enabled_ = false; - setpoint_ = 0.; - input_ = 0.; - output_ = 0.; - integral_ = 0.; + enabled_ = false; + setpoint_ = 0.; + input_ = 0.; + output_ = 0.; + integral_ = 0.; last_error_ = NAN; } diff --git a/main/pid_controller.hh b/main/pid_controller.hh index 360efcd..5d57419 100644 --- a/main/pid_controller.hh +++ b/main/pid_controller.hh @@ -22,16 +22,16 @@ class PIDController { 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_; } - 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_; } - void Setpoint(float setpoint) { setpoint_ = setpoint; } + void Setpoint(float setpoint) { setpoint_ = setpoint; } float Setpoint() const { return setpoint_; } - void Input(float input) { input_ = input; } + void Input(float input) { input_ = input; } float Input() const { return input_; }; float Error() const; @@ -56,7 +56,7 @@ class PIDController { float max_output_; float max_i_error_; - bool enabled_; + bool enabled_; float setpoint_; float input_; float output_; diff --git a/main/u8g2_esp32_hal.c b/main/u8g2_esp32_hal.c index 4ad4282..54cf1b9 100644 --- a/main/u8g2_esp32_hal.c +++ b/main/u8g2_esp32_hal.c @@ -2,19 +2,19 @@ #include #include "esp_log.h" -#include "sdkconfig.h" #include "i2c_mutex.h" +#include "sdkconfig.h" #include "freertos/FreeRTOS.h" #include "freertos/task.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 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 #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; memset(&bus_config, 0, sizeof(spi_bus_config_t)); - 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.miso_io_num = -1; // MISO - bus_config.quadwp_io_num = -1; // Not used - bus_config.quadhd_io_num = -1; // Not used + 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.miso_io_num = -1; // MISO + bus_config.quadwp_io_num = -1; // Not used + bus_config.quadhd_io_num = -1; // Not used // ESP_LOGI(TAG, "... Initializing bus."); ESP_ERROR_CHECK(spi_bus_initialize(HSPI_HOST, &bus_config, 1)); spi_device_interface_config_t dev_config; - dev_config.address_bits = 0; - dev_config.command_bits = 0; - dev_config.dummy_bits = 0; - dev_config.mode = 0; - dev_config.duty_cycle_pos = 0; + dev_config.address_bits = 0; + dev_config.command_bits = 0; + dev_config.dummy_bits = 0; + dev_config.mode = 0; + dev_config.duty_cycle_pos = 0; dev_config.cs_ena_posttrans = 0; - dev_config.cs_ena_pretrans = 0; - dev_config.clock_speed_hz = 10000; - dev_config.spics_io_num = u8x8->pins[U8X8_PIN_CS]; - dev_config.flags = 0; - dev_config.queue_size = 200; - dev_config.pre_cb = NULL; - dev_config.post_cb = NULL; + dev_config.cs_ena_pretrans = 0; + dev_config.clock_speed_hz = 10000; + dev_config.spics_io_num = u8x8->pins[U8X8_PIN_CS]; + dev_config.flags = 0; + dev_config.queue_size = 200; + dev_config.pre_cb = NULL; + dev_config.post_cb = NULL; // ESP_LOGI(TAG, "... Adding device bus."); 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: { spi_transaction_t trans_desc; - trans_desc.addr = 0; - trans_desc.cmd = 0; - trans_desc.flags = 0; - trans_desc.length = 8 * arg_int; // Number of bits NOT number of bytes. + trans_desc.addr = 0; + trans_desc.cmd = 0; + trans_desc.flags = 0; + trans_desc.length = 8 * arg_int; // Number of bits NOT number of bytes. trans_desc.rxlength = 0; trans_desc.tx_buffer = arg_ptr; 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, void *arg_ptr) { #define TXBUF_SIZE 32 - static uint8_t txbuf[TXBUF_SIZE]; + static uint8_t txbuf[TXBUF_SIZE]; static uint8_t *txbuf_ptr; -// ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, -// arg_int, arg_ptr); + // ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg, + // arg_int, arg_ptr); switch (msg) { case U8X8_MSG_BYTE_SET_DC: { if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) { @@ -126,10 +126,10 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, break; i2c_config_t conf; - conf.mode = I2C_MODE_MASTER; - conf.sda_io_num = u8x8->pins[U8X8_PIN_I2C_DATA]; + conf.mode = I2C_MODE_MASTER; + conf.sda_io_num = u8x8->pins[U8X8_PIN_I2C_DATA]; 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; ESP_LOGV(TAG, "clk_speed %d", 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: { 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_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: { 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_ERROR_CHECK(i2c_master_start(handle_i2c)); 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. case U8X8_MSG_GPIO_AND_DELAY_INIT: { uint64_t bitmask = 0; - uint8_t dc = u8x8->pins[U8X8_PIN_DC]; - uint8_t reset = u8x8->pins[U8X8_PIN_RESET]; - uint8_t cs = u8x8->pins[U8X8_PIN_CS]; + uint8_t dc = u8x8->pins[U8X8_PIN_DC]; + uint8_t reset = u8x8->pins[U8X8_PIN_RESET]; + uint8_t cs = u8x8->pins[U8X8_PIN_CS]; if (dc != U8X8_PIN_NONE) { 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; gpioConfig.pin_bit_mask = bitmask; - gpioConfig.mode = GPIO_MODE_OUTPUT; - gpioConfig.pull_up_en = GPIO_PULLUP_DISABLE; + gpioConfig.mode = GPIO_MODE_OUTPUT; + gpioConfig.pull_up_en = GPIO_PULLUP_DISABLE; gpioConfig.pull_down_en = GPIO_PULLDOWN_ENABLE; - gpioConfig.intr_type = GPIO_INTR_DISABLE; + gpioConfig.intr_type = GPIO_INTR_DISABLE; gpio_config(&gpioConfig); break; } diff --git a/main/ugv.cc b/main/ugv.cc index e347150..f5093ce 100644 --- a/main/ugv.cc +++ b/main/ugv.cc @@ -8,19 +8,19 @@ namespace ugv { static const char *TAG = "ugv_main"; -constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; -constexpr float LOOP_HZ = 1000000.f / static_cast(LOOP_PERIOD_US); -constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ; +constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; +constexpr float LOOP_HZ = 1000000.f / static_cast(LOOP_PERIOD_US); +constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ; constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000); constexpr uint64_t NOISE_PERIOD_US = 2000e3; constexpr float NOISE_PERIOD_S = static_cast(NOISE_PERIOD_US) * 1.e-6f; constexpr float ACCEL_NOISE_THRESHOLD = 0.001; -constexpr float GYRO_NOISE_THRESHOLD = 0.3; +constexpr float GYRO_NOISE_THRESHOLD = 0.3; void UpdateLocationFromGPS(comms::messages::Location &location, - const io::GpsData & gps_data) { + const io::GpsData &gps_data) { location.set_fix_quality(gps_data.fix_quality); location.set_latitude(gps_data.latitude); location.set_longitude(gps_data.longitude); @@ -35,8 +35,8 @@ extern "C" void UGV_TickTimeout(void *arg) { UGV::UGV() : angle_controller_(LOOP_PERIOD_S) { SetTarget({34.069022, -118.443067}); - comms = new CommsClass(); - io = new IOClass(); + comms = new CommsClass(); + io = new IOClass(); display = new DisplayClass(comms); SetConfig(DefaultConfig()); @@ -79,15 +79,15 @@ void UGV::Init() { display->Init(); esp_timer_create_args_t timer_args; - timer_args.callback = UGV_TickTimeout; - timer_args.arg = this; + timer_args.callback = UGV_TickTimeout; + timer_args.arg = this; 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_start_periodic(timer_handle, LOOP_PERIOD_US); last_print_ = 0; last_noise_ = 0; - last_left_ = 0; + last_left_ = 0; last_right_ = 0; } @@ -103,7 +103,7 @@ void UGV::UpdateAHRS() { yaw_ += 360.; } pitch_ = ahrs_.getPitch(); - roll_ = ahrs_.getRoll(); + roll_ = ahrs_.getRoll(); } void UGV::DoDebugPrint() { @@ -122,7 +122,7 @@ void UGV::ReadComms() { comms->status.set_pitch_angle(pitch_); comms->status.set_roll_angle(roll_); 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 = (xTaskGetTickCount() - comms->last_packet_tick); if (ticks_since_last_packet >= WATCHDOG_TICKS && @@ -160,31 +160,31 @@ void UGV::OnTick() { UpdateAHRS(); angle_controller_.Input(yaw_); - float drive_power = 0.; - outputs_.left_motor = 0.0; + float drive_power = 0.; + outputs_.left_motor = 0.0; outputs_.right_motor = 0.0; - float pitch = roll_; // TODO: fix quaternion -> YPR - auto min_flip_pitch = conf_.min_flip_pitch(); - bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); + float pitch = roll_; // TODO: fix quaternion -> YPR + auto min_flip_pitch = conf_.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_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate; - last_mpu_ = inputs_.mpu; + io::Vec3f d_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate; + last_mpu_ = inputs_.mpu; accel_noise_accum_ += d_accel.norm2() * LOOP_PERIOD_S; gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S; if (time_us >= last_noise_ + NOISE_PERIOD_US) { accel_noise_ = accel_noise_accum_ / NOISE_PERIOD_S; - gyro_noise_ = gyro_noise_accum_ / NOISE_PERIOD_S; - is_still_ = (accel_noise_ < ACCEL_NOISE_THRESHOLD) && + gyro_noise_ = gyro_noise_accum_ / NOISE_PERIOD_S; + is_still_ = (accel_noise_ < ACCEL_NOISE_THRESHOLD) && (gyro_noise_ < GYRO_NOISE_THRESHOLD); ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f", is_still_ ? "still" : "moving", accel_noise_, gyro_noise_); accel_noise_accum_ = 0; - gyro_noise_accum_ = 0; - last_noise_ = time_us; + gyro_noise_accum_ = 0; + last_noise_ = time_us; } ReadComms(); @@ -202,10 +202,10 @@ void UGV::OnTick() { break; } angle_controller_.Disable(); - TickType_t current_tick = xTaskGetTickCount(); + TickType_t current_tick = xTaskGetTickCount(); TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update; - bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); - bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID; + bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); + bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID; if (not_old && not_invalid) { next_state_ = UGV_State::STATE_TURNING; } @@ -213,7 +213,7 @@ void UGV::OnTick() { } case UGV_State::STATE_FLIPPING: { angle_controller_.Disable(); - outputs_.left_motor = 0.8; + outputs_.left_motor = 0.8; outputs_.right_motor = 0.8; if (!is_upside_down) { next_state_ = UGV_State::STATE_AQUIRING; @@ -232,7 +232,7 @@ void UGV::OnTick() { } 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_.Setpoint(tgt_bearing); @@ -252,7 +252,7 @@ void UGV::OnTick() { } 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()) { ESP_LOGI(TAG, "Finished driving to target"); @@ -268,7 +268,7 @@ void UGV::OnTick() { } case UGV_State::STATE_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); #else angle_controller_.Enable(); @@ -283,8 +283,8 @@ void UGV::OnTick() { } if (angle_controller_.Enabled()) { - float angle_pwr = angle_controller_.Update(); - outputs_.left_motor = drive_power - angle_pwr; + float angle_pwr = angle_controller_.Update(); + outputs_.left_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 && inputs_.mpu.gyro_rate.z == 0) { - outputs_.left_motor = 0; + outputs_.left_motor = 0; outputs_.right_motor = 0; } else { - float dleft = outputs_.left_motor - last_left_; + float dleft = outputs_.left_motor - last_left_; float dright = outputs_.right_motor - last_right_; if (dleft > MAX_ACCEL) { outputs_.left_motor = last_left_ + MAX_ACCEL; @@ -309,7 +309,7 @@ void UGV::OnTick() { } } io->WriteOutputs(outputs_); - last_left_ = outputs_.left_motor; + last_left_ = outputs_.left_motor; last_right_ = outputs_.right_motor; if (current_state_ != next_state_) { diff --git a/main/ugv.hh b/main/ugv.hh index ed4376f..e6bfa64 100644 --- a/main/ugv.hh +++ b/main/ugv.hh @@ -24,37 +24,37 @@ using ugv::io::IOClass; extern "C" void UGV_TickTimeout(void *arg); void UpdateLocationFromGPS(comms::messages::Location &location, - const io::GpsData & gps_data); + const io::GpsData &gps_data); class UGV { private: - CommsClass * comms; - IOClass * io; - DisplayClass * display; + CommsClass *comms; + IOClass *io; + DisplayClass *display; esp_timer_handle_t timer_handle; - LatLong target_; + LatLong target_; config::Config conf_; - PIDController angle_controller_; - Madgwick ahrs_; + PIDController angle_controller_; + Madgwick ahrs_; - io::Inputs inputs_; + io::Inputs inputs_; io::Outputs outputs_; - int64_t last_print_; - UGV_State current_state_; - UGV_State next_state_; - float yaw_; - float pitch_; - float roll_; + int64_t last_print_; + UGV_State current_state_; + UGV_State next_state_; + float yaw_; + float pitch_; + float roll_; io::MpuData last_mpu_; - int64_t last_noise_; - float accel_noise_accum_; - float gyro_noise_accum_; - float accel_noise_; - float gyro_noise_; - bool is_still_; - float last_left_; - float last_right_; + int64_t last_noise_; + float accel_noise_accum_; + float gyro_noise_accum_; + float accel_noise_; + float gyro_noise_; + bool is_still_; + float last_left_; + float last_right_; void UpdateAHRS(); void DoDebugPrint(); diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index b5c591d..11dc706 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -13,7 +13,7 @@ namespace ugv { 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); CommsClass::CommsClass() @@ -39,19 +39,19 @@ void CommsClass::Init() { status.set_state(messages::UGV_State::STATE_IDLE); #ifdef COMMS_SX127X - sx127x_config_t lora_config = sx127x_config_default(); - lora_config.sck_io_num = (gpio_num_t)LORA_SCK; - lora_config.miso_io_num = (gpio_num_t)LORA_MISO; - lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI; - lora_config.cs_io_num = (gpio_num_t)LORA_CS; - lora_config.rst_io_num = (gpio_num_t)LORA_RST; - lora_config.irq_io_num = (gpio_num_t)LORA_IRQ; - lora_config.frequency = LORA_FREQ; - lora_config.tx_power = 17; + sx127x_config_t lora_config = sx127x_config_default(); + lora_config.sck_io_num = (gpio_num_t)LORA_SCK; + lora_config.miso_io_num = (gpio_num_t)LORA_MISO; + lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI; + lora_config.cs_io_num = (gpio_num_t)LORA_CS; + lora_config.rst_io_num = (gpio_num_t)LORA_RST; + lora_config.irq_io_num = (gpio_num_t)LORA_IRQ; + lora_config.frequency = LORA_FREQ; + lora_config.tx_power = 17; lora_config.spreading_factor = 12; lora_config.signal_bandwidth = 10E3; - lora_config.sync_word = 0x34; - lora_config.crc = SX127X_CRC_ENABLED; + lora_config.sync_word = 0x34; + lora_config.crc = SX127X_CRC_ENABLED; ret = sx127x_init(&lora_config, &lora); if (ret != ESP_OK) { @@ -67,10 +67,10 @@ void CommsClass::Init() { ESP_LOGI(TAG, "LoRa initialized"); #else 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_tx_pin = LORA_TX; - ret = lora.Init(lora_config); + ret = lora.Init(lora_config); if (ret != ESP_OK) { const char *error_name = esp_err_to_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 sx127x_read_rssi(lora, &rssi); #else - rssi = 0; + rssi = 0; #endif return rssi; } @@ -121,13 +121,13 @@ void CommsClass::RunTask() { using messages::UGV_Status; TickType_t current_tick = xTaskGetTickCount(); - next_status_send = current_tick; + next_status_send = current_tick; #ifdef COMMS_SX127X sx127x_rx_packet_t rx_packet; #else std::string rx_buf; - int rx_len; + int rx_len; #endif while (true) { @@ -188,7 +188,7 @@ void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) { xSemaphoreTake(mutex, portMAX_DELAY); last_packet_rssi = rx_packet->rssi; - last_packet_snr = rx_packet->snr; + last_packet_snr = rx_packet->snr; xSemaphoreGive(mutex); 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); ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data); - int ret; - size_t decoded_size = (data_size * 4 + 2) / 3; - uint8_t *decoded = new uint8_t[decoded_size]; - size_t decoded_len; + int ret; + size_t decoded_size = (data_size * 4 + 2) / 3; + uint8_t *decoded = new uint8_t[decoded_size]; + size_t decoded_len; ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data, 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); return ESP_ERR_INVALID_SIZE; } - size_t encoded_size = ((size + 6) / 3) * 4; - uint8_t *encoded = new uint8_t[encoded_size]; - size_t encoded_len; - int ret = + size_t encoded_size = ((size + 6) / 3) * 4; + uint8_t *encoded = new uint8_t[encoded_size]; + size_t encoded_len; + int ret = mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data, size); if (ret != 0) { delete[] encoded; diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index bf142b5..ffd47d3 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -16,11 +16,11 @@ namespace ugv { namespace comms { namespace messages = ugv::messages; -namespace config = ugv::config; +namespace config = ugv::config; class CommsClass { 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); CommsClass(); @@ -35,14 +35,14 @@ class CommsClass { public: SemaphoreHandle_t mutex; - TickType_t last_packet_tick; - int32_t last_packet_rssi; - int8_t last_packet_snr; + TickType_t last_packet_tick; + int32_t last_packet_rssi; + int8_t last_packet_snr; - messages::UGV_Status status; + messages::UGV_Status status; messages::DriveHeadingData drive_heading; - messages::TargetLocation* new_target; - config::Config* new_config; + messages::TargetLocation* new_target; + config::Config* new_config; private: #ifdef COMMS_SX127X diff --git a/main/ugv_display.cc b/main/ugv_display.cc index 025a55f..f0a1460 100644 --- a/main/ugv_display.cc +++ b/main/ugv_display.cc @@ -37,7 +37,7 @@ void DisplayClass::Run() { int32_t lora_rssi; uint8_t lora_lna_gain; int32_t last_packet_rssi; - int8_t last_packet_snr; + int8_t last_packet_snr; #endif TickType_t last_packet_tick; messages::UGV_State state; @@ -46,7 +46,7 @@ void DisplayClass::Run() { while (true) { oled->firstPage(); #ifdef COMMS_SX127X - lora_rssi = comms_->ReadRssi(); + lora_rssi = comms_->ReadRssi(); lora_lna_gain = comms_->ReadLnaGain(); #endif @@ -55,7 +55,7 @@ void DisplayClass::Run() { state = comms_->status.state(); #ifdef COMMS_SX127X last_packet_rssi = comms_->last_packet_rssi; - last_packet_snr = comms_->last_packet_snr; + last_packet_snr = comms_->last_packet_snr; #endif comms_->Unlock(); do { @@ -71,7 +71,7 @@ void DisplayClass::Run() { heap_info.total_free_bytes); oled->setCursor(4, 3 * 8); - oled->printf("state: %d", (int) state); + oled->printf("state: %d", (int)state); if (last_packet_tick > 0) { double time_since_last_packet = diff --git a/main/ugv_io.cc b/main/ugv_io.cc index 373c83f..8da03ee 100644 --- a/main/ugv_io.cc +++ b/main/ugv_io.cc @@ -22,11 +22,11 @@ static const char *TAG = "ugv_io"; } \ } -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_DIR = GPIO_NUM_0; -static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12; -static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16; +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_DIR = GPIO_NUM_0; +static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12; +static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16; IOClass IO; @@ -51,11 +51,11 @@ void IOClass::InitMotors() { ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM)); mcpwm_config_t mcpwm_config; - mcpwm_config.frequency = 20000; // 20khz - mcpwm_config.cmpr_a = 0; - mcpwm_config.cmpr_b = 0; + mcpwm_config.frequency = 20000; // 20khz + mcpwm_config.cmpr_a = 0; + mcpwm_config.cmpr_b = 0; 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_1, &mcpwm_config)); } @@ -78,7 +78,7 @@ void IOClass::WriteOutputs(const Outputs &outputs) { ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); ERROR_CHECK(ret); 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; ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir); diff --git a/main/ugv_io.hh b/main/ugv_io.hh index 9073e4e..441ded1 100644 --- a/main/ugv_io.hh +++ b/main/ugv_io.hh @@ -30,7 +30,7 @@ class IOClass { private: UART_GPS gps_; - MPU mpu_; + MPU mpu_; void InitMotors(); }; diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc index 8517a68..151de22 100644 --- a/main/ugv_io_gps.cc +++ b/main/ugv_io_gps.cc @@ -10,13 +10,13 @@ namespace ugv { namespace io { -static constexpr size_t GPS_BUF_SIZE = 1024; -static constexpr uart_port_t GPS_UART = UART_NUM_1; -static constexpr int GPS_UART_TX_PIN = 25; -static constexpr int GPS_UART_RX_PIN = 26; -static constexpr int GPS_UART_BAUD = 9600; -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_BUF_SIZE = 1024; +static constexpr uart_port_t GPS_UART = UART_NUM_1; +static constexpr int GPS_UART_TX_PIN = 25; +static constexpr int GPS_UART_RX_PIN = 26; +static constexpr int GPS_UART_BAUD = 9600; +static constexpr size_t GPS_UART_RX_BUF_SIZE = 1024; +static constexpr size_t GPS_UART_TX_BUF_SIZE = 0; 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"; @@ -27,8 +27,8 @@ const char NMEA_OUTPUT_ALL[] = 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"; -const char NMEA_UPDATE_1HZ[] = "$PMTK220,1000*1F"; -const char NMEA_UPDATE_5HZ[] = "$PMTK220,200*2C"; +const char NMEA_UPDATE_1HZ[] = "$PMTK220,1000*1F"; +const char NMEA_UPDATE_5HZ[] = "$PMTK220,200*2C"; const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F"; const char NMEA_Q_RELEASE[] = "$PMTK605*31"; @@ -47,16 +47,16 @@ void UART_GPS::Init() { esp_err_t ret; mutex_ = xSemaphoreCreateMutex(); - data_ = GpsData{}; + data_ = GpsData{}; uart_config_t gps_uart_config; - gps_uart_config.baud_rate = GPS_UART_BAUD; - gps_uart_config.data_bits = UART_DATA_8_BITS; - gps_uart_config.parity = UART_PARITY_DISABLE; - gps_uart_config.stop_bits = UART_STOP_BITS_1; - gps_uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; + gps_uart_config.baud_rate = GPS_UART_BAUD; + gps_uart_config.data_bits = UART_DATA_8_BITS; + gps_uart_config.parity = UART_PARITY_DISABLE; + gps_uart_config.stop_bits = UART_STOP_BITS_1; + gps_uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; 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); if (ret != ESP_OK) { ESP_LOGE(TAG, "uart_param_config: %d", ret); @@ -115,17 +115,17 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { return; case MINMEA_SENTENCE_RMC: { minmea_sentence_rmc rmc; - bool parse_success; + bool parse_success; parse_success = minmea_parse_rmc(&rmc, line); if (!parse_success) { goto invalid; } xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); - data_.valid = rmc.valid; - data_.latitude = minmea_tocoord(&rmc.latitude); - data_.longitude = minmea_tocoord(&rmc.longitude); - data_.speed = minmea_tofloat(&rmc.speed); - data_.course = minmea_tofloat(&rmc.course); + data_.valid = rmc.valid; + data_.latitude = minmea_tocoord(&rmc.latitude); + data_.longitude = minmea_tocoord(&rmc.longitude); + data_.speed = minmea_tofloat(&rmc.speed); + data_.course = minmea_tofloat(&rmc.course); data_.last_update = xTaskGetTickCount(); ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f", data_.valid ? "valid" : "invalid", data_.latitude, @@ -135,20 +135,20 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { } case MINMEA_SENTENCE_GGA: { minmea_sentence_gga gga; - bool parse_success; + bool parse_success; parse_success = minmea_parse_gga(&gga, line); if (!parse_success) { goto invalid; } 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_.latitude = minmea_tocoord(&gga.latitude); - data_.longitude = minmea_tocoord(&gga.longitude); + data_.latitude = minmea_tocoord(&gga.latitude); + data_.longitude = minmea_tocoord(&gga.longitude); if (gga.fix_quality != 0 && gga.altitude_units != 'M') { 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(); ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f", 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: { minmea_sentence_gsa gsa; - bool parse_success; + bool parse_success; parse_success = minmea_parse_gsa(&gsa, line); if (!parse_success) { goto invalid; } xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); - data_.pdop = minmea_tofloat(&gsa.pdop); - data_.hdop = minmea_tofloat(&gsa.hdop); - data_.vdop = minmea_tofloat(&gsa.vdop); - ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", - data_.pdop, data_.hdop, data_.vdop); + data_.pdop = minmea_tofloat(&gsa.pdop); + data_.hdop = minmea_tofloat(&gsa.hdop); + data_.vdop = minmea_tofloat(&gsa.vdop); + ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", data_.pdop, data_.hdop, + data_.vdop); xSemaphoreGive(mutex_); break; } diff --git a/main/ugv_io_gps.hh b/main/ugv_io_gps.hh index 8e72ebf..3857bc7 100644 --- a/main/ugv_io_gps.hh +++ b/main/ugv_io_gps.hh @@ -9,22 +9,22 @@ namespace ugv { namespace io { enum GpsFixQual { - GPS_FIX_INVALID = 0, // invalid gps fix - GPS_FIX_GPS = 1, // GPS fix - GPS_FIX_DGPS = 2, // differential GPS fix - GPS_FIX_PPS = 3, // PPS fix - GPS_FIX_RTK = 4, // Real Time Kinematic fix - GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix + GPS_FIX_INVALID = 0, // invalid gps fix + GPS_FIX_GPS = 1, // GPS fix + GPS_FIX_DGPS = 2, // differential GPS fix + GPS_FIX_PPS = 3, // PPS fix + GPS_FIX_RTK = 4, // Real Time Kinematic fix + GPS_FIX_FRTK = 5, // Float Real Time Kinematic 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 }; struct GpsData { TickType_t last_update; - bool valid; + bool valid; GpsFixQual fix_quality; - uint8_t num_satellites; + uint8_t num_satellites; float latitude; // degrees +/- float longitude; // degrees +/- @@ -46,17 +46,17 @@ class UART_GPS { void GetData(GpsData& data); private: - TaskHandle_t task_; + TaskHandle_t task_; SemaphoreHandle_t mutex_; - QueueHandle_t uart_queue_; - GpsData data_; - uint8_t* buffer_; + QueueHandle_t uart_queue_; + GpsData data_; + uint8_t* buffer_; esp_err_t WriteCommand(const char* cmd, size_t len); - void HandleUartPattern(); - void ProcessLine(const char* line, size_t len); + void HandleUartPattern(); + void ProcessLine(const char* line, size_t len); - void DoTask(); + void DoTask(); static void TaskEntry(void* arg); }; diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index 959bf5e..b929e79 100644 --- a/main/ugv_io_mpu.cc +++ b/main/ugv_io_mpu.cc @@ -14,23 +14,23 @@ constexpr float M_PI = 3.1415926535897932384626433832795; namespace ugv { namespace io { -static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5; -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::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS; -static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); +static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5; +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::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS; +static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); static const Vec3f ACCEL_OFFSET = {0., 0., 0.}; -static const Mat3f ACCEL_MAT = {-1., 0., 0., 0., -1., 0., 0., 0., -1.}; +static const Mat3f ACCEL_MAT = {-1., 0., 0., 0., -1., 0., 0., 0., -1.}; // static const Vec3f MAG_OFFSET = {-7.79683, 3.6735, 32.3868}; // static const Vec3f MAG_OFFSET = {-118.902, 18.8173, -39.209}; static const Vec3f MAG_OFFSET = {-135.994629, 19.117216, -33.0}; // static const Mat3f MAG_MAT = {0., -0.0281408, 0., -0.0284409, 0., 0., // 0., 0., 0.0261544}; -static const Mat3f MAG_MAT = {0., -0.0335989, 0., -0.0330167, 0., +static const Mat3f MAG_MAT = {0., -0.0335989, 0., -0.0330167, 0., 0., 0., 0., 0.0335989}; static const Vec3f GYRO_OFFSET = {-4.33655, -2.76826, -0.908427}; -static const Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; +static const Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; static const char *TAG = "ugv_io_mpu"; @@ -69,7 +69,7 @@ void MPU::Calibrate() { void MPU::GetData(MpuData &data) { xSemaphoreTake(mutex_, pdMS_TO_TICKS(10)); - data = last_data_; + data = last_data_; last_data_.gyro_rate = {0, 0, 0}; xSemaphoreGive(mutex_); } @@ -83,7 +83,7 @@ bool MPU::Initialize() { mpu_ = new mpud::MPU(*mpu_bus_); esp_err_t ret; - int tries = 0; + int tries = 0; for (; tries < 5; ++tries) { mpu_->getInterruptStatus(); ret = mpu_->testConnection(); @@ -122,8 +122,8 @@ bool MPU::Initialize() { void MPU::DoTask() { ESP_LOGI(TAG, "MPU task start"); mpud::raw_axes_t accel_, gyro_, mag_; - esp_err_t ret; - MpuData data; + esp_err_t ret; + MpuData data; if (!Initialize()) { return; @@ -154,14 +154,14 @@ void MPU::DoTask() { int16_t mz = (static_cast(compass_data[5]) << 8) | static_cast(compass_data[4]); // ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz); - data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); - data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET); + data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); + data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET); data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET); - data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; - data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; - data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; - data.mag = MAG_MAT * (data.mag + MAG_OFFSET); + data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX; + data.mag.y = ((float)my) * MPU_MAG_TO_FLUX; + data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX; + data.mag = MAG_MAT * (data.mag + MAG_OFFSET); xSemaphoreTake(mutex_, pdMS_TO_TICKS(100)); last_data_ = data; xSemaphoreGive(mutex_); diff --git a/main/ugv_io_mpu.hh b/main/ugv_io_mpu.hh index c0a66d2..3f045a7 100644 --- a/main/ugv_io_mpu.hh +++ b/main/ugv_io_mpu.hh @@ -99,14 +99,14 @@ class MPU { void GetData(MpuData& data); private: - mpud::mpu_bus_t* mpu_bus_; - mpud::MPU* mpu_; - TaskHandle_t task_; + mpud::mpu_bus_t* mpu_bus_; + mpud::MPU* mpu_; + TaskHandle_t task_; SemaphoreHandle_t mutex_; - MpuData last_data_; + MpuData last_data_; - void DoTask(); - bool Initialize(); + void DoTask(); + bool Initialize(); static void TaskEntry(void* arg); };