Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[messages] add comp_id to AHRS messages

Browse Source 
To be able to distinguish from which implementation/component the messages was set (needed if running multiple AHRS)
see #1145

Still need possibility to register a message multiple times (from different modules).
This commit is contained in:
Felix Ruess
2015-11-23 22:50:21 +01:00
parent 8a51abbebb
commit 8269b6ebeb
13 changed files with 64 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+5
View File
@@ -1363,6 +1363,7 @@
<field name="body_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="body_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="body_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="comp_id" type="uint8" values="NONE|GENERIC|IR|ICQ|ICE|FC|DCM|FINV|MLKF|GX3"/>
</message>
<message name="AHRS_QUAT_INT" id="157">
@@ -1375,6 +1376,7 @@
<field name="body_qx" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="body_qy" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="body_qz" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="comp_id" type="uint8" values="NONE|GENERIC|IR|ICQ|ICE|FC|DCM|FINV|MLKF|GX3"/>
</message>
<message name="AHRS_RMAT_INT" id="158">
@@ -1396,6 +1398,7 @@
<field name="body_m20" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
<field name="body_m21" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
<field name="body_m22" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
<field name="comp_id" type="uint8" values="NONE|GENERIC|IR|ICQ|ICE|FC|DCM|FINV|MLKF|GX3"/>
</message>
<message name="ROTORCRAFT_NAV_STATUS" id="159">
@@ -1534,6 +1537,7 @@
<field name="phi" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578" />
<field name="theta" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578" />
<field name="psi" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578" />
<field name="comp_id" type="uint8" values="NONE|GENERIC|IR|ICQ|ICE|FC|DCM|FINV|MLKF|GX3"/>
</message>
<message name="AHRS_MEASUREMENT_EULER" id="174">
@@ -1563,6 +1567,7 @@
<field name="bp" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
<field name="bq" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
<field name="br" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
<field name="comp_id" type="uint8" values="NONE|GENERIC|IR|ICQ|ICE|FC|DCM|FINV|MLKF|GX3"/>
</message>
sw/airborne/modules/ins/ahrs_chimu_uart.c
+7 -2
View File
@@ -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
}
sw/airborne/modules/ins/ins_arduimu.c
+2 -1
View File
@@ -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));
}
sw/airborne/modules/ins/ins_arduimu_basic.c
+2 -1
View File
@@ -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,
sw/airborne/subsystems/ahrs.h
+12
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+4 -7
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+2 -6
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c
+4 -7
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+2 -6
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
+8 -9
View File
@@ -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
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+8 -9
View File
@@ -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)
&ltp_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
sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
+6 -6
View File
@@ -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),
sw/logalizer/ahrs2fg.c
+2 -2
View File
@@ -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);