diff --git a/.gitmodules b/.gitmodules index 0eb043b..55da6f1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,4 +9,7 @@ url = https://github.com/natanaeljr/esp32-MPU-driver.git [submodule "components/I2Cbus"] path = components/I2Cbus - url = https://github.com/natanaeljr/esp32-I2Cbus.git \ No newline at end of file + url = https://github.com/natanaeljr/esp32-I2Cbus.git +[submodule "components/minmea/minmea"] + path = components/minmea/minmea + url = https://github.com/kosma/minmea.git diff --git a/components/minmea/CMakeLists.txt b/components/minmea/CMakeLists.txt new file mode 100644 index 0000000..a91095c --- /dev/null +++ b/components/minmea/CMakeLists.txt @@ -0,0 +1,7 @@ +set(COMPONENT_SRCS + "minmea/minmea.c") +set(COMPONENT_ADD_INCLUDEDIRS "minmea") + +register_component() + +component_compile_options(PUBLIC -Dtimegm=mktime) diff --git a/components/minmea/minmea b/components/minmea/minmea new file mode 160000 index 0000000..ae4dd94 --- /dev/null +++ b/components/minmea/minmea @@ -0,0 +1 @@ +Subproject commit ae4dd9442a9041345d5ef108f062e7e4ec6954f2 diff --git a/components/protobuf/protobuf b/components/protobuf/protobuf new file mode 160000 index 0000000..e8ae137 --- /dev/null +++ b/components/protobuf/protobuf @@ -0,0 +1 @@ +Subproject commit e8ae137c96444ea313485ed1118c5e43b2099cf1 diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index ba2c069..ea404b4 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -8,11 +8,12 @@ nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) list(APPEND COMPONENT_SRCS "ugv_main.cc" "ugv_comms.c" "ugv_io.cc" + "ugv_io_gps.cc" "u8g2_esp32_hal.c" "Print.cpp" ${PROTO_SRCS}) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) -set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver") +set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver" "minmea") register_component() diff --git a/main/ugv_io.cc b/main/ugv_io.cc index 9bed146..2e5accb 100644 --- a/main/ugv_io.cc +++ b/main/ugv_io.cc @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -20,19 +21,35 @@ 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 size_t GPS_BUF_SIZE = 1024; +static constexpr uart_port_t GPS_UART = UART_NUM_1; +static constexpr int GPS_UART_TX_PIN = 17; +static constexpr int GPS_UART_RX_PIN = 23; +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_UART_QUEUE_SIZE = 8; + +const char NMEA_OUTPUT_RMCGGA[] = + "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"; +const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n"; +const char NMEA_Q_RELEASE[] = "$PMTK605*31\r\n"; + static const char *TAG = "ugv_io"; -IO io; +IOClass IO; -IO::IO() { mpu_ = new mpud::MPU(); } +IOClass::IOClass() { mpu_ = new mpud::MPU(); gps_ = new UART_GPS(); } -IO::~IO() { - delete mpu_; -} +IOClass::~IOClass() { delete mpu_; delete gps_; } -void IO::Init() { - esp_err_t ret; +void IOClass::Init() { + InitMotors(); + InitMPU(); + gps_->Init(); +} +void IOClass::InitMotors() { gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT); gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT); gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT); @@ -47,30 +64,31 @@ void IO::Init() { mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config); +} + +void IOClass::InitMPU() { + esp_err_t ret; + ret = mpu_->testConnection(); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "MPU not connected"); + return; + } + ret = mpu_->initialize(); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "MPU initialization error"); + return; + } + mpu_->setAccelFullScale(MPU_ACCEL_FS); + mpu_->setGyroFullScale(MPU_GYRO_FS); + ESP_LOGI(TAG, "MPU initialized"); +} - do { - ret = mpu_->testConnection(); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "MPU not connected"); - break; - } - ret = mpu_->initialize(); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "MPU initialization error"); - break; - } - mpu_->setAccelFullScale(MPU_ACCEL_FS); - mpu_->setGyroFullScale(MPU_GYRO_FS); - ESP_LOGI(TAG, "MPU initialized"); - } while (0); - -} // namespace io - -void IO::ReadInputs(Inputs &inputs) { +void IOClass::ReadInputs(Inputs &inputs) { esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_); if (ret != ESP_OK) { ESP_LOGE(TAG, "error reading MPU"); } + gps_->GetInfo(inputs.gps_info); inputs.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); inputs.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); inputs.mag.x = ((float)mag_.x) * MPU_MAG_TO_FLUX; @@ -78,7 +96,7 @@ void IO::ReadInputs(Inputs &inputs) { inputs.mag.z = ((float)mag_.z) * MPU_MAG_TO_FLUX; } -void IO::WriteOutputs(const Outputs &outputs) { +void IOClass::WriteOutputs(const Outputs &outputs) { mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, fabs(outputs.left_motor)); mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B, diff --git a/main/ugv_io.hh b/main/ugv_io.hh index 154d354..7a0f704 100644 --- a/main/ugv_io.hh +++ b/main/ugv_io.hh @@ -1,34 +1,16 @@ #pragma once +#include #include #include +#include "ugv_io_gps.hh" + namespace ugv { namespace io { using mpud::float_axes_t; -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_ESTIMATED = 6, // Estimated fix - GPS_FIX_MANUAL = 7, // Manual fix - GPS_FIX_SIMULATED = 8, // Simulated fix -}; - -struct GpsInfo { - GpsFixQual fix_quality; - uint8_t num_satellites; - - double latitude; // degrees +/- - double longitude; // degrees +/- - double altitude; // meters -}; - struct Inputs { // G's float_axes_t accel; @@ -45,10 +27,10 @@ struct Outputs { float right_motor; // right motor power -1.0 to +1.0 }; -class IO { +class IOClass { public: - explicit IO(); - ~IO(); + explicit IOClass(); + ~IOClass(); void Init(); @@ -58,9 +40,14 @@ class IO { private: mpud::MPU * mpu_; mpud::raw_axes_t accel_, gyro_, mag_; + + UART_GPS *gps_; + + void InitMotors(); + void InitMPU(); }; -extern IO io; +extern IOClass IO; } // namespace io } // namespace ugv diff --git a/main/ugv_io_gps.cc b/main/ugv_io_gps.cc new file mode 100644 index 0000000..f84cc6c --- /dev/null +++ b/main/ugv_io_gps.cc @@ -0,0 +1,216 @@ +#include "ugv_io_gps.hh" + +#include +#include +#include +#include + +#include "minmea.h" + +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 = 17; +static constexpr int GPS_UART_RX_PIN = 23; +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_UART_QUEUE_SIZE = 8; + +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"; +const char NMEA_OUTPUT_RMCGGA[] = + "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"; +const char NMEA_OUTPUT_ALL[] = + "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28"; +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_10HZ[] = "$PMTK220,100*2F"; + +const char NMEA_Q_RELEASE[] = "$PMTK605*31"; + +const char NMEA_END_CMD[] = "\r\n"; + +std::string st; + +static const char *TAG = "ugv_io"; + +UART_GPS::UART_GPS() {} + +UART_GPS::~UART_GPS() {} + +void UART_GPS::Init() { + esp_err_t ret; + + 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.rx_flow_ctrl_thresh = 122; + 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); + return; + } + uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_TX_PIN, UART_PIN_NO_CHANGE, + UART_PIN_NO_CHANGE); + ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE, + GPS_UART_TX_BUF_SIZE, GPS_UART_QUEUE_SIZE, NULL, 0); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "uart_driver_install: %d", ret); + return; + } + ESP_LOGI(TAG, "gps uart configured"); + + mutex_ = xSemaphoreCreateMutex(); + info_ = GpsInfo{}; + + BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024, + this, 1, &this->task_); + if (xRet != pdTRUE) { + ESP_LOGE(TAG, "error creating GPS task"); + return; + } +} + +void UART_GPS::GetInfo(GpsInfo &info) { + xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); + info = GpsInfo(info_); + xSemaphoreGive(mutex_); +} + +esp_err_t UART_GPS::WriteCommand(const char *cmd, size_t len) { + esp_err_t ret; + ret = uart_write_bytes(GPS_UART, cmd, len); + if (ret != ESP_OK) goto err; + ret = uart_write_bytes(GPS_UART, NMEA_END_CMD, sizeof(NMEA_END_CMD)); + if (ret != ESP_OK) goto err; + ret = uart_wait_tx_done(GPS_UART, 1000); + if (ret != ESP_OK) goto err; + + return ESP_OK; + +err: + const char *error_name = esp_err_to_name(ret); + ESP_LOGE(TAG, "WriteCommand error: %s (%d)", error_name, ret); + return ret; +} + +void UART_GPS::ProcessLine(const char *line, size_t len) { + ESP_LOGI(TAG, "gps data: %.*s", len, line); + enum minmea_sentence_id id = minmea_sentence_id(line, false); + switch (id) { + case MINMEA_INVALID: + invalid: + ESP_LOGE(TAG, "invalid nmea sentence: %s", line); + return; + case MINMEA_SENTENCE_RMC: { + minmea_sentence_rmc rmc; + bool parse_success; + parse_success = minmea_parse_rmc(&rmc, line); + if (!parse_success) { + goto invalid; + } + xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); + info_.valid = rmc.valid; + info_.latitude = minmea_tofloat(&rmc.latitude); + info_.longitude = minmea_tofloat(&rmc.longitude); + info_.speed = minmea_tofloat(&rmc.speed); + info_.course = minmea_tofloat(&rmc.course); + info_.last_update = xTaskGetTickCount(); + xSemaphoreGive(mutex_); + } + case MINMEA_SENTENCE_GGA: { + minmea_sentence_gga gga; + bool parse_success; + parse_success = minmea_parse_gga(&gga, line); + if (!parse_success) { + goto invalid; + } + xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000)); + info_.fix_quality = (GpsFixQual)gga.fix_quality; + info_.num_satellites = gga.satellites_tracked; + info_.latitude = minmea_tofloat(&gga.latitude); + info_.longitude = minmea_tofloat(&gga.longitude); + if (gga.altitude_units != 'M') { + ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units); + } + info_.altitude = minmea_tofloat(&gga.altitude); + info_.last_update = xTaskGetTickCount(); + xSemaphoreGive(mutex_); + } + default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line); + } +} + +void UART_GPS::GPS_Task(void *arg) { + UART_GPS *gps = (UART_GPS *)arg; + (void)gps; + + ESP_LOGI(TAG, "gps_task start"); + char * buf1 = (char *)malloc(GPS_BUF_SIZE); + char * buf2 = (char *)malloc(GPS_BUF_SIZE); + char * buf_end = buf1; + size_t read_bytes; + + esp_err_t ret; + ret = gps->WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA)); + ESP_LOGI(TAG, "sent output rmc and gga: %d", ret); + ret = gps->WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ)); + ESP_LOGI(TAG, "sent update 1hz: %d", ret); + + vTaskDelay(pdMS_TO_TICKS(100)); + ret = gps->WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE)); + ESP_LOGI(TAG, "sent q release: %d", ret); + + while (true) { + size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1); + read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining, + portMAX_DELAY); + if (read_bytes <= 0) { + ESP_LOGW(TAG, "GPS error: %d", read_bytes); + continue; + } + ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end); + buf_end += read_bytes; + int lines_received = 0; + for (char *c = buf1; c < buf_end; c++) { + if (*c != '\n') { // wait for \n, if not found loop again + continue; + } + lines_received++; + c++; // now c is one past the end of the whole string including \n + + size_t remaining = buf_end - c; + memcpy(buf2, c, remaining); // copy remaining text from buf1 to buf2 + buf_end = buf2 + remaining; // update buf_end to point to the end of the + // remaining text in buf2 + + char * str_ptr = buf1; + size_t str_len = c - buf1; + str_ptr[str_len] = + '\0'; // append the NULL byte (safe because old data was copied) + gps->ProcessLine(str_ptr, str_len); // process that line + + std::swap(buf1, buf2); // swap buffers + c = buf1; // start over on text which was copied to buf2 (now buf1) + } + if (lines_received == 0) { + ESP_LOGW(TAG, "no lines received, waiting for more data"); + } + } + free(buf1); + free(buf2); + vTaskDelete(NULL); +} + +}; // namespace io +}; // namespace ugv diff --git a/main/ugv_io_gps.hh b/main/ugv_io_gps.hh new file mode 100644 index 0000000..2566289 --- /dev/null +++ b/main/ugv_io_gps.hh @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include +#include + +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_ESTIMATED = 6, // Estimated fix + GPS_FIX_MANUAL = 7, // Manual fix + GPS_FIX_SIMULATED = 8, // Simulated fix +}; + +struct GpsInfo { + TickType_t last_update; + bool valid; + GpsFixQual fix_quality; + uint8_t num_satellites; + + float latitude; // degrees +/- + float longitude; // degrees +/- + float altitude; // meters + float speed; // knots + float course; // degrees clockwise of north +}; + +class UART_GPS { + public: + explicit UART_GPS(); + ~UART_GPS(); + + void Init(); + + void GetInfo(GpsInfo& info); + + private: + TaskHandle_t task_; + SemaphoreHandle_t mutex_; + GpsInfo info_; + + esp_err_t WriteCommand(const char* cmd, size_t len); + void ProcessLine(const char* line, size_t len); + + static void GPS_Task(void* arg); +}; + +} // namespace io +} // namespace ugv diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 4a53ef0..d21cc02 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -1,14 +1,17 @@ -#include #include #include #include +#include "U8g2lib.hh" #include "sx127x_driver.h" #include "sx127x_registers.h" -#include "u8g2_esp32_hal.h" #include "ugv_comms.h" #include "ugv_config.h" -#include "U8g2lib.hh" +#include "ugv_io.hh" + +namespace ugv { + +using ugv::io::IO; static const char *TAG = "ugv_main"; @@ -21,69 +24,12 @@ void setup_oled(void) { oled->setPowerSave(false); } -const size_t GPS_BUF_SIZE = 1024; - -const char NMEA_OUTPUT_RMCGGA[] = - "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"; -const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n"; -const char NMEA_Q_RELEASE[] = "$PMTK605*31\r\n"; - -static void gps_task(void *arg) { - ESP_LOGI(TAG, "gps_task start"); - uint8_t * data = (uint8_t*) malloc(GPS_BUF_SIZE); - size_t read_bytes; - esp_err_t ret; - uart_write_bytes(GPS_UART, NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA)); - ret = uart_wait_tx_done(GPS_UART, 1000); - ESP_LOGI(TAG, "sent output rmc and gga: %d", ret); - uart_write_bytes(GPS_UART, NMEA_SET_UPDATE_1HZ, sizeof(NMEA_SET_UPDATE_1HZ)); - ret = uart_wait_tx_done(GPS_UART, 1000); - ESP_LOGI(TAG, "sent update 1hz: %d", ret); - vTaskDelay(pdMS_TO_TICKS(100)); - uart_write_bytes(GPS_UART, NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE)); - ret = uart_wait_tx_done(GPS_UART, 1000); - ESP_LOGI(TAG, "sent q release: %d", ret); - while (true) { - read_bytes = - uart_read_bytes(GPS_UART, data, GPS_BUF_SIZE, pdMS_TO_TICKS(100)); - if (read_bytes > 0) { - ESP_LOGI(TAG, "gps data: %.*s", read_bytes, data); - } - } -} - -void setup_gps(void) { - uart_config_t gps_uart_config; - esp_err_t ret; - // gps_uart_config.baud_rate = 9600; - gps_uart_config.baud_rate = 57600; - 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; - ret = uart_param_config(GPS_UART, &gps_uart_config); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "uart_param_config: %d", ret); - return; - } - uart_set_pin(GPS_UART, 17, 23, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); - ret = uart_driver_install(GPS_UART, 1024, 0, 8, NULL, 0); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "uart_driver_install: %d", ret); - return; - } - ESP_LOGI(TAG, "gps uart configured"); - xTaskCreate(&gps_task, "gps_task", 2 * 1024, NULL, 1, NULL); -} - void setup(void) { ESP_LOGI(TAG, "setup"); - setup_gps(); setup_oled(); ugv_comms_init(); + IO.Init(); } #define BUF_SZ 32 @@ -96,6 +42,10 @@ void loop(void) { static int8_t last_packet_snr; static char buf[BUF_SZ]; + static io::Inputs inputs; + + IO.ReadInputs(inputs); + // ESP_LOGI(TAG, "inputs %s", inputs.ToString()); oled->firstPage(); sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi); @@ -143,6 +93,8 @@ void loopTask(void *pvUser) { } } +} // namespace ugv + extern "C" void app_main() { - xTaskCreatePinnedToCore(loopTask, "loopTask", 8192, NULL, 1, NULL, 1); + xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1); }