Maybe a second uplink through iridium...

conf/modules/telemetry_intermcu.xml

@@ -18,6 +18,7 @@
   <makefile target="ap">
     <file name="telemetry_intermcu_ap.c"/>
     <file name="short_transport.c" dir="pprzlink/src"/>
+    <define name="TELEMETRY_INTERMCU"/>
   </makefile>
   <makefile target="fbw">
     <file name="telemetry_intermcu_fbw.c"/>

sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c

@@ -36,32 +36,32 @@
 
 #include "firmwares/rotorcraft/autopilot.h"
 
-void firmware_parse_msg(void)
+void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused)))
 {
-  uint8_t msg_id = IdOfPprzMsg(dl_buffer);
+  uint8_t msg_id = IdOfPprzMsg(buf);
 
   /* parse telemetry messages coming from ground station */
   switch (msg_id) {
 
 #ifdef USE_NAVIGATION
     case DL_BLOCK : {
-      if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
-      nav_goto_block(DL_BLOCK_block_id(dl_buffer));
+      if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
+      nav_goto_block(DL_BLOCK_block_id(buf));
     }
     break;
 
     case DL_MOVE_WP : {
-      uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
+      uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
       if (ac_id != AC_ID) { break; }
       if (stateIsLocalCoordinateValid()) {
-        uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
+        uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
         struct LlaCoor_i lla;
-        lla.lat = DL_MOVE_WP_lat(dl_buffer);
-        lla.lon = DL_MOVE_WP_lon(dl_buffer);
+        lla.lat = DL_MOVE_WP_lat(buf);
+        lla.lon = DL_MOVE_WP_lon(buf);
         /* WP_alt from message is alt above MSL in mm
          * lla.alt is above ellipsoid in mm
          */
-        lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
+        lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
                   state.ned_origin_i.lla.alt;
         waypoint_move_lla(wp_id, &lla);
       }
@@ -70,13 +70,13 @@ void firmware_parse_msg(void)
 #endif /* USE_NAVIGATION */
 
     case DL_GUIDED_SETPOINT_NED:
-      if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
+      if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; }
 
-      autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(dl_buffer),
-                              DL_GUIDED_SETPOINT_NED_x(dl_buffer),
-                              DL_GUIDED_SETPOINT_NED_y(dl_buffer),
-                              DL_GUIDED_SETPOINT_NED_z(dl_buffer),
-                              DL_GUIDED_SETPOINT_NED_yaw(dl_buffer));
+      autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(buf),
+                              DL_GUIDED_SETPOINT_NED_x(buf),
+                              DL_GUIDED_SETPOINT_NED_y(buf),
+                              DL_GUIDED_SETPOINT_NED_z(buf),
+                              DL_GUIDED_SETPOINT_NED_yaw(buf));
       break;
 
     default:

sw/airborne/modules/telemetry/telemetry_intermcu_ap.c

@@ -31,6 +31,7 @@
 #include "pprzlink/short_transport.h"
 #include "generated/periodic_telemetry.h"
 #include "subsystems/datalink/telemetry.h"
+#include "subsystems/datalink/datalink.h"
 
 /* Default maximum telemetry message size */
 #ifndef TELEMERTY_INTERMCU_MSG_SIZE
@@ -80,9 +81,9 @@ void telemetry_intermcu_event(void)
 
 }
 
-void telemetry_intermcu_on_msg(uint8_t msg_id __attribute__((unused)), uint8_t* msg __attribute__((unused)), uint8_t size __attribute__((unused)))
+void telemetry_intermcu_on_msg(uint8_t msg_id __attribute__((unused)), uint8_t* msg, uint8_t size __attribute__((unused)))
 {
-
+  DlCheckAndParse(&telemetry_intermcu.dev, &telemetry_intermcu.trans.trans_tx, msg);
 }
 
 static bool telemetry_intermcu_check_free_space(struct telemetry_intermcu_t *p, long *fd __attribute__((unused)), uint16_t len)

sw/airborne/modules/telemetry/telemetry_intermcu_fbw.c

@@ -84,6 +84,8 @@ void telemetry_intermcu_event(void)
     }
 
     /* Forward to AP */
+    pprz_msg_send_IMCU_DATALINK(&(intermcu.transport.trans_tx), intermcu.device,
+                            INTERMCU_FBW, telemetry_intermcu.trans.trans_rx.payload_len, telemetry_intermcu.rx_buffer);
 
     telemetry_intermcu.msg_received = false;
   }

sw/airborne/subsystems/datalink/datalink.c

@@ -52,10 +52,11 @@
 bool datalink_enabled = true;
 #endif
 
