more and better messages

main/messages.proto

@@ -33,6 +33,8 @@ message UGV_Status {
   UGV_State state = 1;
   Location location = 2;
   float yaw_angle = 3;
+  float pitch_angle = 4;
+  float roll_angle = 5;
 }
 
 message UGV_Message {

main/ugv.cc

@@ -89,16 +89,21 @@ void UGV::DoDebugPrint() {
   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());
+  //ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
+  //         ahrs_.getPitch(), ahrs_.getRoll());
 }
 
 void UGV::ReadComms() {
   comms->Lock();
   UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
   comms->status.set_yaw_angle(ahrs_.getYaw());
-  current_state_ = comms->status.state();
+  comms->status.set_pitch_angle(ahrs_.getPitch());
+  comms->status.set_roll_angle(ahrs_.getRoll());
+  UGV_State comms_ugv_state = comms->status.state();
+  if (comms_ugv_state != current_state_) {
+    ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state);
+    current_state_ = comms_ugv_state;
+  }
   if (comms->new_target) {
     SetTarget(*comms->new_target);
     ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude,
@@ -122,7 +127,7 @@ void UGV::OnTick() {
   io->ReadInputs(inputs_);
   UpdateAHRS();
 
-  if (time_us >= last_print_ + 500 * 1000) {
+  if (time_us >= last_print_ + 200 * 1000) {
     DoDebugPrint();
     last_print_ = time_us;
   }
@@ -135,7 +140,7 @@ void UGV::OnTick() {
   outputs_.left_motor  = 0.0;
   outputs_.right_motor = 0.0;
 
-  float pitch          = ahrs_.getPitch();
+  float pitch          = ahrs_.getRoll(); // TODO: fix quaternion -> YPR
   auto  min_flip_pitch = conf_.min_flip_pitch();
   bool  is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
 
@@ -162,8 +167,8 @@ void UGV::OnTick() {
     }
     case UGV_State::STATE_FLIPPING: {
       angle_controller_.Disable();
-      outputs_.left_motor  = -1.0;
-      outputs_.right_motor = -1.0;
+      outputs_.left_motor  = 0.8;
+      outputs_.right_motor = 0.8;
       if (!is_upside_down) {
         next_state_ = UGV_State::STATE_AQUIRING;
         break;
@@ -238,6 +243,9 @@ void UGV::OnTick() {
 
   io->WriteOutputs(outputs_);
 
+  if (current_state_ != next_state_) {
+    ESP_LOGI(TAG, "switching state to %d", next_state_);
+  }
   comms->Lock();
   comms->status.set_state(next_state_);
   comms->Unlock();

main/ugv_comms.cc

@@ -13,7 +13,7 @@
 namespace ugv {
 namespace comms {
 
-static const char *TAG = "ugv_comms";
+static const char *     TAG         = "ugv_comms";
 static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(5000);
 
 CommsClass::CommsClass()
@@ -121,7 +121,7 @@ void CommsClass::RunTask() {
   using messages::UGV_Status;
 
   TickType_t current_tick = xTaskGetTickCount();
-  TickType_t next_send    = current_tick + SEND_PERIOD;
+  next_send               = current_tick + SEND_PERIOD;
 
   esp_err_t ret;
 
@@ -286,6 +286,8 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
   ugv_message.set_command_ack(command.id());
 
   SendPacket(ugv_message);
+
+  next_send = xTaskGetTickCount() + pdMS_TO_TICKS(200);
 }
 
 esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {

main/ugv_comms.hh

@@ -52,6 +52,8 @@ class CommsClass {
 #endif
   TaskHandle_t task_handle;
 
+  TickType_t next_send;
+
   void RunTask();
 
 #ifdef COMMS_SX127X

tools/messages_pb2.py

@@ -23,7 +23,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
   package='ugv.messages',
   syntax='proto3',
   serialized_options=_b('H\003'),
-  serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"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\"q\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.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\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.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*\x8a\x01\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\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x42\x02H\x03P\x00\x62\x06proto3')
+  serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"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\"\x9a\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.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\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.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*\x8a\x01\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\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x42\x02H\x03P\x00\x62\x06proto3')
   ,
   dependencies=[config__pb2.DESCRIPTOR,],
   public_dependencies=[config__pb2.DESCRIPTOR,])
@@ -69,8 +69,8 @@ _UGV_STATE = _descriptor.EnumDescriptor(
   ],
   containing_type=None,
   serialized_options=None,
-  serialized_start=774,
-  serialized_end=940,
+  serialized_start=816,
+  serialized_end=982,
 )
 _sym_db.RegisterEnumDescriptor(_UGV_STATE)
 
@@ -108,8 +108,8 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
   ],
   containing_type=None,
   serialized_options=None,
-  serialized_start=943,
-  serialized_end=1081,
+  serialized_start=985,
+  serialized_end=1123,
 )
 _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
 
@@ -249,6 +249,20 @@ _UGV_STATUS = _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='pitch_angle', full_name='ugv.messages.UGV_Status.pitch_angle', index=3,
+      number=4, 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='roll_angle', full_name='ugv.messages.UGV_Status.roll_angle', index=4,
+      number=5, 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=[
   ],
@@ -261,8 +275,8 @@ _UGV_STATUS = _descriptor.Descriptor(
   extension_ranges=[],
   oneofs=[
   ],
-  serialized_start=189,
-  serialized_end=302,
+  serialized_start=190,
+  serialized_end=344,
 )
 
 
@@ -302,8 +316,8 @@ _UGV_MESSAGE = _descriptor.Descriptor(
       name='ugv_message', full_name='ugv.messages.UGV_Message.ugv_message',
       index=0, containing_type=None, fields=[]),
   ],
-  serialized_start=304,
-  serialized_end=399,
+  serialized_start=346,
+  serialized_end=441,
 )
 
 
@@ -340,8 +354,8 @@ _DRIVEHEADINGDATA = _descriptor.Descriptor(
   extension_ranges=[],
   oneofs=[
   ],
-  serialized_start=401,
-  serialized_end=451,
+  serialized_start=443,
+  serialized_end=493,
 )
 
 
@@ -402,8 +416,8 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
       name='data', full_name='ugv.messages.GroundCommand.data',
       index=0, containing_type=None, fields=[]),
   ],
-  serialized_start=454,
-  serialized_end=688,
+  serialized_start=496,
+  serialized_end=730,
 )
 
 
@@ -436,8 +450,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor(
       name='ground_message', full_name='ugv.messages.GroundMessage.ground_message',
       index=0, containing_type=None, fields=[]),
   ],
-  serialized_start=690,
-  serialized_end=771,
+  serialized_start=732,
+  serialized_end=813,
 )
 
 _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE

