diff --git a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
index 9862663f79..a033a8576b 100644
--- a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
@@ -63,7 +63,9 @@ AggieAir Atomic Tangerine
-
+
+
+
diff --git a/conf/modules/copilot.xml b/conf/modules/copilot.xml
index 3f285e06ef..e5d494e6c6 100644
--- a/conf/modules/copilot.xml
+++ b/conf/modules/copilot.xml
@@ -34,6 +34,7 @@
+
diff --git a/sw/airborne/modules/mission/copilot.h b/sw/airborne/modules/mission/copilot.h
index e9e2d3790f..8d5faaaffe 100644
--- a/sw/airborne/modules/mission/copilot.h
+++ b/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 */
diff --git a/sw/airborne/modules/mission/copilot_common.c b/sw/airborne/modules/mission/copilot_common.c
index 797a0f72ea..82ec1d66dc 100644
--- a/sw/airborne/modules/mission/copilot_common.c
+++ b/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_state,
- &cam_snapshot.snapshot_num,
- &cam_snapshot.snapshot_valid,
- &cam_snapshot.lens_temp,
- &cam_snapshot.array_temp);
+ &cam_snapshot.cam_id, &cam_snapshot.cam_state,
+ &cam_snapshot.snapshot_num, &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.used_mem,
- &cam_payload.used_disk,
- &cam_payload.door_status,
- &cam_payload.error_code);
+ &cam_payload.timestamp, &cam_payload.used_mem, &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.used_mem,
- &copilot_status.used_disk,
- &copilot_status.status,
+ &copilot_status.timestamp, &copilot_status.used_mem,
+ &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 */
+}