From d738c2b4a74b34e75d1688543420fd7ca2486fab Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Wed, 15 May 2019 21:50:38 -0700 Subject: [PATCH] refactor everything --- main/CMakeLists.txt | 1 + main/ugv.cc | 255 ++++++++++++++++++++++++++++++++++++++++ main/ugv.hh | 68 +++++++++++ main/ugv_main.cc | 279 +------------------------------------------- 4 files changed, 328 insertions(+), 275 deletions(-) create mode 100644 main/ugv.cc create mode 100644 main/ugv.hh diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 8298137..a9a9438 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -15,6 +15,7 @@ set(COMPONENT_SRCS "e32_driver.cc" "pid_controller.cc" "lat_long.cc" + "ugv.cc" ) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") diff --git a/main/ugv.cc b/main/ugv.cc new file mode 100644 index 0000000..cc5777b --- /dev/null +++ b/main/ugv.cc @@ -0,0 +1,255 @@ +#include "ugv.hh" + +#include "ugv_comms.hh" +#include "ugv_display.hh" +#include "ugv_io.hh" + +namespace ugv { + +static const char *TAG = "ugv_main"; + +constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; +constexpr float LOOP_PERIOD_S = 1000000.f / static_cast(LOOP_PERIOD_US); + +void UpdateLocationFromGPS(comms::messages::Location &location, + 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); + location.set_altitude(gps_data.altitude); +} + +extern "C" void UGV_TickTimeout(void *arg) { + UGV *ugv = (UGV *)arg; + ugv->OnTick(); +} + +UGV::UGV() : angle_controller_(LOOP_PERIOD_S) { + SetTarget({34.069022, -118.443067}); + + comms = new CommsClass(); + io = new IOClass(); + display = new DisplayClass(comms); + + SetConfig(DefaultConfig()); +} + +config::Config UGV::DefaultConfig() { + config::Config c; + + auto *apid = c.mutable_angle_pid(); + apid->set_kp(0.10); + apid->set_ki(0.0); + apid->set_kd(0.4); + apid->set_max_output(0.5); + apid->set_max_i_error(15.0); + + c.set_min_target_dist(10.0); + c.set_min_flip_pitch(90.0); + return c; +} + +void UGV::SetConfig(const config::Config &conf) { + auto &apid = conf.angle_pid(); + angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd()); + angle_controller_.MaxOutput(apid.max_output()); + angle_controller_.MaxIError(apid.max_i_error()); + conf_ = conf; +} + +void UGV::SetTarget(LatLong target) { target_ = target; } + +void UGV::Init() { + esp_timer_init(); + + ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency + + io->Init(); + comms->Init(); + display->Init(); + + esp_timer_create_args_t timer_args; + timer_args.callback = UGV_TickTimeout; + timer_args.arg = this; + timer_args.dispatch_method = ESP_TIMER_TASK; + 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; +} + +void UGV::UpdateAHRS() { + io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel, + &m = inputs_.mpu.mag; + ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z); +} + +void UGV::DoDebugPrint() { + auto &mpu = inputs_.mpu; + ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", + mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x, + mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z); + ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), + ahrs_.getPitch(), ahrs_.getRoll()); + ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error()); +} + +void UGV::ReadComms() { + comms->Lock(); + UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps); + comms->status.set_yaw_angle(ahrs_.getYaw()); + current_state_ = comms->status.state(); + if (comms->new_target) { + SetTarget(*comms->new_target); + ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude, + target_.longitude); + delete comms->new_target; + comms->new_target = nullptr; + } + if (comms->new_config) { + ESP_LOGI(TAG, "Updating config"); + SetConfig(*comms->new_config); + delete comms->new_config; + comms->new_config = nullptr; + } + comms->Unlock(); +} + +void UGV::OnTick() { + ESP_LOGV(TAG, "OnTick"); + int64_t time_us = esp_timer_get_time(); + // float time_s = ((float)time_us) / 1e6; + io->ReadInputs(inputs_); + UpdateAHRS(); + + if (time_us >= last_print_ + 500 * 1000) { + DoDebugPrint(); + last_print_ = time_us; + } + + ReadComms(); + next_state_ = current_state_; + + angle_controller_.Input(ahrs_.getYaw()); + float drive_power = 0.; + outputs_.left_motor = 0.0; + outputs_.right_motor = 0.0; + + float pitch = ahrs_.getPitch(); + auto min_flip_pitch = conf_.min_flip_pitch(); + bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); + + switch (current_state_) { + default: + ESP_LOGW(TAG, "unhandled state: %d", current_state_); + // fall through + case UGV_State::STATE_IDLE: + case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break; + case UGV_State::STATE_AQUIRING: { + if (is_upside_down) { + next_state_ = UGV_State::STATE_FLIPPING; + break; + } + angle_controller_.Disable(); + 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; + if (not_old && not_invalid) { + next_state_ = UGV_State::STATE_TURNING; + } + break; + } + case UGV_State::STATE_FLIPPING: { + angle_controller_.Disable(); + outputs_.left_motor = -1.0; + outputs_.right_motor = -1.0; + if (!is_upside_down) { + next_state_ = UGV_State::STATE_AQUIRING; + break; + } + break; + } + case UGV_State::STATE_TURNING: { + if (is_upside_down) { + next_state_ = UGV_State::STATE_FLIPPING; + break; + } + if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) { + next_state_ = UGV_State::STATE_AQUIRING; + break; + } + + LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; + float tgt_bearing = current_pos.bearing_toward(target_); + angle_controller_.Enable(); + angle_controller_.Setpoint(tgt_bearing); + + if (fabs(angle_controller_.Error()) <= 5.0) { + next_state_ = UGV_State::STATE_DRIVING; + } + break; + } + case UGV_State::STATE_DRIVING: { + if (is_upside_down) { + next_state_ = UGV_State::STATE_FLIPPING; + break; + } + if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) { + next_state_ = UGV_State::STATE_AQUIRING; + break; + } + + LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; + float tgt_dist = current_pos.distance_to(target_); + + if (tgt_dist <= conf_.min_target_dist()) { + ESP_LOGI(TAG, "Finished driving to target"); + next_state_ = UGV_State::STATE_FINISHED; + break; + } + + float tgt_bearing = current_pos.bearing_toward(target_); + angle_controller_.Enable(); + angle_controller_.Setpoint(tgt_bearing); + break; + } + case UGV_State::STATE_TEST: +#ifdef BASIC_TEST + outputs.left_motor = sinf(time_s * PI); + outputs.right_motor = cosf(time_s * PI); +#else + angle_controller_.Enable(); + angle_controller_.Setpoint(90.0); +#endif + break; + case UGV_State::STATE_DRIVE_HEADING: + angle_controller_.Enable(); + angle_controller_.Setpoint(comms->drive_heading.heading()); + drive_power = comms->drive_heading.power(); + break; + } + + if (angle_controller_.Enabled()) { + float angle_pwr = angle_controller_.Update(); + outputs_.left_motor = drive_power - angle_pwr; + outputs_.right_motor = drive_power + angle_pwr; + } + + io->WriteOutputs(outputs_); + + comms->Lock(); + comms->status.set_state(next_state_); + comms->Unlock(); +} + +UGV *the_ugv; + +void Start(void) { + ESP_LOGI(TAG, "Starting UAS UGV"); + the_ugv = new UGV(); + the_ugv->Init(); + ESP_LOGI(TAG, "Setup finished"); +} + +} // namespace ugv \ No newline at end of file diff --git a/main/ugv.hh b/main/ugv.hh new file mode 100644 index 0000000..643a7a9 --- /dev/null +++ b/main/ugv.hh @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +#include "MadgwickAHRS.h" +#include "config.pb.h" +#include "lat_long.hh" +#include "pid_controller.hh" +#include "ugv_io.hh" + +namespace ugv { + +namespace comms { +class CommsClass; +} + +class DisplayClass; + +using ugv::comms::CommsClass; +using ugv::comms::messages::UGV_State; +using ugv::io::IOClass; + +extern "C" void UGV_TickTimeout(void *arg); + +void UpdateLocationFromGPS(comms::messages::Location &location, + const io::GpsData & gps_data); + +class UGV { + private: + CommsClass * comms; + IOClass * io; + DisplayClass * display; + esp_timer_handle_t timer_handle; + + LatLong target_; + config::Config conf_; + PIDController angle_controller_; + Madgwick ahrs_; + + io::Inputs inputs_; + io::Outputs outputs_; + int64_t last_print_; + UGV_State current_state_; + UGV_State next_state_; + + void UpdateAHRS(); + void DoDebugPrint(); + void ReadComms(); + + public: + explicit UGV(); + + static config::Config DefaultConfig(); + + void SetConfig(const config::Config &conf); + void SetTarget(LatLong target); + + void Init(); + + void OnTick(); +}; + +extern UGV *the_ugv; + +void Start(void); + +} // namespace ugv diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 1d3553e..cdbbd11 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -1,282 +1,11 @@ -#include -#include -#include - -#include "MadgwickAHRS.h" -#include "config.pb.h" #include "i2c_mutex.h" -#include "ugv_comms.hh" -#include "ugv_display.hh" -#include "ugv_io.hh" -#include "pid_controller.hh" -#include "lat_long.hh" - -namespace ugv { - -using ugv::comms::CommsClass; -using ugv::comms::messages::UGV_State; -using ugv::io::IOClass; - -static const char *TAG = "ugv_main"; +#include "ugv.hh" extern "C" { SemaphoreHandle_t i2c_mutex; } -constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; -constexpr float LOOP_PERIOD_S = 1000000.f / static_cast(LOOP_PERIOD_US); - -extern "C" void OnTimeout(void *arg); - -void UpdateLocationFromGPS(comms::messages::Location &location, - 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); - location.set_altitude(gps_data.altitude); +extern "C" void app_main() { + i2c_mutex = xSemaphoreCreateMutex(); + ugv::Start(); } - -struct State { - public: - CommsClass * comms; - IOClass * io; - DisplayClass * display; - esp_timer_handle_t timer_handle; - - io::Inputs inputs_; - io::Outputs outputs_; - int64_t last_print_; - Madgwick ahrs_; - LatLong target_; - PIDController angle_controller_; - config::Config conf_; - - State() : angle_controller_(LOOP_PERIOD_S) { - SetTarget({34.069022, -118.443067}); - - comms = new CommsClass(); - io = new IOClass(); - display = new DisplayClass(comms); - - SetConfig(DefaultConfig()); - } - - static config::Config DefaultConfig() { - config::Config c; - - auto *apid = c.mutable_angle_pid(); - apid->set_kp(0.10); - apid->set_ki(0.0); - apid->set_kd(0.4); - apid->set_max_output(0.5); - apid->set_max_i_error(15.0); - - c.set_min_target_dist(10.0); - c.set_min_flip_pitch(90.0); - return c; - } - - void SetConfig(const config::Config &conf) { - auto &apid = conf.angle_pid(); - angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd()); - angle_controller_.MaxOutput(apid.max_output()); - angle_controller_.MaxIError(apid.max_i_error()); - conf_ = conf; - } - - void SetTarget(LatLong target) { target_ = target; } - - void Init() { - esp_timer_init(); - i2c_mutex = xSemaphoreCreateMutex(); - - ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency - - io->Init(); - comms->Init(); - display->Init(); - - esp_timer_create_args_t timer_args; - timer_args.callback = OnTimeout; - timer_args.arg = this; - timer_args.dispatch_method = ESP_TIMER_TASK; - 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; - } - - void OnTick() { - ESP_LOGV(TAG, "OnTick"); - int64_t time_us = esp_timer_get_time(); - // float time_s = ((float)time_us) / 1e6; - io->ReadInputs(inputs_); - { - io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel, - &m = inputs_.mpu.mag; - ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z); - } - if (time_us >= last_print_ + 500 * 1000) { // 1s - auto &mpu = inputs_.mpu; - ESP_LOGD( - TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", - mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x, - mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z); - ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), - ahrs_.getPitch(), ahrs_.getRoll()); - ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error()); - last_print_ = time_us; - } - - comms->Lock(); - UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps); - comms->status.set_yaw_angle(ahrs_.getYaw()); - UGV_State ugv_state = comms->status.state(); - if (comms->new_target) { - SetTarget(*comms->new_target); - ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude, - target_.longitude); - delete comms->new_target; - comms->new_target = nullptr; - } - if (comms->new_config) { - ESP_LOGI(TAG, "Updating config"); - SetConfig(*comms->new_config); - delete comms->new_config; - comms->new_config = nullptr; - } - comms->Unlock(); - UGV_State next_state = ugv_state; - - angle_controller_.Input(ahrs_.getYaw()); - float drive_power = 0.; - outputs_.left_motor = 0.0; - outputs_.right_motor = 0.0; - - float pitch = ahrs_.getPitch(); - - auto min_flip_pitch = conf_.min_flip_pitch(); - bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); - - switch (ugv_state) { - default: - ESP_LOGW(TAG, "unhandled state: %d", ugv_state); - // fall through - case UGV_State::STATE_IDLE: - case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break; - case UGV_State::STATE_AQUIRING: { - if (is_upside_down) { - next_state = UGV_State::STATE_FLIPPING; - break; - } - angle_controller_.Disable(); - 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; - if (not_old && not_invalid) { - next_state = UGV_State::STATE_TURNING; - } - break; - } - case UGV_State::STATE_FLIPPING: { - angle_controller_.Disable(); - outputs_.left_motor = -1.0; - outputs_.right_motor = -1.0; - if (!is_upside_down) { - next_state = UGV_State::STATE_AQUIRING; - break; - } - break; - } - case UGV_State::STATE_TURNING: { - if (is_upside_down) { - next_state = UGV_State::STATE_FLIPPING; - break; - } - if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) { - next_state = UGV_State::STATE_AQUIRING; - break; - } - - LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; - float tgt_bearing = current_pos.bearing_toward(target_); - angle_controller_.Enable(); - angle_controller_.Setpoint(tgt_bearing); - - if (fabs(angle_controller_.Error()) <= 5.0) { - next_state = UGV_State::STATE_DRIVING; - } - break; - } - case UGV_State::STATE_DRIVING: { - if (is_upside_down) { - next_state = UGV_State::STATE_FLIPPING; - break; - } - if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) { - next_state = UGV_State::STATE_AQUIRING; - break; - } - - LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; - float tgt_dist = current_pos.distance_to(target_); - - if (tgt_dist <= conf_.min_target_dist()) { - ESP_LOGI(TAG, "Finished driving to target"); - next_state = UGV_State::STATE_FINISHED; - break; - } - - float tgt_bearing = current_pos.bearing_toward(target_); - angle_controller_.Enable(); - angle_controller_.Setpoint(tgt_bearing); - break; - } - case UGV_State::STATE_TEST: -#ifdef BASIC_TEST - outputs.left_motor = sinf(time_s * PI); - outputs.right_motor = cosf(time_s * PI); -#else - angle_controller_.Enable(); - angle_controller_.Setpoint(90.0); -#endif - break; - case UGV_State::STATE_DRIVE_HEADING: - angle_controller_.Enable(); - angle_controller_.Setpoint(comms->drive_heading.heading()); - drive_power = comms->drive_heading.power(); - break; - } - - if (angle_controller_.Enabled()) { - float angle_pwr = angle_controller_.Update(); - outputs_.left_motor = drive_power - angle_pwr; - outputs_.right_motor = drive_power + angle_pwr; - } - - io->WriteOutputs(outputs_); - - comms->Lock(); - comms->status.set_state(next_state); - comms->Unlock(); - } -}; - -extern "C" void OnTimeout(void *arg) { - State *state = (State *)arg; - state->OnTick(); -} - -State *state; - -void Setup(void) { - ESP_LOGI(TAG, "Starting UAS UGV"); - state = new State(); - state->Init(); - ESP_LOGI(TAG, "Setup finished"); -} - -} // namespace ugv - -extern "C" void app_main() { ugv::Setup(); }