diff --git a/conf/messages.xml b/conf/messages.xml index 2b6a26a1bd..ba9b5b9853 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1363,6 +1363,7 @@ + @@ -1375,6 +1376,7 @@ + @@ -1396,6 +1398,7 @@ + @@ -1534,6 +1537,7 @@ + @@ -1563,6 +1567,7 @@ + diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index c97ca4fdd9..7d5d83f6df 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -30,6 +30,8 @@ CHIMU_PARSER_DATA CHIMU_DATA; INS_FORMAT ins_roll_neutral; INS_FORMAT ins_pitch_neutral; +static uint8_t ahrs_chimu_id = AHRS_COMP_ID_CHIMU; + struct AhrsChimu ahrs_chimu; static bool_t ahrs_chimu_enable_output(bool_t enable) @@ -104,8 +106,11 @@ void parse_ins_msg(void) } #if CHIMU_DOWNLINK_IMMEDIATE - DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, - &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); + DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, + &CHIMU_DATA.m_attitude.euler.phi, + &CHIMU_DATA.m_attitude.euler.theta, + &CHIMU_DATA.m_attitude.euler.psi, + &ahrs_chimu_id); #endif } diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index d5eb2bc12f..043a88f257 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -215,6 +215,7 @@ void IMU_Daten_verarbeiten(void) att.psi = 0.; imu_daten_angefordert = FALSE; stateSetNedToBodyEulers_f(&att); + uint8_t arduimu_id = 102; - RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi)); + RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi, &arduimu_id)); } diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 85b7c2a6ba..9303dfd4b4 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -194,7 +194,8 @@ void ArduIMU_event(void) ardu_ins_trans.status = I2CTransDone; #ifdef ARDUIMU_SYNC_SEND - //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); + // uint8_t arduimu_id = 102; + //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 1675c4a512..617bd0b377 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -29,6 +29,18 @@ #include "std.h" +#define AHRS_COMP_ID_NONE 0 +#define AHRS_COMP_ID_GENERIC 1 +#define AHRS_COMP_ID_IR 2 +#define AHRS_COMP_ID_ICQ 3 +#define AHRS_COMP_ID_ICE 4 +#define AHRS_COMP_ID_FC 4 +#define AHRS_COMP_ID_DCM 6 +#define AHRS_COMP_ID_FINV 7 +#define AHRS_COMP_ID_MLKF 8 +#define AHRS_COMP_ID_GX3 9 +#define AHRS_COMP_ID_CHIMU 10 + /* include actual (primary) implementation header */ #ifdef AHRS_TYPE_H #include AHRS_TYPE_H diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c index 28a2de9dc3..ce962c049a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -37,6 +37,7 @@ PRINT_CONFIG_VAR(AHRS_FC_OUTPUT_ENABLED) /** if TRUE with push the estimation results to the state interface */ static bool_t ahrs_fc_output_enabled; static uint32_t ahrs_fc_last_stamp; +static uint8_t ahrs_fc_id = AHRS_COMP_ID_FC; static void compute_body_orientation_and_rates(void); @@ -58,7 +59,8 @@ static void send_att(struct transport_tx *trans, struct link_device *dev) &euler_i.psi, &(eulers_body->phi), &(eulers_body->theta), - &(eulers_body->psi)); + &(eulers_body->psi) + &ahrs_fc_id); } static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) @@ -67,20 +69,15 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); } -#ifndef AHRS_FC_FILTER_ID -#define AHRS_FC_FILTER_ID 5 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_FC_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_fc.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_fc_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_fc_id, &mde, &val); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c index d7e4a11226..b735c2b014 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c @@ -37,6 +37,7 @@ PRINT_CONFIG_VAR(AHRS_DCM_OUTPUT_ENABLED) /** if TRUE with push the estimation results to the state interface */ static bool_t ahrs_dcm_output_enabled; static uint32_t ahrs_dcm_last_stamp; +static uint8_t ahrs_dcm_id = AHRS_COMP_ID_DCM; static void set_body_orientation_and_rates(void); @@ -44,20 +45,15 @@ static void set_body_orientation_and_rates(void); #include "subsystems/datalink/telemetry.h" #include "mcu_periph/sys_time.h" -#ifndef AHRS_DCM_FILTER_ID -#define AHRS_DCM_FILTER_ID 6 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_DCM_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_dcm.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_dcm_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_dcm_id, &mde, &val); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c index 1ce0b5adaf..e8eebe41ee 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c @@ -41,6 +41,7 @@ PRINT_CONFIG_VAR(AHRS_INV_OUTPUT_ENABLED) static bool_t ahrs_finv_output_enabled; /** last gyro msg timestamp */ static uint32_t ahrs_finv_last_stamp = 0; +static uint8_t ahrs_finv_id = AHRS_COMP_ID_FINV; static void compute_body_orientation_and_rates(void); @@ -60,7 +61,8 @@ static void send_att(struct transport_tx *trans, struct link_device *dev) &euler_i.psi, &(eulers_body->phi), &(eulers_body->theta), - &(eulers_body->psi)); + &(eulers_body->psi), + &ahrs_finv_id); } static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) @@ -71,20 +73,15 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) &ahrs_float_inv.mag_h.z); } -#ifndef AHRS_FINV_FILTER_ID -#define AHRS_FINV_FILTER_ID 7 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_FINV_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_float_inv.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_finv_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_finv_id, &mde, &val); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c index 409ea5614d..b53983895a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -38,6 +38,7 @@ PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED) /** if TRUE with push the estimation results to the state interface */ static bool_t ahrs_mlkf_output_enabled; static uint32_t ahrs_mlkf_last_stamp; +static uint8_t ahrs_mlkf_id = AHRS_COMP_ID_MLKF; static void set_body_state_from_quat(void); @@ -51,20 +52,15 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); } -#ifndef AHRS_MLKF_FILTER_ID -#define AHRS_MLKF_FILTER_ID 6 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_MLKF_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_mlkf.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_mlkf_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_mlkf_id, &mde, &val); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c index 772ffb5b0c..4a5c7c643b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c @@ -37,6 +37,7 @@ PRINT_CONFIG_VAR(AHRS_ICE_OUTPUT_ENABLED) /** if TRUE with push the estimation results to the state interface */ static bool_t ahrs_ice_output_enabled; static uint32_t ahrs_ice_last_stamp; +static uint8_t ahrs_ice_id = AHRS_COMP_ID_ICE; static void set_body_state_from_euler(void); @@ -62,7 +63,8 @@ static void send_filter(struct transport_tx *trans, struct link_device *dev) &ahrs_ice.residual.psi, &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, - &ahrs_ice.gyro_bias.r); + &ahrs_ice.gyro_bias.r, + &ahrs_ice_id); } static void send_euler(struct transport_tx *trans, struct link_device *dev) @@ -74,29 +76,26 @@ static void send_euler(struct transport_tx *trans, struct link_device *dev) &ahrs_ice.ltp_to_imu_euler.psi, &(eulers->phi), &(eulers->theta), - &(eulers->psi)); + &(eulers->psi), + &ahrs_ice_id); } static void send_bias(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, - &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); + &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, + &ahrs_ice.gyro_bias.r, &ahrs_ice_id); } -#ifndef AHRS_ICE_FILTER_ID -#define AHRS_ICE_FILTER_ID 4 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_ICE_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_ice.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_ice_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ice_id, &mde, &val); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c index 59ce974aa0..161b9409b6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c @@ -37,6 +37,7 @@ PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED) /** if TRUE with push the estimation results to the state interface */ static bool_t ahrs_icq_output_enabled; static uint32_t ahrs_icq_last_stamp; +static uint8_t ahrs_icq_id = AHRS_COMP_ID_ICQ; static void set_body_state_from_quat(void); @@ -57,7 +58,8 @@ static void send_quat(struct transport_tx *trans, struct link_device *dev) &(quat->qi), &(quat->qx), &(quat->qy), - &(quat->qz)); + &(quat->qz), + &ahrs_icq_id); } static void send_euler(struct transport_tx *trans, struct link_device *dev) @@ -71,13 +73,15 @@ static void send_euler(struct transport_tx *trans, struct link_device *dev) <p_to_imu_euler.psi, &(eulers->phi), &(eulers->theta), - &(eulers->psi)); + &(eulers->psi), + &ahrs_icq_id); } static void send_bias(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, - &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q, &ahrs_icq.gyro_bias.r); + &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q, + &ahrs_icq.gyro_bias.r, &ahrs_icq_id); } static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) @@ -90,20 +94,15 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) &h_float.x, &h_float.y, &h_float.z); } -#ifndef AHRS_ICQ_FILTER_ID -#define AHRS_ICQ_FILTER_ID 3 -#endif - static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { - uint8_t id = AHRS_ICQ_FILTER_ID; uint8_t mde = 3; uint16_t val = 0; if (!ahrs_icq.is_aligned) { mde = 2; } uint32_t t_diff = get_sys_time_usec() - ahrs_icq_last_stamp; /* set lost if no new gyro measurements for 50ms */ if (t_diff > 50000) { mde = 5; } - pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_icq_id, &mde, &val); } #endif diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c index 252e5c8b4a..9e21128ded 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c @@ -41,24 +41,24 @@ gboolean timeout_callback(gpointer data) #endif #if AHRS_TYPE == AHRS_TYPE_ICQ - IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d", + IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d %d", ahrs_impl.gyro_bias.p, ahrs_impl.gyro_bias.q, - ahrs_impl.gyro_bias.r); + ahrs_impl.gyro_bias.r, 1); #endif #if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 struct Int32Rates bias_i; RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias); - IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d", + IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d %d", bias_i.p, bias_i.q, - bias_i.r); + bias_i.r, 1); #endif - IvySendMsg("183 AHRS_EULER %f %f %f", + IvySendMsg("183 AHRS_EULER %f %f %f %d", ahrs_float.ltp_to_imu_euler.phi, ahrs_float.ltp_to_imu_euler.theta, - ahrs_float.ltp_to_imu_euler.psi); + ahrs_float.ltp_to_imu_euler.psi, 1); IvySendMsg("183 NPS_RATE_ATTITUDE %f %f %f %f %f %f", DegOfRad(aos.imu_rates.p), diff --git a/sw/logalizer/ahrs2fg.c b/sw/logalizer/ahrs2fg.c index 2db694fc5d..0688f5430f 100644 --- a/sw/logalizer/ahrs2fg.c +++ b/sw/logalizer/ahrs2fg.c @@ -179,8 +179,8 @@ int main ( int argc, char** argv) { IvyInit ("ahrs2fg", "ahrs2fg READY", NULL, NULL, NULL, NULL); IvyBindMsg(on_ATTITUDE, chan, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", ac_id); - IvyBindMsg(on_AHRS_EULER_INT, chan, "^%d AHRS_EULER_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); - IvyBindMsg(on_AHRS_QUAT_INT, chan, "^%d AHRS_QUAT_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); + IvyBindMsg(on_AHRS_EULER_INT, chan, "^%d AHRS_EULER_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); + IvyBindMsg(on_AHRS_QUAT_INT, chan, "^%d AHRS_QUAT_INT (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); IvyBindMsg(on_ROTORCRAFT_FP, chan, "^%d ROTORCRAFT_FP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", ac_id); if (verbose) { printf("Ivy options: %s\n", IvyBus);