From f01cc5de9a937d6adcf75ad7ab93815cd4c7b493 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Wed, 22 May 2019 18:45:55 -0700 Subject: [PATCH] more and better messages --- main/messages.proto | 2 ++ main/ugv.cc | 24 +++++++++++++++-------- main/ugv_comms.cc | 6 ++++-- main/ugv_comms.hh | 2 ++ tools/messages_pb2.py | 44 ++++++++++++++++++++++++++++--------------- tools/ugv_cmd.py | 12 ++++++++---- 6 files changed, 61 insertions(+), 29 deletions(-) diff --git a/main/messages.proto b/main/messages.proto index 485e891..8d2a41b 100644 --- a/main/messages.proto +++ b/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 { diff --git a/main/ugv.cc b/main/ugv.cc index cc5777b..cd38722 100644 --- a/main/ugv.cc +++ b/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(); diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index beeadfe..b333cb1 100644 --- a/main/ugv_comms.cc +++ b/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) { diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index f2a96dd..5b248c6 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -52,6 +52,8 @@ class CommsClass { #endif TaskHandle_t task_handle; + TickType_t next_send; + void RunTask(); #ifdef COMMS_SX127X diff --git a/tools/messages_pb2.py b/tools/messages_pb2.py index ce48587..0ed1544 100644 --- a/tools/messages_pb2.py +++ b/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 diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py index 19c7144..53babfa 100755 --- a/tools/ugv_cmd.py +++ b/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()