[telemetry] new periodic telemetry system

- only FW currently, rotorcraft coming soon
- compared to before:
 - no macros (easier to debug)
 - the default channel/device cannot be set for all the messages at the
   same time (it will be possible when we finally replace messages
   macros and define proper C struct for that)
 - the register function only register for a single process (not
   possible to reuse the function for an other process unless you
   explicitely register for it)

conf/firmwares/subsystems/fixedwing/autopilot.makefile

@@ -115,6 +115,7 @@ endif
 # Main
 #
 ns_srcs	   	+= $(SRC_FIRMWARE)/main.c
+ns_srcs	   	+= subsystems/datalink/telemetry.c
 
 #
 # LEDs
@@ -156,6 +157,7 @@ fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 ##
 
 ap_CFLAGS 		+= -DAP
+ap_CFLAGS 		+= -DDefaultPeriodic='&telemetry_Ap'
 ap_srcs 		+= $(SRC_FIRMWARE)/main_ap.c
 ap_srcs 		+= $(SRC_FIRMWARE)/autopilot.c
 ap_srcs			+= $(SRC_FIRMWARE)/ap_downlink.c
@@ -201,8 +203,9 @@ sim.srcs 		+= $(fbw_srcs) $(ap_srcs)
 sim.CFLAGS 		+= -DSITL
 sim.srcs 		+= $(SRC_ARCH)/sim_ap.c
 
-sim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
-sim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
+sim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Ap'
+sim.srcs 		+= subsystems/datalink/telemetry.c subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c
+sim.srcs		+= $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
 
 sim.srcs 		+= subsystems/settings.c
 sim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c

conf/telemetry/default_fixedwing.xml

@@ -14,8 +14,6 @@
       <message name="CIRCLE"              period="1.05"/>
       <message name="DESIRED"             period="1.05"/>
       <message name="BAT"                 period="1.1"/>
-      <message name="BARO_MS5534A"        period="1.0"/>
-      <message name="SCP_STATUS"          period="1.0"/>
       <message name="SEGMENT"             period="1.2"/>
       <message name="CALIBRATION"         period="2.1"/>
       <message name="NAVIGATION_REF"      period="9."/>

conf/telemetry/default_fixedwing_imu.xml

@@ -14,8 +14,6 @@
       <message name="CIRCLE"              period="1.05"/>
       <message name="DESIRED"             period="0.2"/>
       <message name="BAT"                 period="1.1"/>
-      <message name="BARO_MS5534A"        period="1.0"/>
-      <message name="SCP_STATUS"          period="1.0"/>
       <message name="SEGMENT"             period="1.2"/>
       <message name="CALIBRATION"         period="2.1"/>
       <message name="NAVIGATION_REF"      period="9."/>

conf/telemetry/default_fixedwing_imu_9k6.xml

@@ -14,8 +14,6 @@
       <message name="CIRCLE"              period="1.05"/>
       <message name="DESIRED"             period="1.05"/>
       <message name="BAT"                 period="1.1"/>
-      <message name="BARO_MS5534A"        period="1.4"/>
-      <message name="SCP_STATUS"          period="1.3"/>
       <message name="SEGMENT"             period="1.2"/>
       <message name="CALIBRATION"         period="2.1"/>
       <message name="NAVIGATION_REF"      period="9."/>

sw/airborne/firmwares/fixedwing/ap_downlink.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2006- Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2006-2013 Pascal Brisset, Antoine Drouin, Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -22,300 +22,20 @@
 /**
  * @file firmwares/fixedwing/ap_downlink.h
  *
- * Set of macros defining the periodic telemetry messages of AP process.
- *
- * The PeriodicSendAp() macro is generated from the telemetry description
- * (named in conf.xml, usually in conf/telemetry directory). This macro
- * is a sequence of calls to PERIODIC_SEND_message() which have to be defined
- * in the present file.
+ * The periodic function is generated from the telemetry xml file.
+ * Each subsystem of the autopilot can register function that
+ * will be called if needed.
  *
  */
 
 #ifndef AP_DOWNLINK_H
 #define AP_DOWNLINK_H
 
-#include <inttypes.h>
-
-#include "generated/airframe.h"
-#include "state.h"
-
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #include "subsystems/datalink/downlink.h"
 
-#include "messages.h"
 #include "generated/periodic_telemetry.h"
 
