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

more and better messages

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-22 18:45:55 -07:00
parent cfe26b125f
commit f01cc5de9a
6 changed files with 61 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/messages.proto
View File

@ -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 {

24
main/ugv.cc
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)",
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();

6
main/ugv_comms.cc
View File

@ -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) {

2
main/ugv_comms.hh
View File

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

44
tools/messages_pb2.py
View File

@ -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

12
tools/ugv_cmd.py
View File

@ -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()