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

58
main/MadgwickAHRS.cpp
Unescape View File

@ -36,12 +36,12 @@ @@ -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, @@ -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, @@ -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, @@ -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;
}

2
main/MadgwickAHRS.h
Unescape View File

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

14
main/Print.cpp
Unescape View File

@ -41,8 +41,8 @@ size_t Print::write(const uint8_t *buffer, size_t size) { @@ -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) { @@ -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) { @@ -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) { @@ -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) { @@ -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

8
main/Print.h
Unescape View File

@ -31,7 +31,7 @@ @@ -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 { @@ -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);
}

32
main/U8g2lib.hh
Unescape View File

@ -6,7 +6,7 @@ @@ -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 { @@ -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 { @@ -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 { @@ -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 { @@ -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, @@ -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);

74
main/e32_driver.cc
Unescape View File

@ -5,40 +5,40 @@ @@ -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(); } @@ -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: @@ -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) { @@ -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) { @@ -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) { @@ -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) { @@ -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;

36
main/e32_driver.hh
Unescape View File

@ -9,16 +9,16 @@ namespace e32 { @@ -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 { @@ -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 { @@ -80,7 +80,7 @@ class E32_Driver {
void Flush();
private:
bool initialized_;
bool initialized_;
Config config_;
Params params_;

24
main/lat_long.cc
Unescape View File

@ -1,30 +1,30 @@ @@ -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;

10
main/pid_controller.cc
Unescape View File

@ -34,11 +34,11 @@ float PIDController::Error() const { @@ -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;
}

10
main/pid_controller.hh
Unescape View File

@ -22,16 +22,16 @@ class PIDController { @@ -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 { @@ -56,7 +56,7 @@ class PIDController {
float max_output_;
float max_i_error_;
bool enabled_;
bool enabled_;
float setpoint_;
float input_;
float output_;

76
main/u8g2_esp32_hal.c
Unescape View File

@ -2,19 +2,19 @@ @@ -2,19 +2,19 @@
#include <string.h>
#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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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;
}

72
main/ugv.cc
Unescape View File

@ -8,19 +8,19 @@ namespace ugv { @@ -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<float>(LOOP_PERIOD_US);
constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000);
constexpr uint64_t NOISE_PERIOD_US = 2000e3;
constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f;
constexpr float ACCEL_NOISE_THRESHOLD = 0.001;
constexpr float GYRO_NOISE_THRESHOLD = 0.3;
constexpr float GYRO_NOISE_THRESHOLD = 0.3;
void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) {
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) { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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_) {

44
main/ugv.hh
Unescape View File

@ -24,37 +24,37 @@ using ugv::io::IOClass; @@ -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();

52
main/ugv_comms.cc
Unescape View File

@ -13,7 +13,7 @@ @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -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) { @@ -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) { @@ -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) { @@ -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;

16
main/ugv_comms.hh
Unescape View File

@ -16,11 +16,11 @@ namespace ugv { @@ -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 { @@ -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

8
main/ugv_display.cc
Unescape View File

@ -37,7 +37,7 @@ void DisplayClass::Run() { @@ -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() { @@ -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() { @@ -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() { @@ -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 =

20
main/ugv_io.cc
Unescape View File

@ -22,11 +22,11 @@ static const char *TAG = "ugv_io"; @@ -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() { @@ -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) { @@ -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);

2
main/ugv_io.hh
Unescape View File

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

66
main/ugv_io_gps.cc
Unescape View File

@ -10,13 +10,13 @@ @@ -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[] = @@ -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() { @@ -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) { @@ -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) { @@ -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) { @@ -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;
}

32
main/ugv_io_gps.hh
Unescape View File

@ -9,22 +9,22 @@ namespace ugv { @@ -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 { @@ -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);
};

36
main/ugv_io_mpu.cc
Unescape View File

@ -14,23 +14,23 @@ constexpr float M_PI = 3.1415926535897932384626433832795; @@ -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() { @@ -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() { @@ -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() { @@ -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() { @@ -154,14 +154,14 @@ void MPU::DoTask() {
int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) |
static_cast<int16_t>(compass_data[4]);
// ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
data.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_);

12
main/ugv_io_mpu.hh
Unescape View File

@ -99,14 +99,14 @@ class MPU { @@ -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);
};

Write Preview
Loading…
Cancel
Save