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