send config over rf link

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()

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

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

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

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

main/ugv_main.cc

@@ -1,13 +1,14 @@
 #include <esp_log.h>
 #include <esp_timer.h>
+#include <math.h>
+
 #include "MadgwickAHRS.h"
+#include "config.pb.h"
 #include "i2c_mutex.h"
 #include "ugv_comms.hh"
 #include "ugv_display.hh"
 #include "ugv_io.hh"
 
-#include <math.h>
-
 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();
   }
 };