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

clang-format without silly alignment

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

4
.clang-format
View File

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

58
main/MadgwickAHRS.cpp
View File

@ -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;
}

2
main/MadgwickAHRS.h
View File

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

14
main/Print.cpp
View File

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

8
main/Print.h
View File

@ -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);
}

32
main/U8g2lib.hh
View File

@ -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);

74
main/e32_driver.cc
View File

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

36
main/e32_driver.hh
View File

@ -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_;

24
main/lat_long.cc
View File

@ -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
View File

@ -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
View File

@ -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_;

76
main/u8g2_esp32_hal.c
View File

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

72
main/ugv.cc
View File

@ -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) {
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_) {

44
main/ugv.hh
View File

@ -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
View File

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

16
main/ugv_comms.hh
View File

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

8
main/ugv_display.cc
View File

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

20
main/ugv_io.cc
View File

@ -22,11 +22,11 @@ static const char *TAG = "ugv_io";
} \
}
static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0;
static constexpr 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);

2
main/ugv_io.hh
View File

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

66
main/ugv_io_gps.cc
View File

@ -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;
}

32
main/ugv_io_gps.hh
View File

@ -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);
};

36
main/ugv_io_mpu.cc
View File

@ -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<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
View File

@ -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);
};