-#if defined DOWNLINK
-#define Downlink(x) x
-#else
-#define Downlink(x) {}
-#endif
-
-#define PERIODIC_SEND_ALIVE(_trans, _dev)  DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM);
-
-#define PERIODIC_SEND_BAT(_trans, _dev) {                      \
-    int16_t amps = (int16_t) (current/10);				\
-    Downlink({ int16_t e = energy;                      \
-        DOWNLINK_SEND_BAT(_trans, _dev,                        \
-                          &v_ctl_throttle_slewed,       \
-                          &vsupply,                     \
-                          &amps,                        \
-                          &autopilot_flight_time,       \
-                          &kill_throttle,				\
-                          &block_time,					\
-                          &stage_time,					\
-                          &e);                          \
-      });                                               \
-  }
-
-#ifdef MCU_SPI_LINK
-#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) DOWNLINK_SEND_DEBUG_MCU_LINK(_trans, _dev, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
-#else
-#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) {}
-#endif
-
-
-#define PERIODIC_SEND_DOWNLINK(_trans, _dev) { \
-  static uint16_t last; \
-  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; \
-  last = downlink_nb_bytes; \
-  DOWNLINK_SEND_DOWNLINK(_trans, _dev, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
-}
-
-
-#define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \
-    struct FloatEulers* att = stateGetNedToBodyEulers_f(); \
-    DOWNLINK_SEND_ATTITUDE(_trans, _dev, &(att->phi), &(att->psi), &(att->theta)); \
-})
-
-
-#ifndef USE_AIRSPEED
-#define PERIODIC_SEND_DESIRED(_trans, _dev) { float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;  DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);}
-#else
-#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);
-#endif
-
-
-#if USE_INFRARED
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); }
-#elif USE_IMU && USE_AHRS
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &val); }
-#else
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) {}
-#endif
-
-#define PERIODIC_SEND_NAVIGATION_REF(_trans, _dev)  DOWNLINK_SEND_NAVIGATION_REF(_trans, _dev, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
-
-
-#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); \
-}
-
-
-#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \
-  static uint8_t i; \
-  i++; if (i >= nb_waypoint) i = 0; \
-  DownlinkSendWp(_trans, _dev, i);	    \
-}
-
-#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
-#include "rc_settings.h"
-#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
-#define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val);
-#else
-#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) {                         \
-    uint8_t rc_settings_mode_none = 0;                                  \
-    DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode_none, &mcu1_status); \
-  }
-#define PERIODIC_SEND_SETTINGS(_trans, _dev) {}
-#endif
-
-#if USE_INFRARED || USE_INFRARED_TELEMETRY
-#include "subsystems/sensors/infrared.h"
-#define PERIODIC_SEND_IR_SENSORS(_trans, _dev) DOWNLINK_SEND_IR_SENSORS(_trans, _dev, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
-#else
-#define PERIODIC_SEND_IR_SENSORS(_trans, _dev) ;
-#endif
-
-#define PERIODIC_SEND_ADC(_trans, _dev) {}
-
-
-#define PERIODIC_SEND_CALIBRATION(_trans, _dev) DOWNLINK_SEND_CALIBRATION(_trans, _dev, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
-
-#define PERIODIC_SEND_CIRCLE(_trans, _dev) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_trans, _dev, &nav_circle_x, &nav_circle_y, &nav_circle_radius); }
-
-#define PERIODIC_SEND_SEGMENT(_trans, _dev) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_trans, _dev, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
-
-#ifdef IMU_TYPE_H
-#  ifdef INS_MODULE_H
-#  include "modules/ins/ins_module.h"
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &ins_p, &ins_q, &ins_r)}
-#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &ins_ax, &ins_ay, &ins_az)}
-#    define PERIODIC_SEND_IMU_MAG(_trans, _dev) { DOWNLINK_SEND_IMU_MAG(_trans, _dev, &ins_mx, &ins_my, &ins_mz)}
-#  else
-#    include "subsystems/imu.h"
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
-#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &accel_float.x, &accel_float.y, &accel_float.z)}
-#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
-#    ifdef USE_MAGNETOMETER
-#      define PERIODIC_SEND_IMU_MAG(_trans, _dev) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_trans, _dev, &mag_float.x, &mag_float.y, &mag_float.z)}
-#    else
-#      define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
-#    endif
-#  endif
-#else
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {}
-#    define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
-#endif
-
-#ifdef IMU_ANALOG
-#define PERIODIC_SEND_IMU(_trans, _dev) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_trans, _dev, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
-#else
-#define PERIODIC_SEND_IMU(_trans, _dev) {}
-#endif
-
-#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z))
-
-#define SEND_NAVIGATION(_trans, _dev) Downlink({ \
-    uint8_t _circle_count = NavCircleCount(); \
-    struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
-    DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \
-    })
-
-#define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev)
-
-
-#if defined CAM || defined MOBILE_CAM
-#define SEND_CAM(_trans, _dev) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); int16_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_trans, _dev, &phi, &theta, &x, &y);})
-#define PERIODIC_SEND_CAM_POINT(_trans, _dev) DOWNLINK_SEND_CAM_POINT(_trans, _dev, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
-#else
-#define SEND_CAM(_trans, _dev) {}
-#define PERIODIC_SEND_CAM_POINT(_trans, _dev) {}
-#endif
-
-#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev) /** generated from the xml settings config in conf/settings */
-
-#define PERIODIC_SEND_SURVEY(_trans, _dev) { \
-  if (nav_survey_active) \
-    DOWNLINK_SEND_SURVEY(_trans, _dev, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \
-  }
-
-#define PERIODIC_SEND_RANGEFINDER(_trans, _dev) DOWNLINK_SEND_RANGEFINDER(_trans, _dev, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
-
-#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
-
-#if USE_GPS || defined SITL
-#define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
-#else
-#define PERIODIC_SEND_GPS_SOL(_trans, _dev) {}
-#endif
-
-#define PERIODIC_SEND_GPS(_trans, _dev) {                                      \
-    static uint8_t i;                                                   \
-    int16_t climb = -gps.ned_vel.z;                                     \
-    int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));             \
-    DOWNLINK_SEND_GPS(_trans, _dev, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
-    if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
-    if (i >= gps.nb_channels * 2) i = 0;                                    \
-    if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { \
-      DOWNLINK_SEND_SVINFO(_trans, _dev, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
-    }                                                                   \
-    i++;                                                                \
-}
-
-#if USE_GPS
-#define PERIODIC_SEND_GPS_INT(_trans, _dev) {   \
-  DOWNLINK_SEND_GPS_INT( _trans, _dev,          \
-                         &gps.ecef_pos.x,       \
-                         &gps.ecef_pos.y,       \
-                         &gps.ecef_pos.z,       \
-                         &gps.lla_pos.lat,      \
-                         &gps.lla_pos.lon,      \
-                         &gps.lla_pos.alt,      \
-                         &gps.hmsl,             \
-                         &gps.ecef_vel.x,       \
-                         &gps.ecef_vel.y,       \
-                         &gps.ecef_vel.z,       \
-                         &gps.pacc,             \
-                         &gps.sacc,             \
-                         &gps.tow,              \
-                         &gps.pdop,             \
-                         &gps.num_sv,           \
-                         &gps.fix);             \
-  }
-
-#define PERIODIC_SEND_GPS_LLA(_trans, _dev) {               \
-    uint8_t err = 0;                                        \
-    int16_t climb = -gps.ned_vel.z;                         \
-    int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \
-    DOWNLINK_SEND_GPS_LLA( _trans, _dev,                    \
-                           &gps.lla_pos.lat,                \
-                           &gps.lla_pos.lon,                \
-                           &gps.lla_pos.alt,                \
-                           &course,                         \
-                           &gps.gspeed,                     \
-                           &climb,                          \
-                           &gps.week,                       \
-                           &gps.tow,                        \
-                           &gps.fix,                        \
-                           &err);                           \
-  }
-#endif
-
-#if USE_BARO_MS5534A
-//#include "baro_MS5534A.h"
-#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) DOWNLINK_SEND_BARO_MS5534A(_trans, _dev, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
-#else
-#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) {}
-#endif
-
-#if USE_BARO_SCP
-#include "baro_scp.h"
-#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) DOWNLINK_SEND_SCP_STATUS(_trans, _dev, &baro_scp_pressure, &baro_scp_temperature)
-#else
-#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {}
-#endif
-
-#if USE_BAROMETER
-#include "subsystems/sensors/baro.h"
-#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {  \
-    DOWNLINK_SEND_BARO_RAW(_trans, _dev,        \
-                           &baro.absolute,      \
-                           &baro.differential); \
-  }
-#else
-#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {}
-#endif
-
-#ifdef MEASURE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_trans, _dev) { \
-  float* airspeed = stateGetAirspeed_f(); \
-  DOWNLINK_SEND_AIRSPEED (_trans, _dev, airspeed, airspeed, airspeed, airspeed); \
-}
-#elif USE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
-#else
-#define PERIODIC_SEND_AIRSPEED(_trans, _dev) {}
-#endif
-
-#define PERIODIC_SEND_ENERGY(_trans, _dev) Downlink({ const int16_t e = energy; const float vsup = ((float)vsupply) / 10.0f; const float curs = ((float) current)/1000.0f;  const float power = vsup * curs; DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &curs, &e, &power); })
-
-
-#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
-#define PERIODIC_SEND_H_CTL_A(_trans, _dev) DOWNLINK_SEND_H_CTL_A(_trans, _dev, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
-
-
 #endif /* AP_DOWNLINK_H */

