diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile index efa3ababb4..735f1e4b5a 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile @@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile index 1202eac965..101367d89b 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile @@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile index 9f45359b39..be6fb91b62 100644 --- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile @@ -19,10 +19,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile index b195db2edd..0e349069a8 100644 --- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile @@ -18,10 +18,11 @@ ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\" AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ifdef AHRS_PROPAGATE_FREQUENCY diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 8c16d90c4c..0527a5e946 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -27,7 +27,6 @@ * Propagation can be done in rotation matrix or quaternion representation. */ -#include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_float_cmpl.h" #include "subsystems/ahrs/ahrs_float_utils.h" #include "math/pprz_algebra_float.h" @@ -38,7 +37,6 @@ #include "subsystems/gps.h" #endif #include "state.h" -#include "subsystems/abi.h" //#include "../../test/pprz_algebra_print.h" @@ -90,165 +88,16 @@ static inline void compute_body_orientation_and_rates(void); struct AhrsFloatCmpl ahrs_fc; - -/** ABI binding for IMU data. - * Used for gyro, accel and mag ABI messages. - */ -#ifndef AHRS_FC_IMU_ID -#define AHRS_FC_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_fc_init(void) { -#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl propagation.") - /* timestamp in usec when last callback was received */ - static uint32_t last_stamp = 0; - - if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_fc_propagate((struct Int32Rates*)gyro, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_cmpl propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - if (ahrs_fc.status == AHRS_FC_RUNNING) { - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - ahrs_fc_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 float_cmpl accel update.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_fc_update_accel((struct Int32Vect3*)accel, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.") -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - if (ahrs_fc.status == AHRS_FC_RUNNING) { - const float dt = 1. / (AHRS_CORRECT_FREQUENCY); - ahrs_fc_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 float_cmpl mag update.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_fc_update_mag((struct Int32Vect3*)mag, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.") -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) - if (ahrs_fc.status == AHRS_FC_RUNNING) { - const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); - ahrs_fc_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_fc.status != AHRS_FC_RUNNING) { - ahrs_fc_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, - (struct Int32Vect3*)lp_mag); - } -} - -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" - -static void send_att(struct transport_tx *trans, struct link_device *dev) { - struct FloatEulers ltp_to_imu_euler; - float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat); - struct Int32Eulers euler_i; - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi)); -} - -static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_GEO_MAG(trans, dev, AC_ID, - &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); -} - -// TODO convert from float to int if we really need this one -/* -static void send_rmat(struct transport_tx *trans, struct link_device *dev) { - struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); - pprz_msg_send_AHRS_RMAT(trans, dev, AC_ID, - &ahrs_fc.ltp_to_imu_rmat.m[0], - &ahrs_fc.ltp_to_imu_rmat.m[1], - &ahrs_fc.ltp_to_imu_rmat.m[2], - &ahrs_fc.ltp_to_imu_rmat.m[3], - &ahrs_fc.ltp_to_imu_rmat.m[4], - &ahrs_fc.ltp_to_imu_rmat.m[5], - &ahrs_fc.ltp_to_imu_rmat.m[6], - &ahrs_fc.ltp_to_imu_rmat.m[7], - &ahrs_fc.ltp_to_imu_rmat.m[8], - &(att_rmat->m[0]), - &(att_rmat->m[1]), - &(att_rmat->m[2]), - &(att_rmat->m[3]), - &(att_rmat->m[4]), - &(att_rmat->m[5]), - &(att_rmat->m[6]), - &(att_rmat->m[7]), - &(att_rmat->m[8])); -} -*/ -#endif - -void ahrs_fc_register(void) -{ - ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps); -} - -void ahrs_fc_init(struct OrientationReps* body_to_imu) -{ - /* save body_to_imu pointer */ - ahrs_fc.body_to_imu = body_to_imu; - ahrs_fc.status = AHRS_FC_UNINIT; + ahrs_fc.is_aligned = FALSE; ahrs_fc.ltp_vel_norm_valid = FALSE; ahrs_fc.heading_aligned = FALSE; - /* Set ltp_to_imu so that body is zero */ - memcpy(&ahrs_fc.ltp_to_imu_quat, orientationGetQuat_f(ahrs_fc.body_to_imu), - sizeof(struct FloatQuat)); - memcpy(&ahrs_fc.ltp_to_imu_rmat, orientationGetRMat_f(ahrs_fc.body_to_imu), - sizeof(struct FloatRMat)); + /* init ltp_to_imu quaternion as zero/identity rotation */ + float_quat_identity(&ahrs_fc.ltp_to_imu_quat); FLOAT_RATES_ZERO(ahrs_fc.imu_rate); @@ -270,19 +119,6 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu) ahrs_fc.accel_cnt = 0; ahrs_fc.mag_cnt = 0; - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); - register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); -#endif } bool_t ahrs_fc_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, @@ -311,6 +147,7 @@ bool_t ahrs_fc_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0); ahrs_fc.status = AHRS_FC_RUNNING; + ahrs_fc.is_aligned = TRUE; return TRUE; } @@ -385,7 +222,7 @@ void ahrs_fc_update_accel(struct Int32Vect3* accel, float dt) { /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(ahrs_fc.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ @@ -653,7 +490,7 @@ void ahrs_fc_realign_heading(float heading) { float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(ahrs_fc.body_to_imu); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); @@ -670,14 +507,32 @@ void ahrs_fc_realign_heading(float heading) { static inline void compute_body_orientation_and_rates(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(ahrs_fc.body_to_imu); + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; - struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(ahrs_fc.body_to_imu); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); stateSetBodyRates_f(&body_rate); } + +void ahrs_fc_set_body_to_imu(struct OrientationReps* body_to_imu) +{ + ahrs_fc_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_fc_set_body_to_imu_quat(struct FloatQuat* q_b2i) +{ + orientationSetQuat_f(&ahrs_fc.body_to_imu, q_b2i); + + if (!ahrs_fc.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_fc.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_fc.body_to_imu), + sizeof(struct FloatQuat)); + memcpy(&ahrs_fc.ltp_to_imu_rmat, orientationGetRMat_f(&ahrs_fc.body_to_imu), + sizeof(struct FloatRMat)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index e62d4432a1..ae2f956d2f 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -27,10 +27,12 @@ * Propagation can be done in rotation matrix or quaternion representation. */ -#ifndef AHRS_FLOAT_CMPL -#define AHRS_FLOAT_CMPL +#ifndef AHRS_FLOAT_CMPL_H +#define AHRS_FLOAT_CMPL_H #include "std.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" enum AhrsFCStatus { AHRS_FC_UNINIT, @@ -66,17 +68,17 @@ struct AhrsFloatCmpl { 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 AhrsFCStatus status; + bool_t is_aligned; }; extern struct AhrsFloatCmpl ahrs_fc; -#define DefaultAhrsImpl ahrs_fc - -extern void ahrs_fc_register(void); -extern void ahrs_fc_init(struct OrientationReps* body_to_imu); +extern void ahrs_fc_init(void); +extern void ahrs_fc_set_body_to_imu(struct OrientationReps* body_to_imu); +extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat* q_b2i); extern bool_t ahrs_fc_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); extern void ahrs_fc_propagate(struct Int32Rates* gyro, float dt); @@ -98,4 +100,4 @@ void ahrs_fc_update_heading(float heading); void ahrs_fc_realign_heading(float heading); -#endif /* AHRS_FLOAT_CMPL_RMAT */ +#endif /* AHRS_FLOAT_CMPL_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c new file mode 100644 index 0000000000..980b895fed --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -0,0 +1,169 @@ +/* + * 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_float_cmpl_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_float_cmpl_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_att(struct transport_tx *trans, struct link_device *dev) { + struct FloatEulers ltp_to_imu_euler; + float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat); + struct Int32Eulers euler_i; + EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); + struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + &euler_i.phi, + &euler_i.theta, + &euler_i.psi, + &(eulers_body->phi), + &(eulers_body->theta), + &(eulers_body->psi)); +} + +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, + &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); +} +#endif + + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_FC_IMU_ID +#define AHRS_FC_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_FC propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_fc_propagate((struct Int32Rates*)gyro, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.") +PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + if (ahrs_fc.status == AHRS_FC_RUNNING) { + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + ahrs_fc_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 float_cmpl accel update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_fc_update_accel((struct Int32Vect3*)accel, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.") +PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + if (ahrs_fc.is_aligned) { + const float dt = 1. / (AHRS_CORRECT_FREQUENCY); + ahrs_fc_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 float_cmpl mag update.") + static uint32_t last_stamp = 0; + if (last_stamp > 0 && ahrs_fc.is_aligned) { + float dt = (float)(*stamp - last_stamp) * 1e-6; + ahrs_fc_update_mag((struct Int32Vect3*)mag, dt); + } + last_stamp = *stamp; +#else +PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.") +PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + if (ahrs_fc.is_aligned) { + const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); + ahrs_fc_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_fc.is_aligned) { + ahrs_fc_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_fc_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f); +} + +void ahrs_fc_register(void) +{ + ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_FC_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_EULER_INT", send_att); + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h new file mode 100644 index 0000000000..b6ae7ed973 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_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_float_cmpl_wrapper.h + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#ifndef AHRS_FLOAT_CMPL_WRAPPER_H +#define AHRS_FLOAT_CMPL_WRAPPER_H + +#include "subsystems/ahrs/ahrs_float_cmpl.h" + +#define DefaultAhrsImpl ahrs_fc + +extern void ahrs_fc_register(void); + +#endif /* AHRS_FLOAT_CMPL_WRAPPER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h index 7554859455..4c623a75e3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h @@ -14,9 +14,8 @@ * 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, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with Paparazzi; see the file COPYING. If not, see + * . */ /**