[telemetry] fixing and cleaning a few stuff

sw/airborne/boards/ardrone/navdata.c

@@ -154,7 +154,7 @@ static void send_navdata(struct transport_tx *trans, struct device *dev) {
       &nav_port.checksum_errors);
 }
 
-static void send_fliter_status(void) {
+static void send_filter_status(struct transport_tx *trans, struct device *dev) {
   uint8_t mde = 3;
   if (ahrs.status == AHRS_UNINIT) mde = 2;
   if (imu_lost) mde = 5;
@@ -231,7 +231,7 @@ bool_t navdata_init()
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
-  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
+  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
 #endif
 
   return TRUE;
@@ -324,7 +324,7 @@ static void mag_freeze_check(void) {
 
         uint8_t mde = 5;
         uint16_t val = 0;
-        pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
+        DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val);
 
         // stop acquisition
         uint8_t cmd=0x02;

sw/airborne/firmwares/beth/overo_test_uart.c

@@ -146,9 +146,9 @@ static void main_periodic(int my_sig_num) {
 
 #include "subsystems/datalink/downlink.h"
 #if 0
-uint8_t downlink_nb_ovrn;
+uint8_t downlink.nb_ovrn;
-uint16_t downlink_nb_bytes;
+uint16_t downlink.nb_bytes;
-uint16_t downlink_nb_msgs;
+uint16_t downlink.nb_msgs;
 
 #define __Transport(dev, _x) dev##_x
 #define _Transport(dev, _x) __Transport(dev, _x)
@@ -161,11 +161,11 @@ uint16_t downlink_nb_msgs;
 #define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x))
 #define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x))
 
-#define DownlinkOverrun(_chan) downlink_nb_ovrn++;
+#define DownlinkOverrun(_chan) downlink.nb_ovrn++;
-#define DownlinkCountBytes(_chan, _n) downlink_nb_bytes += _n;
+#define DownlinkCountBytes(_chan, _n) downlink.nb_bytes += _n;
 
 #define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \
-  downlink_nb_msgs++; \
+  downlink.nb_msgs++; \
   Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \
   Transport(_chan, PutUint8(AC_ID)); \
   Transport(_chan, PutNamedUint8(_name, msg_id)); \

sw/airborne/firmwares/fixedwing/autopilot.c

@@ -138,10 +138,10 @@ static void send_airspeed(struct transport_tx *trans __attribute__((unused)), st
 
 static void send_downlink(struct transport_tx *trans, struct device *dev) {
   static uint16_t last;
-  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
+  uint16_t rate = (downlink.nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
-  last = downlink_nb_bytes;
+  last = downlink.nb_bytes;
   pprz_msg_send_DOWNLINK(trans, dev, AC_ID,
-      &downlink_nb_ovrn, &rate, &downlink_nb_msgs);
+      &downlink.nb_ovrn, &rate, &downlink.nb_msgs);
 }
 
 void autopilot_init(void) {

sw/airborne/firmwares/fixedwing/datalink.c

@@ -139,7 +139,7 @@ void dl_parse_msg(void) {
     DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
   } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
     nav_goto_block(DL_BLOCK_block_id(dl_buffer));
-    SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+    SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
   } else
 #endif /** NAV */
 #ifdef WIND_INFO

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -73,12 +73,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
 
 // datalink & telemetry
 #include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/downlink.h"
 #include "subsystems/datalink/telemetry.h"
 #include "subsystems/settings.h"
-#include "subsystems/datalink/pprz_transport.h"
-#include "subsystems/datalink/xbee.h"
-#include "subsystems/datalink/w5100.h"
-#include "subsystems/datalink/ivy_transport.h"
 
 // modules & settings
 #include "generated/modules.h"
@@ -244,20 +241,7 @@ void init_ap( void ) {
   /** - start interrupt task */
   mcu_int_enable();
 
-#if defined DATALINK
+  downlink_init();
-#if DATALINK == PPRZ
-  pprz_transport_init();
-#endif
-#if DATALINK == XBEE
-  xbee_init();
-#endif
-#if DATALINK == W5100
-  w5100_init();
-#endif
-#endif /* DATALINK */
-#if SITL
-  ivy_transport_init();
-#endif
 
 #if defined AEROCOMM_DATA_PIN
   IO0DIR |= _BV(AEROCOMM_DATA_PIN);
@@ -513,7 +497,7 @@ void navigation_task( void ) {
 #endif
 
 #ifndef PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode)
-  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+  SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
 #endif
 
   /* The nav task computes only nav_altitude. However, we are interested

sw/airborne/firmwares/fixedwing/nav.c

@@ -457,18 +457,18 @@ static void send_nav_ref(struct transport_tx *trans, struct device *dev) {
 }
 
 static void send_nav(struct transport_tx *trans, struct device *dev) {
-  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+  SEND_NAVIGATION(trans, dev);
 }
 
 static void send_wp_moved(struct transport_tx *trans, struct device *dev) {
   static uint8_t i;
   i++; if (i >= nb_waypoint) i = 0;
-  DownlinkSendWp(DefaultChannel, DefaultDevice, i);
+  DownlinkSendWp(trans, dev, i);
 }
 
 bool_t DownlinkSendWpNr(uint8_t _wp)
 {
-  DownlinkSendWp(DefaultChannel, DefaultDevice, _wp);
+  DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
   return FALSE;
 }
 

sw/airborne/firmwares/fixedwing/nav.h

@@ -206,7 +206,7 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
     struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
     float dist_wp = sqrtf(dist2_to_wp); \
     float dist_home = sqrtf(dist2_to_home); \
-    DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \
+    pprz_msg_send_NAVIGATION(_trans, _dev, AC_ID, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \
 }
 
 extern bool_t DownlinkSendWpNr(uint8_t _wp);
@@ -214,7 +214,7 @@ extern bool_t DownlinkSendWpNr(uint8_t _wp);
 #define DownlinkSendWp(_trans, _dev, i) {	   \
   float x = nav_utm_east0 +  waypoints[i].x; \
   float y = nav_utm_north0 + waypoints[i].y; \
-  DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
+  pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
 }
 
 #endif /* NAV_H */

sw/airborne/firmwares/rotorcraft/main.c

@@ -41,10 +41,8 @@
 
 #include "subsystems/datalink/telemetry.h"
 #include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/downlink.h"
 #include "subsystems/settings.h"
-#include "subsystems/datalink/pprz_transport.h"
-#include "subsystems/datalink/xbee.h"
-#include "subsystems/datalink/ivy_transport.h"
 
 #include "subsystems/commands.h"
 #include "subsystems/actuators.h"
@@ -173,15 +171,7 @@ STATIC_INLINE void main_init( void ) {
 
   mcu_int_enable();
 
-#if DATALINK == PPRZ
+  downlink_init();
-  pprz_transport_init();
-#endif
-#if DATALINK == XBEE
-  xbee_init();
-#endif
-#if SITL
-  ivy_transport_init();
-#endif
 
   // register the timers for the periodic functions
   main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);

sw/airborne/fms/udp_transport.h

@@ -35,10 +35,10 @@ extern uint8_t udpt_ck_a, udpt_ck_b;
     if (udpt_tx_buf_idx) {					\
       int len;	  						\
       len = network_write(network, updt_tx_buf, udpt_tx_buf_idx);	\
-      downlink_nb_bytes += udpt_tx_buf_idx;			\
+      downlink.nb_bytes += udpt_tx_buf_idx;			\
-      downlink_nb_msgs++;					\
+      downlink.nb_msgs++;					\
       if (len != udpt_tx_buf_idx)				\
-  downlink_nb_ovrn++;					\
+  downlink.nb_ovrn++;					\
       udpt_tx_buf_idx = 0;					\
     }								\
   }

sw/airborne/mcu_periph/i2c.c

@@ -190,7 +190,7 @@ static void send_i2c3_err(struct transport_tx *trans, struct device *dev) {
 #endif /* USE_I2C3 */
 
 #if PERIODIC_TELEMETRY
-static void send_i2c_err(struct transport_tx *trans, struct device *dev) {
+static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), struct device *dev __attribute__((unused))) {
   static uint8_t _i2c_nb_cnt = 0;
   switch (_i2c_nb_cnt) {
     case 0:

sw/airborne/modules/digital_cam/hackhd.h

@@ -58,7 +58,7 @@ enum hackhd_status {
 struct HackHD {
   enum hackhd_status status;
   uint32_t timer;
-  uint32_t photo_nr;
+  int16_t photo_nr;
   uint32_t autoshoot;
   struct EnuCoor_f last_shot_pos;
   uint32_t log_delay;

sw/airborne/subsystems/datalink/downlink.c

@@ -26,8 +26,31 @@
  */
 
 
-#include "std.h"
+#include "subsystems/datalink/downlink.h"
+
+struct downlink downlink;
+
+void downlink_init(void)
+{
+  downlink.nb_ovrn = 0;
+  downlink.nb_bytes = 0;
+  downlink.nb_msgs = 0;
+
+#if defined DATALINK
+#if DATALINK == PPRZ
+  pprz_transport_init();
+#endif
+#if DATALINK == XBEE
+  xbee_init();
+#endif
+#if DATALINK == W5100
+  w5100_init();
+#endif
+#endif
+
+#if SITL
+  ivy_transport_init();
+#endif
+
+}
 
-uint8_t downlink_nb_ovrn;
-uint16_t downlink_nb_bytes;
-uint16_t downlink_nb_msgs;

sw/airborne/subsystems/datalink/downlink.h

@@ -93,9 +93,16 @@
 #define DefaultDevice DOWNLINK_DEVICE
 #endif
 
-/** Counter of messages not sent because of unavailibity of the output buffer*/
+/** Downlink structure */
-extern uint8_t downlink_nb_ovrn;
+struct downlink {
-extern uint16_t downlink_nb_bytes;
+  uint8_t nb_ovrn;    ///< Counter of messages not sent because of unavailibity of the output buffer
-extern uint16_t downlink_nb_msgs;
+  uint16_t nb_bytes;  ///< Number of bytes send over telemetry
+  uint16_t nb_msgs;   ///< Number of messages send over telemetry
+};
+
+extern struct downlink downlink;
+
+// Init function
+extern void downlink_init(void);
 
 #endif /* DOWNLINK_H */

sw/airborne/subsystems/datalink/ivy_transport.c

@@ -140,12 +140,12 @@ static void end_message(struct ivy_transport *trans, struct device *dev __attrib
 
 static void overrun(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)))
 {
-  downlink_nb_ovrn++;
+  downlink.nb_ovrn++;
 }
 
 static void count_bytes(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes)
 {
-  downlink_nb_bytes += bytes;
+  downlink.nb_bytes += bytes;
 }
 
 static int check_available_space(struct ivy_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused)))

sw/airborne/subsystems/datalink/pprz_transport.c

@@ -77,6 +77,7 @@ static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uin
 
 static void start_message(struct pprz_transport *trans, struct device *dev, uint8_t payload_len)
 {
+  downlink.nb_msgs++;
   dev->transmit(dev->periph, STX);
   const uint8_t msg_len = size_of(trans, payload_len);
   dev->transmit(dev->periph, msg_len);
@@ -93,12 +94,12 @@ static void end_message(struct pprz_transport *trans, struct device *dev)
 
 static void overrun(struct pprz_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)))
 {
-  downlink_nb_ovrn++;
+  downlink.nb_ovrn++;
 }
 
 static void count_bytes(struct pprz_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes)
 {
-  downlink_nb_bytes += bytes;
+  downlink.nb_bytes += bytes;
 }
 
 static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct device *dev, uint8_t bytes)

sw/airborne/subsystems/datalink/pprzlog_transport.c

@@ -75,6 +75,7 @@ static uint8_t size_of(struct pprzlog_transport *trans __attribute__((unused)),
 
 static void start_message(struct pprzlog_transport *trans, struct device *dev, uint8_t payload_len)
 {
+  downlink.nb_msgs++;
   dev->transmit(dev->periph, STX_LOG);
   const uint8_t msg_len = size_of(trans, payload_len);
   trans->ck = 0;

sw/airborne/subsystems/datalink/xbee.c

@@ -81,6 +81,7 @@ static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uin
 
 static void start_message(struct xbee_transport *trans, struct device *dev, uint8_t payload_len)
 {
+  downlink.nb_msgs++;
   dev->transmit(dev->periph, XBEE_START);
   const uint16_t len = payload_len + XBEE_API_OVERHEAD;
   dev->transmit(dev->periph, (len >> 8));
@@ -99,12 +100,12 @@ static void end_message(struct xbee_transport *trans, struct device *dev)
 
 static void overrun(struct xbee_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)))
 {
-  downlink_nb_ovrn++;
+  downlink.nb_ovrn++;
 }
 
 static void count_bytes(struct xbee_transport *trans __attribute__((unused)), struct device *dev __attribute__((unused)), uint8_t bytes)
 {
-  downlink_nb_bytes += bytes;
+  downlink.nb_bytes += bytes;
 }
 
 static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct device *dev, uint8_t bytes)

sw/simulator/nps/nps_ivy_common.c

@@ -1,6 +1,7 @@
 #include "nps_ivy.h"
 
 #include <stdlib.h>
+#include <stdio.h>
 #include <Ivy/ivy.h>
 #include <Ivy/ivyglibloop.h>