diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index d0c0480..dabdf3d 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -18,6 +18,8 @@ set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) +proto_generate_cpp(config.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) +message(STATUS "PROTO_SRCS: ${PROTO_SRCS}") list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS}) register_component() diff --git a/main/config.proto b/main/config.proto new file mode 100644 index 0000000..9e266df --- /dev/null +++ b/main/config.proto @@ -0,0 +1,20 @@ +syntax = "proto3"; + +package ugv.config; + +option optimize_for = LITE_RUNTIME; + +message PidParams { + float kp = 1; + float ki = 2; + float kd = 3; + float max_output = 4; + float max_i_error = 5; +} + +message Config { + float min_target_dist = 1; + float drive_power = 2; + PidParams angle_pid = 3; + float min_flip_pitch = 4; +} diff --git a/main/messages.proto b/main/messages.proto index 92dc51a..485e891 100644 --- a/main/messages.proto +++ b/main/messages.proto @@ -1,6 +1,8 @@ syntax = "proto3"; -package uas.ugv.messages; +package ugv.messages; + +import public "config.proto"; option optimize_for = LITE_RUNTIME; @@ -15,6 +17,11 @@ enum UGV_State { STATE_DRIVE_HEADING = 7; } +message TargetLocation { + float latitude = 1; + float longitude = 2; +} + message Location { uint32 fix_quality = 1; float latitude = 2; @@ -40,6 +47,8 @@ enum GroundCommandType { CMD_DRIVE_TO_TARGET = 1; CMD_TEST = 2; CMD_DRIVE_HEADING = 3; + CMD_SET_TARGET = 4; + CMD_SET_CONFIG = 5; } message DriveHeadingData { @@ -52,6 +61,8 @@ message GroundCommand { GroundCommandType type = 2; oneof data { DriveHeadingData drive_heading = 3; + TargetLocation target_location = 4; + config.Config config = 5; } } diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 49b82d6..beeadfe 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -14,24 +14,29 @@ namespace ugv { namespace comms { static const char *TAG = "ugv_comms"; +static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(5000); CommsClass::CommsClass() - : location(), - ugv_state(messages::STATE_IDLE), - last_packet_tick(0), + : last_packet_tick(0), last_packet_rssi(INT32_MIN), - last_packet_snr(INT8_MIN) { + last_packet_snr(INT8_MIN), + status(), + drive_heading(), + new_target(nullptr), + new_config(nullptr) { + status.set_state(messages::STATE_IDLE); mutex = xSemaphoreCreateMutex(); } void CommsClass::Init() { esp_err_t ret; - location.set_fix_quality(0); - location.set_latitude(43.65); - location.set_longitude(-116.20); - location.set_altitude(2730); - ugv_state = messages::UGV_State::STATE_IDLE; + auto *loc = status.mutable_location(); + loc->set_fix_quality(0); + loc->set_latitude(43.65); + loc->set_longitude(-116.20); + loc->set_altitude(2730); + status.set_state(messages::UGV_State::STATE_IDLE); #ifdef COMMS_SX127X sx127x_config_t lora_config = sx127x_config_default(); @@ -115,9 +120,8 @@ void CommsClass::RunTask() { using messages::UGV_State; using messages::UGV_Status; - TickType_t send_period = pdMS_TO_TICKS(2000); TickType_t current_tick = xTaskGetTickCount(); - TickType_t next_send = current_tick + send_period; + TickType_t next_send = current_tick + SEND_PERIOD; esp_err_t ret; @@ -130,7 +134,6 @@ void CommsClass::RunTask() { UGV_Message ugv_message; std::string ugv_message_data; - UGV_Status *status = ugv_message.mutable_status(); while (true) { TickType_t delay_ticks = next_send - current_tick; @@ -146,8 +149,8 @@ void CommsClass::RunTask() { // receiving packet data now rx_len = lora.ReadLn(rx_buf, delay_ticks); if (rx_len <= 0) { - ESP_LOGI(TAG, "timeout for packet rx"); - lora.Flush(); + ESP_LOGV(TAG, "timeout for packet rx"); + // lora.Flush(); } else { ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str()); HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size()); @@ -162,9 +165,7 @@ void CommsClass::RunTask() { } Lock(); - status->mutable_location()->CopyFrom(this->location); - status->set_yaw_angle(this->yaw_angle); - status->set_state(this->ugv_state); + ugv_message.mutable_status()->CopyFrom(this->status); Unlock(); ugv_message.SerializeToString(&ugv_message_data); ret = SendPacket(ugv_message_data); @@ -175,7 +176,7 @@ void CommsClass::RunTask() { } current_tick = xTaskGetTickCount(); - next_send = current_tick + send_period; + next_send = current_tick + SEND_PERIOD; } } @@ -237,24 +238,43 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { void CommsClass::HandleCommand(const messages::GroundCommand &command) { ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type()); + using messages::UGV_State; xSemaphoreTake(mutex, pdMS_TO_TICKS(10)); switch (command.type()) { - case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break; + case messages::CMD_DISABLE: status.set_state(UGV_State::STATE_IDLE); break; case messages::CMD_DRIVE_TO_TARGET: - ugv_state = messages::STATE_AQUIRING; + status.set_state(UGV_State::STATE_AQUIRING); break; - case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break; + case messages::CMD_TEST: status.set_state(UGV_State::STATE_TEST); break; case messages::CMD_DRIVE_HEADING: { if (command.has_drive_heading()) { - this->ugv_state = messages::STATE_DRIVE_HEADING; + status.set_state(UGV_State::STATE_DRIVE_HEADING); this->drive_heading = command.drive_heading(); } else { - this->ugv_state = messages::STATE_IDLE; + status.set_state(UGV_State::STATE_IDLE); ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING"); } break; } + case messages::CMD_SET_TARGET: { + if (command.has_target_location()) { + if (!new_target) { + new_target = new messages::TargetLocation(); + } + new_target->CopyFrom(command.target_location()); + } + break; + } + case messages::CMD_SET_CONFIG: { + if (command.has_config()) { + if (!new_config) { + new_config = new config::Config(); + } + new_config->CopyFrom(command.config()); + } + break; + } default: ESP_LOGW(TAG, "unhandled command type: %d", command.type()); xSemaphoreGive(mutex); diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index d98cf8d..f2a96dd 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -15,7 +15,8 @@ namespace ugv { namespace comms { -namespace messages = uas::ugv::messages; +namespace messages = ugv::messages; +namespace config = ugv::config; class CommsClass { public: @@ -33,14 +34,15 @@ class CommsClass { uint8_t ReadLnaGain(); public: - SemaphoreHandle_t mutex; - messages::Location location; - float yaw_angle; - messages::UGV_State ugv_state; - TickType_t last_packet_tick; - int32_t last_packet_rssi; - int8_t last_packet_snr; + SemaphoreHandle_t mutex; + TickType_t last_packet_tick; + int32_t last_packet_rssi; + int8_t last_packet_snr; + + messages::UGV_Status status; messages::DriveHeadingData drive_heading; + messages::TargetLocation* new_target; + config::Config* new_config; private: #ifdef COMMS_SX127X diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 0aeed4c..b030146 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -1,13 +1,14 @@ #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 - namespace ugv { using ugv::comms::CommsClass; @@ -29,14 +30,6 @@ static const float RAD_PER_DEG = PI / 180.f; // Radius of earth in meters static const float EARTH_RAD = 6372795.f; -static const float DRIVE_POWER = 0.5; -static const float ANGLE_P = 0.02; -static const float ANGLE_I = 0.000005; -static const float ANGLE_D = -0.01; -static const float MAX_ANGLE_POWER = 0.3; -static const float MAX_ANGLE_I_ERROR = 15.0; -static const float MIN_DIST = 10.0; - extern "C" void OnTimeout(void *arg); void UpdateLocationFromGPS(comms::messages::Location &location, @@ -52,9 +45,14 @@ struct LatLong { float latitude; float longitude; + inline LatLong() : LatLong(0., 0.) {} + inline LatLong(double latitude_, double longitude_) : latitude(latitude_), longitude(longitude_) {} + inline LatLong(const comms::messages::TargetLocation &loc) + : latitude(loc.latitude()), longitude(loc.longitude()) {} + /** * Return distance from this LatLong to target, in meters */ @@ -93,7 +91,23 @@ struct LatLong { class PIDController { public: - explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.); + explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.); + + void DeltaT(float dt) { + dt_ = dt; + Reset(); + } + float DeltaT() { return dt_; } + + float GetP() { return kp_; } + float GetI() { return ki_; } + float GetD() { return kd_; } + + void SetPID(float kp, float ki, float kd) { + kp_ = kp; + ki_ = ki; + kd_ = kd; + } void MaxOutput(float max_output) { max_output_ = max_output; } float MaxOutput() const { return max_output_; } @@ -183,17 +197,18 @@ float PIDController::Update() { } float error = Error(); + output_ += kp_ * error; + if (fabsf(error) > max_i_error_) { integral_ = 0.; } else { integral_ += error * dt_; + output_ += ki_ * integral_; } - - output_ += kp_ * error; - output_ += ki_ * integral_; if (!isnan(last_error_)) { output_ += kd_ * (error - last_error_); } + output_ = clamp_mag(output_, max_output_); last_error_ = error; @@ -217,24 +232,49 @@ struct State { 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_; + io::Inputs inputs_; + io::Outputs outputs_; + int64_t last_print_; + Madgwick ahrs_; + LatLong target_; + PIDController angle_controller_; + config::Config conf_; - State() - : target{34.069022, -118.443067}, - angle_controller_(LOOP_PERIOD_S, ANGLE_P, ANGLE_I, ANGLE_D) { - angle_controller_.MaxOutput(MAX_ANGLE_POWER); - angle_controller_.MaxIError(MAX_ANGLE_I_ERROR); + 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(); @@ -252,47 +292,60 @@ struct State { 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_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); + // 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; + 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 - ESP_LOGD(TAG, - "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", - inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z, - inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y, - inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y, - inputs.mpu.mag.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; + last_print_ = time_us; } comms->Lock(); - UpdateLocationFromGPS(comms->location, inputs.gps); - comms->yaw_angle = ahrs_.getYaw(); - UGV_State ugv_state = comms->ugv_state; + 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 drive_power = 0.; + outputs_.left_motor = 0.0; + outputs_.right_motor = 0.0; float pitch = ahrs_.getPitch(); - bool is_upside_down = (pitch > 90.) || (pitch < -90.); + auto min_flip_pitch = conf_.min_flip_pitch(); + bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); switch (ugv_state) { default: @@ -307,9 +360,9 @@ struct State { } angle_controller_.Disable(); TickType_t current_tick = xTaskGetTickCount(); - TickType_t ticks_since_gps = current_tick - inputs.gps.last_update; + 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_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID; if (not_old && not_invalid) { next_state = UGV_State::STATE_TURNING; } @@ -317,8 +370,8 @@ struct State { } case UGV_State::STATE_FLIPPING: { angle_controller_.Disable(); - outputs.left_motor = -1.0; - outputs.right_motor = -1.0; + outputs_.left_motor = -1.0; + outputs_.right_motor = -1.0; if (!is_upside_down) { next_state = UGV_State::STATE_AQUIRING; break; @@ -330,13 +383,13 @@ struct State { next_state = UGV_State::STATE_FLIPPING; break; } - if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { + 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); + 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); @@ -350,21 +403,21 @@ struct State { next_state = UGV_State::STATE_FLIPPING; break; } - if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { + 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); + LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude}; + float tgt_dist = current_pos.distance_to(target_); - if (tgt_dist <= MIN_DIST) { + 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); + float tgt_bearing = current_pos.bearing_toward(target_); angle_controller_.Enable(); angle_controller_.Setpoint(tgt_bearing); break; @@ -386,15 +439,15 @@ struct State { } if (angle_controller_.Enabled()) { - float angle_pwr = angle_controller_.Update(); - outputs.left_motor = drive_power - angle_pwr; - outputs.right_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; } - io->WriteOutputs(outputs); + io->WriteOutputs(outputs_); comms->Lock(); - comms->ugv_state = next_state; + comms->status.set_state(next_state); comms->Unlock(); } };