amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

more and better messages

master
Alex Mikhalev 6 years ago
parent
cfe26b125f
commit
f01cc5de9a
6 changed files with 61 additions and 29 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/messages.proto
  2. 24
      main/ugv.cc
  3. 6
      main/ugv_comms.cc
  4. 2
      main/ugv_comms.hh
  5. 44
      tools/messages_pb2.py
  6. 12
      tools/ugv_cmd.py

2
main/messages.proto
Unescape View File

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

24
main/ugv.cc
Unescape View File

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

6
main/ugv_comms.cc
Unescape View File

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

2
main/ugv_comms.hh
Unescape View File

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

44
tools/messages_pb2.py
Unescape View File

@ -23,7 +23,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
package='ugv.messages', package='ugv.messages',
syntax='proto3', syntax='proto3',
serialized_options=_b('H\003'), 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,], dependencies=[config__pb2.DESCRIPTOR,],
public_dependencies=[config__pb2.DESCRIPTOR,]) public_dependencies=[config__pb2.DESCRIPTOR,])
@ -69,8 +69,8 @@ _UGV_STATE = _descriptor.EnumDescriptor(
], ],
containing_type=None, containing_type=None,
serialized_options=None, serialized_options=None,
serialized_start=774, serialized_start=816,
serialized_end=940, serialized_end=982,
) )
_sym_db.RegisterEnumDescriptor(_UGV_STATE) _sym_db.RegisterEnumDescriptor(_UGV_STATE)
@ -108,8 +108,8 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
], ],
containing_type=None, containing_type=None,
serialized_options=None, serialized_options=None,
serialized_start=943, serialized_start=985,
serialized_end=1081, serialized_end=1123,
) )
_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
@ -249,6 +249,20 @@ _UGV_STATUS = _descriptor.Descriptor(
message_type=None, enum_type=None, containing_type=None, message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None, is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR), 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=[ extensions=[
], ],
@ -261,8 +275,8 @@ _UGV_STATUS = _descriptor.Descriptor(
extension_ranges=[], extension_ranges=[],
oneofs=[ oneofs=[
], ],
serialized_start=189, serialized_start=190,
serialized_end=302, serialized_end=344,
) )
@ -302,8 +316,8 @@ _UGV_MESSAGE = _descriptor.Descriptor(
name='ugv_message', full_name='ugv.messages.UGV_Message.ugv_message', name='ugv_message', full_name='ugv.messages.UGV_Message.ugv_message',
index=0, containing_type=None, fields=[]), index=0, containing_type=None, fields=[]),
], ],
serialized_start=304, serialized_start=346,
serialized_end=399, serialized_end=441,
) )
@ -340,8 +354,8 @@ _DRIVEHEADINGDATA = _descriptor.Descriptor(
extension_ranges=[], extension_ranges=[],
oneofs=[ oneofs=[
], ],
serialized_start=401, serialized_start=443,
serialized_end=451, serialized_end=493,
) )
@ -402,8 +416,8 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
name='data', full_name='ugv.messages.GroundCommand.data', name='data', full_name='ugv.messages.GroundCommand.data',
index=0, containing_type=None, fields=[]), index=0, containing_type=None, fields=[]),
], ],
serialized_start=454, serialized_start=496,
serialized_end=688, serialized_end=730,
) )
@ -436,8 +450,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor(
name='ground_message', full_name='ugv.messages.GroundMessage.ground_message', name='ground_message', full_name='ugv.messages.GroundMessage.ground_message',
index=0, containing_type=None, fields=[]), index=0, containing_type=None, fields=[]),
], ],
serialized_start=690, serialized_start=732,
serialized_end=771, serialized_end=813,
) )
_UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE

12
tools/ugv_cmd.py
Unescape View File

@ -58,12 +58,14 @@ exit, q, C-c, C-d: Quit the program
self.ugv.write_command(messages.CMD_DISABLE) self.ugv.write_command(messages.CMD_DISABLE)
def set_target(self, lat=34.068415, long=-118.443217): def set_target(self, lat=34.068415, long=-118.443217):
lat = float(lat)
long = float(long)
cmd = messages.GroundCommand() cmd = messages.GroundCommand()
cmd.type = messages.CMD_SET_TARGET cmd.type = messages.CMD_SET_TARGET
cmd.target_location.latitude = lat cmd.target_location.latitude = lat
cmd.target_location.longitude = long cmd.target_location.longitude = long
self.ugv.write_command(cmd) 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): def set_config(self):
with open('./tools/config.yml', 'r') as configfile: 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") log.info("updated config")
def drive_heading(self, heading=65, power=0.0): def drive_heading(self, heading=65, power=0.0):
heading = float(heading)
power = float(power)
cmd = messages.GroundCommand() cmd = messages.GroundCommand()
cmd.type = messages.CMD_DRIVE_HEADING cmd.type = messages.CMD_DRIVE_HEADING
cmd.drive_heading.heading = float(heading) cmd.drive_heading.heading = heading
cmd.drive_heading.power = float(power) cmd.drive_heading.power = power
self.ugv.write_command(cmd) 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): def drive_to_target(self):
cmd = messages.GroundCommand() cmd = messages.GroundCommand()

Write Preview
Loading…
Cancel
Save