sw/airborne/firmwares/fixedwing/autopilot.c

@@ -28,8 +28,14 @@
 
 #include "firmwares/fixedwing/autopilot.h"
 
+#include "state.h"
+#include "firmwares/fixedwing/ap_downlink.h"
+#include "subsystems/nav.h"
+#include "generated/settings.h"
+
 uint8_t pprz_mode;
 bool_t kill_throttle;
+uint8_t  mcu1_status;
 
 bool_t launch;
 
@@ -39,12 +45,105 @@ uint16_t autopilot_flight_time;
 uint8_t lateral_mode;
 
 uint16_t vsupply;
+int32_t current;
 float energy;
 
 bool_t gps_lost;
 
 bool_t power_switch;
 
+static void send_alive(void) {
+  DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
+}
+
+#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
+#include "rc_settings.h"
+static void send_rc_settings(void) {
+  if (!RcSettingsOff())
+    DOWNLINK_SEND_SETTINGS(DefaultChannel, DefaultDevice, &slider_1_val, &slider_2_val);
+}
+#else
+uint8_t rc_settings_mode = 0;
+#endif
+
+static void send_mode(void) {
+  DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice,
+      &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
+}
+
+static void send_attitude(void) {
+  struct FloatEulers* att = stateGetNedToBodyEulers_f();
+  DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice,
+      &(att->phi), &(att->psi), &(att->theta));
+};
+
+static void send_estimator(void) {
+  DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice,
+      &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
+}
+
+static void send_bat(void) {
+  int16_t amps = (int16_t) (current/10);
+  int16_t e = energy;
+  DOWNLINK_SEND_BAT(DefaultChannel, DefaultDevice,
+      &v_ctl_throttle_slewed, &vsupply, &amps,
+      &autopilot_flight_time, &kill_throttle,
+      &block_time, &stage_time, &e);
+}
+
+static void send_energy(void) {
+  const int16_t e = energy;
+  const float vsup = ((float)vsupply) / 10.0f;
+  const float curs = ((float)current) / 1000.0f;
+  const float power = vsup * curs;
+  DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power);
+}
+
+static void send_dl_value(void) {
+  PeriodicSendDlValue(DefaultChannel, DefaultDevice);
+}
+
+// FIXME not the best place
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#include CTRL_TYPE_H
+static void send_desired(void) {
+#ifndef USE_AIRSPEED
+  float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
+#endif
+  DOWNLINK_SEND_DESIRED(DefaultChannel, DefaultDevice,
+      &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint,
+      &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint,
+      &v_ctl_auto_airspeed_setpoint);
+}
+
+static void send_airspeed(void) {
+#ifdef MEASURE_AIRSPEED
+  float* airspeed = stateGetAirspeed_f();
+  DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed);
+#elif USE_AIRSPEED
+  DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice,
+      stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint,
+      &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint);
+#endif
+}
+
+static void send_downlink(void) {
+  static uint16_t last;
+  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
+  last = downlink_nb_bytes;
+  DOWNLINK_SEND_DOWNLINK(DefaultChannel, DefaultDevice,
+      &downlink_nb_ovrn, &rate, &downlink_nb_msgs);
+}
+
+#if USE_BAROMETER
+// FIXME we really need a baro subsystem
+#include "subsystems/sensors/baro.h"
+static void send_baro_raw(void) {
+  DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,
+      &baro.absolute, &baro.differential);
+}
+#endif
+
 void autopilot_init(void) {
   pprz_mode = PPRZ_MODE_AUTO2;
   kill_throttle = FALSE;
@@ -57,5 +156,22 @@ void autopilot_init(void) {
 
   power_switch = FALSE;
 
+  /* register some periodic message */
+  register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
+  register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode);
+  register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink);
+  register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
+  register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator);
+  register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed);
+  register_periodic_telemetry(DefaultPeriodic, "BAT", send_bat);
+  register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
+  register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
+  register_periodic_telemetry(DefaultPeriodic, "DESIRED", send_desired);
+#if USE_BAROMETER
+  register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
+#endif
+#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
+  register_periodic_telemetry(DefaultPeriodic, "RC_SETTINGS", send_rc_settings);
+#endif
 }
 

sw/airborne/firmwares/fixedwing/autopilot.h

@@ -61,6 +61,7 @@ extern void autopilot_init(void);
 
 extern uint8_t pprz_mode;
 extern bool_t kill_throttle;
