From 53a1034988994809f3eb2f269041ff3e6a310914 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Thu, 3 Jan 2019 18:12:04 -0700
Subject: [PATCH] add command handling

---
 main/ugv_comms.c | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/main/ugv_comms.c b/main/ugv_comms.c
index 9730962..c028ed6 100644
--- a/main/ugv_comms.c
+++ b/main/ugv_comms.c
@@ -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);
+}
\ No newline at end of file