diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c
index 91033349c9..47f42b6d83 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot.c
@@ -135,15 +135,6 @@ static void send_downlink(void) {
       &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;
@@ -167,9 +158,6 @@ void autopilot_init(void) {
   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
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 5cc48326e8..70a5c2000b 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -139,7 +139,7 @@ static void send_fp(void) {
       &guidance_h_pos_sp.y,
       &guidance_h_pos_sp.x,
       &carrot_up,
-      &guidance_h_command_body.psi,
+      &guidance_h_heading_sp,
       &stabilization_cmd[COMMAND_THRUST],
       &autopilot_flight_time);
 }
@@ -184,12 +184,6 @@ static void send_rotorcraft_cmd(void) {
       &stabilization_cmd[COMMAND_THRUST]);
 }
 
-// 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);
-}
 
 void autopilot_init(void) {
   /* mode is finally set at end of init if MODE_STARTUP is not KILL */
@@ -223,7 +217,6 @@ void autopilot_init(void) {
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
   register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
-  register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
 #ifdef ACTUATORS
   register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators);
 #endif
diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c
index 95a88c2541..0c6fa7353a 100644
--- a/sw/airborne/mcu_periph/i2c.c
+++ b/sw/airborne/mcu_periph/i2c.c
@@ -129,7 +129,7 @@ static void send_i2c2_err(void) {
   uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
   const uint8_t _bus2 = 2;
   DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
-      $i2c2_queue_full_cnt,
+      &i2c2_queue_full_cnt,
       &i2c2_ack_fail_cnt,
       &i2c2_miss_start_stop_cnt,
       &i2c2_arb_lost_cnt,
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index 5105c12b33..dbdaffcfcd 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -123,6 +123,7 @@ static inline void ahrs_update_mag_2d(void);
 static void send_quat(void) {
   struct Int32Quat* quat = stateGetNedToBodyQuat_i();
   DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice,
+      &ahrs_impl.weight,
       &ahrs_impl.ltp_to_imu_quat.qi,
       &ahrs_impl.ltp_to_imu_quat.qx,
       &ahrs_impl.ltp_to_imu_quat.qy,
diff --git a/sw/airborne/subsystems/air_data.c b/sw/airborne/subsystems/air_data.c
index e7cd6b068f..fb966e6acf 100644
--- a/sw/airborne/subsystems/air_data.c
+++ b/sw/airborne/subsystems/air_data.c
@@ -30,6 +30,7 @@
 
 #include "subsystems/air_data.h"
 #include "subsystems/abi.h"
+#include "subsystems/datalink/telemetry.h"
 
 /** global AirData state
  */
@@ -51,5 +52,10 @@ static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const flo
  */
 void air_data_init( void ) {
   AbiBindMsgBARO_ABS(AIR_DATA_BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb);
+  register_periodic_telemetry(DefaultPeriodic, "BARO_RAW", send_baro_raw);
 }
 
+void send_baro_raw(void) {
+  DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,
+                         &air_data.pressure, &air_data.differential);
+}
diff --git a/sw/airborne/subsystems/air_data.h b/sw/airborne/subsystems/air_data.h
index b4963c4832..e39666d9fa 100644
--- a/sw/airborne/subsystems/air_data.h
+++ b/sw/airborne/subsystems/air_data.h
@@ -52,5 +52,7 @@ extern struct AirData air_data;
  */
 extern void air_data_init( void );
 
+extern void send_baro_raw(void);
+
 #endif /* AIR_DATA_H */
 
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 1044c02cd6..7d784fb7e9 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -95,22 +95,22 @@ struct InsInt ins_impl;
 
 static void send_ins(void) {
   DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice,
-      &ins_ltp_pos.x, &ins_ltp_pos.y, &ins_ltp_pos.z,
-      &ins_ltp_speed.x, &ins_ltp_speed.y, &ins_ltp_speed.z,
-      &ins_ltp_accel.x, &ins_ltp_accel.y, &ins_ltp_accel.z);
+      &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z,
+      &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z,
+      &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z);
 }
 
 static void send_ins_z(void) {
   DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
-      &ins_baro_alt, &ins_ltp_pos.z, &ins_ltp_speed.z, &ins_ltp_accel.z);
+      &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
 }
 
 static void send_ins_ref(void) {
-  if (ins_ltp_initialised) {
+  if (ins_impl.ltp_initialized) {
     DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
-        &ins_ltp_def.ecef.x, &ins_ltp_def.ecef.y, &ins_ltp_def.ecef.z,
-        &ins_ltp_def.lla.lat, &ins_ltp_def.lla.lon, &ins_ltp_def.lla.alt,
-        &ins_ltp_def.hmsl, &ins_qfe);
+        &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z,
+        &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt,
+        &ins_impl.ltp_def.hmsl, &ins_impl.qfe);
   }
 }
 #endif