lots of work on driving and fixing stuff

e32_client/messages_pb2.py

@@ -21,7 +21,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
   package='uas.ugv.messages',
   syntax='proto3',
   serialized_options=_b('H\003'),
-  serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"y\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"N\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*f\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04*K\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x42\x02H\x03\x62\x06proto3')
+  serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"y\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\x93\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\x12;\n\rdrive_heading\x18\x03 \x01(\x0b\x32\".uas.ugv.messages.DriveHeadingDataH\x00\x42\x06\n\x04\x64\x61ta\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*b\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x42\x02H\x03\x62\x06proto3')
 )
 
 _UGV_STATE = _descriptor.EnumDescriptor(
@@ -50,11 +50,23 @@ _UGV_STATE = _descriptor.EnumDescriptor(
       name='STATE_TEST', index=4, number=4,
       serialized_options=None,
       type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_FLIPPING', index=5, number=5,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_TURNING', index=6, number=6,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_DRIVE_HEADING', index=7, number=7,
+      serialized_options=None,
+      type=None),
   ],
   containing_type=None,
   serialized_options=None,
-  serialized_start=515,
+  serialized_start=638,
-  serialized_end=617,
+  serialized_end=804,
 )
 _sym_db.RegisterEnumDescriptor(_UGV_STATE)
 
@@ -77,11 +89,15 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
       name='CMD_TEST', index=2, number=2,
       serialized_options=None,
       type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_DRIVE_HEADING', index=3, number=3,
+      serialized_options=None,
+      type=None),
   ],
   containing_type=None,
   serialized_options=None,
-  serialized_start=619,
+  serialized_start=806,
-  serialized_end=694,
+  serialized_end=904,
 )
 _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
 
@@ -91,9 +107,13 @@ STATE_AQUIRING = 1
 STATE_DRIVING = 2
 STATE_FINISHED = 3
 STATE_TEST = 4
+STATE_FLIPPING = 5
+STATE_TURNING = 6
+STATE_DRIVE_HEADING = 7
 CMD_DISABLE = 0
 CMD_DRIVE_TO_TARGET = 1
 CMD_TEST = 2
+CMD_DRIVE_HEADING = 3
 
 
 
@@ -235,6 +255,44 @@ _UGV_MESSAGE = _descriptor.Descriptor(
 )
 
 
+_DRIVEHEADINGDATA = _descriptor.Descriptor(
+  name='DriveHeadingData',
+  full_name='uas.ugv.messages.DriveHeadingData',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='heading', full_name='uas.ugv.messages.DriveHeadingData.heading', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='power', full_name='uas.ugv.messages.DriveHeadingData.power', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=348,
+  serialized_end=398,
+)
+
+
 _GROUNDCOMMAND = _descriptor.Descriptor(
   name='GroundCommand',
   full_name='uas.ugv.messages.GroundCommand',
@@ -256,6 +314,13 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
       message_type=None, enum_type=None, containing_type=None,
       is_extension=False, extension_scope=None,
       serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='drive_heading', full_name='uas.ugv.messages.GroundCommand.drive_heading', index=2,
+      number=3, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
   ],
   extensions=[
   ],
@@ -267,9 +332,12 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
   syntax='proto3',
   extension_ranges=[],
   oneofs=[
+    _descriptor.OneofDescriptor(
+      name='data', full_name='uas.ugv.messages.GroundCommand.data',
+      index=0, containing_type=None, fields=[]),
   ],
-  serialized_start=348,
+  serialized_start=401,
-  serialized_end=426,
+  serialized_end=548,
 )
 
 
@@ -302,8 +370,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor(
       name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message',
       index=0, containing_type=None, fields=[]),
   ],
-  serialized_start=428,
+  serialized_start=550,
-  serialized_end=513,
+  serialized_end=635,
 )
 
 _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE
@@ -316,6 +384,10 @@ _UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append(
   _UGV_MESSAGE.fields_by_name['command_ack'])
 _UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message']
 _GROUNDCOMMAND.fields_by_name['type'].enum_type = _GROUNDCOMMANDTYPE
+_GROUNDCOMMAND.fields_by_name['drive_heading'].message_type = _DRIVEHEADINGDATA
+_GROUNDCOMMAND.oneofs_by_name['data'].fields.append(
+  _GROUNDCOMMAND.fields_by_name['drive_heading'])
+_GROUNDCOMMAND.fields_by_name['drive_heading'].containing_oneof = _GROUNDCOMMAND.oneofs_by_name['data']
 _GROUNDMESSAGE.fields_by_name['command'].message_type = _GROUNDCOMMAND
 _GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append(
   _GROUNDMESSAGE.fields_by_name['command'])
