diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile index 6f2de55e7a..c7574641d4 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile @@ -22,9 +22,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile index 44357ace4d..c574244c72 100644 --- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile @@ -19,9 +19,10 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index db1f35b3a5..7ffb8ce9ad 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -32,7 +32,6 @@ #include "subsystems/ahrs/ahrs_int_cmpl_quat.h" #include "subsystems/ahrs/ahrs_int_utils.h" -#include "subsystems/abi.h" #include "state.h" #if USE_GPS @@ -109,159 +108,18 @@ static inline void set_body_state_from_quat(void); static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3* mag, float dt); static inline void ahrs_icq_update_mag_2d(struct Int32Vect3* mag, float dt); -/** ABI binding for IMU data. - * Used for gyro, accel and mag ABI messages. - */ -#ifndef AHRS_ICQ_IMU_ID -#define AHRS_ICQ_IMU_ID ABI_BROADCAST -#endif -static abi_event gyro_ev; -static abi_event accel_ev; -static abi_event mag_ev; - -static abi_event aligner_ev; - -static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, - const struct Int32Rates* gyro) +void ahrs_icq_init(void) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat propagation.") - /* timestamp in usec when last callback was received */ - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_icq_propagate((struct Int32Rates*)gyro, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_quat propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - if (ahrs_icq.status == AHRS_ICQ_RUNNING) { - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - ahrs_icq_propagate((struct Int32Rates*)gyro, dt); - } -#endif -} - -static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, - const struct Int32Vect3* accel) -{ -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_icq_update_accel((struct Int32Vect3*)accel, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.") -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - if (ahrs_icq.status == AHRS_ICQ_RUNNING) { - const float dt = 1. / (AHRS_CORRECT_FREQUENCY); - ahrs_icq_update_accel((struct Int32Rates*)accel, dt); - } -#endif -} - -static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, - const struct Int32Vect3* mag) -{ -#if USE_MAGNETOMETER -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_icq_update_mag((struct Int32Vect3*)mag, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.") -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) - if (ahrs_icq.status == AHRS_ICQ_RUNNING) { - const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); - ahrs_icq_update_mag((struct Int32Vect3*)mag, dt); - } -#endif -#endif -} - -static void aligner_cb(uint8_t __attribute__((unused)) sender_id, - const uint32_t* stamp __attribute__((unused)), - const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel, - const struct Int32Vect3* lp_mag) -{ - if (ahrs_icq.status != AHRS_ICQ_RUNNING) { - ahrs_icq_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, - (struct Int32Vect3*)lp_mag); - } -} - -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" - -static void send_quat(struct transport_tx *trans, struct link_device *dev) { - struct Int32Quat* quat = stateGetNedToBodyQuat_i(); - pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, - &ahrs_icq.weight, - &ahrs_icq.ltp_to_imu_quat.qi, - &ahrs_icq.ltp_to_imu_quat.qx, - &ahrs_icq.ltp_to_imu_quat.qy, - &ahrs_icq.ltp_to_imu_quat.qz, - &(quat->qi), - &(quat->qx), - &(quat->qy), - &(quat->qz)); -} - -static void send_euler(struct transport_tx *trans, struct link_device *dev) { - struct Int32Eulers ltp_to_imu_euler; - int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat); - struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - <p_to_imu_euler.phi, - <p_to_imu_euler.theta, - <p_to_imu_euler.psi, - &(eulers->phi), - &(eulers->theta), - &(eulers->psi)); -} - -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); -} - -static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { - struct FloatVect3 h_float; - h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x); - h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y); - h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z); - pprz_msg_send_GEO_MAG(trans, dev, AC_ID, - &h_float.x, &h_float.y, &h_float.z); -} -#endif - -void ahrs_icq_register(void) -{ - ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps); -} - - -void ahrs_icq_init(struct OrientationReps* body_to_imu) { - - /* save body_to_imu pointer */ - ahrs_icq.body_to_imu = body_to_imu; ahrs_icq.status = AHRS_ICQ_UNINIT; + ahrs_icq.is_aligned = FALSE; ahrs_icq.ltp_vel_norm_valid = FALSE; ahrs_icq.heading_aligned = FALSE; - /* set ltp_to_imu so that body is zero */ - memcpy(&ahrs_icq.ltp_to_imu_quat, orientationGetQuat_i(ahrs_icq.body_to_imu), - sizeof(struct Int32Quat)); + /* init ltp_to_imu quaternion as zero/identity rotation */ + int32_quat_identity(&ahrs_icq.ltp_to_imu_quat); + INT_RATES_ZERO(ahrs_icq.imu_rate); INT_RATES_ZERO(ahrs_icq.gyro_bias); @@ -290,22 +148,6 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) { ahrs_icq.accel_cnt = 0; ahrs_icq.mag_cnt = 0; - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_IMU_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat); - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); - register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif - } @@ -334,6 +176,7 @@ bool_t ahrs_icq_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28); ahrs_icq.status = AHRS_ICQ_RUNNING; + ahrs_icq.is_aligned = TRUE; return TRUE; } @@ -429,7 +272,7 @@ void ahrs_icq_update_accel(struct Int32Vect3* accel, float dt) { /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(ahrs_icq.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement @@ -779,7 +622,7 @@ void ahrs_icq_realign_heading(int32_t heading) { QUAT_COPY(ltp_to_body_quat, q); /* compute ltp to imu rotations */ - struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(ahrs_icq.body_to_imu); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); int32_quat_comp(&ahrs_icq.ltp_to_imu_quat, <p_to_body_quat, body_to_imu_quat); /* Set state */ @@ -793,15 +636,31 @@ void ahrs_icq_realign_heading(int32_t heading) { static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct Int32Quat ltp_to_body_quat; - struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(ahrs_icq.body_to_imu); + struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu); int32_quat_comp_inv(<p_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_i(<p_to_body_quat); /* compute body rates */ struct Int32Rates body_rate; - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(ahrs_icq.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); } + +void ahrs_icq_set_body_to_imu(struct OrientationReps* body_to_imu) +{ + ahrs_icq_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_icq_set_body_to_imu_quat(struct FloatQuat* q_b2i) +{ + orientationSetQuat_f(&ahrs_icq.body_to_imu, q_b2i); + + if (!ahrs_icq.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_icq.ltp_to_imu_quat, orientationGetQuat_i(&ahrs_icq.body_to_imu), + sizeof(struct Int32Quat)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h index e1224b484c..e26da6a787 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h @@ -34,6 +34,7 @@ #include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_orientation_conversion.h" enum AhrsICQStatus { AHRS_ICQ_UNINIT, @@ -96,17 +97,17 @@ struct AhrsIntCmplQuat { uint16_t accel_cnt; ///< number of propagations since last accel update uint16_t mag_cnt; ///< number of propagations since last mag update - struct OrientationReps* body_to_imu; + struct OrientationReps body_to_imu; enum AhrsICQStatus status; ///< status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING + bool_t is_aligned; }; extern struct AhrsIntCmplQuat ahrs_icq; -#define DefaultAhrsImpl ahrs_icq - -extern void ahrs_icq_register(void); -extern void ahrs_icq_init(struct OrientationReps* body_to_imu); +extern void ahrs_icq_init(void); +extern void ahrs_icq_set_body_to_imu(struct OrientationReps* body_to_imu); +extern void ahrs_icq_set_body_to_imu_quat(struct FloatQuat* q_b2i); extern bool_t ahrs_icq_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); extern void ahrs_icq_propagate(struct Int32Rates* gyro, float dt); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c new file mode 100644 index 0000000000..821aa460f9 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c @@ -0,0 +1,191 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +static void send_quat(struct transport_tx *trans, struct link_device *dev) { + struct Int32Quat* quat = stateGetNedToBodyQuat_i(); + pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, + &ahrs_icq.weight, + &ahrs_icq.ltp_to_imu_quat.qi, + &ahrs_icq.ltp_to_imu_quat.qx, + &ahrs_icq.ltp_to_imu_quat.qy, + &ahrs_icq.ltp_to_imu_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} + +static void send_euler(struct transport_tx *trans, struct link_device *dev) { + struct Int32Eulers ltp_to_imu_euler; + int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat); + struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + <p_to_imu_euler.phi, + <p_to_imu_euler.theta, + <p_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); +} + +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); +} + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { + struct FloatVect3 h_float; + h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x); + h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y); + h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z); + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &h_float.x, &h_float.y, &h_float.z); +} +#endif + + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_ICQ_IMU_ID +#define AHRS_ICQ_IMU_ID ABI_BROADCAST +#endif +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) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_icq_propagate((struct Int32Rates*)gyro, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.") +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_icq.status == AHRS_ICQ_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_icq_propagate((struct Int32Rates*)gyro, dt); + } +#endif +} + +static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, + const struct Int32Vect3* accel) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_icq_update_accel((struct Int32Vect3*)accel, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.") +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_CORRECT_FREQUENCY); + ahrs_icq_update_accel((struct Int32Vect3*)accel, dt); + } +#endif +} + +static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp, + const struct Int32Vect3* mag) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) +PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_icq.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_icq_update_mag((struct Int32Vect3*)mag, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.") +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + if (ahrs_icq.is_aligned) { + const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); + ahrs_icq_update_mag((struct Int32Vect3*)mag, dt); + } +#endif +} + +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + const uint32_t* stamp __attribute__((unused)), + const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel, + const struct Int32Vect3* lp_mag) +{ + if (!ahrs_icq.is_aligned) { + ahrs_icq_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, + (struct Int32Vect3*)lp_mag); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + const struct FloatQuat* q_b2i_f) +{ + ahrs_icq_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f); +} + +void ahrs_icq_register(void) +{ + ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_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, "AHRS_QUAT_INT", send_quat); + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); + register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h new file mode 100644 index 0000000000..e64c3a9b04 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#ifndef AHRS_INT_CMPL_QUAT_WRAPPER_H +#define AHRS_INT_CMPL_QUAT_WRAPPER_H + +#include "subsystems/ahrs/ahrs_int_cmpl_quat.h" + +#define DefaultAhrsImpl ahrs_icq + +extern void ahrs_icq_register(void); + +#endif /* AHRS_INT_CMPL_QUAT_WRAPPER_H */