diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index 966860b8b7..37af8b7b81 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -11,7 +11,24 @@ - + + + + + + diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.c b/sw/airborne/modules/datalink/extra_pprz_dl.c index 0ae723bfa1..724aabc661 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.c +++ b/sw/airborne/modules/datalink/extra_pprz_dl.c @@ -45,8 +45,22 @@ #include "modules/datalink/extra_pprz_dl.h" #include "subsystems/datalink/telemetry.h" +#include "subsystems/datalink/datalink.h" +#include "pprzlink/pprz_transport.h" + +#if USE_UDP +#include "mcu_periph/udp.h" +#endif + +#if USE_USB_SERIAL +#include "mcu_periph/usb_serial.h" +#endif +bool extra_dl_msg_available; +uint8_t extra_dl_buffer[MSG_SIZE] __attribute__((aligned)); + +/* PPRZ transport structure */ struct pprz_transport extra_pprz_tp; void extra_pprz_dl_init(void) @@ -54,6 +68,12 @@ void extra_pprz_dl_init(void) pprz_transport_init(&extra_pprz_tp); } +extern void extra_pprz_dl_event(void) +{ + pprz_check_and_parse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp, extra_dl_buffer, &extra_dl_msg_available); + DlCheckAndParse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp.trans_tx, extra_dl_buffer); +} + void extra_pprz_dl_periodic(void) { #if PERIODIC_TELEMETRY && defined(TELEMETRY_PROCESS_Extra) @@ -62,3 +82,21 @@ void extra_pprz_dl_periodic(void) #endif } + +void extra_pprz_dl_parse_payload_cmd(void) +{ + // check if the command it meant for me + if (AC_ID != DL_PAYLOAD_COMMAND_ac_id(extra_dl_buffer)){ + return; + } + + // check if the payload length it correct + if (EXPECTED_PAYLOAD_LENGTH != DL_PAYLOAD_COMMAND_command_length(extra_dl_buffer)){ + return; + } + + // optionally we can check for PAYLOAD_COMMAND version etc, depending on what we define in the packet + if (DL_PAYLOAD_COMMAND_command(extra_dl_buffer)[PAYLOAD_CMD_IDX] == PAYLOAD_CMD_INFO){ + // TODO: do something + } +} diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index faa940f136..b0c14515d0 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -27,34 +27,29 @@ #ifndef EXTRA_PPRZ_DL_H #define EXTRA_PPRZ_DL_H -#include "subsystems/datalink/datalink.h" -#include "pprzlink/pprz_transport.h" +// example of checking for correct PAYLOAD_COMMAND message +#define EXPECTED_PAYLOAD_LENGTH 8 -#if USE_UDP -#include "mcu_periph/udp.h" -#endif +// example of a payload command +#define PAYLOAD_CMD_INFO 1 -#if USE_USB_SERIAL -#include "mcu_periph/usb_serial.h" -#endif +// so we don't have to remember the index of bytes +#define PAYLOAD_CMD_IDX 0 -/* PPRZ transport structure */ -extern struct pprz_transport extra_pprz_tp; - -/* Datalink Event */ -#define ExtraDatalinkEvent() { \ - pprz_check_and_parse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp, dl_buffer, &dl_msg_available); \ - DlCheckAndParse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp.trans_tx, dl_buffer); \ - } +/** Datalink Event */ +void extra_pprz_dl_event(void); /** Init function */ -extern void extra_pprz_dl_init(void); +void extra_pprz_dl_init(void); /** Periodic function * * should be called at TELEMETRY_FREQUENCY */ -extern void extra_pprz_dl_periodic(void); +void extra_pprz_dl_periodic(void); + +/** Process payload commands */ +void extra_pprz_dl_parse_payload_cmd(void); #endif /* EXTRA_PPRZ_DL_H */