+extern uint8_t  mcu1_status;
 
 /** flight time in seconds. */
 extern uint16_t autopilot_flight_time;
@@ -88,6 +89,11 @@ extern uint8_t lateral_mode;
  */
 extern uint16_t vsupply;
 
+/** Supply current in milliAmpere.
+ * This the ap copy of the measurement from fbw
+ */
+extern int32_t current;	// milliAmpere
+
 /** Fuel consumption (mAh)
  * TODO: move to electrical subsystem
  */

sw/airborne/firmwares/fixedwing/datalink.c

@@ -140,7 +140,8 @@ 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);
+    // FIXME
+    //SEND_NAVIGATION(DefaultChannel, DefaultDevice);
   } else
 #endif /** NAV */
 #ifdef WIND_INFO

sw/airborne/firmwares/fixedwing/fbw_downlink.h

@@ -34,67 +34,14 @@
 #ifndef FBW_DOWNLINK_H
 #define FBW_DOWNLINK_H
 
-#include <inttypes.h>
-#include "messages.h"
-#include "generated/periodic_telemetry.h"
-#include "generated/airframe.h"
-#include "subsystems/commands.h"
-#include "subsystems/actuators.h"
-
-#include "mcu_periph/uart.h"
-#include "firmwares/fixedwing/main_fbw.h"
-#include "subsystems/radio_control.h"
-#include "subsystems/electrical.h"
-#include "inter_mcu.h"
-
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE
 #endif
 #include "subsystems/datalink/downlink.h"
-
-#define PERIODIC_SEND_COMMANDS(_trans, _dev) DOWNLINK_SEND_COMMANDS(_trans, _dev, COMMANDS_NB, commands)
-
-#ifdef RADIO_CONTROL
-#define PERIODIC_SEND_FBW_STATUS(_trans, _dev) DOWNLINK_SEND_FBW_STATUS(_trans, _dev, &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current)
-#ifdef RADIO_CONTROL_TYPE_PPM
-#define PERIODIC_SEND_PPM(_trans, _dev) {                           \
-  uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL];        \
-  for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++)               \
-    ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]); \
-  DOWNLINK_SEND_PPM(_trans, _dev,                            \
-                    &radio_control.frame_rate,               \
-                    PPM_NB_CHANNEL,                          \
-                    ppm_pulses_usec);                        \
-}
-#else
-#define PERIODIC_SEND_PPM(_trans, _dev) {}
-#endif
-#define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
-#else // RADIO_CONTROL
-#define PERIODIC_SEND_FBW_STATUS(_trans, _dev) { uint8_t dummy = 0; DOWNLINK_SEND_FBW_STATUS(_trans, _dev, &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); }
-#define PERIODIC_SEND_PPM(_trans, _dev) {}
-#define PERIODIC_SEND_RC(_trans, _dev) {}
-#endif // RADIO_CONTROL
-
-#ifdef ACTUATORS
-#define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators)
-#else
-#define PERIODIC_SEND_ACTUATORS(_trans, _dev) {}
-#endif
-
-#ifdef BRICOLAGE_ADC
-extern uint16_t adc0_val[];
-
-#define PERIODIC_SEND_ADC(_trans, _dev) {			\
-    static const uint8_t mcu = 0;			\
-    DOWNLINK_SEND_ADC(_trans, _dev, &mcu, 8, adc0_val);	\
-  }
-#else
-#define PERIODIC_SEND_ADC(_trans, _dev) {}
-#endif
+#include "generated/periodic_telemetry.h"
 
 static inline void fbw_downlink_periodic_task(void) {
-  PeriodicSendFbw(DefaultChannel,DefaultDevice)
+  periodic_telemetry_send_Fbw();
 }
 
 

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -116,6 +116,15 @@ static inline void on_accel_event( void );
 static inline void on_mag_event( void );
 volatile uint8_t ahrs_timeout_counter = 0;
 
+//FIXME not the correct place
+static void send_fliter_status(void) {
+  uint8_t mde = 3;
+  if (ahrs.status == AHRS_UNINIT) mde = 2;
+  if (ahrs_timeout_counter > 10) mde = 5;
+  uint16_t val = 0;
+  DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val);
+}
+
 #endif // USE_AHRS && USE_IMU
 
 #if USE_GPS
@@ -130,17 +139,10 @@ static inline void on_baro_dif_event( void );
 // what version is this ????
 static const uint16_t version = 1;
 
-static uint8_t  mcu1_status;
-
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
 static uint8_t  mcu1_ppm_cpt;
 #endif
 
-/** Supply current in milliAmpere.
- * This the ap copy of the measurement from fbw
- */
-static int32_t current;	// milliAmpere
-
 
 tid_t modules_tid;     ///< id for modules_periodic_task() timer
 tid_t telemetry_tid;   ///< id for telemetry_periodic() timer
@@ -172,6 +174,10 @@ void init_ap( void ) {
   ahrs_init();
 #endif
 
+#if USE_AHRS && USE_IMU
+  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status);
+#endif
+
 #if USE_BAROMETER
   baro_init();
 #endif
@@ -228,6 +234,7 @@ void init_ap( void ) {
 #ifdef TRAFFIC_INFO
   traffic_info_init();
 #endif
+
 }
 
 
@@ -352,8 +359,9 @@ static inline void telecommand_task( void ) {
 #endif
   }
   mode_changed |= mcu1_status_update();
-  if ( mode_changed )
-    PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+  // FIXME
+  //if ( mode_changed )
+  //  PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
 
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
   /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
@@ -407,7 +415,8 @@ void reporting_task( void ) {
   }
   /** then report periodicly */
   else {
-    PeriodicSendAp(DefaultChannel, DefaultDevice);
+    //PeriodicSendAp(DefaultChannel, DefaultDevice);
+    periodic_telemetry_send_Ap();
   }
 }
 
@@ -431,7 +440,8 @@ void navigation_task( void ) {
       if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
         last_pprz_mode = pprz_mode;
         pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
-        PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+        // FIXME
+        //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
         gps_lost = TRUE;
       }
     } else if (gps_lost) { /* GPS is ok */
@@ -439,7 +449,8 @@ void navigation_task( void ) {
       pprz_mode = last_pprz_mode;
       gps_lost = FALSE;
 
-      PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
+      // FIXME
+      //PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
     }
   }
 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
