diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile index 91de1f8a5e..ca5fb490fb 100644 --- a/conf/firmwares/setup.makefile +++ b/conf/firmwares/setup.makefile @@ -84,7 +84,7 @@ setup_actuators.CFLAGS += -DUSE_$(MODEM_PORT) setup_actuators.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) setup_actuators.srcs += mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c -setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=$(MODEM_PORT) -DDOWNLINK_AP_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) +setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ setup_actuators.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ifneq ($(SYS_TIME_LED),none) diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile index 7f8f2a8a1c..76f2f7d4c8 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile @@ -4,7 +4,7 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=$(MODEM_PORT) -DDOWNLINK_AP_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile index 4baf02135f..dfb990a788 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -4,7 +4,7 @@ #serial USB (e.g. /dev/ttyACM0) ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile index 8e70ae50c3..41478cd1ea 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile @@ -7,7 +7,7 @@ W5100_SUBNET ?= "255,255,255,0" W5100_MULTICAST_IP ?= "224,1,1,11" W5100_MULTICAST_PORT ?= "1234" -ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=W5100 -DDOWNLINK_AP_DEVICE=W5100 +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=W5100 ap.CFLAGS += -DDOWNLINK_TRANSPORT=W5100Transport -DDATALINK=W5100 ap.CFLAGS += -DW5100_IP=$(W5100_IP) -DW5100_SUBNET=$(W5100_SUBNET) -DW5100_MULTICAST_IP=$(W5100_MULTICAST_IP) -DW5100_MULTICAST_PORT=$(W5100_MULTICAST_PORT) ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile index 10c21c7b73..67b0a2302e 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile @@ -5,7 +5,7 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=$(MODEM_PORT) -DDOWNLINK_AP_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT) +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT) ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c ap.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 9e72afab8e..5653554b63 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -36,11 +36,11 @@ #include #include "navdata.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - int nav_fd; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_navdata(void) { DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice, &navdata->taille, @@ -72,6 +72,7 @@ static void send_navdata(void) { &navdata->mz, &navdata->chksum); } +#endif int navdata_init() { @@ -122,7 +123,10 @@ int navdata_init() previousUltrasoundHeight = 0; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); +#endif + return 0; } diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index c1ec18744a..a531aab806 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -36,12 +36,6 @@ #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; @@ -149,6 +143,9 @@ inline static void h_ctl_pitch_loop( void ); #define H_CTL_PITCH_KFFD 0. #endif +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_calibration(void) { DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } @@ -162,7 +159,7 @@ 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) } - +#endif void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; @@ -211,9 +208,11 @@ void h_ctl_init( void ) { v_ctl_pitch_dash_trim = 0.; #endif +#if DOWNLINK 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); +#endif } /** diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 7fc2b91452..ea24c6851c 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -35,12 +35,6 @@ #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; @@ -118,9 +112,13 @@ float h_ctl_roll_rate_gain; static float nav_ratio; #endif +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_calibration(void) { DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } +#endif void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; @@ -178,7 +176,9 @@ void h_ctl_init( void ) { nav_ratio=0; #endif +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration); +#endif } /** diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 7d58a2c06e..d4a4e02c70 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -35,10 +35,10 @@ #include "subsystems/actuators.h" #include "subsystems/electrical.h" #include "subsystems/settings.h" +#include "subsystems/datalink/telemetry.h" #include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/guidance.h" #include "firmwares/rotorcraft/stabilization.h" -#include "firmwares/rotorcraft/telemetry.h" #include "led.h" uint8_t autopilot_mode; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 8295fb538d..696aec76f6 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -34,10 +34,6 @@ #include "generated/airframe.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - /* error if some gains are negative */ #if (GUIDANCE_H_PGAIN < 0) || \ (GUIDANCE_H_DGAIN < 0) || \ @@ -93,6 +89,9 @@ static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_gh(void) { struct NedCoor_i* pos = stateGetPositionNed_i(); DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice, @@ -127,6 +126,7 @@ static void send_href(void) { &guidance_h_pos_sp.y, &guidance_h_pos_ref.y, &guidance_h_speed_ref.y, &guidance_h_accel_ref.y); } +#endif void guidance_h_init(void) { @@ -144,9 +144,11 @@ void guidance_h_init(void) { transition_percentage = 0; transition_theta_offset = 0; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_INT", send_gh); register_periodic_telemetry(DefaultPeriodic, "HOVER_LOOP", send_hover_loop); register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_REF", send_href); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index a832a74bac..10f23cdddc 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -39,9 +39,6 @@ #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - /* warn if some gains are still negative */ #if (GUIDANCE_V_HOVER_KP < 0) || \ @@ -96,6 +93,9 @@ int32_t guidance_v_z_sum_err; __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight); +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_vert_loop(void) { DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice, &guidance_v_z_sp, &guidance_v_zd_sp, @@ -112,6 +112,7 @@ static void send_vert_loop(void) { &guidance_v_fb_cmd, &guidance_v_delta_t); } +#endif void guidance_v_init(void) { @@ -129,7 +130,9 @@ void guidance_v_init(void) { gv_adapt_init(); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 5a0cb0e942..c0fe0297f8 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -33,8 +33,7 @@ #include "mcu_periph/sys_time.h" #include "led.h" -#include "subsystems/datalink/downlink.h" -#include "firmwares/rotorcraft/telemetry.h" +#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/datalink.h" #include "subsystems/settings.h" #include "subsystems/datalink/xbee.h" diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 037d09409f..de2b555c4c 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -44,9 +44,6 @@ #include "math/pprz_algebra_int.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - const uint8_t nb_waypoint = NB_WAYPOINT; struct EnuCoor_i waypoints[NB_WAYPOINT]; @@ -86,6 +83,9 @@ static inline void nav_set_altitude( void ); #define ARRIVED_AT_WAYPOINT (3 << 8) #define CARROT_DIST (12 << 8) +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_nav_status(void) { DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice, &block_time, &stage_time, @@ -115,6 +115,7 @@ static void send_wp_moved(void) { &(waypoints[i].y), &(waypoints[i].z)); } +#endif void nav_init(void) { // convert to @@ -148,8 +149,10 @@ void nav_init(void) { nav_leg_progress = 0; nav_leg_length = 1; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status); register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved); +#endif } static inline void nav_advance_carrot(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 9b6f69632f..b71c846bf5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -35,6 +35,9 @@ struct FloatEulers stabilization_att_sum_err; float stabilization_att_fb_cmd[COMMANDS_NB]; float stabilization_att_ff_cmd[COMMANDS_NB]; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_att(void) { struct FloatRates* body_rate = stateGetBodyRates_i(); struct FloatEulers* att = stateGetNedToBodyEulers_i(); @@ -75,7 +78,7 @@ static void send_att_ref(void) { &stab_att_ref_accel.q, &stab_att_ref_accel.r); } - +#endif void stabilization_attitude_init(void) { @@ -103,8 +106,10 @@ void stabilization_attitude_init(void) { FLOAT_EULERS_ZERO( stabilization_att_sum_err ); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index f44894f2cf..8e08d952b9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -27,9 +27,6 @@ #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - struct Int32AttitudeGains stabilization_gains; /* warn if some gains are still negative */ @@ -57,6 +54,9 @@ static inline void reset_psi_ref_from_body(void) { stab_att_ref_accel.r = 0; } +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_att(void) { struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i(); @@ -95,6 +95,7 @@ static void send_att_ref(void) { &stab_att_ref_accel.q, &stab_att_ref_accel.r); } +#endif void stabilization_attitude_init(void) { @@ -124,8 +125,10 @@ void stabilization_attitude_init(void) { INT_EULERS_ZERO( stabilization_att_sum_err ); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index a9aa344003..b9842627dc 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -32,9 +32,6 @@ #include "state.h" #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - struct Int32AttitudeGains stabilization_gains = { {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN }, {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN }, @@ -67,6 +64,9 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB]; #define GAIN_PRESCALER_D 48 #define GAIN_PRESCALER_I 48 +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_att(void) { //FIXME really use this message here ? struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i(); @@ -102,6 +102,7 @@ static void send_att_ref(void) { &(quat->qy), &(quat->qz)); } +#endif void stabilization_attitude_init(void) { @@ -121,8 +122,10 @@ void stabilization_attitude_enter(void) { INT32_QUAT_ZERO(stabilization_att_sum_err_quat); INT_EULERS_ZERO(stabilization_att_sum_err); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_AHRS_REF_QUAT", send_att_ref); +#endif } void stabilization_attitude_set_failsafe_setpoint(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index edb587551d..0bb9aa4be4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -34,9 +34,6 @@ #include "subsystems/radio_control.h" #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - #define F_UPDATE_RES 9 #define REF_DOT_FRAC 11 #define REF_FRAC 16 @@ -122,6 +119,9 @@ struct Int32Rates stabilization_rate_ff_cmd; (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R) +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_rate(void) { DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice, &stabilization_rate_sp.p, @@ -144,6 +144,7 @@ static void send_rate(void) { &stabilization_rate_fb_cmd.r, &stabilization_cmd[COMMAND_THRUST]); } +#endif void stabilization_rate_init(void) { @@ -166,7 +167,9 @@ void stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_refdot); INT_RATES_ZERO(stabilization_rate_sum_err); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate); +#endif } diff --git a/sw/airborne/link_mcu_spi.c b/sw/airborne/link_mcu_spi.c index e9b7284ab2..363d2dc827 100644 --- a/sw/airborne/link_mcu_spi.c +++ b/sw/airborne/link_mcu_spi.c @@ -100,20 +100,18 @@ 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; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + 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); } +#endif void link_mcu_init(void) { link_mcu_nb_err = 0; @@ -128,7 +126,9 @@ void link_mcu_init(void) { link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "DEBUG_MCU_LINK", send_debug_link); +#endif } void link_mcu_send(void) { diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index dcc1965daf..c30e06bd56 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -27,13 +27,15 @@ #include "mcu_periph/i2c.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" +#endif #ifdef USE_I2C0 struct i2c_periph i2c0; +#if DOWNLINK static void send_i2c0_err(void) { uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; @@ -57,6 +59,7 @@ static void send_i2c0_err(void) { &i2c0_last_unexpected_event, &_bus0); } +#endif void i2c0_init(void) { i2c_init(&i2c0); @@ -70,6 +73,7 @@ void i2c0_init(void) { struct i2c_periph i2c1; +#if DOWNLINK static void send_i2c1_err(void) { uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; @@ -93,6 +97,7 @@ static void send_i2c1_err(void) { &i2c1_last_unexpected_event, &_bus1); } +#endif void i2c1_init(void) { i2c_init(&i2c1); @@ -106,6 +111,7 @@ void i2c1_init(void) { struct i2c_periph i2c2; +#if DOWNLINK static void send_i2c2_err(void) { uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; @@ -129,6 +135,7 @@ static void send_i2c2_err(void) { &i2c2_last_unexpected_event, &_bus2); } +#endif void i2c2_init(void) { i2c_init(&i2c2); @@ -137,6 +144,7 @@ void i2c2_init(void) { #endif /* USE_I2C2 */ +#if DOWNLINK static void send_i2c_err(void) { static uint8_t _i2c_nb_cnt = 0; switch (_i2c_nb_cnt) { @@ -162,14 +170,17 @@ static void send_i2c_err(void) { if (_i2c_nb_cnt == 3) _i2c_nb_cnt = 0; } +#endif void i2c_init(struct i2c_periph* p) { p->trans_insert_idx = 0; p->trans_extract_idx = 0; p->status = I2CIdle; +#if DOWNLINK // the first to register do it for the others register_periodic_telemetry(DefaultPeriodic, "I2C_ERRORS", send_i2c_err); +#endif } diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index 76a662008f..87667602f9 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -22,12 +22,14 @@ #include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" +#endif #ifdef USE_UART0 struct uart_periph uart0; +#if DOWNLINK static void send_uart0_err(void) { uint16_t ore = uart0.ore; uint16_t ne_err = uart0.ne_err; @@ -36,12 +38,14 @@ static void send_uart0_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus0); } +#endif #endif #ifdef USE_UART1 struct uart_periph uart1; +#if DOWNLINK static void send_uart1_err(void) { uint16_t ore = uart1.ore; uint16_t ne_err = uart1.ne_err; @@ -50,12 +54,14 @@ static void send_uart1_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus1); } +#endif #endif #ifdef USE_UART2 struct uart_periph uart2; +#if DOWNLINK static void send_uart2_err(void) { uint16_t ore = uart2.ore; uint16_t ne_err = uart2.ne_err; @@ -64,12 +70,14 @@ static void send_uart2_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus2); } +#endif #endif #ifdef USE_UART3 struct uart_periph uart3; +#if DOWNLINK static void send_uart3_err(void) { uint16_t ore = uart3.ore; uint16_t ne_err = uart3.ne_err; @@ -78,12 +86,14 @@ static void send_uart3_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus3); } +#endif #endif #ifdef USE_UART4 struct uart_periph uart4; +#if DOWNLINK static void send_uart4_err(void) { uint16_t ore = uart4.ore; uint16_t ne_err = uart4.ne_err; @@ -92,12 +102,14 @@ static void send_uart4_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus4); } +#endif #endif #ifdef USE_UART5 struct uart_periph uart5; +#if DOWNLINK static void send_uart5_err(void) { uint16_t ore = uart5.ore; uint16_t ne_err = uart5.ne_err; @@ -106,12 +118,14 @@ static void send_uart5_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus5); } +#endif #endif #ifdef USE_UART6 struct uart_periph uart6; +#if DOWNLINK static void send_uart6_err(void) { const uint8_t _bus6 = 6; uint16_t ore = uart6.ore; @@ -120,9 +134,11 @@ static void send_uart6_err(void) { DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, &ore, &ne_err, &fe_err, &_bus6); } +#endif #endif +#if DOWNLINK static void send_uart_err(void) { static uint8_t uart_nb_cnt = 0; switch (uart_nb_cnt) { @@ -160,6 +176,7 @@ static void send_uart_err(void) { if (uart_nb_cnt == 6) uart_nb_cnt = 0; } +#endif void uart_periph_init(struct uart_periph* p) { p->rx_insert_idx = 0; @@ -171,8 +188,10 @@ void uart_periph_init(struct uart_periph* p) { p->ne_err = 0; p->fe_err = 0; +#if DOWNLINK // the first to register do it for the others register_periodic_telemetry(DefaultPeriodic, "UART_ERRORS", send_uart_err); +#endif } bool_t uart_check_free_space(struct uart_periph* p, uint8_t len) { diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 4278ca5cca..51da63b2e8 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -35,11 +35,7 @@ #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" +#include "subsystems/datalink/telemetry.h" #ifdef TEST_CAM float test_cam_estimator_x; diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index 296c428918..a73ca98e40 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -29,8 +29,7 @@ #include "subsystems/ins/hf_float.h" #endif -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" +#include "subsystems/datalink/telemetry.h" struct FloatVect3 target_pos_ned; struct FloatVect3 target_speed_ned; diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 1315cdbafd..c9a29f859a 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -27,8 +27,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "std.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" +#include "subsystems/datalink/telemetry.h" uint8_t rotorcraft_cam_mode; diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index 8f8c8171b3..61082cba5b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -32,9 +32,6 @@ #include "subsystems/imu.h" #include "led.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - struct AhrsAligner ahrs_aligner; #define SAMPLES_NB PERIODIC_FREQUENCY @@ -45,6 +42,9 @@ static struct Int32Vect3 mag_sum; static int32_t ref_sensor_samples[SAMPLES_NB]; static uint32_t samples_idx; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_aligner(void) { DOWNLINK_SEND_FILTER_ALIGNER(DefaultChannel, DefaultDevice, &ahrs_aligner.lp_gyro.p, @@ -57,6 +57,7 @@ static void send_aligner(void) { &ahrs_aligner.low_noise_cnt, &ahrs_aligner.status); } +#endif void ahrs_aligner_init(void) { @@ -68,7 +69,9 @@ void ahrs_aligner_init(void) { ahrs_aligner.noise = 0; ahrs_aligner.low_noise_cnt = 0; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "FILTER_ALIGNER", send_aligner); +#endif } #ifndef LOW_NOISE_THRESHOLD diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index b1ae8ce052..491d53b18e 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -33,13 +33,13 @@ #include "boards/ardrone/at_com.h" #include "subsystems/electrical.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - struct AhrsARDrone ahrs_impl; struct AhrsAligner ahrs_aligner; unsigned char buffer[2048]; //Packet buffer +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_ahrs_ad2(void) { DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice, &ahrs_impl.state, @@ -56,6 +56,7 @@ static void send_ahrs_ad2(void) { &ahrs_impl.altitude, &ahrs_impl.battery); } +#endif void ahrs_init(void) { init_at_com(); @@ -66,7 +67,9 @@ void ahrs_init(void) { ahrs.status = AHRS_RUNNING; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2); +#endif } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index dfa8bfd179..c661f4c960 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -40,10 +40,6 @@ #include "subsystems/gps.h" #endif -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - - //#include "../../test/pprz_algebra_print.h" #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT @@ -87,6 +83,9 @@ static inline void compute_body_orientation_and_rates(void); struct AhrsFloatCmpl ahrs_impl; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_att(void) { struct FloatEulers ltp_to_imu_euler; FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); @@ -127,6 +126,7 @@ static void send_rmat(void) { &(att_rmat->m[8])); } */ +#endif void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -151,7 +151,9 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); +#endif } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c index f2beda4628..92424b9990 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -35,9 +35,6 @@ #include "generated/airframe.h" #include "math/pprz_algebra_float.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - #include @@ -217,6 +214,9 @@ float bafl_R_mag; INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \ } +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_lkf(void) { DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, DefaultChannel, DefaultDevice, @@ -277,7 +277,7 @@ static void send_lkf_mag(void) { &bafl_b_m_err.q, &bafl_b_m_err.r); } - +#endif void ahrs_init(void) { int i, j; @@ -315,10 +315,12 @@ void ahrs_init(void) { FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF", send_lkf); register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_DEBUG", send_lkf_debug); register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_ACC_DBG", send_lkf_acc); register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_MAG_DBG", send_lkf_mag); +#endif } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index 8212ba58ad..12b19da23a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -37,14 +37,11 @@ #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; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_infrared(void) { DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top); @@ -56,14 +53,17 @@ static void send_status(void) { if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast); } +#endif void ahrs_init(void) { ahrs.status = AHRS_UNINIT; heading = 0.; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared); register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status); +#endif } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 02a195be8f..254107a9d6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -38,9 +38,6 @@ #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - #ifndef FACE_REINJ_1 #define FACE_REINJ_1 1024 #endif @@ -77,6 +74,9 @@ static inline void set_body_state_from_euler(void); while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \ } +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_filter(void) { DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice, &ahrs_impl.ltp_to_imu_euler.phi, @@ -111,6 +111,7 @@ static void send_bias(void) { DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); } +#endif void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -124,9 +125,11 @@ void ahrs_init(void) { ahrs_impl.mag_offset = AHRS_MAG_OFFSET; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); +#endif } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 32cccd7ab4..7eafe7ebc8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -43,9 +43,6 @@ #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - //#include "../../test/pprz_algebra_print.h" static inline void ahrs_update_mag_full(void); @@ -79,6 +76,9 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; static inline void set_body_state_from_quat(void); +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_quat(void) { struct Int32Quat* quat = stateGetNedToBodyQuat_i(); DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice, @@ -109,6 +109,7 @@ static void send_bias(void) { DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, &ahrs_impl.gyro_bias.p, &ahrs_impl.gyro_bias.q, &ahrs_impl.gyro_bias.r); } +#endif void ahrs_init(void) { @@ -138,9 +139,11 @@ void ahrs_init(void) { VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); +#endif } diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 58e18c51bc..844793dca2 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -26,12 +26,6 @@ #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) @@ -40,6 +34,9 @@ struct GpsState gps; struct GpsTimeSync gps_time_sync; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_gps(void) { static uint8_t i; int16_t climb = -gps.ned_vel.z; @@ -97,6 +94,7 @@ static void send_gps_lla(void) { static void send_gps_sol(void) { DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); } +#endif void gps_init(void) { gps.fix = GPS_FIX_NONE; @@ -108,10 +106,12 @@ void gps_init(void) { gps_impl_init(); #endif +#if DOWNLINK 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); +#endif } uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 6900162b6f..daf7e478b2 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -26,11 +26,8 @@ #include "subsystems/imu.h" -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" static void send_accel_raw(void) { DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, @@ -93,6 +90,8 @@ static void send_mag_calib(void) { } #endif +#endif + struct Imu imu; void imu_init(void) { @@ -123,6 +122,7 @@ 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); +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); @@ -134,6 +134,7 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_CURRENT_CALIBRATION", send_mag_calib); +#endif #endif imu_impl_init(); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index fcfe5ddea1..1fa57f3d44 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -36,9 +36,6 @@ #include "generated/airframe.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - #ifdef SITL #include #define DBG_LEVEL 1 @@ -240,6 +237,9 @@ static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel); static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel); +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_hff(void) { DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice, &b2_hff_state.x, @@ -271,6 +271,8 @@ static void send_hff_gps(void) { } #endif +#endif + void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { Rgps_pos = HFF_R_POS; Rgps_vel = HFF_R_SPEED; @@ -311,11 +313,13 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { b2_hff_lost_counter = 0; b2_hff_lost_limit = HFF_LOST_LIMIT; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "HFF", send_hff); register_periodic_telemetry(DefaultPeriodic, "HFF_DBG", send_hff_debug); #ifdef GPS_LAG register_periodic_telemetry(DefaultPeriodic, "HFF_GPS", send_hff_gps); #endif +#endif } static inline void b2_hff_init_x(float init_x, float init_xdot) { diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 90e3d9fe89..7ff6420411 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -51,9 +51,6 @@ #include "generated/flight_plan.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - #ifndef USE_INS_NAV_INIT #define USE_INS_NAV_INIT TRUE PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") @@ -86,6 +83,9 @@ struct NedCoor_i ins_ltp_pos; struct NedCoor_i ins_ltp_speed; struct NedCoor_i ins_ltp_accel; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_ins(void) { DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, &ins_ltp_pos.x, &ins_ltp_pos.y, &ins_ltp_pos.z, @@ -106,6 +106,7 @@ static void send_ins_ref(void) { &ins_ltp_def.hmsl, &ins_qfe); } } +#endif void ins_init() { #if USE_INS_NAV_INIT @@ -146,9 +147,11 @@ void ins_init() { // TODO correct init ins.status = INS_RUNNING; +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z); register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); +#endif } void ins_periodic( void ) { diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index b30a234baa..3fd4bc2cfd 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -39,9 +39,6 @@ #include "subsystems/datalink/downlink.h" #endif -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - /* X = [ z zdot accel_bias baro_offset ] @@ -74,12 +71,16 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; float vff_z_meas; float vff_z_meas_baro; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_vffe(void) { DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset); } +#endif void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) { vff_z = init_z; @@ -93,7 +94,9 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b vff_P[i][i] = INIT_PXX; } +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "VFF_EXTENDED", send_vffe); +#endif } diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c index d8b2a87015..e49d12a567 100644 --- a/sw/airborne/subsystems/ins/vf_float.c +++ b/sw/airborne/subsystems/ins/vf_float.c @@ -28,9 +28,6 @@ #include "subsystems/ins/vf_float.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic_telemetry.h" - /* X = [ z zdot bias ] @@ -69,11 +66,15 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; float vff_z_meas; +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_vff(void) { DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z, &vff_zdot, &vff_bias, &vff_P[0][0], &vff_P[1][1], &vff_P[2][2]); } +#endif void vff_init(float init_z, float init_zdot, float init_bias) { vff_z = init_z; @@ -86,7 +87,9 @@ void vff_init(float init_z, float init_zdot, float init_bias) { vff_P[i][i] = VF_FLOAT_INIT_PXX; } +#if DOWNLINK register_periodic_telemetry(DefaultPeriodic, "VFF", send_vff); +#endif } diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index e3cab5e156..19f2f56347 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -36,12 +36,6 @@ #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; @@ -440,6 +434,9 @@ void nav_periodic_task(void) { /** * \brief Periodic telemetry */ +#if DOWNLINK +#include "subsystems/datalink/telemetry.h" + static void send_nav_ref(void) { DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0); @@ -475,6 +472,7 @@ static void send_survey(void) { &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); } } +#endif /** * \brief Navigation Initialisation @@ -493,12 +491,14 @@ void nav_init(void) { nav_ground_speed_setpoint = NOMINAL_AIRSPEED; #endif +#if DOWNLINK 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); +#endif } /** diff --git a/sw/airborne/subsystems/radio_control/ppm.c b/sw/airborne/subsystems/radio_control/ppm.c index 58d622b133..bb7231eb7c 100644 --- a/sw/airborne/subsystems/radio_control/ppm.c +++ b/sw/airborne/subsystems/radio_control/ppm.c @@ -22,20 +22,17 @@ #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" +uint16_t ppm_pulses[ PPM_NB_CHANNEL ]; +volatile bool_t ppm_frame_available; +#if DOWNLINK #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; +#include "subsystems/datalink/telemetry.h" static void send_ppm(void) { uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; @@ -44,10 +41,13 @@ static void send_ppm(void) { DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, &radio_control.frame_rate, PPM_NB_CHANNEL, ppm_pulses_usec); } +#endif void radio_control_impl_init(void) { ppm_frame_available = FALSE; ppm_arch_init(); +#if DOWNLINK register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_ppm); +#endif }