mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-21 03:43:26 +08:00
[ahrs] add (yet another) AHRS based on an invariant filter
This commit is contained in:
Gautier Hattenberger
@@ -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
|
||||
@@ -0,0 +1,17 @@
|
||||
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="invariant">
|
||||
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ahrs_float_inv.reset" shortname="reset"/>
|
||||
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.lx" shortname="lx" module="subsystems/ahrs/ahrs_float_invariant"/>
|
||||
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.ly" shortname="ly"/>
|
||||
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.lz" shortname="lz"/>
|
||||
<dl_setting MAX=".1" MIN="0." STEP="0.001" VAR="ahrs_float_inv.gains.mx" shortname="mx"/>
|
||||
<dl_setting MAX=".1" MIN="0." STEP="0.001" VAR="ahrs_float_inv.gains.my" shortname="my"/>
|
||||
<dl_setting MAX=".1" MIN="0." STEP="0.001" VAR="ahrs_float_inv.gains.mz" shortname="mz"/>
|
||||
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.n" shortname="n"/>
|
||||
<dl_setting MAX="1." MIN="0." STEP="0.01" VAR="ahrs_float_inv.gains.o" shortname="o"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/ahrs/ahrs_float_invariant.c
|
||||
* @author Jean-Philippe Condomines <jp.condomines@gmail.com>
|
||||
*
|
||||
* 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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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 */
|
||||
|
||||
@@ -0,0 +1,237 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2015 Jean-Philippe Condomines, Gautier Hattenberger
|
||||
* 2015 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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 */
|
||||
Reference in New Issue
Block a user