[extra_dl] Extend module functionality (#2042)

use dedicated buffer

conf/modules/extra_dl.xml

@@ -11,7 +11,16 @@
   </header>
   <init fun="extra_pprz_dl_init()"/>
   <periodic fun="extra_pprz_dl_periodic()" freq="TELEMETRY_FREQUENCY" autorun="TRUE"/>
-  <event fun="ExtraDatalinkEvent()"/>
+
+  <!--
+  The event function listens on the extra telemetry port (from the payload computer for example)
+  and parses the received "datalink" messages, such as MOVE_WP.
+  We need this function to be able to listen to the extra telemetry port  
+
+  As a result all messages normally handled by the autopilot (such as MOVE_WP) are processed.
+  -->
+  <event fun="extra_pprz_dl_event()"/>
+
   <makefile target="ap">
     <configure name="EXTRA_DL_PORT" default="uart1" case="upper|lower"/>
     <define name="EXTRA_DOWNLINK_DEVICE" value="$(EXTRA_DL_PORT_LOWER)"/>

sw/airborne/arch/sim/sim_ap.c

@@ -137,7 +137,7 @@ value set_datalink_message(value s)
   }
 
   dl_msg_available = true;
-  DlCheckAndParse(&(DOWNLINK_DEVICE).device, &ivy_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&(DOWNLINK_DEVICE).device, &ivy_tp.trans_tx, dl_buffer, &dl_msg_available);
 
   return Val_unit;
 }

sw/airborne/modules/datalink/bluegiga_dl.c

@@ -38,6 +38,6 @@ void bluegiga_dl_init(void)
 void bluegiga_dl_event(void)
 {
   pprz_check_and_parse(&DOWNLINK_DEVICE.device, &pprz_bg_tp, dl_buffer, &dl_msg_available);
-  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_bg_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_bg_tp.trans_tx, dl_buffer, &dl_msg_available);
 }
 

sw/airborne/modules/datalink/extra_pprz_dl.c

@@ -46,6 +46,8 @@
 #include "modules/datalink/extra_pprz_dl.h"
 #include "subsystems/datalink/telemetry.h"
 
+bool extra_dl_msg_available;
+uint8_t extra_dl_buffer[MSG_SIZE]  __attribute__((aligned));
 
 struct pprz_transport extra_pprz_tp;
 
@@ -54,6 +56,14 @@ void extra_pprz_dl_init(void)
   pprz_transport_init(&extra_pprz_tp);
 }
 
+
+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, &extra_dl_msg_available);
+}
+
+
 void extra_pprz_dl_periodic(void)
 {
 #if PERIODIC_TELEMETRY && defined(TELEMETRY_PROCESS_Extra)
@@ -61,4 +71,3 @@ void extra_pprz_dl_periodic(void)
   periodic_telemetry_send_Extra(DefaultPeriodic, &extra_pprz_tp.trans_tx, &(EXTRA_DOWNLINK_DEVICE).device);
 #endif
 }
-

sw/airborne/modules/datalink/extra_pprz_dl.h

@@ -41,11 +41,9 @@
 /* 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);

sw/airborne/modules/datalink/pprz_dl.c

@@ -36,6 +36,6 @@ void pprz_dl_init(void)
 void pprz_dl_event(void)
 {
   pprz_check_and_parse(&DOWNLINK_DEVICE.device, &pprz_tp, dl_buffer, &dl_msg_available);
-  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_tp.trans_tx, dl_buffer, &dl_msg_available);
 }
 

sw/airborne/modules/datalink/xbee_dl.c

@@ -53,6 +53,6 @@ void xbee_dl_init(void)
 void xbee_dl_event(void)
 {
   xbee_check_and_parse(&(XBEE_UART).device, &xbee_tp, dl_buffer, &dl_msg_available);
-  DlCheckAndParse(&(XBEE_UART).device, &xbee_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&(XBEE_UART).device, &xbee_tp.trans_tx, dl_buffer, &dl_msg_available);
 }
 

sw/airborne/modules/nav/nav_geofence.h

@@ -31,9 +31,8 @@
  * GEOFENCE_DATALINK_LOST_TIME is defined in the airframe config file
  */
 
-#include <stdbool.h>
-
 #ifdef GEOFENCE_DATALINK_LOST_TIME
+#include "subsystems/datalink/datalink.h"
 /*
  * from the airfame config file:
  * go to HOME mode if datalink lost for GEOFENCE_DATALINK_LOST_TIME

sw/airborne/subsystems/datalink/datalink.h

@@ -79,7 +79,7 @@ EXTERN bool datalink_enabled;
 }
 
 /** Check for new message and parse */
-static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
+static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf, bool *msg_available)
 {
   // make it possible to disable datalink in NPS sim
 #if USE_NPS
@@ -88,11 +88,11 @@ static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx
   }
 #endif
 
-  if (dl_msg_available) {
+  if (*msg_available) {
     datalink_time = 0;
     datalink_nb_msgs++;
     dl_parse_msg(dev, trans, buf);
-    dl_msg_available = false;
+    *msg_available = false;
   }
 }
 

sw/airborne/subsystems/datalink/superbitrf.c

@@ -288,7 +288,7 @@ void superbitrf_dl_init(void)
  */
 void superbitrf_dl_event(void)
 {
-  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_srf_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&DOWNLINK_DEVICE.device, &pprz_srf_tp.trans_tx, dl_buffer, &dl_msg_available);
 }
 
 void superbitrf_set_mfg_id(uint32_t id)

sw/airborne/subsystems/datalink/w5100.h

@@ -104,7 +104,7 @@ static inline void w5100_check_and_parse(struct link_device *dev, struct pprz_tr
 static inline w5100_event(void)
 {
   w5100_check_and_parse(&(W5100).device, &pprz_w5100_tp);
-  DlCheckAndParse(&(W5100).device, &pprz_w5100_tp.trans_tx, dl_buffer);
+  DlCheckAndParse(&(W5100).device, &pprz_w5100_tp.trans_tx, dl_buffer, &dl_msg_available);
 }
 
 #endif /* W5100_H */