diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile index 9f49d234d1..c661f74922 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile @@ -9,6 +9,6 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) 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.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main' +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile index 46da240113..6eab128eb7 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -3,8 +3,8 @@ ifeq ($(ARCH), lpc21) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DDefaultPeriodic='&telemetry_Main' +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile index fa355be53b..e77ef74406 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_udp.makefile @@ -2,7 +2,7 @@ # Udp telemetry ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=Udp -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=UDP -ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/udp.c subsystems/datalink/pprz_transport.c +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=UDP -DDefaultPeriodic='&telemetry_Main' +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/udp.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c ap.srcs += fms/fms_network.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile index 8b70a32c30..c83162d86e 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile @@ -10,6 +10,6 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD) 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.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE -DDefaultPeriodic='&telemetry_Main' +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 8231957d70..9e72afab8e 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -36,8 +36,43 @@ #include #include "navdata.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + int nav_fd; +static void send_navdata(void) { + DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice, + &navdata->taille, + &navdata->nu_trame, + &navdata->ax, + &navdata->ay, + &navdata->az, + &navdata->vx, + &navdata->vy, + &navdata->vz, + &navdata->temperature_acc, + &navdata->temperature_gyro, + &navdata->ultrasound, + &navdata->us_debut_echo, + &navdata->us_fin_echo, + &navdata->us_association_echo, + &navdata->us_distance_echo, + &navdata->us_curve_time, + &navdata->us_curve_value, + &navdata->us_curve_ref, + &navdata->nb_echo, + &navdata->sum_echo, + &navdata->gradient, + &navdata->flag_echo_ini, + &navdata->pressure, + &navdata->temperature_pressure, + &navdata->mx, + &navdata->my, + &navdata->mz, + &navdata->chksum); +} + int navdata_init() { port = malloc(sizeof(navdata_port)); @@ -87,6 +122,7 @@ int navdata_init() previousUltrasoundHeight = 0; + register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); return 0; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index fa4538cfda..7d58a2c06e 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -28,12 +28,17 @@ #include "firmwares/rotorcraft/autopilot.h" +#include "mcu_periph/uart.h" #include "subsystems/radio_control.h" #include "subsystems/gps.h" #include "subsystems/commands.h" +#include "subsystems/actuators.h" +#include "subsystems/electrical.h" +#include "subsystems/settings.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; @@ -73,6 +78,95 @@ static inline int ahrs_is_aligned(void) { #include "autopilot_arming_yaw.h" #endif +static void send_alive(void) { + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); +} + +static void send_status(void) { + uint32_t imu_nb_err = 0; + uint8_t _twi_blmc_nb_err = 0; +#if USE_GPS + uint8_t fix = gps.fix; +#else + uint8_t fix = GPS_FIX_NONE; +#endif + uint16_t time_sec = sys_time.nb_sec; + DOWNLINK_SEND_ROTORCRAFT_STATUS(DefaultChannel, DefaultDevice, + &imu_nb_err, &_twi_blmc_nb_err, + &radio_control.status, &radio_control.frame_rate, + &fix, &autopilot_mode, + &autopilot_in_flight, &autopilot_motors_on, + &guidance_h_mode, &guidance_v_mode, + &electrical.vsupply, &time_sec); +} + +static void send_fp(void) { + int32_t carrot_up = -guidance_v_z_sp; + DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice, + &(stateGetPositionEnu_i()->x), + &(stateGetPositionEnu_i()->y), + &(stateGetPositionEnu_i()->z), + &(stateGetSpeedEnu_i()->x), + &(stateGetSpeedEnu_i()->y), + &(stateGetSpeedEnu_i()->z), + &(stateGetNedToBodyEulers_i()->phi), + &(stateGetNedToBodyEulers_i()->theta), + &(stateGetNedToBodyEulers_i()->psi), + &guidance_h_pos_sp.y, + &guidance_h_pos_sp.x, + &carrot_up, + &guidance_h_command_body.psi, + &stabilization_cmd[COMMAND_THRUST], + &autopilot_flight_time); +} + +#ifdef RADIO_CONTROL +static void send_rc(void) { + DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); +} + +static void send_rotorcraft_rc(void) { +#ifdef RADIO_KILL_SWITCH + int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH]; +#else + int16_t _kill_switch = 42; +#endif + DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice, + &radio_control.values[RADIO_ROLL], + &radio_control.values[RADIO_PITCH], + &radio_control.values[RADIO_YAW], + &radio_control.values[RADIO_THROTTLE], + &radio_control.values[RADIO_MODE], + &_kill_switch, + &radio_control.status); +} +#endif + +#ifdef ACTUATORS +static void send_actuators(void) { + DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); +} +#endif + +static void send_dl_value(void) { + PeriodicSendDlValue(DefaultChannel, DefaultDevice); +} + +static void send_rotorcraft_cmd(void) { + DOWNLINK_SEND_ROTORCRAFT_CMD(DefaultChannel, DefaultDevice, + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &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) { autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; @@ -89,6 +183,20 @@ void autopilot_init(void) { LED_ON(POWER_SWITCH_LED); // POWER OFF #endif autopilot_arming_init(); + + register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); + register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status); + 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 +#ifdef RADIO_CONTROL + register_periodic_telemetry(DefaultPeriodic, "RC", send_rc); + register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 44d11f3b29..8295fb538d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -34,6 +34,10 @@ #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) || \ @@ -89,6 +93,40 @@ static void guidance_h_hover_enter(void); static void guidance_h_nav_enter(void); static inline void transition_run(void); +static void send_gh(void) { + struct NedCoor_i* pos = stateGetPositionNed_i(); + DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice, + &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, + &guidance_h_pos_ref.x, &guidance_h_pos_ref.y, + &(pos->x), &(pos->y)); +} + +static void send_hover_loop(void) { + struct NedCoor_i* pos = stateGetPositionNed_i(); + struct NedCoor_i* speed = stateGetSpeedNed_i(); + struct NedCoor_i* accel = stateGetAccelNed_i(); + DOWNLINK_SEND_HOVER_LOOP(DefaultChannel, DefaultDevice, + &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, + &(pos->x), &(pos->y), + &(speed->x), &(speed->y), + &(accel->x), &(accel->y), + &guidance_h_pos_err.x, &guidance_h_pos_err.y, + &guidance_h_speed_err.x, &guidance_h_speed_err.y, + &guidance_h_pos_err_sum.x, &guidance_h_pos_err_sum.y, + &guidance_h_nav_err.x, &guidance_h_nav_err.y, + &guidance_h_command_earth.x, &guidance_h_command_earth.y, + &guidance_h_command_body.phi, + &guidance_h_command_body.theta, + &guidance_h_command_body.psi); +} + +static void send_href(void) { + DOWNLINK_SEND_GUIDANCE_H_REF_INT(DefaultChannel, DefaultDevice, + &guidance_h_pos_sp.x, &guidance_h_pos_ref.x, + &guidance_h_speed_ref.x, &guidance_h_accel_ref.x, + &guidance_h_pos_sp.y, &guidance_h_pos_ref.y, + &guidance_h_speed_ref.y, &guidance_h_accel_ref.y); +} void guidance_h_init(void) { @@ -105,6 +143,10 @@ void guidance_h_init(void) { guidance_h_again = GUIDANCE_H_AGAIN; transition_percentage = 0; transition_theta_offset = 0; + + 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); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 2287d88252..a832a74bac 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -39,6 +39,9 @@ #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) || \ @@ -93,6 +96,22 @@ int32_t guidance_v_z_sum_err; __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight); +static void send_vert_loop(void) { + DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice, + &guidance_v_z_sp, &guidance_v_zd_sp, + &(stateGetPositionNed_i()->z), + &(stateGetSpeedNed_i()->z), + &(stateGetAccelNed_i()->z), + &guidance_v_z_ref, &guidance_v_zd_ref, + &guidance_v_zdd_ref, + &gv_adapt_X, + &gv_adapt_P, + &gv_adapt_Xmeas, + &guidance_v_z_sum_err, + &guidance_v_ff_cmd, + &guidance_v_fb_cmd, + &guidance_v_delta_t); +} void guidance_v_init(void) { @@ -109,6 +128,8 @@ void guidance_v_init(void) { #endif gv_adapt_init(); + + register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop); } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 544073e355..5a0cb0e942 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -57,10 +57,13 @@ #include "firmwares/rotorcraft/autopilot.h" +#include "subsystems/radio_control.h" + #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/guidance.h" #include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ins.h" #include "state.h" @@ -212,7 +215,7 @@ STATIC_INLINE void main_periodic( void ) { } STATIC_INLINE void telemetry_periodic(void) { - PeriodicSendMain(DefaultChannel,DefaultDevice); + periodic_telemetry_send_Main(); } STATIC_INLINE void failsafe_check( void ) { diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index f78723abbf..037d09409f 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -44,6 +44,9 @@ #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]; @@ -83,6 +86,36 @@ static inline void nav_set_altitude( void ); #define ARRIVED_AT_WAYPOINT (3 << 8) #define CARROT_DIST (12 << 8) +static void send_nav_status(void) { + DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice, + &block_time, &stage_time, + &nav_block, &nav_stage, + &horizontal_mode); + if (horizontal_mode == HORIZONTAL_MODE_ROUTE) { + float sx = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].x); + float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y); + float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x); + float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y); + DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey); + } + else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { + float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x); + float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y); + float r = POS_FLOAT_OF_BFP(nav_circle_radius); + DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r); + } +} + +static void send_wp_moved(void) { + static uint8_t i; + i++; if (i >= nb_waypoint) i = 0; + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, + &i, + &(waypoints[i].x), + &(waypoints[i].y), + &(waypoints[i].z)); +} + void nav_init(void) { // convert to const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS; @@ -115,6 +148,8 @@ void nav_init(void) { nav_leg_progress = 0; nav_leg_length = 1; + register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status); + register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved); } 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 b2f9f24178..9b6f69632f 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,47 @@ struct FloatEulers stabilization_att_sum_err; float stabilization_att_fb_cmd[COMMANDS_NB]; float stabilization_att_ff_cmd[COMMANDS_NB]; +static void send_att(void) { + struct FloatRates* body_rate = stateGetBodyRates_i(); + struct FloatEulers* att = stateGetNedToBodyEulers_i(); + float foo; + DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice, + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err_eulers.phi, + &stabilization_att_sum_err_eulers.theta, + &stabilization_att_sum_err_eulers.psi, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &foo, &foo, &foo); +} + +static void send_att_ref(void) { + DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice, + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stab_att_ref_euler.phi, + &stab_att_ref_euler.theta, + &stab_att_ref_euler.psi, + &stab_att_ref_rate.p, + &stab_att_ref_rate.q, + &stab_att_ref_rate.r, + &stab_att_ref_accel.p, + &stab_att_ref_accel.q, + &stab_att_ref_accel.r); +} + void stabilization_attitude_init(void) { @@ -62,6 +103,8 @@ void stabilization_attitude_init(void) { FLOAT_EULERS_ZERO( stabilization_att_sum_err ); + register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); + register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); } 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 12effe492a..f44894f2cf 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -27,6 +27,9 @@ #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 */ @@ -54,6 +57,45 @@ static inline void reset_psi_ref_from_body(void) { stab_att_ref_accel.r = 0; } +static void send_att(void) { + struct Int32Rates* body_rate = stateGetBodyRates_i(); + struct Int32Eulers* att = stateGetNedToBodyEulers_i(); + DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err.phi, + &stabilization_att_sum_err.theta, + &stabilization_att_sum_err.psi, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW]); +} + +static void send_att_ref(void) { + DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice, + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stab_att_ref_euler.phi, + &stab_att_ref_euler.theta, + &stab_att_ref_euler.psi, + &stab_att_ref_rate.p, + &stab_att_ref_rate.q, + &stab_att_ref_rate.r, + &stab_att_ref_accel.p, + &stab_att_ref_accel.q, + &stab_att_ref_accel.r); +} + void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); @@ -82,6 +124,8 @@ void stabilization_attitude_init(void) { INT_EULERS_ZERO( stabilization_att_sum_err ); + register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); + register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); } 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 773bd481e8..a9aa344003 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -32,6 +32,9 @@ #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 }, @@ -64,6 +67,42 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB]; #define GAIN_PRESCALER_D 48 #define GAIN_PRESCALER_I 48 +static void send_att(void) { //FIXME really use this message here ? + struct Int32Rates* body_rate = stateGetBodyRates_i(); + struct Int32Eulers* att = stateGetNedToBodyEulers_i(); + DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err.phi, + &stabilization_att_sum_err.theta, + &stabilization_att_sum_err.psi, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW]); +} + +static void send_att_ref(void) { + struct Int32Quat* quat = stateGetNedToBodyQuat_i(); + DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice, + &stab_att_ref_quat.qi, + &stab_att_ref_quat.qx, + &stab_att_ref_quat.qy, + &stab_att_ref_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} + void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); @@ -81,6 +120,9 @@ void stabilization_attitude_enter(void) { INT32_QUAT_ZERO(stabilization_att_sum_err_quat); INT_EULERS_ZERO(stabilization_att_sum_err); + + register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); + register_periodic_telemetry(DefaultPeriodic, "STAB_AHRS_REF_QUAT", send_att_ref); } 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 486ca4deb9..edb587551d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -34,6 +34,9 @@ #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 @@ -119,6 +122,28 @@ struct Int32Rates stabilization_rate_ff_cmd; (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R) +static void send_rate(void) { + DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice, + &stabilization_rate_sp.p, + &stabilization_rate_sp.q, + &stabilization_rate_sp.r, + &stabilization_rate_ref.p, + &stabilization_rate_ref.q, + &stabilization_rate_ref.r, + &stabilization_rate_refdot.p, + &stabilization_rate_refdot.q, + &stabilization_rate_refdot.r, + &stabilization_rate_sum_err.p, + &stabilization_rate_sum_err.q, + &stabilization_rate_sum_err.r, + &stabilization_rate_ff_cmd.p, + &stabilization_rate_ff_cmd.q, + &stabilization_rate_ff_cmd.r, + &stabilization_rate_fb_cmd.p, + &stabilization_rate_fb_cmd.q, + &stabilization_rate_fb_cmd.r, + &stabilization_cmd[COMMAND_THRUST]); +} void stabilization_rate_init(void) { @@ -140,6 +165,8 @@ void stabilization_rate_init(void) { INT_RATES_ZERO(stabilization_rate_ref); INT_RATES_ZERO(stabilization_rate_refdot); INT_RATES_ZERO(stabilization_rate_sum_err); + + register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate); } diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index b35591458d..5eba238f31 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -29,1051 +29,25 @@ #include "subsystems/datalink/downlink.h" #include "generated/periodic_telemetry.h" -#ifdef RADIO_CONTROL -#include "subsystems/radio_control.h" -#endif +// FIXME no idea where to put this one +//#ifndef AHRS_FLOAT +//#define PERIODIC_SEND_ROTORCRAFT_TUNE_HOVER(DefaultChannel, DefaultDevice) { +// DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(DefaultChannel, DefaultDevice, +// &radio_control.values[RADIO_ROLL], +// &radio_control.values[RADIO_PITCH], +// &radio_control.values[RADIO_YAW], +// &stabilization_cmd[COMMAND_ROLL], +// &stabilization_cmd[COMMAND_PITCH], +// &stabilization_cmd[COMMAND_YAW], +// &stabilization_cmd[COMMAND_THRUST], +// &ahrs_impl.ltp_to_imu_euler.phi, +// &ahrs_impl.ltp_to_imu_euler.theta, +// &ahrs_impl.ltp_to_imu_euler.psi, +// &(stateGetNedToBodyEulers_i()->phi), +// &(stateGetNedToBodyEulers_i()->theta), +// &(stateGetNedToBodyEulers_i()->psi)); +//} +//#endif -#include "state.h" - -#include "firmwares/rotorcraft/autopilot.h" -#include "firmwares/rotorcraft/guidance.h" - -#include "subsystems/actuators.h" - -#include "mcu_periph/sys_time.h" -#include "subsystems/electrical.h" -#include "subsystems/imu.h" -#if USE_GPS -#include "subsystems/gps.h" -#endif -#include "subsystems/ins.h" -#include "subsystems/ahrs.h" -// I2C Error counters -#include "mcu_periph/i2c.h" - -#define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM) - -#if USE_GPS -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \ - uint32_t imu_nb_err = 0; \ - uint8_t _twi_blmc_nb_err = 0; \ - uint16_t time_sec = sys_time.nb_sec; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \ - &imu_nb_err, \ - &_twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &gps.fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &electrical.vsupply, \ - &time_sec \ - ); \ - } -#else /* !USE_GPS */ -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \ - uint32_t imu_nb_err = 0; \ - uint8_t twi_blmc_nb_err = 0; \ - uint8_t fix = GPS_FIX_NONE; \ - uint16_t time_sec = sys_time.nb_sec; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev, \ - &imu_nb_err, \ - &twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &electrical.vsupply, \ - &time_sec \ - ); \ - } -#endif /* USE_GPS */ - -#ifdef RADIO_CONTROL -#define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values) -#if defined RADIO_KILL_SWITCH -#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &radio_control.values[RADIO_KILL_SWITCH]) -#else /* ! RADIO_KILL_SWITCH */ -#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) { \ - int16_t foo = -42; \ - SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &foo) \ -} -#endif /* !RADIO_KILL_SWITCH */ -#define SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev, _kill_switch) { \ - DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev, \ - &radio_control.values[RADIO_ROLL], \ - &radio_control.values[RADIO_PITCH], \ - &radio_control.values[RADIO_YAW], \ - &radio_control.values[RADIO_THROTTLE], \ - &radio_control.values[RADIO_MODE], \ - _kill_switch, \ - &radio_control.status);} -#else /* ! RADIO_CONTROL */ -#define PERIODIC_SEND_RC(_trans, _dev) {} -#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) {} -#endif - -#ifdef RADIO_CONTROL_TYPE_PPM -#define PERIODIC_SEND_PPM(_trans, _dev) { \ - uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; \ - for (int i=0;ip), &(body_rate->q), &(body_rate->r), \ - &(att->phi), &(att->theta), &(att->psi), \ - &stab_att_sp_euler.phi, \ - &stab_att_sp_euler.theta, \ - &stab_att_sp_euler.psi, \ - &stabilization_att_sum_err.phi, \ - &stabilization_att_sum_err.theta, \ - &stabilization_att_sum_err.psi, \ - &stabilization_att_fb_cmd[COMMAND_ROLL], \ - &stabilization_att_fb_cmd[COMMAND_PITCH], \ - &stabilization_att_fb_cmd[COMMAND_YAW], \ - &stabilization_att_ff_cmd[COMMAND_ROLL], \ - &stabilization_att_ff_cmd[COMMAND_PITCH], \ - &stabilization_att_ff_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW]); \ -} - -#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \ - DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(_trans, _dev, \ - &stab_att_sp_euler.phi, \ - &stab_att_sp_euler.theta, \ - &stab_att_sp_euler.psi, \ - &stab_att_ref_euler.phi, \ - &stab_att_ref_euler.theta, \ - &stab_att_ref_euler.psi, \ - &stab_att_ref_rate.p, \ - &stab_att_ref_rate.q, \ - &stab_att_ref_rate.r, \ - &stab_att_ref_accel.p, \ - &stab_att_ref_accel.q, \ - &stab_att_ref_accel.r); \ - } - -#elif defined STABILIZATION_ATTITUDE_TYPE_FLOAT - -#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ - struct FloatRates* body_rate = stateGetBodyRates_f(); \ - struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ - float foo; \ - DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \ - &(body_rate->p), &(body_rate->q), &(body_rate->r), \ - &(att->phi), &(att->theta), &(att->psi), \ - &stab_att_ref_euler.phi, \ - &stab_att_ref_euler.theta, \ - &stab_att_ref_euler.psi, \ - &stabilization_att_sum_err_eulers.phi, \ - &stabilization_att_sum_err_eulers.theta, \ - &stabilization_att_sum_err_eulers.psi, \ - &stabilization_att_fb_cmd[COMMAND_ROLL], \ - &stabilization_att_fb_cmd[COMMAND_PITCH], \ - &stabilization_att_fb_cmd[COMMAND_YAW], \ - &stabilization_att_ff_cmd[COMMAND_ROLL], \ - &stabilization_att_ff_cmd[COMMAND_PITCH], \ - &stabilization_att_ff_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW], \ - &foo, &foo, &foo); \ - } - -#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \ - DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(_trans, _dev, \ - &stab_att_sp_euler.phi, \ - &stab_att_sp_euler.theta, \ - &stab_att_sp_euler.psi, \ - &stab_att_ref_euler.phi, \ - &stab_att_ref_euler.theta, \ - &stab_att_ref_euler.psi, \ - &stab_att_ref_rate.p, \ - &stab_att_ref_rate.q, \ - &stab_att_ref_rate.r, \ - &stab_att_ref_accel.p, \ - &stab_att_ref_accel.q, \ - &stab_att_ref_accel.r); \ - } - -#endif /* STABILIZATION_ATTITUDE_TYPE_FLOAT */ - - -#include "subsystems/ahrs/ahrs_aligner.h" -#define PERIODIC_SEND_FILTER_ALIGNER(_trans, _dev) { \ - DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \ - &ahrs_aligner.lp_gyro.p, \ - &ahrs_aligner.lp_gyro.q, \ - &ahrs_aligner.lp_gyro.r, \ - &imu.gyro.p, \ - &imu.gyro.q, \ - &imu.gyro.r, \ - &ahrs_aligner.noise, \ - &ahrs_aligner.low_noise_cnt, \ - &ahrs_aligner.status); \ - } - - -#define PERIODIC_SEND_ROTORCRAFT_CMD(_trans, _dev) { \ - DOWNLINK_SEND_ROTORCRAFT_CMD(_trans, _dev, \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_THRUST]); \ - } - - -#if USE_AHRS_CMPL_EULER -#include "subsystems/ahrs/ahrs_int_cmpl_euler.h" -#define PERIODIC_SEND_FILTER(_trans, _dev) { \ - DOWNLINK_SEND_FILTER(_trans, _dev, \ - &ahrs_impl.ltp_to_imu_euler.phi, \ - &ahrs_impl.ltp_to_imu_euler.theta, \ - &ahrs_impl.ltp_to_imu_euler.psi, \ - &ahrs_impl.measure.phi, \ - &ahrs_impl.measure.theta, \ - &ahrs_impl.measure.psi, \ - &ahrs_impl.hi_res_euler.phi, \ - &ahrs_impl.hi_res_euler.theta, \ - &ahrs_impl.hi_res_euler.psi, \ - &ahrs_impl.residual.phi, \ - &ahrs_impl.residual.theta, \ - &ahrs_impl.residual.psi, \ - &ahrs_impl.gyro_bias.p, \ - &ahrs_impl.gyro_bias.q, \ - &ahrs_impl.gyro_bias.r); \ - } -#else -#define PERIODIC_SEND_FILTER(_trans, _dev) {} -#endif - -#if USE_AHRS_ARDRONE2 -#include "subsystems/ahrs/ahrs_ardrone2.h" -#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_ARDRONE2(_trans, _dev, \ - &ahrs_impl.state, \ - &ahrs_impl.control_state, \ - &ahrs_impl.eulers.phi, \ - &ahrs_impl.eulers.theta, \ - &ahrs_impl.eulers.psi, \ - &ahrs_impl.speed.x, \ - &ahrs_impl.speed.y, \ - &ahrs_impl.speed.z, \ - &ahrs_impl.accel.x, \ - &ahrs_impl.accel.y, \ - &ahrs_impl.accel.z, \ - &ahrs_impl.altitude, \ - &ahrs_impl.battery); \ - } -#else -#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) {} -#endif - -#if USE_AHRS_CMPL_EULER || USE_AHRS_CMPL_QUAT -#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev, \ - &ahrs_impl.gyro_bias.p, \ - &ahrs_impl.gyro_bias.q, \ - &ahrs_impl.gyro_bias.r); \ - } -#else -#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) {} -#endif - -#if USE_AHRS_LKF -#include "subsystems/ahrs.h" -#include "ahrs/ahrs_float_lkf.h" -#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, \ - _trans, _dev, \ - &bafl_eulers.theta, \ - &bafl_eulers.psi, \ - &bafl_quat.qi, \ - &bafl_quat.qx, \ - &bafl_quat.qy, \ - &bafl_quat.qz, \ - &bafl_rates.p, \ - &bafl_rates.q, \ - &bafl_rates.r, \ - &bafl_accel_measure.x, \ - &bafl_accel_measure.y, \ - &bafl_accel_measure.z, \ - &bafl_mag.x, \ - &bafl_mag.y, \ - &bafl_mag.z); \ - } -#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_DEBUG(_trans, _dev, \ - &bafl_X[0], \ - &bafl_X[1], \ - &bafl_X[2], \ - &bafl_bias.p, \ - &bafl_bias.q, \ - &bafl_bias.r, \ - &bafl_qnorm, \ - &bafl_phi_accel, \ - &bafl_theta_accel, \ - &bafl_P[0][0], \ - &bafl_P[1][1], \ - &bafl_P[2][2], \ - &bafl_P[3][3], \ - &bafl_P[4][4], \ - &bafl_P[5][5]); \ - } -#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_trans, _dev, \ - &bafl_q_a_err.qi, \ - &bafl_q_a_err.qx, \ - &bafl_q_a_err.qy, \ - &bafl_q_a_err.qz, \ - &bafl_b_a_err.p, \ - &bafl_b_a_err.q, \ - &bafl_b_a_err.r); \ - } -#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_trans, _dev, \ - &bafl_q_m_err.qi, \ - &bafl_q_m_err.qx, \ - &bafl_q_m_err.qy, \ - &bafl_q_m_err.qz, \ - &bafl_b_m_err.p, \ - &bafl_b_m_err.q, \ - &bafl_b_m_err.r); \ - } -#else -#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) {} -#endif - -#if defined STABILIZATION_ATTITUDE_TYPE_QUAT && defined STABILIZATION_ATTITUDE_TYPE_INT -#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev, \ - &stab_att_ref_quat.qi, \ - &stab_att_ref_quat.qx, \ - &stab_att_ref_quat.qy, \ - &stab_att_ref_quat.qz, \ - &(stateGetNedToBodyQuat_i()->qi), \ - &(stateGetNedToBodyQuat_i()->qx), \ - &(stateGetNedToBodyQuat_i()->qy), \ - &(stateGetNedToBodyQuat_i()->qz)); \ - } -#else -#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) {} -#endif /* STABILIZATION_ATTITUDE_TYPE_QUAT */ - -#ifndef AHRS_FLOAT -#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ - &ahrs_impl.ltp_to_imu_quat.qi, \ - &ahrs_impl.ltp_to_imu_quat.qx, \ - &ahrs_impl.ltp_to_imu_quat.qy, \ - &ahrs_impl.ltp_to_imu_quat.qz, \ - &(stateGetNedToBodyQuat_i()->qi), \ - &(stateGetNedToBodyQuat_i()->qx), \ - &(stateGetNedToBodyQuat_i()->qy), \ - &(stateGetNedToBodyQuat_i()->qz)); \ - } -#endif - -#if USE_AHRS_CMPL_EULER -#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ - &ahrs_impl.ltp_to_imu_euler.phi, \ - &ahrs_impl.ltp_to_imu_euler.theta, \ - &ahrs_impl.ltp_to_imu_euler.psi, \ - &(stateGetNedToBodyEulers_i()->phi), \ - &(stateGetNedToBodyEulers_i()->theta), \ - &(stateGetNedToBodyEulers_i()->psi)); \ - } -#else -#ifndef AHRS_FLOAT -#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ - struct Int32Eulers ltp_to_imu_euler; \ - INT32_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); \ - DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ - <p_to_imu_euler.phi, \ - <p_to_imu_euler.theta, \ - <p_to_imu_euler.psi, \ - &(stateGetNedToBodyEulers_i()->phi), \ - &(stateGetNedToBodyEulers_i()->theta), \ - &(stateGetNedToBodyEulers_i()->psi)); \ -} -#else -#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ - struct FloatEulers ltp_to_imu_euler; \ - FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); \ - struct Int32Eulers euler_i; \ - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); \ - DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ - &euler_i.phi, \ - &euler_i.theta, \ - &euler_i.psi, \ - &(stateGetNedToBodyEulers_i()->phi), \ - &(stateGetNedToBodyEulers_i()->theta), \ - &(stateGetNedToBodyEulers_i()->psi)); \ - } -#endif -#endif - -#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \ - struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); \ - DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \ - &ahrs_impl.ltp_to_imu_rmat.m[0], \ - &ahrs_impl.ltp_to_imu_rmat.m[1], \ - &ahrs_impl.ltp_to_imu_rmat.m[2], \ - &ahrs_impl.ltp_to_imu_rmat.m[3], \ - &ahrs_impl.ltp_to_imu_rmat.m[4], \ - &ahrs_impl.ltp_to_imu_rmat.m[5], \ - &ahrs_impl.ltp_to_imu_rmat.m[6], \ - &ahrs_impl.ltp_to_imu_rmat.m[7], \ - &ahrs_impl.ltp_to_imu_rmat.m[8], \ - &(att_rmat->m[0]), \ - &(att_rmat->m[1]), \ - &(att_rmat->m[2]), \ - &(att_rmat->m[3]), \ - &(att_rmat->m[4]), \ - &(att_rmat->m[5]), \ - &(att_rmat->m[6]), \ - &(att_rmat->m[7]), \ - &(att_rmat->m[8])); \ -} - - - -#if USE_VFF -#include "subsystems/ins/vf_float.h" -#define PERIODIC_SEND_VFF(_trans, _dev) { \ - DOWNLINK_SEND_VFF(_trans, _dev, \ - &vff_z_meas, \ - &vff_z, \ - &vff_zdot, \ - &vff_bias, \ - & vff_P[0][0], \ - & vff_P[1][1], \ - & vff_P[2][2]); \ - } -#else -#define PERIODIC_SEND_VFF(_trans, _dev) {} -#endif - -#if USE_VFF_EXTENDED -#include "subsystems/ins/vf_extended_float.h" -#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) { \ - DOWNLINK_SEND_VFF_EXTENDED(_trans, _dev, \ - &vff_z_meas, \ - &vff_z_meas_baro, \ - &vff_z, \ - &vff_zdot, \ - &vff_bias, \ - &vff_offset); \ - } -#else -#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) {} -#endif - -#if USE_HFF -#include "subsystems/ins/hf_float.h" -#define PERIODIC_SEND_HFF(_trans, _dev) { \ - DOWNLINK_SEND_HFF(_trans, _dev, \ - &b2_hff_state.x, \ - &b2_hff_state.y, \ - &b2_hff_state.xdot, \ - &b2_hff_state.ydot, \ - &b2_hff_state.xdotdot, \ - &b2_hff_state.ydotdot); \ - } -#define PERIODIC_SEND_HFF_DBG(_trans, _dev) { \ - DOWNLINK_SEND_HFF_DBG(_trans, _dev, \ - &b2_hff_x_meas, \ - &b2_hff_y_meas, \ - &b2_hff_xd_meas, \ - &b2_hff_yd_meas, \ - &b2_hff_state.xP[0][0], \ - &b2_hff_state.yP[0][0], \ - &b2_hff_state.xP[1][1], \ - &b2_hff_state.yP[1][1]); \ - } -#ifdef GPS_LAG -#define PERIODIC_SEND_HFF_GPS(_trans, _dev) { \ - DOWNLINK_SEND_HFF_GPS(_trans, _dev, \ - &(b2_hff_rb_last->lag_counter), \ - &lag_counter_err, \ - &save_counter); \ - } -#else -#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {} -#endif -#else -#define PERIODIC_SEND_HFF(_trans, _dev) {} -#define PERIODIC_SEND_HFF_DBG(_trans, _dev) {} -#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {} -#endif - -#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \ - DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_ref.x, \ - &guidance_h_pos_ref.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y); \ - } - -#define PERIODIC_SEND_INS_Z(_trans, _dev) { \ - DOWNLINK_SEND_INS_Z(_trans, _dev, \ - &ins_baro_alt, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z); \ - } - -#define PERIODIC_SEND_INS(_trans, _dev) { \ - DOWNLINK_SEND_INS(_trans, _dev, \ - &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); \ - } - -#define PERIODIC_SEND_INS_REF(_trans, _dev) { \ - if (ins_ltp_initialised) \ - DOWNLINK_SEND_INS_REF(_trans, _dev, \ - &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); \ - } - -#define PERIODIC_SEND_VERT_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_VERT_LOOP(_trans, _dev, \ - &guidance_v_z_sp, \ - &guidance_v_zd_sp, \ - &ins_ltp_pos.z, \ - &ins_ltp_speed.z, \ - &ins_ltp_accel.z, \ - &guidance_v_z_ref, \ - &guidance_v_zd_ref, \ - &guidance_v_zdd_ref, \ - &gv_adapt_X, \ - &gv_adapt_P, \ - &gv_adapt_Xmeas, \ - &guidance_v_z_sum_err, \ - &guidance_v_ff_cmd, \ - &guidance_v_fb_cmd, \ - &guidance_v_delta_t); \ - } - -#define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \ - DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_sp.y, \ - &ins_ltp_pos.x, \ - &ins_ltp_pos.y, \ - &ins_ltp_speed.x, \ - &ins_ltp_speed.y, \ - &ins_ltp_accel.x, \ - &ins_ltp_accel.y, \ - &guidance_h_pos_err.x, \ - &guidance_h_pos_err.y, \ - &guidance_h_speed_err.x, \ - &guidance_h_speed_err.y, \ - &guidance_h_pos_err_sum.x, \ - &guidance_h_pos_err_sum.y, \ - &guidance_h_nav_err.x, \ - &guidance_h_nav_err.y, \ - &guidance_h_command_earth.x, \ - &guidance_h_command_earth.y, \ - &guidance_h_command_body.phi, \ - &guidance_h_command_body.theta, \ - &guidance_h_command_body.psi); \ - } - -#define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \ - DOWNLINK_SEND_GUIDANCE_H_REF_INT(_trans, _dev, \ - &guidance_h_pos_sp.x, \ - &guidance_h_pos_ref.x, \ - &guidance_h_speed_ref.x, \ - &guidance_h_accel_ref.x, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_ref.y, \ - &guidance_h_speed_ref.y, \ - &guidance_h_accel_ref.y); \ -} - -#include "firmwares/rotorcraft/navigation.h" -#define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) { \ - int32_t carrot_up = -guidance_v_z_sp; \ - DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev, \ - &(stateGetPositionEnu_i()->x), \ - &(stateGetPositionEnu_i()->y), \ - &(stateGetPositionEnu_i()->z), \ - &(stateGetSpeedEnu_i()->x), \ - &(stateGetSpeedEnu_i()->y), \ - &(stateGetSpeedEnu_i()->z), \ - &(stateGetNedToBodyEulers_i()->phi), \ - &(stateGetNedToBodyEulers_i()->theta), \ - &(stateGetNedToBodyEulers_i()->psi), \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_sp.x, \ - &carrot_up, \ - &guidance_h_command_body.psi, \ - &stabilization_cmd[COMMAND_THRUST], \ - &autopilot_flight_time); \ - } - -#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); \ - static uint8_t i; \ - static uint8_t last_cnos[GPS_NB_CHANNELS]; \ - if (i == gps.nb_channels) i = 0; \ - if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ - 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); \ - last_cnos[i] = gps.svinfos[i].cno; \ - } \ - i++; \ - } -#else -#define PERIODIC_SEND_GPS_INT(_trans, _dev) {} -#endif - -#include "firmwares/rotorcraft/navigation.h" -#define PERIODIC_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev) { \ - DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev, \ - &block_time, \ - &stage_time, \ - &nav_block, \ - &nav_stage, \ - &horizontal_mode); \ - if (horizontal_mode == HORIZONTAL_MODE_ROUTE) { \ - float sx = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].x); \ - float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y); \ - float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x); \ - float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y); \ - DOWNLINK_SEND_SEGMENT(_trans, _dev, &sx, &sy, &ex, &ey); \ - } \ - else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { \ - float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x); \ - float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y); \ - float r = POS_FLOAT_OF_BFP(nav_circle_radius); \ - DOWNLINK_SEND_CIRCLE(_trans, _dev, &cx, &cy, &r); \ - } \ - } - -#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \ - static uint8_t i; \ - i++; if (i >= nb_waypoint) i = 0; \ - DOWNLINK_SEND_WP_MOVED_ENU(_trans, _dev, \ - &i, \ - &(waypoints[i].x), \ - &(waypoints[i].y), \ - &(waypoints[i].z)); \ - } - -#if USE_CAM -#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) DOWNLINK_SEND_ROTORCRAFT_CAM(_trans, _dev,&rotorcraft_cam_tilt,&rotorcraft_cam_pan); -#else -#define PERIODIC_SEND_ROTORCRAFT_CAM(_trans, _dev) {} -#endif - -#ifndef AHRS_FLOAT -#define PERIODIC_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev) { \ - DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(_trans, _dev, \ - &radio_control.values[RADIO_ROLL], \ - &radio_control.values[RADIO_PITCH], \ - &radio_control.values[RADIO_YAW], \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_THRUST], \ - &ahrs_impl.ltp_to_imu_euler.phi, \ - &ahrs_impl.ltp_to_imu_euler.theta, \ - &ahrs_impl.ltp_to_imu_euler.psi, \ - &(stateGetNedToBodyEulers_i()->phi), \ - &(stateGetNedToBodyEulers_i()->theta), \ - &(stateGetNedToBodyEulers_i()->psi)); \ - } -#endif - -#ifdef USE_I2C0 -#define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) { \ - uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; \ - uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; \ - uint16_t i2c0_arb_lost_cnt = i2c0.errors->arb_lost_cnt; \ - uint16_t i2c0_over_under_cnt = i2c0.errors->over_under_cnt; \ - uint16_t i2c0_pec_recep_cnt = i2c0.errors->pec_recep_cnt; \ - uint16_t i2c0_timeout_tlow_cnt = i2c0.errors->timeout_tlow_cnt; \ - uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt; \ - uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; \ - uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; \ - const uint8_t _bus0 = 0; \ - DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ - &i2c0_ack_fail_cnt, \ - &i2c0_miss_start_stop_cnt, \ - &i2c0_arb_lost_cnt, \ - &i2c0_over_under_cnt, \ - &i2c0_pec_recep_cnt, \ - &i2c0_timeout_tlow_cnt, \ - &i2c0_smbus_alert_cnt, \ - &i2c0_unexpected_event_cnt, \ - &i2c0_last_unexpected_event, \ - &_bus0); \ - } -#else -#define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) {} -#endif - -#ifdef USE_I2C1 -#define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) { \ - uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; \ - uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; \ - uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt; \ - uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt; \ - uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt; \ - uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt; \ - uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; \ - uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; \ - uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; \ - const uint8_t _bus1 = 1; \ - DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ - &i2c1_ack_fail_cnt, \ - &i2c1_miss_start_stop_cnt, \ - &i2c1_arb_lost_cnt, \ - &i2c1_over_under_cnt, \ - &i2c1_pec_recep_cnt, \ - &i2c1_timeout_tlow_cnt, \ - &i2c1_smbus_alert_cnt, \ - &i2c1_unexpected_event_cnt, \ - &i2c1_last_unexpected_event, \ - &_bus1); \ - } -#else -#define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) {} -#endif - -#ifdef USE_I2C2 -#define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) { \ - uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; \ - uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; \ - uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; \ - uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; \ - uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; \ - uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; \ - uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; \ - uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; \ - uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; \ - const uint8_t _bus2 = 2; \ - DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ - &i2c2_ack_fail_cnt, \ - &i2c2_miss_start_stop_cnt, \ - &i2c2_arb_lost_cnt, \ - &i2c2_over_under_cnt, \ - &i2c2_pec_recep_cnt, \ - &i2c2_timeout_tlow_cnt, \ - &i2c2_smbus_alert_cnt, \ - &i2c2_unexpected_event_cnt, \ - &i2c2_last_unexpected_event, \ - &_bus2); \ - } -#else -#define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) {} -#endif - -#ifndef SITL -#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \ - static uint8_t _i2c_nb_cnt = 0; \ - switch (_i2c_nb_cnt) { \ - case 0: \ - PERIODIC_SEND_I2C0_ERRORS(_trans, _dev); break; \ - case 1: \ - PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); break; \ - case 2: \ - PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); break; \ - default: \ - break; \ - } \ - _i2c_nb_cnt++; \ - if (_i2c_nb_cnt == 3) \ - _i2c_nb_cnt = 0; \ - } -#else -#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) {} -#endif - -#ifdef BOOZ2_TRACK_CAM -#include "cam_track.h" -#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_NPS_SPEED_POS(_trans, _dev, \ - &target_accel_ned.x, \ - &target_accel_ned.y, \ - &target_accel_ned.z, \ - &target_speed_ned.x, \ - &target_speed_ned.y, \ - &target_speed_ned.z, \ - &target_pos_ned.x, \ - &target_pos_ned.y, \ - &target_pos_ned.z) -#else -#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) {} -#endif - -#ifdef ARDRONE2_RAW -#include "navdata.h" -#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \ - &navdata->taille, \ - &navdata->nu_trame, \ - &navdata->ax, \ - &navdata->ay, \ - &navdata->az, \ - &navdata->vx, \ - &navdata->vy, \ - &navdata->vz, \ - &navdata->temperature_acc, \ - &navdata->temperature_gyro, \ - &navdata->ultrasound, \ - &navdata->us_debut_echo, \ - &navdata->us_fin_echo, \ - &navdata->us_association_echo, \ - &navdata->us_distance_echo, \ - &navdata->us_curve_time, \ - &navdata->us_curve_value, \ - &navdata->us_curve_ref, \ - &navdata->nb_echo, \ - &navdata->sum_echo, \ - &navdata->gradient, \ - &navdata->flag_echo_ini, \ - &navdata->pressure, \ - &navdata->temperature_pressure, \ - &navdata->mx, \ - &navdata->my, \ - &navdata->mz, \ - &navdata->chksum \ - ) -#else -#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {} -#endif - -#include "generated/settings.h" -#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev) - -/* - * Sending of UART errors. - */ -#ifdef USE_UART1 -#define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \ - const uint8_t _bus1 = 1; \ - DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart1.ore, \ - &uart1.ne_err, \ - &uart1.fe_err, \ - &_bus1); \ - } -#else -#define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) {} -#endif - -#ifdef USE_UART2 -#define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \ - const uint8_t _bus2 = 2; \ - DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart2.ore, \ - &uart2.ne_err, \ - &uart2.fe_err, \ - &_bus2); \ - } -#else -#define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) {} -#endif - -#ifdef USE_UART3 -#define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \ - const uint8_t _bus3 = 3; \ - DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart3.ore, \ - &uart3.ne_err, \ - &uart3.fe_err, \ - &_bus3); \ - } -#else -#define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) {} -#endif - -#ifdef USE_UART5 -#define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \ - const uint8_t _bus5 = 5; \ - DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart5.ore, \ - &uart5.ne_err, \ - &uart5.fe_err, \ - &_bus5); \ - } -#else -#define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) {} -#endif - - -#ifndef SITL -#define PERIODIC_SEND_UART_ERRORS(_trans, _dev) { \ - static uint8_t uart_nb_cnt = 0; \ - switch (uart_nb_cnt) { \ - case 0: \ - PERIODIC_SEND_UART1_ERRORS(_trans, _dev); break; \ - case 1: \ - PERIODIC_SEND_UART2_ERRORS(_trans, _dev); break; \ - case 2: \ - PERIODIC_SEND_UART3_ERRORS(_trans, _dev); break; \ - case 3: \ - PERIODIC_SEND_UART5_ERRORS(_trans, _dev); break; \ - default: break; \ - } \ - uart_nb_cnt++; \ - if (uart_nb_cnt == 4) \ - uart_nb_cnt = 0; \ - } -#else -#define PERIODIC_SEND_UART_ERRORS(_trans, _dev) {} -#endif #endif /* TELEMETRY_H */ diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index 897278d41c..dcc1965daf 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -27,10 +27,37 @@ #include "mcu_periph/i2c.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #ifdef USE_I2C0 struct i2c_periph i2c0; +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; + uint16_t i2c0_arb_lost_cnt = i2c0.errors->arb_lost_cnt; + uint16_t i2c0_over_under_cnt = i2c0.errors->over_under_cnt; + uint16_t i2c0_pec_recep_cnt = i2c0.errors->pec_recep_cnt; + uint16_t i2c0_timeout_tlow_cnt = i2c0.errors->timeout_tlow_cnt; + uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt; + uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; + uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; + const uint8_t _bus0 = 0; + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + &i2c0_ack_fail_cnt, + &i2c0_miss_start_stop_cnt, + &i2c0_arb_lost_cnt, + &i2c0_over_under_cnt, + &i2c0_pec_recep_cnt, + &i2c0_timeout_tlow_cnt, + &i2c0_smbus_alert_cnt, + &i2c0_unexpected_event_cnt, + &i2c0_last_unexpected_event, + &_bus0); +} + void i2c0_init(void) { i2c_init(&i2c0); i2c0_hw_init(); @@ -43,6 +70,30 @@ void i2c0_init(void) { struct i2c_periph i2c1; +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; + uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt; + uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt; + uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt; + uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt; + uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; + uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; + uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; + const uint8_t _bus1 = 1; + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + &i2c1_ack_fail_cnt, + &i2c1_miss_start_stop_cnt, + &i2c1_arb_lost_cnt, + &i2c1_over_under_cnt, + &i2c1_pec_recep_cnt, + &i2c1_timeout_tlow_cnt, + &i2c1_smbus_alert_cnt, + &i2c1_unexpected_event_cnt, + &i2c1_last_unexpected_event, + &_bus1); +} + void i2c1_init(void) { i2c_init(&i2c1); i2c1_hw_init(); @@ -55,6 +106,30 @@ void i2c1_init(void) { struct i2c_periph i2c2; +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; + uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; + uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; + uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; + uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; + uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; + uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; + uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; + const uint8_t _bus2 = 2; + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + &i2c2_ack_fail_cnt, + &i2c2_miss_start_stop_cnt, + &i2c2_arb_lost_cnt, + &i2c2_over_under_cnt, + &i2c2_pec_recep_cnt, + &i2c2_timeout_tlow_cnt, + &i2c2_smbus_alert_cnt, + &i2c2_unexpected_event_cnt, + &i2c2_last_unexpected_event, + &_bus2); +} + void i2c2_init(void) { i2c_init(&i2c2); i2c2_hw_init(); @@ -62,10 +137,39 @@ void i2c2_init(void) { #endif /* USE_I2C2 */ +static void send_i2c_err(void) { + static uint8_t _i2c_nb_cnt = 0; + switch (_i2c_nb_cnt) { + case 0: +#if USE_I2C0 + send_i2c0_err(); +#endif + break; + case 1: +#if USE_I2C1 + send_i2c1_err(); +#endif + break; + case 2: +#if USE_I2C2 + send_i2c2_err(); +#endif + break; + default: + break; + } + _i2c_nb_cnt++; + if (_i2c_nb_cnt == 3) + _i2c_nb_cnt = 0; +} + void i2c_init(struct i2c_periph* p) { p->trans_insert_idx = 0; p->trans_extract_idx = 0; p->status = I2CIdle; + + // the first to register do it for the others + register_periodic_telemetry(DefaultPeriodic, "I2C_ERRORS", send_i2c_err); } diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index 49abe70e44..76a662008f 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -22,34 +22,145 @@ #include "mcu_periph/uart.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #ifdef USE_UART0 struct uart_periph uart0; + +static void send_uart0_err(void) { + uint16_t ore = uart0.ore; + uint16_t ne_err = uart0.ne_err; + uint16_t fe_err = uart0.fe_err; + const uint8_t _bus0 = 0; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus0); +} + #endif #ifdef USE_UART1 struct uart_periph uart1; + +static void send_uart1_err(void) { + uint16_t ore = uart1.ore; + uint16_t ne_err = uart1.ne_err; + uint16_t fe_err = uart1.fe_err; + const uint8_t _bus1 = 1; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus1); +} + #endif #ifdef USE_UART2 struct uart_periph uart2; + +static void send_uart2_err(void) { + uint16_t ore = uart2.ore; + uint16_t ne_err = uart2.ne_err; + uint16_t fe_err = uart2.fe_err; + const uint8_t _bus2 = 2; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus2); +} + #endif #ifdef USE_UART3 struct uart_periph uart3; + +static void send_uart3_err(void) { + uint16_t ore = uart3.ore; + uint16_t ne_err = uart3.ne_err; + uint16_t fe_err = uart3.fe_err; + const uint8_t _bus3 = 3; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus3); +} + #endif #ifdef USE_UART4 struct uart_periph uart4; + +static void send_uart4_err(void) { + uint16_t ore = uart4.ore; + uint16_t ne_err = uart4.ne_err; + uint16_t fe_err = uart4.fe_err; + const uint8_t _bus4 = 4; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus4); +} + #endif #ifdef USE_UART5 struct uart_periph uart5; + +static void send_uart5_err(void) { + uint16_t ore = uart5.ore; + uint16_t ne_err = uart5.ne_err; + uint16_t fe_err = uart5.fe_err; + const uint8_t _bus5 = 5; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus5); +} + #endif #ifdef USE_UART6 struct uart_periph uart6; + +static void send_uart6_err(void) { + const uint8_t _bus6 = 6; + uint16_t ore = uart6.ore; + uint16_t ne_err = uart6.ne_err; + uint16_t fe_err = uart6.fe_err; + DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + &ore, &ne_err, &fe_err, &_bus6); +} + #endif +static void send_uart_err(void) { + static uint8_t uart_nb_cnt = 0; + switch (uart_nb_cnt) { +#if USE_UART0 + case 0: + send_uart0_err(); break; +#endif +#if USE_UART1 + case 1: + send_uart1_err(); break; +#endif +#if USE_UART2 + case 2: + send_uart2_err(); break; +#endif +#if USE_UART3 + case 3: + send_uart3_err(); break; +#endif +#if USE_UART4 + case 4: + send_uart4_err(); break; +#endif +#if USE_UART5 + case 5: + send_uart5_err(); break; +#endif +#if USE_UART6 + case 6: + send_uart6_err(); break; +#endif + default: break; + } + uart_nb_cnt++; + if (uart_nb_cnt == 6) + uart_nb_cnt = 0; +} + void uart_periph_init(struct uart_periph* p) { p->rx_insert_idx = 0; p->rx_extract_idx = 0; @@ -59,6 +170,9 @@ void uart_periph_init(struct uart_periph* p) { p->ore = 0; p->ne_err = 0; p->fe_err = 0; + + // the first to register do it for the others + register_periodic_telemetry(DefaultPeriodic, "UART_ERRORS", send_uart_err); } bool_t uart_check_free_space(struct uart_periph* p, uint8_t len) { diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index ae7a29abda..296c428918 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -29,6 +29,9 @@ #include "subsystems/ins/hf_float.h" #endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + struct FloatVect3 target_pos_ned; struct FloatVect3 target_speed_ned; struct FloatVect3 target_accel_ned; @@ -54,6 +57,13 @@ volatile uint8_t cam_msg_received; uint8_t cam_status; uint8_t cam_data_len; +static void send_cam_track(void) { + DOWNLINK_SEND_NPS_SPEED_POS(DefaultChannel, DefaultDevice, + &target_accel_ned.x, &target_accel_ned.y, &target_accel_ned.z, + &target_speed_ned.x, &target_speed_ned.y, &target_speed_ned.z, + &target_pos_ned.x, &target_pos_ned.y, &target_pos_ned.z); +} + void track_init(void) { ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground) @@ -61,6 +71,7 @@ void track_init(void) { cam_status = UNINIT; cam_data_len = CAM_DATA_LEN; + register_periodic_telemetry(DefaultPeriodic, "CAM_TRACK", send_cam_track); } #include diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 6b7f3b5bf3..1315cdbafd 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -27,6 +27,9 @@ #include "firmwares/rotorcraft/navigation.h" #include "std.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + uint8_t rotorcraft_cam_mode; #define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p @@ -49,6 +52,11 @@ int16_t rotorcraft_cam_pan; #define ROTORCRAFT_CAM_PAN_MIN 0 #define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI +static void send_cam(void) { + DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice, + &rotorcraft_cam_tilt,&rotorcraft_cam_pan); +} + void rotorcraft_cam_init(void) { rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE); #if ROTORCRAFT_CAM_USE_TILT @@ -59,6 +67,8 @@ void rotorcraft_cam_init(void) { #endif rotorcraft_cam_tilt = 0; rotorcraft_cam_pan = 0; + + register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam); } void rotorcraft_cam_periodic(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index ee1ae63e14..8f8c8171b3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -32,6 +32,9 @@ #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 @@ -42,6 +45,19 @@ static struct Int32Vect3 mag_sum; static int32_t ref_sensor_samples[SAMPLES_NB]; static uint32_t samples_idx; +static void send_aligner(void) { + DOWNLINK_SEND_FILTER_ALIGNER(DefaultChannel, DefaultDevice, + &ahrs_aligner.lp_gyro.p, + &ahrs_aligner.lp_gyro.q, + &ahrs_aligner.lp_gyro.r, + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r, + &ahrs_aligner.noise, + &ahrs_aligner.low_noise_cnt, + &ahrs_aligner.status); +} + void ahrs_aligner_init(void) { ahrs_aligner.status = AHRS_ALIGNER_RUNNING; @@ -51,6 +67,8 @@ void ahrs_aligner_init(void) { samples_idx = 0; ahrs_aligner.noise = 0; ahrs_aligner.low_noise_cnt = 0; + + register_periodic_telemetry(DefaultPeriodic, "FILTER_ALIGNER", send_aligner); } #ifndef LOW_NOISE_THRESHOLD diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index 95c865fb90..b1ae8ce052 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -33,10 +33,30 @@ #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 +static void send_ahrs_ad2(void) { + DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice, + &ahrs_impl.state, + &ahrs_impl.control_state, + &ahrs_impl.eulers.phi, + &ahrs_impl.eulers.theta, + &ahrs_impl.eulers.psi, + &ahrs_impl.speed.x, + &ahrs_impl.speed.y, + &ahrs_impl.speed.z, + &ahrs_impl.accel.x, + &ahrs_impl.accel.y, + &ahrs_impl.accel.z, + &ahrs_impl.altitude, + &ahrs_impl.battery); +} + void ahrs_init(void) { init_at_com(); @@ -45,6 +65,8 @@ void ahrs_init(void) { at_com_send_ftrim(); ahrs.status = AHRS_RUNNING; + + register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2); } 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 0a17453b3a..dfa8bfd179 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -40,6 +40,9 @@ #include "subsystems/gps.h" #endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + //#include "../../test/pprz_algebra_print.h" @@ -84,6 +87,47 @@ static inline void compute_body_orientation_and_rates(void); struct AhrsFloatCmpl ahrs_impl; +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); + struct Int32Eulers euler_i; + EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); + struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); + DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + &euler_i.phi, + &euler_i.theta, + &euler_i.psi, + &(eulers_body->phi), + &(eulers_body->theta), + &(eulers_body->psi)); +} + +// TODO convert from float to int if we really need this one +/* +static void send_rmat(void) { + struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); + DOWNLINK_SEND_AHRS_RMAT(DefaultChannel, DefaultDevice, + &ahrs_impl.ltp_to_imu_rmat.m[0], + &ahrs_impl.ltp_to_imu_rmat.m[1], + &ahrs_impl.ltp_to_imu_rmat.m[2], + &ahrs_impl.ltp_to_imu_rmat.m[3], + &ahrs_impl.ltp_to_imu_rmat.m[4], + &ahrs_impl.ltp_to_imu_rmat.m[5], + &ahrs_impl.ltp_to_imu_rmat.m[6], + &ahrs_impl.ltp_to_imu_rmat.m[7], + &ahrs_impl.ltp_to_imu_rmat.m[8], + &(att_rmat->m[0]), + &(att_rmat->m[1]), + &(att_rmat->m[2]), + &(att_rmat->m[3]), + &(att_rmat->m[4]), + &(att_rmat->m[5]), + &(att_rmat->m[6]), + &(att_rmat->m[7]), + &(att_rmat->m[8])); +} +*/ + void ahrs_init(void) { ahrs.status = AHRS_UNINIT; ahrs_impl.ltp_vel_norm_valid = FALSE; @@ -107,6 +151,7 @@ void ahrs_init(void) { ahrs_impl.correct_gravity = FALSE; #endif + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); } 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 e3e38086dc..f2beda4628 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -35,6 +35,9 @@ #include "generated/airframe.h" #include "math/pprz_algebra_float.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #include @@ -214,6 +217,67 @@ float bafl_R_mag; INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \ } +static void send_lkf(void) { + DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, + DefaultChannel, DefaultDevice, + &bafl_eulers.theta, + &bafl_eulers.psi, + &bafl_quat.qi, + &bafl_quat.qx, + &bafl_quat.qy, + &bafl_quat.qz, + &bafl_rates.p, + &bafl_rates.q, + &bafl_rates.r, + &bafl_accel_measure.x, + &bafl_accel_measure.y, + &bafl_accel_measure.z, + &bafl_mag.x, + &bafl_mag.y, + &bafl_mag.z); +} + +static void send_lkf_debug(void) { + DOWNLINK_SEND_AHRS_LKF_DEBUG(DefaultChannel, DefaultDevice, + &bafl_X[0], + &bafl_X[1], + &bafl_X[2], + &bafl_bias.p, + &bafl_bias.q, + &bafl_bias.r, + &bafl_qnorm, + &bafl_phi_accel, + &bafl_theta_accel, + &bafl_P[0][0], + &bafl_P[1][1], + &bafl_P[2][2], + &bafl_P[3][3], + &bafl_P[4][4], + &bafl_P[5][5]); +} + +static void send_lkf_acc(void) { + DOWNLINK_SEND_AHRS_LKF_ACC_DBG(DefaultChannel, DefaultDevice, + &bafl_q_a_err.qi, + &bafl_q_a_err.qx, + &bafl_q_a_err.qy, + &bafl_q_a_err.qz, + &bafl_b_a_err.p, + &bafl_b_a_err.q, + &bafl_b_a_err.r); +} + +static void send_lkf_mag(void) { + DOWNLINK_SEND_AHRS_LKF_MAG_DBG(DefaultChannel, DefaultDevice, + &bafl_q_m_err.qi, + &bafl_q_m_err.qx, + &bafl_q_m_err.qy, + &bafl_q_m_err.qz, + &bafl_b_m_err.p, + &bafl_b_m_err.q, + &bafl_b_m_err.r); +} + void ahrs_init(void) { int i, j; @@ -251,6 +315,10 @@ void ahrs_init(void) { FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz); + 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); } 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 02d31d6d7b..02a195be8f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -38,6 +38,9 @@ #include "generated/airframe.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #ifndef FACE_REINJ_1 #define FACE_REINJ_1 1024 #endif @@ -74,6 +77,41 @@ static inline void set_body_state_from_euler(void); while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \ } +static void send_filter(void) { + DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice, + &ahrs_impl.ltp_to_imu_euler.phi, + &ahrs_impl.ltp_to_imu_euler.theta, + &ahrs_impl.ltp_to_imu_euler.psi, + &ahrs_impl.measure.phi, + &ahrs_impl.measure.theta, + &ahrs_impl.measure.psi, + &ahrs_impl.hi_res_euler.phi, + &ahrs_impl.hi_res_euler.theta, + &ahrs_impl.hi_res_euler.psi, + &ahrs_impl.residual.phi, + &ahrs_impl.residual.theta, + &ahrs_impl.residual.psi, + &ahrs_impl.gyro_bias.p, + &ahrs_impl.gyro_bias.q, + &ahrs_impl.gyro_bias.r); +} + +static void send_euler(void) { + struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); + DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + &ahrs_impl.ltp_to_imu_euler.phi, + &ahrs_impl.ltp_to_imu_euler.theta, + &ahrs_impl.ltp_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); +} + +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); +} + void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -85,6 +123,10 @@ void ahrs_init(void) { ahrs_impl.reinj_1 = FACE_REINJ_1; ahrs_impl.mag_offset = AHRS_MAG_OFFSET; + + 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); } 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 7db726ec22..32cccd7ab4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -43,6 +43,9 @@ #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); @@ -76,6 +79,36 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; static inline void set_body_state_from_quat(void); +static void send_quat(void) { + struct Int32Quat* quat = stateGetNedToBodyQuat_i(); + DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice, + &ahrs_impl.ltp_to_imu_quat.qi, + &ahrs_impl.ltp_to_imu_quat.qx, + &ahrs_impl.ltp_to_imu_quat.qy, + &ahrs_impl.ltp_to_imu_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} + +static void send_euler(void) { + struct Int32Eulers ltp_to_imu_euler; + INT32_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); + struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); + DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + <p_to_imu_euler.phi, + <p_to_imu_euler.theta, + <p_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); +} + +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); +} void ahrs_init(void) { @@ -105,6 +138,10 @@ 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)); + 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); + } void ahrs_align(void) { diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 39883053b6..890f27d768 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -56,6 +56,7 @@ #ifdef USE_USB_SERIAL #include "mcu_periph/usb_serial.h" #endif +#include "mcu_periph/uart.h" #endif /** !SITL */ #ifndef DefaultChannel diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 4d02ce83cc..58e18c51bc 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -52,17 +52,16 @@ static void send_gps(void) { 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); + &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) { + static uint8_t i; + static uint8_t last_cnos[GPS_NB_CHANNELS]; 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, @@ -73,6 +72,15 @@ static void send_gps_int(void) { &gps.pdop, &gps.num_sv, &gps.fix); + if (i == gps.nb_channels) i = 0; + if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { + 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); + last_cnos[i] = gps.svinfos[i].cno; + } + i++; } static void send_gps_lla(void) { diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index fd65d967ae..6900162b6f 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -37,18 +37,11 @@ static void send_accel_raw(void) { &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); +static void send_accel_scaled(void) { + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, + &imu.accel.x, &imu.accel.y, &imu.accel.z); } -#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); @@ -56,6 +49,16 @@ static void send_accel(void) { &accel_float.x, &accel_float.y, &accel_float.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); +} + +static void send_gyro_scaled(void) { + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, + &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); +} + static void send_gyro(void) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); @@ -64,12 +67,30 @@ static void send_gyro(void) { } #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); +} + +static void send_mag_scaled(void) { + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, + &imu.mag.x, &imu.mag.y, &imu.mag.z); +} + 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); } + +// TODO this could be a special module ? +#include "subsystems/electrical.h" +static void send_mag_calib(void) { + DOWNLINK_SEND_IMU_MAG_CURRENT_CALIBRATION(DefaultChannel, DefaultDevice, + &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z, + &electrical.current); +} #endif struct Imu imu; @@ -103,14 +124,16 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") 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_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); + register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); + register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #ifdef USE_MAGNETOMETER + register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); + 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 imu_impl_init(); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 0af2ea4569..fcfe5ddea1 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -36,6 +36,9 @@ #include "generated/airframe.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + #ifdef SITL #include #define DBG_LEVEL 1 @@ -237,7 +240,36 @@ 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); +static void send_hff(void) { + DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice, + &b2_hff_state.x, + &b2_hff_state.y, + &b2_hff_state.xdot, + &b2_hff_state.ydot, + &b2_hff_state.xdotdot, + &b2_hff_state.ydotdot); +} +static void send_hff_debug(void) { + DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice, + &b2_hff_x_meas, + &b2_hff_y_meas, + &b2_hff_xd_meas, + &b2_hff_yd_meas, + &b2_hff_state.xP[0][0], + &b2_hff_state.yP[0][0], + &b2_hff_state.xP[1][1], + &b2_hff_state.yP[1][1]); +} + +#ifdef GPS_LAG +static void send_hff_gps(void) { + DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice, + &(b2_hff_rb_last->lag_counter), + &lag_counter_err, + &save_counter); +} +#endif void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { Rgps_pos = HFF_R_POS; @@ -278,6 +310,12 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { b2_hff_ps_counter = 1; b2_hff_lost_counter = 0; b2_hff_lost_limit = HFF_LOST_LIMIT; + + 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 } 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 3dd2985273..90e3d9fe89 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -47,11 +47,13 @@ #include #endif - #include "math/pprz_geodetic_int.h" #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") @@ -84,6 +86,26 @@ struct NedCoor_i ins_ltp_pos; struct NedCoor_i ins_ltp_speed; struct NedCoor_i ins_ltp_accel; +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); +} + +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); +} + +static void send_ins_ref(void) { + if (ins_ltp_initialised) { + 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); + } +} void ins_init() { #if USE_INS_NAV_INIT @@ -124,6 +146,9 @@ void ins_init() { // TODO correct init ins.status = INS_RUNNING; + 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); } 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 48d96f0b09..b30a234baa 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -39,6 +39,9 @@ #include "subsystems/datalink/downlink.h" #endif +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + /* X = [ z zdot accel_bias baro_offset ] @@ -71,6 +74,13 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; float vff_z_meas; float vff_z_meas_baro; +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); +} + void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) { vff_z = init_z; vff_zdot = init_zdot; @@ -83,6 +93,7 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b vff_P[i][i] = INIT_PXX; } + register_periodic_telemetry(DefaultPeriodic, "VFF_EXTENDED", send_vffe); } @@ -134,7 +145,7 @@ void vff_propagate(float accel) { vff_P[3][3] = FPF33 + Qoffoff; #if DEBUG_VFF_EXTENDED - RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset)); + RunOnceEvery(10, send_vffe()); #endif } diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c index 54bf0c612b..d8b2a87015 100644 --- a/sw/airborne/subsystems/ins/vf_float.c +++ b/sw/airborne/subsystems/ins/vf_float.c @@ -28,6 +28,9 @@ #include "subsystems/ins/vf_float.h" +#include "subsystems/datalink/downlink.h" +#include "generated/periodic_telemetry.h" + /* X = [ z zdot bias ] @@ -66,6 +69,12 @@ float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE]; float vff_z_meas; +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]); +} + void vff_init(float init_z, float init_zdot, float init_bias) { vff_z = init_z; vff_zdot = init_zdot; @@ -77,6 +86,7 @@ void vff_init(float init_z, float init_zdot, float init_bias) { vff_P[i][i] = VF_FLOAT_INIT_PXX; } + register_periodic_telemetry(DefaultPeriodic, "VFF", send_vff); }