-void dl_parse_msg(void)
+
+void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
 {
-  uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
-  uint8_t msg_id = IdOfPprzMsg(dl_buffer);
+  uint8_t sender_id = SenderIdOfPprzMsg(buf);
+  uint8_t msg_id = IdOfPprzMsg(buf);
 
   /* parse telemetry messages coming from other AC */
   if (sender_id != 0) {
@@ -68,24 +69,24 @@ void dl_parse_msg(void)
     /* parse telemetry messages coming from ground station */
     switch (msg_id) {
       case  DL_PING: {
-        DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
+        pprz_msg_send_PONG(trans, dev, AC_ID);
       }
       break;
 
       case DL_SETTING : {
-        if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
-        uint8_t i = DL_SETTING_index(dl_buffer);
-        float var = DL_SETTING_value(dl_buffer);
+        if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
+        uint8_t i = DL_SETTING_index(buf);
+        float var = DL_SETTING_value(buf);
         DlSetting(i, var);
-        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
+        pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
       }
       break;
 
       case DL_GET_SETTING : {
-        if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
-        uint8_t i = DL_GET_SETTING_index(dl_buffer);
+        if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+        uint8_t i = DL_GET_SETTING_index(buf);
         float val = settings_get_value(i);
-        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
+        pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
       }
       break;
 
@@ -95,34 +96,34 @@ void dl_parse_msg(void)
         LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
 #endif
         parse_rc_3ch_datalink(
-          DL_RC_3CH_throttle_mode(dl_buffer),
-          DL_RC_3CH_roll(dl_buffer),
-          DL_RC_3CH_pitch(dl_buffer));
+          DL_RC_3CH_throttle_mode(buf),
+          DL_RC_3CH_roll(buf),
+          DL_RC_3CH_pitch(buf));
         break;
       case DL_RC_4CH :
-        if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
+        if (DL_RC_4CH_ac_id(buf) == AC_ID) {
 #ifdef RADIO_CONTROL_DATALINK_LED
           LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
 #endif
-          parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
-                                DL_RC_4CH_throttle(dl_buffer),
-                                DL_RC_4CH_roll(dl_buffer),
-                                DL_RC_4CH_pitch(dl_buffer),
-                                DL_RC_4CH_yaw(dl_buffer));
+          parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
+                                DL_RC_4CH_throttle(buf),
+                                DL_RC_4CH_roll(buf),
+                                DL_RC_4CH_pitch(buf),
+                                DL_RC_4CH_yaw(buf));
         }
         break;
 #endif // RADIO_CONTROL_TYPE_DATALINK
 
 #if USE_GPS
       case DL_GPS_INJECT : {
         // Check if the GPS is for this AC
-        if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
+        if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
 
         // GPS parse data
         gps_inject_data(
-          DL_GPS_INJECT_packet_id(dl_buffer),
-          DL_GPS_INJECT_data_length(dl_buffer),
-          DL_GPS_INJECT_data(dl_buffer)
+          DL_GPS_INJECT_packet_id(buf),
+          DL_GPS_INJECT_data_length(buf),
+          DL_GPS_INJECT_data(buf)
         );
       }
       break;
@@ -133,13 +134,13 @@ void dl_parse_msg(void)
     }
   }
   /* Parse firmware specific datalink */
-  firmware_parse_msg();
+  firmware_parse_msg(dev, trans, buf);
 
   /* Parse modules datalink */
   modules_parse_datalink(msg_id);
 }
 
 /* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */
-WEAK void firmware_parse_msg(void)
+WEAK void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused)))
 {
-}
+}

sw/airborne/subsystems/datalink/datalink.h

@@ -60,10 +60,10 @@ EXTERN uint16_t datalink_nb_msgs;
 EXTERN uint8_t dl_buffer[MSG_SIZE]  __attribute__((aligned));
 
 /** Should be called when chars are available in dl_buffer */
-EXTERN void dl_parse_msg(void);
+EXTERN void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf);
 
 /** Firmware specfic msg handler */
-EXTERN void firmware_parse_msg(void);
+EXTERN void firmware_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf);
 
 #if USE_NPS
 EXTERN bool datalink_enabled;
@@ -79,7 +79,7 @@ EXTERN bool datalink_enabled;
 }
 
 /** Check for new message and parse */
-static inline void DlCheckAndParse(void)
+static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
 {
   // make it possible to disable datalink in NPS sim
 #if USE_NPS
@@ -91,7 +91,7 @@ static inline void DlCheckAndParse(void)
   if (dl_msg_available) {
     datalink_time = 0;
     datalink_nb_msgs++;
-    dl_parse_msg();
+    dl_parse_msg(dev, trans, buf);
     dl_msg_available = false;
   }
 }
@@ -100,14 +100,14 @@ static inline void DlCheckAndParse(void)
 
 #define DatalinkEvent() {                       \
     pprz_check_and_parse(&(PPRZ_UART).device, &pprz_tp, dl_buffer, &dl_msg_available);      \
-    DlCheckAndParse();                          \
+    DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer);               \
   }
 
 #elif defined DATALINK && DATALINK == XBEE
 
 #define DatalinkEvent() {                       \
     xbee_check_and_parse(&(XBEE_UART).device, &xbee_tp, dl_buffer, &dl_msg_available);      \
-    DlCheckAndParse();                          \
+    DlCheckAndParse(&(DOWNLINK_DEVICE).device, &xbee_tp.trans_tx, dl_buffer);               \
   }
 
 #elif defined DATALINK && DATALINK == W5100
@@ -128,7 +128,7 @@ static inline void DlCheckAndParse(void)
 
 #define DatalinkEvent() {                       \
     pprz_check_and_parse(&(DOWNLINK_DEVICE).device, &pprz_tp, dl_buffer, &dl_msg_available);      \
-    DlCheckAndParse();                          \
+    DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer);                     \
   }
 
 #else

sw/airborne/subsystems/intermcu/intermcu_ap.c

@@ -164,6 +164,16 @@ static inline void intermcu_parse_msg(void (*rc_frame_handler)(void))
       break;
     }
 
+#if TELEMETRY_INTERMCU
+    case DL_IMCU_DATALINK: {
+      uint8_t size = DL_IMCU_TELEMETRY_msg_length(imcu_msg_buf);
+      uint8_t *msg = DL_IMCU_TELEMETRY_msg(imcu_msg_buf);
+      telemetry_intermcu_on_msg(0, msg, size);
+      break;
+    }
+
+#endif
+
 #if IMCU_GPS
     case DL_IMCU_REMOTE_GPS: {
       uint32_t now_ts = get_sys_time_usec();