@@ -456,11 +467,11 @@ void navigation_task( void ) {
   CallTCAS();
 #endif
 
-#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
-  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
-#endif
+//#ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
+//  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+//#endif
 
-  SEND_CAM(DefaultChannel, DefaultDevice);
+  //SEND_CAM(DefaultChannel, DefaultDevice);
 
   /* The nav task computes only nav_altitude. However, we are interested
      by desired_altitude (= nav_alt+alt_shift) in any case.

sw/airborne/firmwares/fixedwing/main_fbw.c

@@ -78,6 +78,35 @@ volatile uint8_t fbw_new_actuators = 0;
 tid_t fbw_periodic_tid; ///< id for periodic_task_fbw() timer
 tid_t electrical_tid;   ///< id for electrical_periodic() timer
 
+/********** PERIODIC MESSAGES ************************************************/
+static void send_commands(void) {
+  DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands);
+}
+
+#ifdef RADIO_CONTROL
+static void send_fbw_status(void) {
+  DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
+      &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current);
+}
+
+static void send_rc(void) {
+  DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values);
+}
+
+#else
+static void send_fbw_status(void) {
+  uint8_t dummy = 0;
+  DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
+      &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current);
+}
+#endif
+
+#ifdef ACTUATORS
+static void send_actuators(void) {
+  DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators);
+}
+#endif
+
 /********** INIT *************************************************************/
 void init_fbw( void ) {
 
@@ -87,14 +116,19 @@ void init_fbw( void ) {
   electrical_init();
 #endif
 
+  register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status);
+  register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands);
+
 #ifdef ACTUATORS
   actuators_init();
   /* Load the failsafe defaults */
   SetCommands(commands_failsafe);
   fbw_new_actuators = 1;
+  register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators);
 #endif
 #ifdef RADIO_CONTROL
   radio_control_init();
+  register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc);
 #endif
 #ifdef INTER_MCU
   inter_mcu_init();

sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c

@@ -36,6 +36,11 @@
 #include CTRL_TYPE_H
 #include "firmwares/fixedwing/autopilot.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
 
 /* outer loop parameters */
 float h_ctl_course_setpoint; /* rad, CW/north */
@@ -144,6 +149,21 @@ inline static void h_ctl_pitch_loop( void );
 #define H_CTL_PITCH_KFFD 0.
 #endif
 