@@ -323,6 +395,7 @@ _GROUNDMESSAGE.fields_by_name['command'].containing_oneof = _GROUNDMESSAGE.oneof
 DESCRIPTOR.message_types_by_name['Location'] = _LOCATION
 DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS
 DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE
+DESCRIPTOR.message_types_by_name['DriveHeadingData'] = _DRIVEHEADINGDATA
 DESCRIPTOR.message_types_by_name['GroundCommand'] = _GROUNDCOMMAND
 DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE
 DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE
@@ -350,6 +423,13 @@ UGV_Message = _reflection.GeneratedProtocolMessageType('UGV_Message', (_message.
   ))
 _sym_db.RegisterMessage(UGV_Message)
 
+DriveHeadingData = _reflection.GeneratedProtocolMessageType('DriveHeadingData', (_message.Message,), dict(
+  DESCRIPTOR = _DRIVEHEADINGDATA,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:uas.ugv.messages.DriveHeadingData)
+  ))
+_sym_db.RegisterMessage(DriveHeadingData)
+
 GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict(
   DESCRIPTOR = _GROUNDCOMMAND,
   __module__ = 'messages_pb2'

e32_client/ugv.py

@@ -35,13 +35,16 @@ class UGVComms(E32):
         data = msg.SerializeToString()
         self.write_base64(data)
 
-    def write_command(self, cmd_type: messages.GroundCommandType, retry=True):
+    def write_command(self, command, retry=True):
         cmdid = self.next_command_id
         self.next_command_id += 1
 
         gmsg = messages.GroundMessage()
+        if type(command) is int:
+            gmsg.command.type = command
+        else:
+            gmsg.command.CopyFrom(command)
         gmsg.command.id = cmdid
-        gmsg.command.type = cmd_type
         self.write_message(gmsg)
         last_write_time = time.time()
         if not retry:
@@ -117,9 +120,13 @@ def main():
     time.sleep(0.2)
     try:
         while True:
-            if ugv.last_status is None or ugv.last_status.state is not messages.STATE_TEST:
+            if ugv.last_status is None or ugv.last_status.state is not messages.STATE_DRIVE_HEADING:
-                ugv.write_command(messages.CMD_TEST)
+                cmd = messages.GroundCommand()
-            time.sleep(2.)
+                cmd.type = messages.CMD_DRIVE_HEADING
+                cmd.drive_heading.heading = -90.0
+                cmd.drive_heading.power = 0.2
+                ugv.write_command(cmd)
+            time.sleep(1.0)
     except KeyboardInterrupt:
         ugv.write_command(messages.CMD_DISABLE)
         print("exiting...")

main/messages.proto

@@ -10,6 +10,9 @@ enum UGV_State {
   STATE_DRIVING = 2;
   STATE_FINISHED = 3;
   STATE_TEST = 4;
+  STATE_FLIPPING = 5;
+  STATE_TURNING = 6;
+  STATE_DRIVE_HEADING = 7;
 }
 
 message Location {
@@ -36,11 +39,20 @@ enum GroundCommandType {
   CMD_DISABLE = 0;
   CMD_DRIVE_TO_TARGET = 1;
   CMD_TEST = 2;
+  CMD_DRIVE_HEADING = 3;
+}
+
+message DriveHeadingData {
+  float heading = 1;
+  float power = 2;
 }
 
 message GroundCommand {
   uint32 id = 1;
   GroundCommandType type = 2;
+  oneof data {
+    DriveHeadingData drive_heading = 3;
+  }
 }
 
 message GroundMessage {

main/ugv_comms.cc

@@ -144,9 +144,9 @@ void CommsClass::RunTask() {
     }
 #else
     // receiving packet data now
-    rx_len = lora.ReadLn(rx_buf, PACKET_RX_TIMEOUT);
+    rx_len = lora.ReadLn(rx_buf, delay_ticks);
     if (rx_len <= 0) {
-      ESP_LOGE(TAG, "timeout for packet rx");
+      ESP_LOGI(TAG, "timeout for packet rx");
       lora.Flush();
     } else {
       ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str());
@@ -200,6 +200,7 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
   xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
   last_packet_tick = xTaskGetTickCount();
   xSemaphoreGive(mutex);
+  ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data);
 
   int      ret;
   size_t   decoded_size = (data_size * 4 + 2) / 3;
@@ -209,7 +210,7 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
   ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data,
                               data_size);
   if (ret != 0) {
-    ESP_LOGE(TAG, "error with base64 decode: %d", ret);
+    ESP_LOGE(TAG, "base64 decode error: %d", ret);
     delete[] decoded;
     return;
   }
@@ -244,6 +245,16 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
       ugv_state = messages::STATE_AQUIRING;
       break;
     case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break;
+    case messages::CMD_DRIVE_HEADING: {
+      if (command.has_drive_heading()) {
+        this->ugv_state     = messages::STATE_DRIVE_HEADING;
+        this->drive_heading = command.drive_heading();
+      } else {
+        this->ugv_state = messages::STATE_IDLE;
+        ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
+      }
+      break;
+    }
     default:
       ESP_LOGW(TAG, "unhandled command type: %d", command.type());
       xSemaphoreGive(mutex);
@@ -265,7 +276,7 @@ esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {
     ESP_LOGE(TAG, "SendPacket size too long: %d", size);
     return ESP_ERR_INVALID_SIZE;
   }
-  size_t   encoded_size = ((size + 2) / 3) * 4;
+  size_t   encoded_size = ((size + 6) / 3) * 4;
   uint8_t *encoded      = new uint8_t[encoded_size];
   size_t   encoded_len;
   int      ret =

main/ugv_comms.hh

@@ -33,13 +33,14 @@ class CommsClass {
   uint8_t ReadLnaGain();
 
  public:
-  SemaphoreHandle_t   mutex;
+  SemaphoreHandle_t          mutex;
-  messages::Location  location;
+  messages::Location         location;
-  float               yaw_angle;
+  float                      yaw_angle;
-  messages::UGV_State ugv_state;
+  messages::UGV_State        ugv_state;
-  TickType_t          last_packet_tick;
+  TickType_t                 last_packet_tick;
-  int32_t             last_packet_rssi;
+  int32_t                    last_packet_rssi;
-  int8_t              last_packet_snr;
+  int8_t                     last_packet_snr;
+  messages::DriveHeadingData drive_heading;
 
  private:
 #ifdef COMMS_SX127X

main/ugv_main.cc

@@ -21,9 +21,21 @@ SemaphoreHandle_t i2c_mutex;
 }
 
 constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
+constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
 static const float PI =
     3.1415926535897932384626433832795028841971693993751058209749445923078164062;
 
+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.05;
+static const float ANGLE_I         = 0.00;
+static const float ANGLE_D         = -0.05;
+static const float MAX_ANGLE_POWER = 0.5;
+static const float MIN_DIST        = 10.0;
+
 extern "C" void OnTimeout(void *arg);
 
 void UpdateLocationFromGPS(comms::messages::Location &location,
@@ -34,24 +46,6 @@ void UpdateLocationFromGPS(comms::messages::Location &location,
   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.05;
-static const float MAX_ANGLE_POWER = 0.3;
-static const float MIN_DIST        = 10.0;
-
-float clamp_mag(float x, float max_mag) {
-  if (x > max_mag)
-    return max_mag;
-  else if (x < -max_mag)
-    return -max_mag;
-  else
-    return x;
-}
-
 struct LatLong {
  public:
   float latitude;
@@ -96,19 +90,134 @@ struct LatLong {
   }
 };
 
+class PIDController {
+ public:
+  explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.);
+
+  void  MaxOutput(float max_output) { max_output_ = max_output; }
+  float MaxOutput() const { return max_output_; }
+
+  void  Setpoint(float setpoint) { setpoint_ = setpoint; }
+  float Setpoint() const { return setpoint_; }
+
+  void  Input(float input) { input_ = input; }
+  float Input() const { return input_; };
+
+  float Error() const {
+    float error = setpoint_ - input_;
+    // TODO: have this be configurable
+    while (error < 180.f) error += 360.f;
+    while (error > 180.f) error -= 360.f;
+    return error;
+  }
+  float Output() const { return output_; };
+
+  float Update();
+  float Update(float input);
+  float Update(float input, float setpoint);
+
+  void Reset();
+  void Enable(bool enable = true) { enabled_ = enable; }
+  void Disable() { Enable(false); }
+  bool Enabled() const { return enabled_; }
+
+ private:
+  static float clamp_mag(float x, float mag);
+
+  float dt_;
+  float kp_;
+  float ki_;
+  float kd_;
+  float max_output_;
+
+  bool  enabled_;
+  float setpoint_;
+  float input_;
+  float output_;
+
+  float integral_;
+  float last_error_;
+};
+
+float PIDController::clamp_mag(float x, float max_mag) {
+  if (x > max_mag)
+    return max_mag;
+  else if (x < -max_mag)
+    return -max_mag;
+  else
+    return x;
+}
+
+PIDController::PIDController(float dt, float kp, float ki, float kd)
+    : dt_(dt),
+      kp_(kp),
+      ki_(ki),
+      kd_(kd),
+      max_output_(INFINITY),
+      enabled_(false),
+      setpoint_(0),
+      input_(0),
+      output_(0),
+      integral_(0),
+      last_error_(0) {}
+
+void PIDController::Reset() {
+  enabled_    = false;
+  setpoint_   = 0.;
+  input_      = 0.;
+  output_     = 0.;
+  integral_   = 0.;
+  last_error_ = NAN;
+}
+
+float PIDController::Update() {
+  output_ = 0.;
+  if (!enabled_) {
+    return output_;
+  }
+  float error = Error();
+
+  integral_ += error * dt_;
+
+  output_ += kp_ * error;
+  output_ += ki_ * integral_;
+  if (!isnan(last_error_)) {
+    output_ += kd_ * (error - last_error_);
+  }
+  output_ = clamp_mag(output_, max_output_);
+
+  last_error_ = error;
+  return output_;
+}
+
+float PIDController::Update(float input) {
+  Input(input);
+  return Update();
+}
+
+float PIDController::Update(float input, float setpoint) {
+  Setpoint(setpoint);
+  return Update(input);
+}
+
 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;
 
-  State() : target{34.069022, -118.443067} {
+  io::Inputs    inputs;
+  io::Outputs   outputs;
+  int64_t       last_print;
+  Madgwick      ahrs_;
+  LatLong       target;
+  PIDController angle_controller_;
+
+  State()
+      : target{34.069022, -118.443067},
+        angle_controller_(LOOP_PERIOD_S, ANGLE_P) {
+    angle_controller_.MaxOutput(MAX_ANGLE_POWER);
     comms   = new CommsClass();
     io      = new IOClass();
     display = new DisplayClass(comms);
@@ -118,8 +227,7 @@ struct State {
     esp_timer_init();
     i2c_mutex = xSemaphoreCreateMutex();
 
-    ahrs_.begin(1000000.f /
+    ahrs_.begin(LOOP_PERIOD_S);  // rough sample frequency
-                static_cast<float>(LOOP_PERIOD_US));  // rough sample frequency
 
     io->Init();
     comms->Init();
@@ -164,23 +272,44 @@ struct State {
     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;
+
     switch (ugv_state) {
       default:
         ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
         // fall through
       case UGV_State::STATE_IDLE:
-      case UGV_State::STATE_FINISHED:
+      case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
-        outputs.left_motor  = 0.0;
-        outputs.right_motor = 0.0;
-        break;
       case UGV_State::STATE_AQUIRING: {
+        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;
-        outputs.left_motor     = 0.0;
-        outputs.right_motor    = 0.0;
         if (not_old && not_invalid) {
+          next_state = UGV_State::STATE_TURNING;
+        }
+        break;
+      }
+      case UGV_State::STATE_FLIPPING: {
+        angle_controller_.Disable();
+        break;
+      }
+      case UGV_State::STATE_TURNING: {
+        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;
@@ -201,15 +330,8 @@ struct State {
         }
 
         float tgt_bearing = current_pos.bearing_toward(target);
-        float cur_bearing = ahrs_.getYaw();
+        angle_controller_.Enable();
-        float angle_delta = tgt_bearing - cur_bearing;
+        angle_controller_.Setpoint(tgt_bearing);
-        if (angle_delta < 180.f) angle_delta += 360.f;
-        if (angle_delta > 180.f) angle_delta -= 360.f;
-        float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
-        float power     = DRIVE_POWER;
-
-        outputs.left_motor  = power - angle_pwr;
-        outputs.right_motor = power + angle_pwr;
         break;
       }
       case UGV_State::STATE_TEST:
@@ -217,19 +339,23 @@ struct State {
         outputs.left_motor  = sinf(time_s * PI);
         outputs.right_motor = cosf(time_s * PI);
 #else
-        float tgt_bearing = 90.0;
+        angle_controller_.Enable();
-        float cur_bearing = ahrs_.getYaw();
+        angle_controller_.Setpoint(90.0);
-        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 = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
-        float power     = 0.0;
-
-        outputs.left_motor  = power - angle_pwr;
-        outputs.right_motor = power + angle_pwr;
 #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();