tools/ugv_cmd.py

@@ -58,12 +58,14 @@ exit, q, C-c, C-d:  Quit the program
         self.ugv.write_command(messages.CMD_DISABLE)
 
     def set_target(self, lat=34.068415, long=-118.443217):
+        lat = float(lat)
+        long = float(long)
         cmd = messages.GroundCommand()
         cmd.type = messages.CMD_SET_TARGET
         cmd.target_location.latitude = lat
         cmd.target_location.longitude = long
         self.ugv.write_command(cmd)
-        log.info("set target to (%d, %d)", lat, long)
+        log.info("set target to (%f, %f)", lat, long)
 
     def set_config(self):
         with open('./tools/config.yml', 'r') as configfile:
@@ -82,12 +84,14 @@ exit, q, C-c, C-d:  Quit the program
         log.info("updated config")
 
     def drive_heading(self, heading=65, power=0.0):
+        heading = float(heading)
+        power = float(power)
         cmd = messages.GroundCommand()
         cmd.type = messages.CMD_DRIVE_HEADING
-        cmd.drive_heading.heading = float(heading)
-        cmd.drive_heading.power = float(power)
+        cmd.drive_heading.heading = heading
+        cmd.drive_heading.power = power
         self.ugv.write_command(cmd)
-        log.info("driving heading %d at power %d", heading, power)
+        log.info("driving heading %f at power %f", heading, power)
 
     def drive_to_target(self):
         cmd = messages.GroundCommand()