From 0972b7f2620676cb9bf179912ec4f24deb7d4474 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 10 Nov 2014 18:58:48 +0100 Subject: [PATCH] [ahrs] use ABI for body_to_imu --- conf/abi.xml | 4 +++ sw/airborne/firmwares/rotorcraft/main.c | 3 +++ sw/airborne/subsystems/ahrs.c | 13 +--------- sw/airborne/subsystems/ahrs.h | 2 +- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c | 25 ++++++++++++------- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h | 3 ++- .../subsystems/ahrs/ahrs_float_mlkf_wrapper.c | 10 +++++++- 7 files changed, 36 insertions(+), 24 deletions(-) diff --git a/conf/abi.xml b/conf/abi.xml index 5d78b46a0c..ba2e08b6c3 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -42,6 +42,10 @@ + + + + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index c6fe4c21a4..aa464b69ad 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -174,6 +174,9 @@ STATIC_INLINE void main_init(void) DefaultAhrsRegister(); + // send body_to_imu from here for now + AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); + #if USE_GPS gps_init(); #endif diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index 5ec5394a23..91c794b0f7 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -27,8 +27,6 @@ #include "subsystems/ahrs.h" #include "subsystems/imu.h" -#include "subsystems/abi.h" -#include "mcu_periph/sys_time.h" struct Ahrs ahrs; @@ -37,16 +35,7 @@ void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps) ahrs.init = init; ahrs.update_gps = update_gps; - // TODO: remove hacks -#if !USE_IMU - struct OrientationReps body_to_imu; - struct FloatEulers eulers_zero = {0., 0., 0.}; - orientationSetEulers_f(&body_to_imu, &eulers_zero); - ahrs.init(&body_to_imu); -#elif !defined SITL || USE_NPS - /* call init function of implementation */ - ahrs.init(&imu.body_to_imu); -#endif + ahrs.init(); ahrs.status = AHRS_REGISTERED; } diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 9ef55d8e60..cae61c1695 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -41,7 +41,7 @@ #include AHRS_TYPE_H #endif -typedef void (*AhrsInit)(struct OrientationReps* body_to_imu); +typedef void (*AhrsInit)(void); typedef void (*AhrsUpdateGps)(void); /** Attitude and Heading Reference System state */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 0e9174ed81..499f55b9cd 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -57,17 +57,12 @@ static inline void set_body_state_from_quat(void); struct AhrsMlkf ahrs_mlkf; -void ahrs_mlkf_init(struct OrientationReps* body_to_imu) { - - /* save body_to_imu */ - ahrs_mlkf_set_body_to_imu(body_to_imu); +void ahrs_mlkf_init(void) { ahrs_mlkf.status = AHRS_MLKF_UNINIT; - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu), - sizeof(struct FloatQuat)); - + /* init ltp_to_imu quaternion as zero/identity rotation */ + float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat); FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate); VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z); @@ -91,9 +86,21 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) { void ahrs_mlkf_set_body_to_imu(struct OrientationReps* body_to_imu) { - orientationSetQuat_f(&ahrs_mlkf.body_to_imu, orientationGetQuat_f(body_to_imu)); + ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); } +void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat* q_b2i) +{ + orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i); + + if (ahrs_mlkf.status == AHRS_MLKF_UNINIT) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu), + sizeof(struct FloatQuat)); + } +} + + bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h index 13f6480584..c20b221431 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h @@ -62,8 +62,9 @@ struct AhrsMlkf { extern struct AhrsMlkf ahrs_mlkf; -extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu); +extern void ahrs_mlkf_init(void); extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps* body_to_imu); +extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat* q_b2i); extern bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); extern void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c index e786ea683c..aefdce4f32 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c @@ -48,8 +48,9 @@ static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { static abi_event gyro_ev; static abi_event accel_ev; static abi_event mag_ev; - static abi_event aligner_ev; +static abi_event body_to_imu_ev; + static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, const struct Int32Rates* gyro) @@ -103,6 +104,12 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, } } +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + const struct FloatQuat* q_b2i_f) +{ + ahrs_mlkf_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f); +} + void ahrs_mlkf_register(void) { ahrs_register_impl(ahrs_mlkf_init, NULL); @@ -114,6 +121,7 @@ void ahrs_mlkf_register(void) AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); + AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);