diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile new file mode 100644 index 0000000000..ce4cadb62a --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile @@ -0,0 +1,46 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# AHRS_H_X +# AHRS_H_Y +# AHRS_H_Z +# + +USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none + +AHRS_FINV_CFLAGS = -DUSE_AHRS +AHRS_FINV_CFLAGS += -DUSE_AHRS_ALIGNER + +ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) + AHRS_FINV_CFLAGS += -DUSE_MAGNETOMETER +endif + +ifneq ($(AHRS_ALIGNER_LED),none) + AHRS_FINV_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) +endif + +ifdef SECONDARY_AHRS +ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat)) +# this is the secondary AHRS +AHRS_FINV_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\" +AHRS_FINV_CFLAGS += -DSECONDARY_AHRS=ahrs_float_invariant +else +# this is the primary AHRS +AHRS_FINV_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\" +AHRS_FINV_CFLAGS += -DPRIMARY_AHRS=ahrs_float_invariant +endif +else +# plain old single AHRS usage +AHRS_FINV_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\" +endif + +AHRS_FINV_SRCS += subsystems/ahrs.c +AHRS_FINV_SRCS += subsystems/ahrs/ahrs_float_invariant.c +AHRS_FINV_SRCS += subsystems/ahrs/ahrs_float_invariant_wrapper.c +AHRS_FINV_SRCS += subsystems/ahrs/ahrs_aligner.c + +# add it for all targets except sim and fbw +ifeq (,$(findstring $(TARGET),sim fbw)) +$(TARGET).CFLAGS += $(AHRS_FINV_CFLAGS) +$(TARGET).srcs += $(AHRS_FINV_SRCS) +endif diff --git a/conf/settings/estimation/ahrs_float_invariant.xml b/conf/settings/estimation/ahrs_float_invariant.xml new file mode 100644 index 0000000000..253ad3d8cb --- /dev/null +++ b/conf/settings/estimation/ahrs_float_invariant.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant.c b/sw/airborne/subsystems/ahrs/ahrs_float_invariant.c new file mode 100644 index 0000000000..847ced8924 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant.c @@ -0,0 +1,398 @@ +/* + * Copyright (C) 2015 Jean-Philippe Condomines, Gautier Hattenberger + * + * 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_invariant.c + * @author Jean-Philippe Condomines + * + * AHRS using invariant filter. + * + */ + +#include "subsystems/ahrs/ahrs_float_invariant.h" + +#include "subsystems/ahrs/ahrs_int_utils.h" +#include "subsystems/ahrs/ahrs_aligner.h" + +#include "generated/airframe.h" + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" +#include "math/pprz_rk_float.h" + +#if SEND_INVARIANT_FILTER +#include "subsystems/datalink/telemetry.h" +#endif + +/*---------Invariant Observers----------- + * + * State vector : + * x = [q0 q1 q2 q3 wb1 wb2 wb3 as cs] + * + * + * Dynamic model (dim = 9) : + * x_qdot = 0.5 * x_quat * ( x_rates - x_bias ); + * x_bias_dot = 0; + * x_asdot = 0; + * x_csdot = 0; + * + * Observation model (dim = 6) : + * ya = as * (q)-1 * A * q; (A : accelerometers) + * yc = cs * (q)-1 * C * q; (C = A x B (cross product), B : magnetometers) + * + *------------------------------------------*/ + +// Default values for the tuning gains +// Tuning parameter of accel and mag on attitude +#ifndef AHRS_INV_LX +#define AHRS_INV_LX 2. * (0.06 + 0.1) +#endif +// Tuning parameter of accel and mag on attitude +#ifndef AHRS_INV_LY +#define AHRS_INV_LY 2. * (0.06 + 0.06) +#endif +// Tuning parameter of accel and mag on attitude +#ifndef AHRS_INV_LZ +#define AHRS_INV_LZ 2. * (0.1 + 0.06) +#endif +// Tuning parameter of accel and mag on gyro bias +#ifndef AHRS_INV_MX +#define AHRS_INV_MX 2. * 0.05 * (0.06 + 0.1) +#endif +// Tuning parameter of accel and mag on gyro bias +#ifndef AHRS_INV_MY +#define AHRS_INV_MY 2. * 0.05 * (0.06 + 0.06) +#endif +// Tuning parameter of accel and mag on gyro bias +#ifndef AHRS_INV_MZ +#define AHRS_INV_MZ 2. * 0.05 * (0.1 + 0.06) +#endif +// Tuning parameter of accel and mag on accel bias +#ifndef AHRS_INV_N +#define AHRS_INV_N 0.25 +#endif +// Tuning parameter of accel and mag on mag bias +#ifndef AHRS_INV_O +#define AHRS_INV_O 0.5 +#endif + +struct AhrsFloatInv ahrs_float_inv; + +/* earth gravity model */ +static const struct FloatVect3 A = { 0.f, 0.f, -9.81f }; + +/* earth magnetic model */ +#define B ahrs_float_inv.mag_h + +/* error computation */ +static inline void error_output(struct AhrsFloatInv *_ins); + +/* propagation model (called by runge-kutta library) */ +static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m); + + +/** Right multiplication by a quaternion. + * vi * q + */ +void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, + struct FloatVect3 *vi); + +/* init state and measurements */ +static inline void init_invariant_state(void) +{ + // init state + float_quat_identity(&ahrs_float_inv.state.quat); + FLOAT_RATES_ZERO(ahrs_float_inv.state.bias); + ahrs_float_inv.state.as = 1.0f; + ahrs_float_inv.state.cs = 1.0f; + + // init measures + FLOAT_VECT3_ZERO(ahrs_float_inv.meas.accel); + FLOAT_VECT3_ZERO(ahrs_float_inv.meas.mag); + +} + +void ahrs_float_invariant_init(void) +{ + // init magnetometers + + ahrs_float_inv.mag_h.x = AHRS_H_X; + ahrs_float_inv.mag_h.y = AHRS_H_Y; + ahrs_float_inv.mag_h.z = AHRS_H_Z; + + // init state and measurements + init_invariant_state(); + + // init gains + ahrs_float_inv.gains.lx = AHRS_INV_LX; + ahrs_float_inv.gains.ly = AHRS_INV_LY; + ahrs_float_inv.gains.lz = AHRS_INV_LZ; + ahrs_float_inv.gains.mx = AHRS_INV_MX; + ahrs_float_inv.gains.my = AHRS_INV_MY; + ahrs_float_inv.gains.mz = AHRS_INV_MZ; + ahrs_float_inv.gains.n = AHRS_INV_N; + ahrs_float_inv.gains.o = AHRS_INV_O; + + ahrs_float_inv.is_aligned = FALSE; + ahrs_float_inv.reset = FALSE; +} + +void ahrs_float_invariant_align(struct Int32Rates *lp_gyro, + struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ + /* Compute an initial orientation from accel and mag directly as quaternion */ + ahrs_float_get_quat_from_accel_mag(&ahrs_float_inv.state.quat, lp_accel, lp_mag); + + /* use average gyro as initial value for bias */ + struct FloatRates bias0; + RATES_COPY(bias0, *lp_gyro); + RATES_FLOAT_OF_BFP(ahrs_float_inv.state.bias, bias0); + + // ins and ahrs are now running + ahrs_float_inv.is_aligned = TRUE; +} + +void ahrs_float_invariant_propagate(struct Int32Rates* gyro, float dt) +{ + // realign all the filter if needed + // a complete init cycle is required + if (ahrs_float_inv.reset) { + ahrs_float_inv.reset = FALSE; + ahrs_float_inv.is_aligned = FALSE; + init_invariant_state(); + } + + // fill command vector + struct Int32Rates gyro_meas_body; + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_float_inv.body_to_imu); + int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); + RATES_FLOAT_OF_BFP(ahrs_float_inv.cmd.rates, gyro_meas_body); + + // update correction gains + error_output(&ahrs_float_inv); + + // propagate model + struct inv_state new_state; + runge_kutta_4_float((float *)&new_state, + (float *)&ahrs_float_inv.state, INV_STATE_DIM, + (float *)&ahrs_float_inv.cmd, INV_COMMAND_DIM, + invariant_model, dt); + ahrs_float_inv.state = new_state; + + // normalize quaternion + FLOAT_QUAT_NORMALIZE(ahrs_float_inv.state.quat); + + //------------------------------------------------------------// + +#if SEND_INVARIANT_FILTER + struct FloatEulers eulers; + float foo = 0.f; + FLOAT_EULERS_OF_QUAT(eulers, ahrs_float_inv.state.quat); + RunOnceEvery(3, + pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device, + AC_ID, + &ahrs_float_inv.state.quat.qi, + &eulers.phi, + &eulers.theta, + &eulers.psi, + &foo, + &foo, + &foo, + &foo, + &foo, + &foo, + &ahrs_float_inv.state.bias.p, + &ahrs_float_inv.state.bias.q, + &ahrs_float_inv.state.bias.r, + &ahrs_float_inv.state.as, + &ahrs_float_inv.state.cs, + &foo, + &foo); + ); +#endif + +} + +void ahrs_float_invariant_update_accel(struct Int32Vect3* accel) +{ + ACCELS_FLOAT_OF_BFP(ahrs_float_inv.meas.accel, *accel); +} + +// assume mag is dead when values are not moving anymore +#define MAG_FROZEN_COUNT 30 + +void ahrs_float_invariant_update_mag(struct Int32Vect3* mag) +{ + static uint32_t mag_frozen_count = MAG_FROZEN_COUNT; + static int32_t last_mx = 0; + + if (last_mx == mag->x) { + mag_frozen_count--; + if (mag_frozen_count == 0) { + // if mag is dead, better set measurements to zero + FLOAT_VECT3_ZERO(ahrs_float_inv.meas.mag); + mag_frozen_count = MAG_FROZEN_COUNT; + } + } else { + // values are moving + struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_float_inv.body_to_imu); + struct Int32Vect3 mag_meas_body; + // new values in body frame + int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, mag); + MAGS_FLOAT_OF_BFP(ahrs_float_inv.meas.mag, mag_meas_body); + // reset counter + mag_frozen_count = MAG_FROZEN_COUNT; + } + last_mx = mag->x; +} + +/** Compute dynamic mode + * + * x_dot = evolution_model + (gain_matrix * error) + */ +static inline void invariant_model(float *o, const float *x, const int n, const float *u, + const int m __attribute__((unused))) +{ + +#pragma GCC diagnostic push // require GCC 4.6 +#pragma GCC diagnostic ignored "-Wcast-qual" + struct inv_state *s = (struct inv_state *)x; + struct inv_command *c = (struct inv_command *)u; +#pragma GCC diagnostic pop // require GCC 4.6 + struct inv_state s_dot; + struct FloatRates rates_unbiased; + struct FloatVect3 tmp_vect; + struct FloatQuat tmp_quat; + + // test accel sensitivity + if (fabs(s->as) < 0.1) { + // too small, return x_dot = 0 to avoid division by 0 + float_vect_zero(o, n); + // TODO set ins state to error + return; + } + + /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */ + RATES_DIFF(rates_unbiased, c->rates, s->bias); + /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */ + float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat)); + + float_quat_vmul_right(&tmp_quat, &(s->quat), &ahrs_float_inv.corr.LE); + QUAT_ADD(s_dot.quat, tmp_quat); + + float norm2_r = 1. - FLOAT_QUAT_NORM2(s->quat); + QUAT_SMUL(tmp_quat, s->quat, norm2_r); + QUAT_ADD(s_dot.quat, tmp_quat); + + /* bias_dot = q-1 * (ME) * q */ + float_quat_vmult(&tmp_vect, &(s->quat), &ahrs_float_inv.corr.ME); + RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z); + + /* as_dot = as * NE */ + s_dot.as = (s->as) * (ahrs_float_inv.corr.NE); + + /* cs_dot = OE */ + s_dot.cs = ahrs_float_inv.corr.OE; + + // set output + memcpy(o, &s_dot, n * sizeof(float)); +} + +/** Compute correction vectors + * E = ( ŷ - y ) + * LE, ME, NE, OE : ( gain matrix * error ) + */ +static inline void error_output(struct AhrsFloatInv *_ahrs) +{ + + struct FloatVect3 YAt, C, YC, YCt, Ec, Ea; + + // test accel sensitivity + if (fabs(_ahrs->state.as) < 0.1) { + // too small, don't do anything to avoid division by 0 + return; + } + + /* C = A X B Cross product */ + VECT3_CROSS_PRODUCT(C, A, B); + /* YC = YA X YB Cross product */ + VECT3_CROSS_PRODUCT(YC, _ahrs->meas.accel, _ahrs->meas.mag); + /* YCt = (1 / cs) * q * YC * q-1 => invariant output on magnetometers */ + struct FloatQuat q_b2n; + float_quat_invert(&q_b2n, &(_ahrs->state.quat)); + float_quat_vmult(&YCt, &q_b2n, &YC); + VECT3_SMUL(YCt, YCt, 1. / (_ahrs->state.cs)); + + /* YAt = q * yA * q-1 => invariant output on accelerometers */ + float_quat_vmult(&YAt, &q_b2n, &(_ahrs->meas.accel)); + VECT3_SMUL(YAt, YAt, 1. / (_ahrs->state.as)); + + /*--------- E = ( ŷ - y ) ----------*/ + + /* EC = ( C - YCt ) */ + VECT3_DIFF(Ec, C, YCt); + /* EA = ( A - YAt ) */ + VECT3_DIFF(Ea, A, YAt); + + /*-------------- Gains --------------*/ + + /**** LaEa + LcEc *****/ + _ahrs->corr.LE.x = (-_ahrs->gains.lx * Ea.y) / (2.f * A.z); + _ahrs->corr.LE.y = (_ahrs->gains.ly * Ea.x) / (2.f * A.z); + _ahrs->corr.LE.z = (-_ahrs->gains.lz * Ec.x) / (B.x * 2.f * A.z); + + /***** MaEa + McEc ********/ + _ahrs->corr.ME.x = (_ahrs->gains.mx * Ea.y) / (2.f * A.z); + _ahrs->corr.ME.y = (-_ahrs->gains.my * Ea.x)/(2.f * A.z); + _ahrs->corr.ME.z = (_ahrs->gains.mz * Ec.x) / (B.x * 2.f * A.z); + + /****** NaEa + NcEc ********/ + _ahrs->corr.NE = (-_ahrs->gains.n * Ea.z) / A.z; + + /****** OaEa + OcEc ********/ + _ahrs->corr.OE = (-_ahrs->gains.o * Ec.y) / (B.x * A.z); +} + + +void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, + struct FloatVect3 *vi) +{ + struct FloatVect3 qvec, v1, v2; + float qi; + + FLOAT_QUAT_EXTRACT(qvec, *q); + qi = - VECT3_DOT_PRODUCT(*vi, qvec); + VECT3_CROSS_PRODUCT(v1, *vi, qvec); + VECT3_SMUL(v2, *vi, q->qi); + VECT3_ADD(v2, v1); + QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z); +} + +void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i) +{ + orientationSetQuat_f(&ahrs_float_inv.body_to_imu, q_b2i); + + if (!ahrs_float_inv.is_aligned) { + /* Set ltp_to_imu so that body is zero */ + memcpy(&ahrs_float_inv.state.quat, q_b2i, sizeof(struct FloatQuat)); + } +} + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant.h b/sw/airborne/subsystems/ahrs/ahrs_float_invariant.h new file mode 100644 index 0000000000..efe1612ac7 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant.h @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2014-2015 Jean-Philippe Condomines, Gautier Hattenberger + * + * 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_invariant.h + * AHRS using invariant filter. + * For more information, please send an email to "jp.condomines@gmail.com" + */ + +#ifndef AHRS_FLOAT_INVARIANT_H +#define AHRS_FLOAT_INVARIANT_H + +#include "subsystems/ahrs.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_orientation_conversion.h" + +/** Invariant filter state dimension + */ +#define INV_STATE_DIM 9 + +/** Invariant filter state + */ +struct inv_state { + struct FloatQuat quat; ///< Estimated attitude (quaternion) + struct FloatRates bias; ///< Estimated gyro biases + float cs; ///< Estimates magnetometer sensitivity + float as; ///< Estimated accelerometer sensitivity +}; + +/** Invariant filter measurement vector dimension + */ +#define INV_MEASURE_DIM 6 + +/** Invariant filter measurement vector + */ +struct inv_measures { + struct FloatVect3 accel; ///< Measured accelerometers + struct FloatVect3 mag; ///< Measured magnetic field +}; + +/** Invariant filter command vector dimension + */ +#define INV_COMMAND_DIM 3 + +/** Invariant filter command vector + */ +struct inv_command { + struct FloatRates rates; ///< Input gyro rates +}; + +/** Invariant filter correction gains + */ +struct inv_correction_gains { + struct FloatVect3 LE; ///< Correction gains on attitude + struct FloatVect3 ME; ///< Correction gains on gyro biases + float NE; ///< Correction gains on accel bias + float OE; ///< Correction gains on magnetometer sensitivity +}; + +/** Invariant filter tuning gains + */ +struct inv_gains { + float lx; ///< Tuning parameter of accel and mag on attitude (longitudinal subsystem) + float ly; ///< Tuning parameter of accel and mag on attitude (lateral subsystem) + float lz; ///< Tuning parameter of accel and mag on attitude (heading subsystem) + float mx; ///< Tuning parameter of accel and mag on gyro bias (longitudinal subsystem) + float my; ///< Tuning parameter of accel and mag on gyro bias (lateral subsystem) + float mz; ///< Tuning parameter of accel and mag on gyro bias (heading subsystem) + float n; ///< Tuning parameter of accel and mag on accel bias (scaling subsystem) + float o; ///< Tuning parameter of accel and mag on mag bias (scaling subsystem) +}; + +/** Invariant filter structure + */ +struct AhrsFloatInv { + struct inv_state state; ///< state vector + struct inv_measures meas; ///< measurement vector + struct inv_command cmd; ///< command vector + struct inv_correction_gains corr; ///< correction gains + struct inv_gains gains; ///< tuning gains + + bool_t reset; ///< flag to request reset/reinit the filter + + /** body_to_imu rotation */ + struct OrientationReps body_to_imu; + + struct FloatVect3 mag_h; + bool_t is_aligned; +}; + +extern struct AhrsFloatInv ahrs_float_inv; + +extern void ahrs_float_invariant_init(void); +extern void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i); +extern void ahrs_float_invariant_align(struct Int32Rates *lp_gyro, + struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag); +extern void ahrs_float_invariant_propagate(struct Int32Rates* gyro, float dt); +extern void ahrs_float_invariant_update_accel(struct Int32Vect3* accel); +extern void ahrs_float_invariant_update_mag(struct Int32Vect3* mag); + +#endif /* AHRS_FLOAT_INVARIANT_H */ + diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c new file mode 100644 index 0000000000..0c75a4c526 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c @@ -0,0 +1,237 @@ +/* + * Copyright (C) 2012-2015 Jean-Philippe Condomines, Gautier Hattenberger + * 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_invariant_wrapper.c + * + * Paparazzi specific wrapper to run INVARIANT ahrs filter. + */ + +#include "subsystems/ahrs/ahrs_float_invariant_wrapper.h" +#include "subsystems/ahrs.h" +#include "subsystems/abi.h" +#include "mcu_periph/sys_time.h" +#include "message_pragmas.h" +#include "state.h" + +#ifndef AHRS_FINV_OUTPUT_ENABLED +#define AHRS_FINV_OUTPUT_ENABLED TRUE +#endif +PRINT_CONFIG_VAR(AHRS_INV_OUTPUT_ENABLED) + +/** if TRUE with push the estimation results to the state interface */ +static bool_t ahrs_finv_output_enabled; +/** last gyro msg timestamp */ +static uint32_t ahrs_finv_last_stamp = 0; + +static void compute_body_orientation_and_rates(void); + +#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_float_inv.state.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_float_inv.mag_h.x, + &ahrs_float_inv.mag_h.y, + &ahrs_float_inv.mag_h.z); +} + +#ifndef AHRS_FINV_FILTER_ID +#define AHRS_FINV_FILTER_ID 7 +#endif + +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t id = AHRS_FINV_FILTER_ID; + uint8_t mde = 3; + uint16_t val = 0; + if (!ahrs_float_inv.is_aligned) { mde = 2; } + uint32_t t_diff = get_sys_time_usec() - ahrs_finv_last_stamp; + /* set lost if no new gyro measurements for 50ms */ + if (t_diff > 50000) { mde = 5; } + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val); +} +#endif + + +/* + * ABI bindings + */ +/** IMU (gyro, accel) */ +#ifndef AHRS_FINV_IMU_ID +#define AHRS_FINV_IMU_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(AHRS_FINV_IMU_ID) + +/** magnetometer */ +#ifndef AHRS_FINV_MAG_ID +#define AHRS_FINV_MAG_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(AHRS_FINV_MAG_ID) + +static abi_event mag_ev; +static abi_event gyro_ev; +static abi_event accel_ev; +static abi_event aligner_ev; +static abi_event body_to_imu_ev; +static abi_event geo_mag_ev; + +/** + * Call ahrs_float_invariant_propagate on new gyro measurements. + * Since acceleration measurement is also needed for propagation, + * use the last stored accel from #ahrs_finv_accel. + */ +static void gyro_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp, struct Int32Rates *gyro) +{ +#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Calculating dt for AHRS float_invariant propagation.") + /* timestamp in usec when last callback was received */ + static uint32_t last_stamp = 0; + + if (last_stamp > 0 && ahrs_float_inv.is_aligned) { + float dt = (float)(stamp - last_stamp) * 1e-6; + ahrs_float_invariant_propagate(gyro, dt); + compute_body_orientation_and_rates(); + } + last_stamp = stamp; +#else + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_invariant propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); + if (ahrs_float_inv.is_aligned) { + ahrs_float_invariant_propagate(gyro, dt); + compute_body_orientation_and_rates(); + } +#endif + + ahrs_finv_last_stamp = stamp; +} + +static void accel_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *accel) +{ + if (ahrs_float_inv.is_aligned) { + ahrs_float_invariant_update_accel(accel); + } +} + +static void mag_cb(uint8_t sender_id __attribute__((unused)), + uint32_t stamp __attribute__((unused)), + struct Int32Vect3 *mag) +{ + if (ahrs_float_inv.is_aligned) { + ahrs_float_invariant_update_mag(mag); + } +} + +static void aligner_cb(uint8_t __attribute__((unused)) sender_id, + uint32_t stamp __attribute__((unused)), + struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, + struct Int32Vect3 *lp_mag) +{ + if (!ahrs_float_inv.is_aligned) { + ahrs_float_invariant_align(lp_gyro, lp_accel, lp_mag); + compute_body_orientation_and_rates(); + } +} + +static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)), + struct FloatQuat *q_b2i_f) +{ + ahrs_float_inv_set_body_to_imu_quat(q_b2i_f); +} + +static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h) +{ + memcpy(&ahrs_float_inv.mag_h, h, sizeof(struct FloatVect3)); +} + +static bool_t ahrs_float_invariant_enable_output(bool_t enable) +{ + ahrs_finv_output_enabled = enable; + return ahrs_finv_output_enabled; +} + +/** + * Compute body orientation and rates from imu orientation and rates + */ +static void compute_body_orientation_and_rates(void) +{ + if (ahrs_finv_output_enabled) { + /* Compute LTP to BODY quaternion */ + struct FloatQuat ltp_to_body_quat; + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu); + float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat); + /* Set state */ + stateSetNedToBodyQuat_f(<p_to_body_quat); + + /* compute body rates */ + struct FloatRates body_rate; + RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias); + struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &body_rate); + stateSetBodyRates_f(&body_rate); + + } +} + + +void ahrs_float_invariant_register(void) +{ + ahrs_finv_output_enabled = AHRS_FINV_OUTPUT_ENABLED; + ahrs_float_invariant_init(); + ahrs_register_impl(ahrs_float_invariant_enable_output); + + /* + * Subscribe to scaled IMU measurements and attach callbacks + */ + AbiBindMsgIMU_MAG_INT32(AHRS_FINV_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO_INT32(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_INT32(AHRS_FINV_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_LOWPASSED(AHRS_FINV_IMU_ID, &aligner_ev, aligner_cb); + AbiBindMsgBODY_TO_IMU_QUAT(AHRS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); + AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att); + register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); +#endif +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.h new file mode 100644 index 0000000000..9458d4d9e5 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2015 Gautier Hattenberger + * + * 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_invariant_wrapper.h + * + * Paparazzi specific wrapper to run INVARIANT ahrs filter. + */ + +#ifndef AHRS_FLOAT_INVARIANT_WRAPPER_H +#define AHRS_FLOAT_INVARIANT_WRAPPER_H + +#include "subsystems/ahrs/ahrs_float_invariant.h" + +#ifndef PRIMARY_AHRS +#define PRIMARY_AHRS ahrs_float_invariant +#endif + +extern void ahrs_float_invariant_register(void); + +#endif /* AHRS_FLOAT_INVARIANT_WRAPPER_H */