+static void send_calibration(void) {
+  DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice,  &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
+}
+
+static void send_tune_roll(void) {
+  DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
+      &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
+}
+
+static void send_ctl_a(void) {
+  DOWNLINK_SEND_H_CTL_A(DefaultChannel, DefaultDevice,
+      &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
+}
+
+
 void h_ctl_init( void ) {
   h_ctl_course_setpoint = 0.;
   h_ctl_course_pre_bank = 0.;
@@ -191,6 +211,9 @@ void h_ctl_init( void ) {
   v_ctl_pitch_dash_trim = 0.;
 #endif
 
+  register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
+  register_periodic_telemetry(DefaultPeriodic, "TUNE_ROLL", send_tune_roll);
+  register_periodic_telemetry(DefaultPeriodic, "H_CTL_A", send_ctl_a);
 }
 
 /**

sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c

@@ -35,6 +35,12 @@
 #include CTRL_TYPE_H
 #include "firmwares/fixedwing/autopilot.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 /* outer loop parameters */
 float h_ctl_course_setpoint; /* rad, CW/north */
 float h_ctl_course_pre_bank;
@@ -112,6 +118,10 @@ float h_ctl_roll_rate_gain;
 static float nav_ratio;
 #endif
 
+static void send_calibration(void) {
+  DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice,  &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
+}
+
 void h_ctl_init( void ) {
   h_ctl_course_setpoint = 0.;
   h_ctl_course_pre_bank = 0.;
@@ -167,6 +177,8 @@ void h_ctl_init( void ) {
 #ifdef AGR_CLIMB
 nav_ratio=0;
 #endif
+
+  register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
 }
 
 /**

sw/airborne/link_mcu_spi.c

@@ -100,9 +100,21 @@ void link_mcu_event_task( void ) {
 #define LINK_MCU_SLAVE_IDX SPI_SLAVE0
 #endif
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 uint8_t link_mcu_nb_err;
 uint8_t link_mcu_fbw_nb_err;
 
+static void send_debug_link(void) {
+  uint8_t mcu1_ppm_cpt_foo = 0; //FIXME
+  DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice,
+      &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo);
+}
+
 void link_mcu_init(void) {
   link_mcu_nb_err = 0;
 
@@ -115,6 +127,8 @@ void link_mcu_init(void) {
   link_mcu_trans.output_buf = (uint8_t*)&link_mcu_from_ap_msg;
   link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH;
   link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH;
+
+  register_periodic_telemetry(DefaultPeriodic, "DEBUG_MCU_LINK", send_debug_link);
 }
 
 void link_mcu_send(void) {

sw/airborne/modules/cam_control/cam.c

@@ -35,6 +35,12 @@
 #include "point.h"
 #endif // POINT_CAM
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 #ifdef TEST_CAM
 float test_cam_estimator_x;
 float test_cam_estimator_y;
@@ -98,8 +104,21 @@ void cam_target(void);
 void cam_waypoint_target(void);
 void cam_ac_target(void);
 
+static void send_cam(void) {
+  SEND_CAM(DefaultChannel, DefaultDevice);
+}
+
+static void send_cam_point(void) {
+  DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice,
+      &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon);
+}
+
+
 void cam_init( void ) {
   cam_mode = CAM_MODE0;
+
+  register_periodic_telemetry(DefaultPeriodic, "CAM", send_cam);
+  register_periodic_telemetry(DefaultPeriodic, "CAM_POINT", send_cam_point);
 }
 
 void cam_periodic( void ) {

sw/airborne/modules/cam_control/cam.h

@@ -96,4 +96,12 @@ extern bool_t video_tx_state;
 
 #endif
 
+#define SEND_CAM(_trans, _dev) { \
+  int16_t x = cam_target_x; \
+  int16_t y = cam_target_y; \
+  int16_t phi = DegOfRad(cam_phi_c); \
+  int16_t theta = DegOfRad(cam_theta_c); \
+  DOWNLINK_SEND_CAM(_trans, _dev, &phi, &theta, &x, &y); \
+}
+
 #endif // CAM_H

sw/airborne/modules/sensors/baro_MS5534A.c

@@ -259,6 +259,9 @@ void baro_MS5534A_event( void ) {
     if (baro_MS5534A_available) {
     //  baro_MS5534A_available = FALSE; // Checked by INS
       baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084;
+#if SENSO_SYNC_SEND
+      DOWNLINK_SEND_BARO_MS5534A(DefaultChannel, DefaultDevice, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z);
+#endif
     //  if (alt_baro_enabled) {
     //    EstimatorSetAlt(baro_MS5534A_z); // Updated by INS
     //  }

sw/airborne/subsystems/ahrs/ahrs_infrared.c

@@ -37,12 +37,33 @@
 
 #include "state.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 float heading;
 
+static void send_infrared(void) {
+  DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice,
+      &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
+}
+
+static void send_status(void) {
+  uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
+  uint8_t mde = 3;
+  if (contrast < 50) mde = 7;
+  DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast);
+}
+
 void ahrs_init(void) {
   ahrs.status = AHRS_UNINIT;
 
   heading = 0.;
+
+  register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
+  register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
 }
 
 void ahrs_align(void) {

sw/airborne/subsystems/datalink/telemetry.c

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "subsystems/datalink/telemetry_common.h"
+
+//struct pprz_telemetry telemetry[PERIODIC_TELEMETRY_NB];
+
+//void periodic_telemetry_init(void) {
+//  telemetry = PERIODIC_TELEMETRY_MESSAGES;
+//}
+
+bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, char * _msg, telemetry_cb _cb) {
+  // look for message name
+  uint8_t i;
+  for (i = 0; i < _pt->nb; i++) {
+    if (str_equal(_pt->msgs[i].msg, _msg)) {
+      // register callback if not already done
+      if (_pt->msgs[i].cb == NULL) {
+        _pt->msgs[i].cb = _cb;
+        return TRUE;
+      }
+      else { return FALSE; }
+    }
+  }
+  // message name is not in telemetry file
+  return FALSE;
+}
+

sw/airborne/subsystems/datalink/telemetry_common.h

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file subsystems/datalink/telemetry_common.h
+ *
+ *  Common tools for periodic telemetry interface
+ *  Allows subsystem to register callback functions
+ */
+
+#ifndef TELEMETRY_COMMON_H
+#define TELEMETRY_COMMON_H
+
+#include <inttypes.h>
+#include "std.h"
+
+/** Telemetry callback definition */
+typedef void (*telemetry_cb)(void);
+
+/** Telemetry header
+ */
+struct telemetry_msg {
+  char msg[64];     ///< name in telemetry xml file
+  telemetry_cb cb;  ///< callback funtion
+};
+
+struct pprz_telemetry {
+  uint8_t nb;       ///< number of messages
+  struct telemetry_msg* msgs; ///< the list of (msg name, callbacks)
+};
+
+/** Telemetry init function
+ */
+//void periodic_telemetry_init(void);
+
+/** Register function
+ * @param _pt periodic telemetry structure to register
+ * @param _msg message name (string) as defined in telemetry xml file
+ * @param _cb callback function, called according to telemetry mode and specified period
+ * @return TRUE if message registered with success, FALSE otherwise
+ */
+bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, char * _msg, telemetry_cb _cb);
+
+
+#endif /* TELEMETRY_COMMON_H */
+

sw/airborne/subsystems/gps.c

@@ -26,6 +26,12 @@
 
 #include "subsystems/gps.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 #include "led.h"
 
 #define MSEC_PER_WEEK (1000*60*60*24*7)
@@ -34,6 +40,56 @@ struct GpsState gps;
 
 struct GpsTimeSync gps_time_sync;
 
+static void send_gps(void) {
+  static uint8_t i;
+  int16_t climb = -gps.ned_vel.z;
+  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
+  DOWNLINK_SEND_GPS(DefaultChannel, DefaultDevice, &gps.fix,
+      &gps.utm_pos.east, &gps.utm_pos.north,
+      &course, &gps.hmsl, &gps.gspeed, &climb,
+      &gps.week, &gps.tow, &gps.utm_pos.zone, &i);
+  if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;
+  if (i >= gps.nb_channels * 2) i = 0;
+  if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) {
+    DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i,
+        &gps.svinfos[i].svid,
+        &gps.svinfos[i].flags,
+        &gps.svinfos[i].qi,
+        &gps.svinfos[i].cno,
+        &gps.svinfos[i].elev,
+        &gps.svinfos[i].azim);
+  }
+  i++;
+}
+
+static void send_gps_int(void) {
+  DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice,
+      &gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z,
+      &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
+      &gps.hmsl,
+      &gps.ecef_vel.x, &gps.ecef_vel.y, &gps.ecef_vel.z,
+      &gps.pacc, &gps.sacc,
+      &gps.tow,
+      &gps.pdop,
+      &gps.num_sv,
+      &gps.fix);
+}
+
+static void send_gps_lla(void) {
+  uint8_t err = 0;
+  int16_t climb = -gps.ned_vel.z;
+  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
+  DOWNLINK_SEND_GPS_LLA(DefaultChannel, DefaultDevice,
+      &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,
+      &course, &gps.gspeed, &climb,
+      &gps.week, &gps.tow,
+      &gps.fix, &err);
+}
+
+static void send_gps_sol(void) {
+  DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv);
+}
+
 void gps_init(void) {
   gps.fix = GPS_FIX_NONE;
   gps.cacc = 0;
@@ -43,6 +99,11 @@ void gps_init(void) {
 #ifdef GPS_TYPE_H
   gps_impl_init();
 #endif
+
+  register_periodic_telemetry(DefaultPeriodic, "GPS", send_gps);
+  register_periodic_telemetry(DefaultPeriodic, "GPS_INT", send_gps_int);
+  register_periodic_telemetry(DefaultPeriodic, "GPS_LLA", send_gps_lla);
+  register_periodic_telemetry(DefaultPeriodic, "GPS_SOL", send_gps_sol);
 }
 
 uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)

