Implement initial driving to target logic

e32_client/ugv.py

@@ -30,6 +30,7 @@ class UGVComms(E32):
         msg_len = int.from_bytes(len_data, byteorder='big')
         data = self.ser.read(size=msg_len)
         if len(data) != msg_len:
+            print("read bad data: ", data)
             self.ser.flush()
             return None
         msg = messages.UGV_Message()
@@ -49,7 +50,7 @@ def __rx_thread_entry(ugv: UGVComms):
 
 
 if __name__ == "__main__":
-    ser = serial.serial_for_url("loop://", baudrate=9600, parity=serial.PARITY_NONE,
+    ser = serial.serial_for_url("/dev/ttyUSB1", baudrate=9600, parity=serial.PARITY_NONE,
                                 stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
                                 timeout=2.0)
     ugv = UGVComms(ser)

main/messages.proto

@@ -5,10 +5,11 @@ package uas.ugv.messages;
 option optimize_for = LITE_RUNTIME;
 
 enum UGV_State {
-  IDLE = 0;
+  STATE_IDLE = 0;
-  AQUIRING = 1;
+  STATE_AQUIRING = 1;
-  DRIVING = 2;
+  STATE_DRIVING = 2;
-  FINISHED = 3;
+  STATE_FINISHED = 3;
+  STATE_TEST = 4;
 }
 
 message Location {
@@ -31,8 +32,9 @@ message UGV_Message {
 }
 
 enum GroundCommandType {
-  DISABLE = 0;
+  CMD_DISABLE = 0;
-  ENABLE = 1;
+  CMD_DRIVE_TO_TARGET = 1;
+  CMD_TEST = 2;
 }
 
 message GroundCommand {

main/ugv_comms.cc

@@ -16,7 +16,7 @@ static const char *TAG = "ugv_comms";
 
 CommsClass::CommsClass()
     : location(),
-      ugv_state(messages::IDLE),
+      ugv_state(messages::STATE_IDLE),
       last_packet_tick(0),
       last_packet_rssi(INT32_MIN),
       last_packet_snr(INT8_MIN) {
@@ -30,7 +30,7 @@ void CommsClass::Init() {
   location.set_latitude(43.65);
   location.set_longitude(-116.20);
   location.set_altitude(2730);
-  ugv_state = messages::UGV_State::IDLE;
+  ugv_state = messages::UGV_State::STATE_IDLE;
 
 #ifdef COMMS_SX127X
   sx127x_config_t lora_config  = sx127x_config_default();
@@ -145,18 +145,18 @@ void CommsClass::RunTask() {
       sx127x_packet_rx_free(&rx_packet);
     }
 #else
-    if (packet_len <= 0) { // need to receive packet size first
+    if (packet_len <= 0) {  // need to receive packet size first
       rx_len = lora.Read(rx_buf, 1, delay_ticks);
       if (rx_len > 0) {
         packet_len = rx_buf[0];
-        if (packet_len > MAX_PACKET_LEN) { // should not be possible
+        if (packet_len > MAX_PACKET_LEN) {  // should not be possible
           ESP_LOGE(TAG, "invalid packet len received: %d", packet_len);
           packet_len = -1;
         } else {
           ESP_LOGV(TAG, "rx packet len: %d", packet_len);
         }
       }
-    } else { // receiving packet data now
+    } else {  // receiving packet data now
       rx_len = lora.Read(rx_buf, packet_len, PACKET_RX_TIMEOUT);
       if (rx_len < packet_len) {
         ESP_LOGE(TAG, "timeout for packet rx");
@@ -165,7 +165,7 @@ void CommsClass::RunTask() {
         ESP_LOGV(TAG, "rx data: %.*s", packet_len, rx_buf);
         HandlePacket(rx_buf, packet_len);
       }
-      packet_len = -1; // wait for new packet len
+      packet_len = -1;  // wait for new packet len
     }
     // TODO: checksum?
 #endif
@@ -239,7 +239,20 @@ 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());
-  // TODO: handle command
+
+  xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+  switch (command.type()) {
+    case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break;
+    case messages::CMD_DRIVE_TO_TARGET:
+      ugv_state = messages::STATE_AQUIRING;
+      break;
+    case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break;
+    default:
+      ESP_LOGW(TAG, "unhandled command type: %d", command.type());
+      xSemaphoreGive(mutex);
+      return;  // early return, no ack
+  }
+  xSemaphoreGive(mutex);
 
   messages::UGV_Message ugv_message;
   ugv_message.set_command_ack(command.id());

main/ugv_main.cc

@@ -11,6 +11,7 @@
 namespace ugv {
 
 using ugv::comms::CommsClass;
+using ugv::comms::messages::UGV_State;
 using ugv::io::IOClass;
 
 static const char *TAG = "ugv_main";
@@ -20,10 +21,71 @@ SemaphoreHandle_t i2c_mutex;
 }
 
 constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
-static const float PI             = atanf(1.0) * 4.0;
+static const float PI =
+    3.1415926535897932384626433832795028841971693993751058209749445923078164062;
 
 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);
+}
+
+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 MIN_DIST    = 10.0;
+
+struct LatLong {
+ public:
+  float latitude;
+  float longitude;
+
+  inline LatLong(double latitude_, double longitude_)
+      : latitude(latitude_), longitude(longitude_) {}
+
+  /**
+   * Return distance from this LatLong to target, in meters
+   */
+  float distance_to(const LatLong &target) const {
+    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 +
+              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 bearing_toward(const LatLong &target) const {
+    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 course = atan2f(num, denom);
+    if (course < 0.0) {
+      course += 2 * PI;
+    }
+    return course / RAD_PER_DEG;
+  }
+};
+
 struct State {
  public:
   CommsClass *       comms;
@@ -34,8 +96,9 @@ struct State {
   io::Outputs        outputs;
   int64_t            last_print;
   Madgwick           ahrs_;
+  LatLong            target;
 
-  State() {
+  State() : target{34.069022, -118.443067} {
     comms   = new CommsClass();
     io      = new IOClass();
     display = new DisplayClass(comms);
@@ -67,9 +130,6 @@ struct State {
     int64_t time_us = esp_timer_get_time();
     float   time_s  = ((float)time_us) / 1e6;
     io->ReadInputs(inputs);
-    outputs.left_motor  = sinf(time_s * PI);
-    outputs.right_motor = cosf(time_s * PI);
-    io->WriteOutputs(outputs);
     {
       io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel,
                 &m = inputs.mpu.mag;
@@ -86,6 +146,60 @@ struct State {
                ahrs_.getPitch(), ahrs_.getRoll());
       last_print = time_us;
     }
+
+    comms->Lock();
+    UpdateLocationFromGPS(comms->location, inputs.gps);
+    UGV_State ugv_state = comms->ugv_state;
+    comms->Unlock();
+
+    switch (ugv_state) {
+      default:
+        ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
+        // fall through
+      case UGV_State::STATE_IDLE:
+      case UGV_State::STATE_FINISHED:
+        outputs.left_motor  = 0.0;
+        outputs.right_motor = 0.0;
+        break;
+      case UGV_State::STATE_AQUIRING: {
+        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;
+        outputs.left_motor     = 0.0;
+        outputs.right_motor    = 0.0;
+        if (not_old && not_invalid) {
+          comms->ugv_state = UGV_State::STATE_DRIVING;
+        }
+        break;
+      }
+      case UGV_State::STATE_DRIVING: {
+        LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude};
+        float   tgt_dist    = current_pos.distance_to(target);
+
+        if (tgt_dist <= MIN_DIST) {
+          ESP_LOGI(TAG, "Finished driving to target");
+          comms->ugv_state = UGV_State::STATE_FINISHED;
+          break;
+        }
+
+        float tgt_bearing = current_pos.bearing_toward(target);
+        float cur_bearing = ahrs_.getYaw();
+        float angle_delta = tgt_bearing - cur_bearing;
+        if (angle_delta < 180.f) angle_delta += 360.f;
+        if (angle_delta > 180.f) angle_delta -= 360.f;
+        float angle_pwr = angle_delta * ANGLE_P;
+
+        outputs.left_motor  = DRIVE_POWER + angle_pwr;
+        outputs.right_motor = DRIVE_POWER - angle_pwr;
+        break;
+      }
+      case UGV_State::STATE_TEST:
+        outputs.left_motor  = sinf(time_s * PI);
+        outputs.right_motor = cosf(time_s * PI);
+        break;
+    }
+    io->WriteOutputs(outputs);
   }
 };