Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-02 13:27:32 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[abi] update ABI bindings to pass single types by value

Browse Source
This commit is contained in:
Felix Ruess
2015-02-05 18:44:10 +01:00
parent 12e256dfa7
commit 9f0240d893
13 changed files with 130 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+7 -7
View File
@@ -22,28 +22,28 @@
<message name="IMU_GYRO_INT32" id="4"> <message name="IMU_GYRO_INT32" id="4">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates"/> <field name="gyro" type="struct Int32Rates *"/>
</message> </message>
<message name="IMU_ACCEL_INT32" id="5"> <message name="IMU_ACCEL_INT32" id="5">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="accel" type="struct Int32Vect3"/> <field name="accel" type="struct Int32Vect3 *"/>
</message> </message>
<message name="IMU_MAG_INT32" id="6"> <message name="IMU_MAG_INT32" id="6">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="mag" type="struct Int32Vect3"/> <field name="mag" type="struct Int32Vect3 *"/>
</message> </message>
<message name="IMU_LOWPASSED" id="7"> <message name="IMU_LOWPASSED" id="7">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates"/> <field name="gyro" type="struct Int32Rates *"/>
<field name="accel" type="struct Int32Vect3"/> <field name="accel" type="struct Int32Vect3 *"/>
<field name="mag" type="struct Int32Vect3"/> <field name="mag" type="struct Int32Vect3 *"/>
</message> </message>
<message name="BODY_TO_IMU_QUAT" id="8"> <message name="BODY_TO_IMU_QUAT" id="8">
<field name="q_b2i_f" type="struct FloatQuat"/> <field name="q_b2i_f" type="struct FloatQuat *"/>
</message> </message>
</msg_class> </msg_class>
sw/airborne/firmwares/fixedwing/main_ap.c
+3 -3
View File
@@ -766,7 +766,7 @@ static inline void on_accel_event(void)
imu_scale_accel(&imu); imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel); AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
} }
static inline void on_gyro_event(void) static inline void on_gyro_event(void)
@@ -778,7 +778,7 @@ static inline void on_gyro_event(void)
imu_scale_gyro(&imu); imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev); AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER #if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
@@ -818,7 +818,7 @@ static inline void on_mag_event(void)
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
imu_scale_mag(&imu); imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag); AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#endif #endif
} }
sw/airborne/firmwares/rotorcraft/main.c
+3 -3
View File
@@ -362,7 +362,7 @@ static inline void on_accel_event( void ) {
imu_scale_accel(&imu); imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel); AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
} }
static inline void on_gyro_event( void ) { static inline void on_gyro_event( void ) {
@@ -371,7 +371,7 @@ static inline void on_gyro_event( void ) {
imu_scale_gyro(&imu); imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev); AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER #if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
@@ -420,7 +420,7 @@ static inline void on_mag_event(void)
// current timestamp // current timestamp
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag); AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#ifdef USE_VEHICLE_INTERFACE #ifdef USE_VEHICLE_INTERFACE
vi_notify_mag_available(); vi_notify_mag_available();
sw/airborne/modules/sensors/mag_hmc58xx.c
+1 -1
View File
@@ -78,7 +78,7 @@ void mag_hmc58xx_module_event(void)
// scale vector // scale vector
imu_scale_mag(&imu); imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, &now_ts, &imu.mag); AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
#endif #endif
#if MODULE_HMC58XX_SYNC_SEND #if MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx_report(); mag_hmc58xx_report();
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+1 -1
View File
@@ -136,7 +136,7 @@ void ahrs_aligner_run(void)
LED_ON(AHRS_ALIGNER_LED); LED_ON(AHRS_ALIGNER_LED);
#endif #endif
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, &now_ts, &ahrs_aligner.lp_gyro, AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro,
&ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
} }
} }
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+22 -23
View File
@@ -69,8 +69,8 @@ static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void gyro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
@@ -78,31 +78,31 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *s
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) { if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_propagate((struct Int32Rates *)gyro, dt); ahrs_fc_propagate(gyro, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.") PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_fc.status == AHRS_FC_RUNNING) { if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_fc_propagate((struct Int32Rates *)gyro, dt); ahrs_fc_propagate(gyro, dt);
} }
#endif #endif
} }
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void accel_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Vect3 *accel) struct Int32Vect3 *accel)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.") PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) { if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt); ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.") PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
@@ -113,42 +113,41 @@ static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *
#endif #endif
} }
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void mag_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.") PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) { if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_update_mag((struct Int32Vect3 *)mag, dt); ahrs_fc_update_mag(mag, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.") PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
if (ahrs_fc.is_aligned) { if (ahrs_fc.is_aligned) {
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
ahrs_fc_update_mag((struct Int32Vect3 *)mag, dt); ahrs_fc_update_mag(mag, dt);
} }
#endif #endif
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_fc.is_aligned) { if (!ahrs_fc.is_aligned) {
ahrs_fc_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ahrs_fc_align(lp_gyro, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
const struct FloatQuat *q_b2i_f) struct FloatQuat *q_b2i_f)
{ {
ahrs_fc_set_body_to_imu_quat((struct FloatQuat *)q_b2i_f); ahrs_fc_set_body_to_imu_quat(q_b2i_f);
} }
void ahrs_fc_register(void) void ahrs_fc_register(void)
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+18 -19
View File
@@ -42,61 +42,60 @@ static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void gyro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
/* timestamp in usec when last callback was received */ /* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_dcm.is_aligned) { if (last_stamp > 0 && ahrs_dcm.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_dcm_propagate((struct Int32Rates *)gyro, dt); ahrs_dcm_propagate(gyro, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.") PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_dcm.is_aligned) { if (ahrs_dcm.is_aligned) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_dcm_propagate((struct Int32Rates *)gyro, dt); ahrs_dcm_propagate(gyro, dt);
} }
#endif #endif
} }
static void accel_cb(uint8_t sender_id __attribute__((unused)), static void accel_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *accel) struct Int32Vect3 *accel)
{ {
if (ahrs_dcm.is_aligned) { if (ahrs_dcm.is_aligned) {
ahrs_dcm_update_accel((struct Int32Vect3 *)accel); ahrs_dcm_update_accel(accel);
} }
} }
static void mag_cb(uint8_t sender_id __attribute__((unused)), static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
if (ahrs_dcm.is_aligned) { if (ahrs_dcm.is_aligned) {
ahrs_dcm_update_mag((struct Int32Vect3 *)mag); ahrs_dcm_update_mag(mag);
} }
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_dcm.is_aligned) { if (!ahrs_dcm.is_aligned) {
ahrs_dcm_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ahrs_dcm_align(lp_gyro, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
const struct FloatQuat *q_b2i_f) struct FloatQuat *q_b2i_f)
{ {
ahrs_dcm_set_body_to_imu_quat((struct FloatQuat *)q_b2i_f); ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
} }
void ahrs_dcm_register(void) void ahrs_dcm_register(void)
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+18 -19
View File
@@ -53,8 +53,8 @@ static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void gyro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
@@ -62,53 +62,52 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *s
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_mlkf.is_aligned) { if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_mlkf_propagate((struct Int32Rates *)gyro, dt); ahrs_mlkf_propagate(gyro, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.") PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) { if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_mlkf_propagate((struct Int32Rates *)gyro, dt); ahrs_mlkf_propagate(gyro, dt);
} }
#endif #endif
} }
static void accel_cb(uint8_t sender_id __attribute__((unused)), static void accel_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *accel) struct Int32Vect3 *accel)
{ {
if (ahrs_mlkf.is_aligned) { if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_accel((struct Int32Vect3 *)accel); ahrs_mlkf_update_accel(accel);
} }
} }
static void mag_cb(uint8_t sender_id __attribute__((unused)), static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
if (ahrs_mlkf.is_aligned) { if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_mag((struct Int32Vect3 *)mag); ahrs_mlkf_update_mag(mag);
} }
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_mlkf.is_aligned) { if (!ahrs_mlkf.is_aligned) {
ahrs_mlkf_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ahrs_mlkf_align(lp_accel, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
const struct FloatQuat *q_b2i_f) struct FloatQuat *q_b2i_f)
{ {
ahrs_mlkf_set_body_to_imu_quat((struct FloatQuat *)q_b2i_f); ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
} }
void ahrs_mlkf_register(void) void ahrs_mlkf_register(void)
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+3 -3
View File
@@ -51,10 +51,10 @@ float heading;
static abi_event gyro_ev; static abi_event gyro_ev;
static void gyro_cb(uint8_t sender_id __attribute__((unused)), static void gyro_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
stateSetBodyRates_i((struct Int32Rates *)gyro); stateSetBodyRates_i(gyro);
} }
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
+15 -16
View File
@@ -85,48 +85,47 @@ static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t sender_id __attribute__((unused)), static void gyro_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
if (ahrs_ice.is_aligned) { if (ahrs_ice.is_aligned) {
ahrs_ice_propagate((struct Int32Rates *)gyro); ahrs_ice_propagate(gyro);
} }
} }
static void accel_cb(uint8_t sender_id __attribute__((unused)), static void accel_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *accel) struct Int32Vect3 *accel)
{ {
if (ahrs_ice.is_aligned) { if (ahrs_ice.is_aligned) {
ahrs_ice_update_accel((struct Int32Vect3 *)accel); ahrs_ice_update_accel(accel);
} }
} }
static void mag_cb(uint8_t sender_id __attribute__((unused)), static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
if (ahrs_ice.is_aligned) { if (ahrs_ice.is_aligned) {
ahrs_ice_update_mag((struct Int32Vect3 *)mag); ahrs_ice_update_mag(mag);
} }
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_ice.is_aligned) { if (!ahrs_ice.is_aligned) {
ahrs_ice_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ahrs_ice_align(lp_gyro, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
const struct FloatQuat *q_b2i_f) struct FloatQuat *q_b2i_f)
{ {
ahrs_ice_set_body_to_imu_quat((struct FloatQuat *)q_b2i_f); ahrs_ice_set_body_to_imu_quat(q_b2i_f);
} }
void ahrs_ice_register(void) void ahrs_ice_register(void)
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+24 -25
View File
@@ -91,8 +91,8 @@ static abi_event aligner_ev;
static abi_event body_to_imu_ev; static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void gyro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Rates *gyro) struct Int32Rates *gyro)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.") PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.")
@@ -100,77 +100,76 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *s
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.is_aligned) { if (last_stamp > 0 && ahrs_icq.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_icq_propagate((struct Int32Rates *)gyro, dt); ahrs_icq_propagate(gyro, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.") PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_icq.status == AHRS_ICQ_RUNNING) { if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_icq_propagate((struct Int32Rates *)gyro, dt); ahrs_icq_propagate(gyro, dt);
} }
#endif #endif
} }
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void accel_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Vect3 *accel) struct Int32Vect3 *accel)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.") PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.is_aligned) { if (last_stamp > 0 && ahrs_icq.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_icq_update_accel((struct Int32Vect3 *)accel, dt); ahrs_icq_update_accel(accel, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.") PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
if (ahrs_icq.is_aligned) { if (ahrs_icq.is_aligned) {
const float dt = 1. / (AHRS_CORRECT_FREQUENCY); const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
ahrs_icq_update_accel((struct Int32Vect3 *)accel, dt); ahrs_icq_update_accel(accel, dt);
} }
#endif #endif
} }
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void mag_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.") PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.")
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.is_aligned) { if (last_stamp > 0 && ahrs_icq.is_aligned) {
float dt = (float)(*stamp - last_stamp) * 1e-6; float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_icq_update_mag((struct Int32Vect3 *)mag, dt); ahrs_icq_update_mag(mag, dt);
} }
last_stamp = *stamp; last_stamp = stamp;
#else #else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.") PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
if (ahrs_icq.is_aligned) { if (ahrs_icq.is_aligned) {
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
ahrs_icq_update_mag((struct Int32Vect3 *)mag, dt); ahrs_icq_update_mag(mag, dt);
} }
#endif #endif
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ahrs_icq.is_aligned) { if (!ahrs_icq.is_aligned) {
ahrs_icq_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ahrs_icq_align(lp_gyro, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
const struct FloatQuat *q_b2i_f) struct FloatQuat *q_b2i_f)
{ {
ahrs_icq_set_body_to_imu_quat((struct FloatQuat *)q_b2i_f); ahrs_icq_set_body_to_imu_quat(q_b2i_f);
} }
void ahrs_icq_register(void) void ahrs_icq_register(void)
sw/airborne/subsystems/ins/ins_float_invariant.c
+12 -13
View File
@@ -186,14 +186,14 @@ static void baro_cb(uint8_t sender_id, float pressure);
#define INS_MAG_ID ABI_BROADCAST #define INS_MAG_ID ABI_BROADCAST
#endif #endif
static abi_event mag_ev; static abi_event mag_ev;
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t *stamp, static void mag_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp,
const struct Int32Vect3 *mag); struct Int32Vect3 *mag);
static abi_event aligner_ev; static abi_event aligner_ev;
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag); struct Int32Vect3 *lp_mag);
/* gps */ /* gps */
@@ -756,21 +756,20 @@ void ins_propagate(float dt)
} }
static void mag_cb(uint8_t sender_id __attribute__((unused)), static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Vect3 *mag) struct Int32Vect3 *mag)
{ {
if (ins_impl.is_aligned) { if (ins_impl.is_aligned) {
ins_float_invariant_update_mag((struct Int32Vect3 *)mag); ins_float_invariant_update_mag(mag);
} }
} }
static void aligner_cb(uint8_t __attribute__((unused)) sender_id, static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t *stamp __attribute__((unused)), uint32_t stamp __attribute__((unused)),
const struct Int32Rates *lp_gyro, const struct Int32Vect3 *lp_accel, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
const struct Int32Vect3 *lp_mag) struct Int32Vect3 *lp_mag)
{ {
if (!ins_impl.is_aligned) { if (!ins_impl.is_aligned) {
ins_float_invariant_align((struct Int32Rates *)lp_gyro, (struct Int32Vect3 *)lp_accel, ins_float_invariant_align(lp_gyro, lp_accel, lp_mag);
(struct Int32Vect3 *)lp_mag);
} }
} }
sw/airborne/test/subsystems/test_ahrs.c
+3 -3
View File
@@ -107,7 +107,7 @@ static inline void on_gyro_event(void)
imu_scale_gyro(&imu); imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev); AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER #if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) { if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
@@ -124,7 +124,7 @@ static inline void on_accel_event(void)
imu_scale_accel(&imu); imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel); AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
} }
static inline void on_mag_event(void) static inline void on_mag_event(void)
@@ -134,7 +134,7 @@ static inline void on_mag_event(void)
imu_scale_mag(&imu); imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag); AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
} }