sw/airborne/subsystems/imu.c

@@ -26,6 +26,52 @@
 
 #include "subsystems/imu.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
+static void send_accel_raw(void) {
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
+      &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
+}
+
+static void send_gyro_raw(void) {
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
+      &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
+}
+
+#ifdef USE_MAGNETOMETER
+static void send_mag_raw(void) {
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
+      &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
+}
+#endif
+
+static void send_accel(void) {
+  struct FloatVect3 accel_float;
+  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
+  DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
+      &accel_float.x, &accel_float.y, &accel_float.z);
+}
+
+static void send_gyro(void) {
+  struct FloatRates gyro_float;
+  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
+  DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
+      &gyro_float.p, &gyro_float.q, &gyro_float.r);
+}
+
+#ifdef USE_MAGNETOMETER
+static void send_mag(void) {
+  struct FloatVect3 mag_float;
+  MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
+  DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice,
+      &mag_float.x, &mag_float.y, &mag_float.z);
+}
+#endif
+
 struct Imu imu;
 
 void imu_init(void) {
@@ -56,6 +102,17 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!")
   INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
   INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
 
+  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
+  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
+#ifdef USE_MAGNETOMETER
+  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
+#endif
+  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
+  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
+#ifdef USE_MAGNETOMETER
+  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
+#endif
+
   imu_impl_init();
 }
 

sw/airborne/subsystems/nav.c

@@ -36,6 +36,12 @@
 #include "inter_mcu.h"
 #include "subsystems/navigation/traffic_info.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+#include "generated/periodic_telemetry.h"
+
 #define RCLost() bit_is_set(fbw_state->status, RADIO_REALLY_LOST)
 
 enum oval_status oval_status;
@@ -133,7 +139,7 @@ void nav_circle_XY(float x, float y, float radius) {
     (dist2_center > Square(abs_radius + dist_carrot)
      || dist2_center < Square(abs_radius - dist_carrot)) ?
     0 :
-    atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius));
+    atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (GRAVITY*radius));
 
   float carrot_angle = dist_carrot / abs_radius;
   carrot_angle = Min(carrot_angle, M_PI/4);
@@ -431,6 +437,44 @@ void nav_periodic_task(void) {
   nav_set_altitude();
 }
 
+/**
+ * \brief Periodic telemetry
+ */
+static void send_nav_ref(void) {
+  DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice,
+      &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
+}
+
+static void send_nav(void) {
+  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
+}
+
+static void send_wp_moved(void) {
+  static uint8_t i;
+  i++; if (i >= nb_waypoint) i = 0;
+  DownlinkSendWp(DefaultChannel, DefaultDevice, i);
+}
+
+static void send_circle(void) {
+  if (nav_in_circle) {
+    DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice,
+        &nav_circle_x, &nav_circle_y, &nav_circle_radius);
+  }
+}
+
+static void send_segment(void) {
+  if (nav_in_segment) {
+    DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice,
+        &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
+  }
+}
+
+static void send_survey(void) {
+  if (nav_survey_active) {
+    DOWNLINK_SEND_SURVEY(DefaultChannel, DefaultDevice,
+        &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
+  }
+}
 
 /**
  *  \brief Navigation Initialisation
@@ -448,6 +492,13 @@ void nav_init(void) {
   nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
   nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
 #endif
+
+  register_periodic_telemetry(DefaultPeriodic, "NAVIGATION_REF", send_nav_ref);
+  register_periodic_telemetry(DefaultPeriodic, "NAVIGATION", send_nav);
+  register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
+  register_periodic_telemetry(DefaultPeriodic, "CIRCLE", send_circle);
+  register_periodic_telemetry(DefaultPeriodic, "SEGMENT", send_segment);
+  register_periodic_telemetry(DefaultPeriodic, "SURVEY", send_survey);
 }
 
 /**

sw/airborne/subsystems/nav.h

@@ -41,7 +41,7 @@
 #include "subsystems/navigation/common_flight_plan.h"
 #include "subsystems/navigation/common_nav.h"
 
-#define G 9.806
+#define GRAVITY 9.806
 #define Square(_x) ((_x)*(_x))
 #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y))
 
@@ -200,4 +200,16 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
 #define GetPosY() (stateGetPositionUtm_f()->east)
 #define GetPosAlt() (stateGetPositionUtm_f()->alt)
 
+#define SEND_NAVIGATION(_trans, _dev) { \
+    uint8_t _circle_count = NavCircleCount(); \
+    struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
+    DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \
+}
+
+#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); \
+}
+
 #endif /* NAV_H */

sw/airborne/subsystems/radio_control/ppm.c

@@ -22,10 +22,32 @@
 #include "subsystems/radio_control.h"
 #include "subsystems/radio_control/ppm.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+
+#ifdef FBW
+#define DOWNLINK_TELEMETRY &telemetry_Fbw
+#else
+#define DOWNLINK_TELEMETRY DefaultPeriodic
+#endif
+#include "generated/periodic_telemetry.h"
+
 uint16_t ppm_pulses[ PPM_NB_CHANNEL ];
 volatile bool_t ppm_frame_available;
 
+static void send_ppm(void) {
+  uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL];
+  for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++)
+    ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]);
+  DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice,
+      &radio_control.frame_rate, PPM_NB_CHANNEL, ppm_pulses_usec);
+}
+
 void radio_control_impl_init(void) {
   ppm_frame_available = FALSE;
   ppm_arch_init();
+
+  register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_ppm);
 }

sw/include/std.h

@@ -239,7 +239,12 @@ typedef uint8_t unit_t;
     }									\
   }
 
-
-
+static inline bool_t str_equal(char * a, char * b) {
+  while (!(*a == 0 && *b == 0)) {
+    if (*a != *b) return FALSE;
+    a++; b++;
+  }
+  return TRUE;
+}
 
 #endif /* STD_H */

