From 18567fcdaf9d765420fa2ba7fc618b57ccb7c50c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 30 Jan 2012 14:46:37 +0100 Subject: [PATCH] rotorcraft: minor cleanup * TelemetryPeriodic not needed anymore * there is no TELEMETRY_STARTUP_DELAY anymore --- sw/airborne/firmwares/rotorcraft/telemetry.h | 740 +++++++++---------- 1 file changed, 367 insertions(+), 373 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index f20b5e103d..8d41af42a1 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -29,6 +29,7 @@ #include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" +#include "generated/periodic.h" #ifdef RADIO_CONTROL #include "subsystems/radio_control.h" @@ -39,12 +40,7 @@ #include "firmwares/rotorcraft/actuators.h" -#ifndef TELEMETRY_STARTUP_DELAY -#define TELEMETRY_STARTUP_DELAY 0 -#endif - -#define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM) - +#include "mcu_periph/sys_time.h" #include "subsystems/electrical.h" #include "subsystems/imu.h" #if USE_GPS @@ -57,24 +53,26 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; +#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; \ 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, \ - &cpu_time_sec \ - ); \ + &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, \ + &sys_time.nb_sec \ + ); \ } #else /* !USE_GPS */ #define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \ @@ -82,19 +80,19 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; uint8_t twi_blmc_nb_err = 0; \ uint8_t fix = GPS_FIX_NONE; \ 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, \ - &cpu_time_sec \ - ); \ + &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, \ + &sys_time.nb_sec \ + ); \ } #endif /* USE_GPS */ @@ -103,20 +101,20 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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) \ +#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);} +#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) {} @@ -138,46 +136,48 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_IMU_GYRO_SCALED(_trans, _dev) { \ DOWNLINK_SEND_IMU_GYRO_SCALED(_trans, _dev, \ - &imu.gyro.p, \ - &imu.gyro.q, \ - &imu.gyro.r); \ + &imu.gyro.p, \ + &imu.gyro.q, \ + &imu.gyro.r); \ } #define PERIODIC_SEND_IMU_ACCEL_SCALED(_trans, _dev) { \ DOWNLINK_SEND_IMU_ACCEL_SCALED(_trans, _dev, \ - &imu.accel.x, \ - &imu.accel.y, \ - &imu.accel.z); \ + &imu.accel.x, \ + &imu.accel.y, \ + &imu.accel.z); \ } #define PERIODIC_SEND_IMU_MAG_SCALED(_trans, _dev) { \ DOWNLINK_SEND_IMU_MAG_SCALED(_trans, _dev, \ - &imu.mag.x, \ - &imu.mag.y, \ - &imu.mag.z); \ + &imu.mag.x, \ + &imu.mag.y, \ + &imu.mag.z); \ } #define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { \ DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, \ - &imu.gyro_unscaled.p, \ - &imu.gyro_unscaled.q, \ - &imu.gyro_unscaled.r); \ + &imu.gyro_unscaled.p, \ + &imu.gyro_unscaled.q, \ + &imu.gyro_unscaled.r); \ } #define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) { \ DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev, \ - &imu.accel_unscaled.x, \ - &imu.accel_unscaled.y, \ - &imu.accel_unscaled.z); \ + &imu.accel_unscaled.x, \ + &imu.accel_unscaled.y, \ + &imu.accel_unscaled.z); \ } #define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) { \ DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev, \ - &imu.mag_unscaled.x, \ - &imu.mag_unscaled.y, \ - &imu.mag_unscaled.z); \ + &imu.mag_unscaled.x, \ + &imu.mag_unscaled.y, \ + &imu.mag_unscaled.z); \ } + +#include "subsystems/sensors/baro.h" #define PERIODIC_SEND_BARO_RAW(_trans, _dev) { \ DOWNLINK_SEND_BARO_RAW(_trans, _dev, \ &baro.absolute, \ @@ -213,87 +213,87 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #ifdef STABILISATION_ATTITUDE_TYPE_INT #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev, \ - &ahrs.body_rate.p, \ - &ahrs.body_rate.q, \ - &ahrs.body_rate.r, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.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]); \ + &ahrs.body_rate.p, \ + &ahrs.body_rate.q, \ + &ahrs.body_rate.r, \ + &ahrs.ltp_to_body_euler.phi, \ + &ahrs.ltp_to_body_euler.theta, \ + &ahrs.ltp_to_body_euler.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); \ + &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 /* STABILISATION_ATTITUDE_TYPE_INT */ #ifdef STABILISATION_ATTITUDE_TYPE_FLOAT #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \ - &ahrs_float.body_rate.p, \ - &ahrs_float.body_rate.q, \ - &ahrs_float.body_rate.r, \ - &ahrs_float.ltp_to_body_euler.phi, \ - &ahrs_float.ltp_to_body_euler.theta, \ - &ahrs_float.ltp_to_body_euler.psi, \ - &stab_att_ref_euler.phi, \ - &stab_att_ref_euler.theta, \ - &stab_att_ref_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]); \ + &ahrs_float.body_rate.p, \ + &ahrs_float.body_rate.q, \ + &ahrs_float.body_rate.r, \ + &ahrs_float.ltp_to_body_euler.phi, \ + &ahrs_float.ltp_to_body_euler.theta, \ + &ahrs_float.ltp_to_body_euler.psi, \ + &stab_att_ref_euler.phi, \ + &stab_att_ref_euler.theta, \ + &stab_att_ref_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_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); \ + &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 /* STABILISATION_ATTITUDE_TYPE_FLOAT */ @@ -302,23 +302,23 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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.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); \ } #define PERIODIC_SEND_BOOZ2_CMD(_trans, _dev) { \ DOWNLINK_SEND_BOOZ2_CMD(_trans, _dev, \ - &stabilization_cmd[COMMAND_ROLL], \ - &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW], \ - &stabilization_cmd[COMMAND_THRUST]); \ + &stabilization_cmd[COMMAND_ROLL], \ + &stabilization_cmd[COMMAND_PITCH], \ + &stabilization_cmd[COMMAND_YAW], \ + &stabilization_cmd[COMMAND_THRUST]); \ } @@ -326,21 +326,21 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #include "subsystems/ahrs/ahrs_int_cmpl_euler.h" #define PERIODIC_SEND_FILTER(_trans, _dev) { \ DOWNLINK_SEND_FILTER(_trans, _dev, \ - &ahrs.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.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); \ + &ahrs.ltp_to_imu_euler.phi, \ + &ahrs.ltp_to_imu_euler.theta, \ + &ahrs.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) {} @@ -351,60 +351,60 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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); \ + _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_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_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); \ +#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) {} @@ -416,58 +416,58 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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, \ - &ahrs.ltp_to_body_quat.qi, \ - &ahrs.ltp_to_body_quat.qx, \ - &ahrs.ltp_to_body_quat.qy, \ - &ahrs.ltp_to_body_quat.qz); \ + &stab_att_ref_quat.qi, \ + &stab_att_ref_quat.qx, \ + &stab_att_ref_quat.qy, \ + &stab_att_ref_quat.qz, \ + &ahrs.ltp_to_body_quat.qi, \ + &ahrs.ltp_to_body_quat.qx, \ + &ahrs.ltp_to_body_quat.qy, \ + &ahrs.ltp_to_body_quat.qz); \ } #define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) { \ DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev, \ - &ahrs.ltp_to_imu_quat.qi, \ - &ahrs.ltp_to_imu_quat.qx, \ - &ahrs.ltp_to_imu_quat.qy, \ - &ahrs.ltp_to_imu_quat.qz, \ - &ahrs.ltp_to_body_quat.qi, \ - &ahrs.ltp_to_body_quat.qx, \ - &ahrs.ltp_to_body_quat.qy, \ - &ahrs.ltp_to_body_quat.qz); \ + &ahrs.ltp_to_imu_quat.qi, \ + &ahrs.ltp_to_imu_quat.qx, \ + &ahrs.ltp_to_imu_quat.qy, \ + &ahrs.ltp_to_imu_quat.qz, \ + &ahrs.ltp_to_body_quat.qi, \ + &ahrs.ltp_to_body_quat.qx, \ + &ahrs.ltp_to_body_quat.qy, \ + &ahrs.ltp_to_body_quat.qz); \ } #define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) { \ DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev, \ - &ahrs.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.ltp_to_imu_euler.psi, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi); \ + &ahrs.ltp_to_imu_euler.phi, \ + &ahrs.ltp_to_imu_euler.theta, \ + &ahrs.ltp_to_imu_euler.psi, \ + &ahrs.ltp_to_body_euler.phi, \ + &ahrs.ltp_to_body_euler.theta, \ + &ahrs.ltp_to_body_euler.psi); \ } #define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) { \ DOWNLINK_SEND_AHRS_RMAT(_trans, _dev, \ - &ahrs.ltp_to_imu_rmat.m[0], \ - &ahrs.ltp_to_imu_rmat.m[1], \ - &ahrs.ltp_to_imu_rmat.m[2], \ - &ahrs.ltp_to_imu_rmat.m[3], \ - &ahrs.ltp_to_imu_rmat.m[4], \ - &ahrs.ltp_to_imu_rmat.m[5], \ - &ahrs.ltp_to_imu_rmat.m[6], \ - &ahrs.ltp_to_imu_rmat.m[7], \ - &ahrs.ltp_to_imu_rmat.m[8], \ - &ahrs.ltp_to_body_rmat.m[0], \ - &ahrs.ltp_to_body_rmat.m[1], \ - &ahrs.ltp_to_body_rmat.m[2], \ - &ahrs.ltp_to_body_rmat.m[3], \ - &ahrs.ltp_to_body_rmat.m[4], \ - &ahrs.ltp_to_body_rmat.m[5], \ - &ahrs.ltp_to_body_rmat.m[6], \ - &ahrs.ltp_to_body_rmat.m[7], \ - &ahrs.ltp_to_body_rmat.m[8]); \ + &ahrs.ltp_to_imu_rmat.m[0], \ + &ahrs.ltp_to_imu_rmat.m[1], \ + &ahrs.ltp_to_imu_rmat.m[2], \ + &ahrs.ltp_to_imu_rmat.m[3], \ + &ahrs.ltp_to_imu_rmat.m[4], \ + &ahrs.ltp_to_imu_rmat.m[5], \ + &ahrs.ltp_to_imu_rmat.m[6], \ + &ahrs.ltp_to_imu_rmat.m[7], \ + &ahrs.ltp_to_imu_rmat.m[8], \ + &ahrs.ltp_to_body_rmat.m[0], \ + &ahrs.ltp_to_body_rmat.m[1], \ + &ahrs.ltp_to_body_rmat.m[2], \ + &ahrs.ltp_to_body_rmat.m[3], \ + &ahrs.ltp_to_body_rmat.m[4], \ + &ahrs.ltp_to_body_rmat.m[5], \ + &ahrs.ltp_to_body_rmat.m[6], \ + &ahrs.ltp_to_body_rmat.m[7], \ + &ahrs.ltp_to_body_rmat.m[8]); \ } @@ -476,13 +476,13 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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]); \ + &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) {} @@ -500,7 +500,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &b2_hff_state.ydotdot); \ } #define PERIODIC_SEND_HFF_DBG(_trans, _dev) { \ - DOWNLINK_SEND_HFF_DBG(_trans, _dev, \ + DOWNLINK_SEND_HFF_DBG(_trans, _dev, \ &b2_hff_x_meas, \ &b2_hff_y_meas, \ &b2_hff_xd_meas, \ @@ -513,9 +513,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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); \ + &b2_hff_rb_last->lag_counter, \ + &lag_counter_err, \ + &save_counter); \ } #else #define PERIODIC_SEND_HFF_GPS(_trans, _dev) {} @@ -528,87 +528,87 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_GUIDANCE(_trans, _dev) { \ DOWNLINK_SEND_GUIDANCE(_trans, _dev, \ - &guidance_h_cur_pos.x, \ - &guidance_h_cur_pos.y, \ - &guidance_h_held_pos.x, \ - &guidance_h_held_pos.y); \ + &guidance_h_cur_pos.x, \ + &guidance_h_cur_pos.y, \ + &guidance_h_held_pos.x, \ + &guidance_h_held_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); \ + &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); \ + &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) { \ 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); \ + &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); \ + &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); \ + &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) { \ @@ -627,20 +627,20 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) { \ int32_t carrot_up = -guidance_v_z_sp; \ DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev, \ - &ins_enu_pos.x, \ - &ins_enu_pos.y, \ - &ins_enu_pos.z, \ - &ins_enu_speed.x, \ - &ins_enu_speed.y, \ - &ins_enu_speed.z, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi, \ - &guidance_h_pos_sp.y, \ - &guidance_h_pos_sp.x, \ - &carrot_up, \ - &guidance_h_command_body.psi, \ - &stabilization_cmd[COMMAND_THRUST], \ + &ins_enu_pos.x, \ + &ins_enu_pos.y, \ + &ins_enu_pos.z, \ + &ins_enu_speed.x, \ + &ins_enu_speed.y, \ + &ins_enu_speed.z, \ + &ahrs.ltp_to_body_euler.phi, \ + &ahrs.ltp_to_body_euler.theta, \ + &ahrs.ltp_to_body_euler.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); \ } @@ -679,11 +679,11 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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); \ + &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); \ @@ -703,10 +703,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; 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)); \ + &i, \ + &(waypoints[i].x), \ + &(waypoints[i].y), \ + &(waypoints[i].z)); \ } #if USE_CAM @@ -715,34 +715,34 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) {} #endif -#define PERIODIC_SEND_BOOZ2_TUNE_HOVER(_trans, _dev) { \ - DOWNLINK_SEND_BOOZ2_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.ltp_to_imu_euler.phi, \ - &ahrs.ltp_to_imu_euler.theta, \ - &ahrs.ltp_to_imu_euler.psi, \ - &ahrs.ltp_to_body_euler.phi, \ - &ahrs.ltp_to_body_euler.theta, \ - &ahrs.ltp_to_body_euler.psi \ - ); \ +#define PERIODIC_SEND_BOOZ2_TUNE_HOVER(_trans, _dev) { \ + DOWNLINK_SEND_BOOZ2_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.ltp_to_imu_euler.phi, \ + &ahrs.ltp_to_imu_euler.theta, \ + &ahrs.ltp_to_imu_euler.psi, \ + &ahrs.ltp_to_body_euler.phi, \ + &ahrs.ltp_to_body_euler.theta, \ + &ahrs.ltp_to_body_euler.psi \ + ); \ } -#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \ - DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ - &i2c_errc_ack_fail, \ - &i2c_errc_miss_start_stop, \ - &i2c_errc_arb_lost, \ - &i2c_errc_over_under, \ - &i2c_errc_pec_recep, \ - &i2c_errc_timeout_tlow, \ - &i2c_errc_smbus_alert \ - ); \ +#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \ + DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c_errc_ack_fail, \ + &i2c_errc_miss_start_stop, \ + &i2c_errc_arb_lost, \ + &i2c_errc_over_under, \ + &i2c_errc_pec_recep, \ + &i2c_errc_timeout_tlow, \ + &i2c_errc_smbus_alert \ + ); \ } //TODO replace by BOOZ_EXTRA_ADC @@ -771,10 +771,4 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #include "generated/settings.h" #define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev) -#include "generated/periodic.h" -#define TelemetryPeriodic() { \ - PeriodicSendMain(DefaultChannel,DefaultDevice); \ - } - - #endif /* TELEMETRY_H */