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 */ +}