sw/tools/gen_periodic.ml

@@ -44,7 +44,7 @@ let output_modes = fun out_h process_name modes freq modules ->
   List.iter
     (fun mode ->
       let mode_name = ExtXml.attrib mode "name" in
-      lprintf out_h "if (telemetry_mode_%s == TELEMETRY_MODE_%s_%s) {\\\n" process_name process_name mode_name;
+      lprintf out_h "if (telemetry_mode_%s == TELEMETRY_MODE_%s_%s) {\n" process_name process_name mode_name;
       right ();
 
       (** Filter message list to remove messages linked to unloaded modules *)
@@ -62,7 +62,7 @@ let output_modes = fun out_h process_name modes freq modules ->
       List.iter (fun m ->
         let v = sprintf "i%d" m in
         let _type = if m >= 256 then "uint16_t" else "uint8_t" in
-        lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\\\n" _type v v v m v;
+        lprintf out_h "static %s %s = 0; %s++; if (%s>=%d) %s=0;\n" _type v v v m v;
       ) modulos;
 
       (** For each message in this mode *)
@@ -78,17 +78,23 @@ let output_modes = fun out_h process_name modes freq modules ->
           let message_phase = try int_of_float (float_of_string (ExtXml.attrib message "phase")*.float_of_int freq) with _ -> !i in
           phase := message_phase;
           let else_ = if List.mem_assoc p !l && not (List.mem (p, !phase) !l) then "else " else "" in
-          lprintf out_h "%sif (i%d == %d) {\\\n" else_ p !phase;
+          lprintf out_h "%sif (i%d == %d) {\n" else_ p !phase;
           l := (p, !phase) :: !l;
           i := !i + freq/10;
           right ();
-          lprintf out_h "PERIODIC_SEND_%s(_trans, _dev);\\\n" message_name;
+          lprintf out_h "if (telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb != NULL)\n" process_name (String.uppercase process_name) message_name;
+          right ();
+          lprintf out_h "telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb();\n" process_name (String.uppercase process_name) message_name;
           left ();
-          lprintf out_h "} \\\n"
+          fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n";
+          lprintf out_h "else peridodic_telemetry_err_report(TELEMETRY_%s_MSG_%s_ID);\n" (String.uppercase process_name) message_name;
+          fprintf out_h "#endif\n";
+          left ();
+          lprintf out_h "}\n"
         )
         messages;
       left ();
-      lprintf out_h "}\\\n")
+      lprintf out_h "}\n")
     modes
 
 let write_settings = fun xml_file out_set telemetry_xml ->
@@ -141,24 +147,18 @@ let _ =
   fprintf out_h "#ifndef _VAR_PERIODIC_H_\n";
   fprintf out_h "#define _VAR_PERIODIC_H_\n\n";
   fprintf out_h "#include \"std.h\"\n";
-  fprintf out_h "#include \"generated/airframe.h\"\n\n";
+  fprintf out_h "#include \"generated/airframe.h\"\n";
+  fprintf out_h "#include \"subsystems/datalink/telemetry_common.h\"\n\n";
 
   (** For each process *)
   List.iter
     (fun process ->
       let process_name = ExtXml.attrib process "name" in
 
-      fprintf out_h "\n/* Macros for %s process */\n" process_name;
-      fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name);
-      fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name);
-      fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name);
-      fprintf out_h "#endif\n";
-      fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name);
-      fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name);
-      fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name;
-      fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name);
-
       let modes = Xml.children process in
+      let messages = Hashtbl.create 5 in
+
+      fprintf out_h "\n/* Periodic telemetry: %s process */\n" process_name;
 
       let i = ref 0 in
       (** For each mode of this process *)
@@ -170,12 +170,41 @@ let _ =
           (fun x ->
             let p = ExtXml.attrib x "period"
             and n = ExtXml.attrib x "name" in
-            Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name !i) (sprintf "(%s)" p))
+            (* add message to the list if it exists *)
+            if not (Hashtbl.mem messages n) then Hashtbl.add messages n ();
+            Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name !i) (sprintf "(%s)" p)) (* FIXME really needed ? *)
           (Xml.children mode);
         incr i)
         modes;
 
-      lprintf out_h "#define PeriodicSend%s(_trans, _dev) {  /* %dHz */ \\\n" process_name freq;
+      let i = ref 0 in
+      (* Print message ID and total number *)
+      Hashtbl.iter (fun n _ ->
+        Xml2h.define (sprintf "TELEMETRY_%s_MSG_%s_ID" (String.uppercase process_name) n) (sprintf "%d" !i);
+        incr i;
+      ) messages;
+      Xml2h.define (sprintf "TELEMETRY_%s_NB_MSG" (String.uppercase process_name)) (sprintf "%d" !i);
+
+      (* Structure initialization *)
+      fprintf out_h "#define TELEMETRY_%s_STRUCT { \\\n" (String.uppercase process_name);
+      Hashtbl.iter (fun n _ -> fprintf out_h "  { \"%s\", NULL }, \\\n" n) messages;
+      fprintf out_h "};\n";
+
+      fprintf out_h "\n/* Functions for %s process */\n" process_name;
+      fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name);
+      fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name);
+      fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name);
+      fprintf out_h "#endif\n";
+      fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name);
+      fprintf out_h "struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG] = TELEMETRY_%s_STRUCT;\n" process_name (String.uppercase process_name) (String.uppercase process_name);
+      fprintf out_h "struct pprz_telemetry telemetry_%s = { TELEMETRY_%s_NB_MSG, telemetry_msg_%s };\n" process_name (String.uppercase process_name) process_name;
+      fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name);
+      fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name;
+      fprintf out_h "extern struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG];\n" process_name (String.uppercase process_name);
+      fprintf out_h "extern struct pprz_telemetry telemetry_%s;\n" process_name;
+      fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name);
+
+      lprintf out_h "static inline void periodic_telemetry_send_%s(void) {  /* %dHz */\n" process_name freq; (*TODO pass transport+device *)
       right ();
       output_modes out_h process_name modes freq modules_name;
       left ();