Add copilot support for forwarding paylaod commands to the mission computer and back (#2302)

conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml

@@ -63,7 +63,9 @@ AggieAir Atomic Tangerine
     <module name="nav" type="skid_landing"/>
 
     <module name="sys_mon"/>
-    <module name="copilot"/>
+    <module name="copilot">
+      <define name="FORWARD_PAYLOAD" value="TRUE"/>
+    </module>
     <module name="extra_dl">
       <!-- in order to use uart1 without chibios we need to remap the peripheral-->
       <define name="REMAP_UART1" value="TRUE"/>

conf/modules/copilot.xml

@@ -34,6 +34,7 @@
   <datalink message="CAMERA_PAYLOAD_DL" fun="copilot_parse_cam_payload_dl(buf)"/>
   <datalink message="COPILOT_STATUS_DL" fun="copilot_parse_copilot_status_dl(buf)"/>
   <datalink message="MOVE_WP" fun="copilot_parse_move_wp_dl(buf)"/>
+  <datalink message="PAYLOAD_COMMAND" fun="copilot_parse_payload_command_dl(buf)"/>
   <makefile target="ap">
     <file name="copilot_common.c"/>
   </makefile>

sw/airborne/modules/mission/copilot.h

@@ -92,6 +92,7 @@ void copilot_parse_cam_snapshot_dl(uint8_t *buf);
 void copilot_parse_cam_payload_dl(uint8_t *buf);
 void copilot_parse_copilot_status_dl(uint8_t *buf);
 void copilot_parse_move_wp_dl(uint8_t *buf);
+void copilot_parse_payload_command_dl(uint8_t *buf);
 
 #endif /* COPILOT_H */
 

sw/airborne/modules/mission/copilot_common.c

@@ -76,35 +76,26 @@ void copilot_init(void)
 void copilot_periodic(void)
 {
   PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx);
-  if (send_cam_snapshot)
+  if (send_cam_snapshot) {
-  {
     // send down to GCS
     DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
-        &cam_snapshot.cam_id,
+        &cam_snapshot.cam_id, &cam_snapshot.cam_state,
-        &cam_snapshot.cam_state,
+        &cam_snapshot.snapshot_num, &cam_snapshot.snapshot_valid,
-        &cam_snapshot.snapshot_num,
+        &cam_snapshot.lens_temp, &cam_snapshot.array_temp);
-        &cam_snapshot.snapshot_valid,
-        &cam_snapshot.lens_temp,
-        &cam_snapshot.array_temp);
 
     send_cam_snapshot = false;
   }
   PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx);
 
-
   PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx);
-  if (send_cam_payload)
+  if (send_cam_payload) {
-  {
     // NOTE: to send the message over the EXTRA_DL port
     // use "DOWNLINK_SEND_CAMERA_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,"
 
     // send down to GCS
     DOWNLINK_SEND_CAMERA_PAYLOAD(DefaultChannel, DefaultDevice,
-        &cam_payload.timestamp,
+        &cam_payload.timestamp, &cam_payload.used_mem, &cam_payload.used_disk,
-        &cam_payload.used_mem,
+        &cam_payload.door_status, &cam_payload.error_code);
-        &cam_payload.used_disk,
-        &cam_payload.door_status,
-        &cam_payload.error_code);
 
     send_cam_payload = false;
   }
@@ -112,13 +103,10 @@ void copilot_periodic(void)
 
   PPRZ_MUTEX_LOCK(copilot_status_mtx);
   // send down to GCS
-  if (send_copilot_status)
+  if (send_copilot_status) {
-  {
     DOWNLINK_SEND_COPILOT_STATUS(DefaultChannel, DefaultDevice,
-        &copilot_status.timestamp,
+        &copilot_status.timestamp, &copilot_status.used_mem,
-        &copilot_status.used_mem,
+        &copilot_status.used_disk, &copilot_status.status,
-        &copilot_status.used_disk,
-        &copilot_status.status,
         &copilot_status.error_code);
 
     send_copilot_status = false;
@@ -188,3 +176,30 @@ void copilot_parse_copilot_status_dl(uint8_t *buf)
 
   PPRZ_MUTEX_UNLOCK(copilot_status_mtx);
 }
+
+/**
+ * Pass through PAYLOAD_COMMAND message
+ * and send it as PAYLOAD msg over telemetry
+ */
+void copilot_parse_payload_command_dl(uint8_t *buf __attribute__((unused)))
+{
+#if FORWARD_PAYLOAD
+MESSAGE("Forwarding PAYLOAD_COMMAND messages.")
+  // Check if message was for us
+  if (DL_PAYLOAD_COMMAND_ac_id(buf) == AC_ID) {
+    uint8_t len = DL_PAYLOAD_COMMAND_command_length(buf);
+
+    if (buf == extra_dl_buffer) {
+       // Message came from extra_dl, forward to GCS via telemetry
+      DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice,
+            len, DL_PAYLOAD_COMMAND_command(buf));
+    }
+
+    if (buf == dl_buffer) {
+      // Message came from GCS/Telemetry, forward to extra_dl
+      DOWNLINK_SEND_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
+            len, DL_PAYLOAD_COMMAND_command(buf));
+    }
+  }
+#endif /* FORWARD_PAYLOAD */
+}