diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile index 80f9d1e062..656d4dd064 100644 --- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile @@ -17,9 +17,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_euler.h\" +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\" AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 1bc7e4740e..27eb3f574b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -61,165 +61,30 @@ static inline void set_body_state_from_euler(void); } -/** ABI binding for IMU data. - * Used for gyro, accel and mag ABI messages. - */ -#ifndef AHRS_ICE_IMU_ID -#define AHRS_ICE_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_ice_init(void) { - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_ice_propagate((struct Int32Rates*)gyro, dt); - } - last_stamp = *stamp; -} - -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_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler propagation.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_ice_update_accel((struct Int32Vect3*)accel, dt); - } - last_stamp = *stamp; -#else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_euler propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) - if (ahrs_ice.status == AHRS_ICE_RUNNING) { - const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - ahrs_ice_propagate((struct Int32Rates*)gyro, 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_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler accel update.") - static uint32_t last_stamp = 0; - if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) { - float dt = (float)(*stamp - last_stamp) * 1e-6; - ahrs_ice_update_mag((struct Int32Vect3*)mag, 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_ice.status == AHRS_ICE_RUNNING) { - const float dt = 1. / (AHRS_CORRECT_FREQUENCY); - ahrs_ice_update_accel((struct Int32Rates*)accel, 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_ice.status != AHRS_ICE_RUNNING) { - ahrs_ice_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel, - (struct Int32Vect3*)lp_mag); - } -} - -#if PERIODIC_TELEMETRY -#include "subsystems/datalink/telemetry.h" - -static void send_filter(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_FILTER(trans, dev, AC_ID, - &ahrs_ice.ltp_to_imu_euler.phi, - &ahrs_ice.ltp_to_imu_euler.theta, - &ahrs_ice.ltp_to_imu_euler.psi, - &ahrs_ice.measure.phi, - &ahrs_ice.measure.theta, - &ahrs_ice.measure.psi, - &ahrs_ice.hi_res_euler.phi, - &ahrs_ice.hi_res_euler.theta, - &ahrs_ice.hi_res_euler.psi, - &ahrs_ice.residual.phi, - &ahrs_ice.residual.theta, - &ahrs_ice.residual.psi, - &ahrs_ice.gyro_bias.p, - &ahrs_ice.gyro_bias.q, - &ahrs_ice.gyro_bias.r); -} - -static void send_euler(struct transport_tx *trans, struct link_device *dev) { - struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &ahrs_ice.ltp_to_imu_euler.phi, - &ahrs_ice.ltp_to_imu_euler.theta, - &ahrs_ice.ltp_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_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); -} -#endif - -void ahrs_ice_register(void) -{ - ahrs_register_impl(ahrs_ice_init, NULL); -} - -void ahrs_ice_init(struct OrientationReps* body_to_imu) -{ - /* save body_to_imu pointer */ - ahrs_ice.body_to_imu = body_to_imu; - ahrs_ice.status = AHRS_ICE_UNINIT; + ahrs_ice.is_aligned = FALSE; - /* set ltp_to_imu so that body is zero */ - memcpy(&ahrs_ice.ltp_to_imu_euler, orientationGetEulers_i(ahrs_ice.body_to_imu), - sizeof(struct Int32Eulers)); + /* init ltp_to_imu to zero */ + INT_EULERS_ZERO(ahrs_ice.ltp_to_imu_euler) INT_RATES_ZERO(ahrs_ice.imu_rate); INT_RATES_ZERO(ahrs_ice.gyro_bias); ahrs_ice.reinj_1 = FACE_REINJ_1; ahrs_ice.mag_offset = AHRS_MAG_OFFSET; - - /* - * Subscribe to scaled IMU measurements and attach callbacks - */ - AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_ICE_IMU_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter); - register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); - register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); -#endif } bool_t ahrs_ice_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag) { - get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi, &ahrs_ice.hi_res_euler.theta, lp_accel); + get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi, + &ahrs_ice.hi_res_euler.theta, lp_accel); get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi, - ahrs_ice.hi_res_euler.phi/F_UPDATE, ahrs_ice.hi_res_euler.theta/F_UPDATE, lp_mag); + ahrs_ice.hi_res_euler.phi/F_UPDATE, + ahrs_ice.hi_res_euler.theta/F_UPDATE, lp_mag); EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler); EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler); @@ -232,6 +97,7 @@ bool_t ahrs_ice_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro); ahrs_ice.status = AHRS_ICE_RUNNING; + ahrs_ice.is_aligned = TRUE; return TRUE; } @@ -286,7 +152,7 @@ static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int3 * */ -void ahrs_ice_propagate(struct Int32Rates* gyro, float dt __attribute__((unused))) { +void ahrs_ice_propagate(struct Int32Rates* gyro) { /* unbias gyro */ struct Int32Rates uf_rate; @@ -336,7 +202,7 @@ void ahrs_ice_propagate(struct Int32Rates* gyro, float dt __attribute__((unused) } -void ahrs_ice_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused))) { +void ahrs_ice_update_accel(struct Int32Vect3* accel) { #if USE_NOISE_CUT || USE_NOISE_FILTER static struct Int32Vect3 last_accel = { 0, 0, 0 }; @@ -357,7 +223,7 @@ void ahrs_ice_update_accel(struct Int32Vect3* accel, float dt __attribute__((unu } -void ahrs_ice_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused))) { +void ahrs_ice_update_mag(struct Int32Vect3* mag) { get_psi_measurement_from_mag(&ahrs_ice.measurement.psi, ahrs_ice.ltp_to_imu_euler.phi, ahrs_ice.ltp_to_imu_euler.theta, mag); @@ -406,7 +272,7 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag( /* Rotate angles and rates from imu to body frame and set state */ static void set_body_state_from_euler(void) { - struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(ahrs_ice.body_to_imu); + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu); struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; /* Compute LTP to IMU rotation matrix */ int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler); @@ -422,3 +288,19 @@ static void set_body_state_from_euler(void) { stateSetBodyRates_i(&body_rate); } + +void ahrs_ice_set_body_to_imu(struct OrientationReps* body_to_imu) +{ + ahrs_ice_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu)); +} + +void ahrs_ice_set_body_to_imu_quat(struct FloatQuat* q_b2i) +{ + orientationSetQuat_f(&ahrs_ice.body_to_imu, q_b2i); + + if (!ahrs_ice.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_ice.ltp_to_imu_euler, orientationGetEulers_i(&ahrs_ice.body_to_imu), + sizeof(struct Int32Eulers)); + } +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h index e3da309a1d..a30f117d71 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h @@ -33,6 +33,7 @@ #include "subsystems/ahrs.h" #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_orientation_conversion.h" enum AhrsICEStatus { AHRS_ICE_UNINIT, @@ -50,21 +51,21 @@ struct AhrsIntCmplEuler { int32_t reinj_1; float mag_offset; - struct OrientationReps* body_to_imu; + struct OrientationReps body_to_imu; enum AhrsICEStatus status; + bool_t is_aligned; }; extern struct AhrsIntCmplEuler ahrs_ice; -#define DefaultAhrsImpl ahrs_ice - -extern void ahrs_ice_register(void); -extern void ahrs_ice_init(struct OrientationReps* body_to_imu); +extern void ahrs_ice_init(void); +extern void ahrs_ice_set_body_to_imu(struct OrientationReps* body_to_imu); +extern void ahrs_ice_set_body_to_imu_quat(struct FloatQuat* q_b2i); extern bool_t ahrs_ice_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel, struct Int32Vect3* lp_mag); -extern void ahrs_ice_propagate(struct Int32Rates* gyro, float dt); -extern void ahrs_ice_update_accel(struct Int32Vect3* accel, float dt); -extern void ahrs_ice_update_mag(struct Int32Vect3* mag, float dt); +extern void ahrs_ice_propagate(struct Int32Rates* gyro); +extern void ahrs_ice_update_accel(struct Int32Vect3* accel); +extern void ahrs_ice_update_mag(struct Int32Vect3* mag); #endif /* AHRS_INT_CMPL_EULER_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c new file mode 100644 index 0000000000..d72b717b75 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c @@ -0,0 +1,147 @@ +/* + * 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_euler_wrapper.c + * + * Paparazzi specific wrapper to run floating point complementary filter. + */ + +#include "subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "state.h" + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_filter(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_FILTER(trans, dev, AC_ID, + &ahrs_ice.ltp_to_imu_euler.phi, + &ahrs_ice.ltp_to_imu_euler.theta, + &ahrs_ice.ltp_to_imu_euler.psi, + &ahrs_ice.measure.phi, + &ahrs_ice.measure.theta, + &ahrs_ice.measure.psi, + &ahrs_ice.hi_res_euler.phi, + &ahrs_ice.hi_res_euler.theta, + &ahrs_ice.hi_res_euler.psi, + &ahrs_ice.residual.phi, + &ahrs_ice.residual.theta, + &ahrs_ice.residual.psi, + &ahrs_ice.gyro_bias.p, + &ahrs_ice.gyro_bias.q, + &ahrs_ice.gyro_bias.r); +} + +static void send_euler(struct transport_tx *trans, struct link_device *dev) { + struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, + &ahrs_ice.ltp_to_imu_euler.phi, + &ahrs_ice.ltp_to_imu_euler.theta, + &ahrs_ice.ltp_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_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); +} +#endif + +/** ABI binding for IMU data. + * Used for gyro, accel and mag ABI messages. + */ +#ifndef AHRS_ICE_IMU_ID +#define AHRS_ICE_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 sender_id __attribute__((unused)), + const uint32_t* stamp __attribute__((unused)), + const struct Int32Rates* gyro) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_propagate((struct Int32Rates*)gyro); + } + +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + const uint32_t* stamp __attribute__((unused)), + const struct Int32Vect3* accel) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_update_accel((struct Int32Vect3*)accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + const uint32_t* stamp __attribute__((unused)), + const struct Int32Vect3* mag) +{ + if (ahrs_ice.is_aligned) { + ahrs_ice_update_mag((struct Int32Vect3*)mag); + } +} + +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_ice.is_aligned) { + ahrs_ice_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_ice_set_body_to_imu_quat((struct FloatQuat*)q_b2i_f); +} + +void ahrs_ice_register(void) +{ + ahrs_register_impl(ahrs_ice_init, NULL); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_INT32(AHRS_ICE_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, "FILTER", send_filter); + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler); + register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h new file mode 100644 index 0000000000..051ce689f2 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_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_euler_wrapper.h + * + * Paparazzi specific wrapper to run floating point DCM filter. + */ + +#ifndef AHRS_INT_CMPL_EULER_WRAPPER_H +#define AHRS_INT_CMPL_EULER_WRAPPER_H + +#include "subsystems/ahrs/ahrs_int_cmpl_euler.h" + +#define DefaultAhrsImpl ahrs_ice + +extern void ahrs_ice_register(void); + +#endif /* AHRS_INT_CMPL_EULER_WRAPPER_H */