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

add command handling

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-03 18:12:04 -07:00
parent c5584ad801
commit 53a1034988
1 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
main/ugv_comms.c
View File

@ -15,6 +15,8 @@ static uint16_t packet_num;
static void ugv_comms_task(void *arg);
static void ugv_comms_handle_packet(ugv_comms_state_t * st,
sx127x_rx_packet_t *rx_packet);
static void ugv_comms_handle_command(ugv_comms_state_t * st,
uas_ugv_GroundCommand *command);
void ugv_comms_init() {
ugv_comms_state_t *st = &ugv_comms_state;
@ -120,4 +122,38 @@ static void ugv_comms_handle_packet(ugv_comms_state_t * st,
st->last_packet_rssi = rx_packet->rssi;
st->last_packet_snr = rx_packet->snr;
xSemaphoreGive(st->mutex);
uas_ugv_GroundMessage ground_message = uas_ugv_GroundMessage_init_default;
pb_istream_t istream;
bool pb_ret;
istream =
pb_istream_from_buffer((pb_byte_t *)rx_packet->data, rx_packet->data_len);
pb_ret = pb_decode(&istream, uas_ugv_GroundMessage_fields, &ground_message);
if (!pb_ret) {
ESP_LOGE(TAG, "rx invalid protobuf");
return;
}
switch (ground_message.which_ground_message) {
case uas_ugv_GroundMessage_command_tag:
ugv_comms_handle_command(st, &ground_message.ground_message.command);
break;
default:
ESP_LOGE(TAG, "invalid ground message: %d",
ground_message.which_ground_message);
break;
}
}
static void ugv_comms_handle_command(ugv_comms_state_t * st,
uas_ugv_GroundCommand *command) {
ESP_LOGI(TAG, "rx command id %d type %d", command->id, command->type);
// TODO: handle command
uas_ugv_UGV_Message ugv_message = uas_ugv_UGV_Message_init_default;
ugv_message.which_ugv_message = uas_ugv_UGV_Message_command_ack_tag;
ugv_message.ugv_message.command_ack = command->id;
sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields, &ugv_message, 0);
}