mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -1,12 +0,0 @@
|
||||
<!-- DOCTYPE flight_plan SYSTEM "flight_plan.dtd" -->
|
||||
|
||||
<flight_plan NAME="GRZ" LON0="1.27289" MAX_DIST_FROM_HOME="1000" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
|
||||
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="120.0" alt="250."/>
|
||||
</waypoints>
|
||||
|
||||
<blocks>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
|
||||
@@ -1,85 +0,0 @@
|
||||
<!DOCTYPE settings SYSTEM "settings.dtd">
|
||||
|
||||
<!-- A conf for a quadrirotor -->
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
|
||||
<dl_settings name="trim">
|
||||
<dl_setting var="trim_roll" min="-1000" step="10" max="1000"/>
|
||||
<dl_setting var="trim_pitch" min="-1000" step="10" max="1000"/>
|
||||
<dl_setting var="trim_yaw" min="-1000" step="10" max="1000"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="rate">
|
||||
|
||||
<dl_setting VAR="roll_dot_pid.p_gain" MAX="6000" STEP="100" MIN="2000"/>
|
||||
<dl_setting VAR="roll_dot_pid.i_gain" MAX="750" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="roll_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="roll_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="pitch_dot_pid.p_gain" MAX="-2000" STEP="100" MIN="-16000"/>
|
||||
<dl_setting VAR="pitch_dot_pid.i_gain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="pitch_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="pitch_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="yaw_dot_pid.p_gain" MAX="-2000" STEP="100" MIN="-6000"/>
|
||||
<dl_setting VAR="yaw_dot_pid.i_gain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="yaw_dot_pid.d_gain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="yaw_dot_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="att">
|
||||
|
||||
<dl_setting VAR="roll_pid.p_gain" MAX="-0.1" STEP="0.1" MIN="-5."/>
|
||||
<dl_setting VAR="roll_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="roll_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="pitch_pid.p_gain" MAX="-.1" STEP="0.1" MIN="-15."/>
|
||||
<dl_setting VAR="pitch_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="pitch_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="yaw_pid.p_gain" MAX="-0.1" STEP="0.1" MIN="-5."/>
|
||||
<dl_setting VAR="yaw_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="yaw_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="yaw_pid.set_point" MAX="3" STEP="0.1" MIN="-3"/>
|
||||
<dl_setting VAR="pitch_pid.set_point" MAX="0.3" STEP="0.05" MIN="-0.3"/>
|
||||
<dl_setting VAR="roll_pid.set_point" MAX="0.3" STEP="0.05" MIN="-0.3"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="alt">
|
||||
|
||||
<dl_setting VAR="ctl_grz_z_dot_pgain" MAX="-500" STEP="100" MIN="-6000"/>
|
||||
<dl_setting VAR="ctl_grz_z_dot_igain" MAX="5" STEP="0.05" MIN="0"/>
|
||||
<dl_setting VAR="ctl_grz_z_dot_dgain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="ctl_grz_throttle_level" MAX="10000" STEP="100" MIN="3000"/>
|
||||
<dl_setting VAR="ctl_grz_z_dot_sum_err" MAX="1000" STEP="10" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="ctl_grz_z_pgain" MAX="-0.1" STEP="0.05" MIN="-2"/>
|
||||
<dl_setting VAR="ctl_grz_z_igain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
<dl_setting VAR="ctl_grz_z_dgain" MAX="10" STEP="0.5" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings name="speed">
|
||||
|
||||
<dl_setting VAR="ve_pid.p_gain" MAX="-0.01" STEP="0.01" MIN="-0.5"/>
|
||||
<dl_setting VAR="ve_pid.i_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="ve_pid.d_gain" MAX="10" STEP="0.1" MIN="0"/>
|
||||
|
||||
<dl_setting VAR="estimator_hspeed_mod" MAX="2" STEP="0.1" MIN="0"/>
|
||||
<dl_setting VAR="estimator_hspeed_dir" MAX="3" STEP="0.1" MIN="-3"/>
|
||||
|
||||
<dl_setting VAR="ve_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
<dl_setting VAR="vn_pid.sum_err" MAX="1" STEP="1" MIN="0"/>
|
||||
<dl_setting VAR="sum_err_reset" MAX="1" STEP="1" MIN="0"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
</settings>
|
||||
@@ -1,34 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
|
||||
<telemetry>
|
||||
<process name="Ap">
|
||||
<mode name="default">
|
||||
<message name="DL_VALUE" period="1"/>
|
||||
<message name="PPRZ_MODE" period="5"/>
|
||||
<message name="RANGEFINDER" period="0.2"/>
|
||||
<message name="GRZ_RATE_LOOP" period="0.5"/>
|
||||
<message name="SPEED_LOOP" period="0.5"/>
|
||||
<message name="ATTITUDE" period="0.5"/>
|
||||
<message name="GRZ_ATTITUDE_LOOP" period="0.5"/>
|
||||
<message name="GRZ_MEASURE" period="0.5"/>
|
||||
</mode>
|
||||
<mode name="debug">
|
||||
<message name="PPRZ_MODE" period="5"/>
|
||||
</mode>
|
||||
</process>
|
||||
<process name="Fbw">
|
||||
<mode name="default">
|
||||
<message name="COMMANDS" period="1"/>
|
||||
<message name="FBW_STATUS" period="1"/>
|
||||
<message name="ACTUATORS" period="5"/>
|
||||
<message name="RC" period="1"/>
|
||||
</mode>
|
||||
<mode name="debug">
|
||||
<message name="PPM" period="0.5"/>
|
||||
<message name="RC" period="0.5"/>
|
||||
<message name="COMMANDS" period="0.5"/>
|
||||
<message name="FBW_STATUS" period="1"/>
|
||||
<message name="ACTUATORS" period="5"/>
|
||||
</mode>
|
||||
</process>
|
||||
</telemetry>
|
||||
-1633
File diff suppressed because it is too large
Load Diff
@@ -1,103 +0,0 @@
|
||||
/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
|
||||
* $Id$
|
||||
*
|
||||
* Fast AHRS object almost ready for use on microcontrollers
|
||||
* Fixed-point mode can be use !
|
||||
*
|
||||
* (c) 2003 Trammell Hudson <hudson@rotomotion.com>
|
||||
* (c) 2005 Jean-Pierre Dumont <jpxDOTdumontATwanadooDOTfr>
|
||||
*
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** \file ahrs.h
|
||||
* \brief Attitude Heading Reference System (gyros, accels and magneto
|
||||
* filtered through a Kalman)
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __AHRS_
|
||||
#define __AHRS_
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
typedef float real_t;
|
||||
#define real_t_min 0.000000001
|
||||
typedef uint8_t index_t;
|
||||
|
||||
|
||||
/*
|
||||
* We have seven variables in our state -- the quaternion attitude
|
||||
* estimate and three gyro bias values. The state time update equation
|
||||
* comes from the IMU gyro sensor readings:
|
||||
*
|
||||
* Q_dot = Wxq(pqr) * Q
|
||||
* Bias_dot = 0
|
||||
*/
|
||||
|
||||
|
||||
extern real_t X[7];
|
||||
extern real_t quat[4];
|
||||
extern real_t q0;
|
||||
extern real_t q1;
|
||||
extern real_t q2;
|
||||
extern real_t q3;
|
||||
extern real_t bias[3];
|
||||
extern real_t bias_p;
|
||||
extern real_t bias_q;
|
||||
extern real_t bias_r;
|
||||
|
||||
extern real_t pqr[3];
|
||||
extern real_t accel[3];
|
||||
|
||||
extern int16_t accel_raw[3];
|
||||
extern int16_t gyro_raw[3];
|
||||
extern int16_t accel_raw_zero[3];
|
||||
extern int16_t gyro_raw_zero[3];
|
||||
|
||||
/*
|
||||
* The euler estimate will be updated less frequently than the
|
||||
* quaternion, but some applications are ok with that.
|
||||
*/
|
||||
extern real_t ahrs_euler[3];
|
||||
|
||||
/** @name ahrs_state possible values */
|
||||
//@{
|
||||
#define AHRS_NOT_INITIALIZED 0
|
||||
#define AHRS_IMU_CALIBRATION 1
|
||||
#define AHRS_RUNNING 2
|
||||
//@}
|
||||
|
||||
/** Current internal state */
|
||||
extern uint8_t ahrs_state;
|
||||
|
||||
|
||||
/** Restarts the ahrs with zero_calibration phase */
|
||||
void ahrs_init( uint8_t do_zero_calibration );
|
||||
|
||||
/** Udates state with accels and compas (if available) */
|
||||
void ahrs_update( void );
|
||||
|
||||
#if (!defined IMU_GYROS_CONNECTED_TO_AP) || (IMU_GYROS_CONNECTED_TO_AP == 0)
|
||||
|
||||
/** Function to be called when gyro data are available (through the link to
|
||||
the fbw mcu */
|
||||
void ahrs_gyro_update( void );
|
||||
#endif //IMU_GYROS_CONNECTED_TO_AP
|
||||
|
||||
#endif //__AHRS_
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,33 +0,0 @@
|
||||
#ifndef AHRS_H
|
||||
#define AHRS_H
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#define index_t uint8_t
|
||||
|
||||
/* our state */
|
||||
extern float q0, q1, q2, q3;
|
||||
extern float bias_p, bias_q, bias_r;
|
||||
/* we maintain eulers angles */
|
||||
extern float ahrs_phi, ahrs_theta, ahrs_psi;
|
||||
/* we maintain unbiased rates */
|
||||
extern float ahrs_p, ahrs_q, ahrs_r;
|
||||
|
||||
extern float P[7][7]; /* covariance */
|
||||
extern float A[4][7]; /* jacobian */
|
||||
|
||||
extern void ahrs_init( const int16_t *mag, const float* accel, const float* gyro );
|
||||
extern void ahrs_predict( const float* gyro );
|
||||
|
||||
extern void ahrs_roll_update( const float* accel);
|
||||
extern void ahrs_pitch_update( const float* accel);
|
||||
extern void ahrs_yaw_update( const int16_t* mag);
|
||||
|
||||
#if 0
|
||||
extern float ahrs_roll_of_accel( const float* accel_cal );
|
||||
extern float ahrs_pitch_of_accel( const float* accel_cal);
|
||||
extern float ahrs_yaw_of_mag( const int16_t *mag);
|
||||
#endif
|
||||
|
||||
#endif /* AHRS_H */
|
||||
@@ -0,0 +1,439 @@
|
||||
//#include "ahrs_quat_fast_ekf.h"
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <inttypes.h>
|
||||
#include <string.h>
|
||||
|
||||
/* Time step */
|
||||
#define afe_dt 4e-3
|
||||
|
||||
/* We have seven variables in our state -- the quaternion attitude
|
||||
* estimate and three gyro bias values
|
||||
*/
|
||||
FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
|
||||
//FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
|
||||
|
||||
/* We maintain unbiased rates. */
|
||||
//FLOAT_T afe_p, afe_q, afe_r;
|
||||
|
||||
/* We maintain eulers */
|
||||
//FLOAT_T afe_phi, afe_theta, afe_psi;
|
||||
|
||||
/* time derivative of state
|
||||
* we know that bias_p_dot = bias_q_dot = bias_r_dot = 0
|
||||
*/
|
||||
FLOAT_T afe_q0_dot, afe_q1_dot, afe_q2_dot, afe_q3_dot;
|
||||
|
||||
/*
|
||||
* The Direction Cosine Matrix is used to help rotate measurements
|
||||
* to and from the body frame. We only need five elements from it,
|
||||
* so those are computed explicitly rather than the entire matrix
|
||||
*
|
||||
* External routines might want these (to until sensor readings),
|
||||
* so we export them.
|
||||
*/
|
||||
FLOAT_T afe_dcm00;
|
||||
FLOAT_T afe_dcm01;
|
||||
FLOAT_T afe_dcm02;
|
||||
FLOAT_T afe_dcm12;
|
||||
FLOAT_T afe_dcm22;
|
||||
|
||||
/*
|
||||
* Covariance matrix and covariance matrix derivative
|
||||
*/
|
||||
FLOAT_T afe_P[7][7];
|
||||
FLOAT_T afe_Pdot[7][7];
|
||||
|
||||
/*
|
||||
* F represents the Jacobian of the derivative of the system with respect
|
||||
* its states. We do not allocate the bottom three rows since we know that
|
||||
* the derivatives of bias_dot are all zero.
|
||||
*/
|
||||
FLOAT_T afe_F[4][7];
|
||||
|
||||
/*
|
||||
* Kalman filter variables.
|
||||
*/
|
||||
FLOAT_T afe_PHt[7];
|
||||
FLOAT_T afe_K[7];
|
||||
FLOAT_T afe_E;
|
||||
|
||||
/*
|
||||
* H represents the Jacobian of the measurements of the attitude
|
||||
* with respect to the states of the filter. We do not allocate the bottom
|
||||
* three rows since we know that the attitude measurements have no
|
||||
* relationship to gyro bias.
|
||||
*/
|
||||
FLOAT_T afe_H[4];
|
||||
/* temporary variable used during state covariance update */
|
||||
FLOAT_T afe_FP[4][7];
|
||||
|
||||
/*
|
||||
* Q is our estimate noise variance. It is supposed to be an NxN
|
||||
* matrix, but with elements only on the diagonals. Additionally,
|
||||
* since the quaternion has no expected noise (we can't directly measure
|
||||
* it), those are zero. For the gyro, we expect around 5 deg/sec noise,
|
||||
* which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
|
||||
*/
|
||||
/* I measured about 0.009 rad/s noise */
|
||||
#define afe_Q_gyro 8e-03
|
||||
|
||||
/*
|
||||
* R is our measurement noise estimate. Like Q, it is supposed to be
|
||||
* an NxN matrix with elements on the diagonals. However, since we can
|
||||
* not directly measure the gyro bias, we have no estimate for it.
|
||||
* We only have an expected noise in the pitch and roll accelerometers
|
||||
* and in the compass.
|
||||
*/
|
||||
#define AFE_R_PHI 1.3 * 1.3
|
||||
#define AFE_R_THETA 1.3 * 1.3
|
||||
#define AFE_R_PSI 2.5 * 2.5
|
||||
|
||||
#include "ahrs_quat_fast_ekf_utils.h"
|
||||
|
||||
/*
|
||||
* Call ahrs_state_update every dt seconds with the raw body frame angular
|
||||
* rates. It updates the attitude state estimate via this function:
|
||||
*
|
||||
* quat_dot = Wxq(pqr) * quat
|
||||
* bias_dot = 0
|
||||
*
|
||||
* Since F also contains Wxq, we fill it in here and then reuse the computed
|
||||
* values. This avoids the extra floating point math.
|
||||
*
|
||||
* Wxq is the quaternion omega matrix:
|
||||
*
|
||||
* [ 0, -p, -q, -r ]
|
||||
* 1/2 * [ p, 0, r, -q ]
|
||||
* [ q, -r, 0, p ]
|
||||
* [ r, q, -p, 0 ]
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
* [ 0 -p -q -r q1 q2 q3]
|
||||
* F = 1/2 * [ p 0 r -q -q0 q3 -q2]
|
||||
* [ q -r 0 p -q3 q0 q1]
|
||||
* [ r q -p 0 q2 -q1 -q0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
* [ 0 0 0 0 0 0 0]
|
||||
*
|
||||
*/
|
||||
void afe_predict( const FLOAT_T* gyro ) {
|
||||
/* Unbias our gyro values */
|
||||
booz_ahrs_p = gyro[0] - booz_ahrs_bp;
|
||||
booz_ahrs_q = gyro[1] - booz_ahrs_bq;
|
||||
booz_ahrs_r = gyro[2] - booz_ahrs_br;
|
||||
|
||||
/* compute F
|
||||
F is only needed later on to update the state covariance P.
|
||||
However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
|
||||
compute the time derivative of the quaternion, so we compute F now */
|
||||
|
||||
/* Fill in Wxq(pqr) into F */
|
||||
afe_F[0][0] = afe_F[1][1] = afe_F[2][2] = afe_F[3][3] = 0;
|
||||
afe_F[1][0] = afe_F[2][3] = booz_ahrs_p * 0.5;
|
||||
afe_F[2][0] = afe_F[3][1] = booz_ahrs_q * 0.5;
|
||||
afe_F[3][0] = afe_F[1][2] = booz_ahrs_r * 0.5;
|
||||
|
||||
afe_F[0][1] = afe_F[3][2] = -afe_F[1][0];
|
||||
afe_F[0][2] = afe_F[1][3] = -afe_F[2][0];
|
||||
afe_F[0][3] = afe_F[2][1] = -afe_F[3][0];
|
||||
/* Fill in [4:6][0:3] region */
|
||||
afe_F[0][4] = afe_F[2][6] = afe_q1 * 0.5;
|
||||
afe_F[0][5] = afe_F[3][4] = afe_q2 * 0.5;
|
||||
afe_F[0][6] = afe_F[1][5] = afe_q3 * 0.5;
|
||||
|
||||
afe_F[1][4] = afe_F[2][5] = afe_F[3][6] = afe_q0 * -0.5;
|
||||
afe_F[3][5] = -afe_F[0][4];
|
||||
afe_F[1][6] = -afe_F[0][5];
|
||||
afe_F[2][4] = -afe_F[0][6];
|
||||
|
||||
/* update state */
|
||||
/* quat_dot = Wxq(pqr) * quat */
|
||||
afe_q0_dot = afe_F[0][1] * afe_q1 + afe_F[0][2] * afe_q2 + afe_F[0][3] * afe_q3;
|
||||
afe_q1_dot = afe_F[1][0] * afe_q0 + afe_F[1][2] * afe_q2 + afe_F[1][3] * afe_q3;
|
||||
afe_q2_dot = afe_F[2][0] * afe_q0 + afe_F[2][1] * afe_q1 + afe_F[2][3] * afe_q3;
|
||||
afe_q3_dot = afe_F[3][0] * afe_q0 + afe_F[3][1] * afe_q1 + afe_F[3][2] * afe_q2;
|
||||
|
||||
/* quat = quat + quat_dot * dt */
|
||||
afe_q0 += afe_q0_dot * afe_dt;
|
||||
afe_q1 += afe_q1_dot * afe_dt;
|
||||
afe_q2 += afe_q2_dot * afe_dt;
|
||||
afe_q3 += afe_q3_dot * afe_dt;
|
||||
|
||||
/* normalize quaternion */
|
||||
AFE_NORM_QUAT();
|
||||
/* */
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
|
||||
#ifdef EKF_UPDATE_DISCRETE
|
||||
/*
|
||||
* update covariance
|
||||
* Pdot = F*P*F' + Q
|
||||
* P += Pdot * dt
|
||||
*/
|
||||
uint8_t i, j, k;
|
||||
for (i=0; i<4; i++) {
|
||||
for (j=0; j<7; j++) {
|
||||
afe_FP[i][j] = 0.;
|
||||
for (k=0; k<7; k++) {
|
||||
afe_FP[i][j] += afe_F[i][k] * afe_P[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i=0; i<4; i++) {
|
||||
for (j=0; j<4; j++) {
|
||||
afe_Pdot[i][j] = 0.;
|
||||
for (k=0; k<7; k++) {
|
||||
afe_Pdot[i][j] += afe_FP[i][k] * afe_F[j][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
/* add Q !!! added below*/
|
||||
// Pdot[4][4] = afe_Q_gyro;
|
||||
// Pdot[5][5] = afe_Q_gyro;
|
||||
// Pdot[6][6] = afe_Q_gyro;
|
||||
|
||||
/* P = P + Pdot * dt */
|
||||
for (i=0; i<4; i++)
|
||||
for (j=0; j<4; j++)
|
||||
afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
|
||||
afe_P[4][4] += afe_Q_gyro * afe_dt;
|
||||
afe_P[5][5] += afe_Q_gyro * afe_dt;
|
||||
afe_P[6][6] += afe_Q_gyro * afe_dt;
|
||||
#endif
|
||||
|
||||
#ifdef EKF_UPDATE_CONTINUOUS
|
||||
/* Pdot = F*P + F'*P + Q */
|
||||
memset( afe_Pdot, 0, sizeof(afe_Pdot) );
|
||||
/*
|
||||
* Compute the A*P+P*At for the bottom rows of P and A_tranpose
|
||||
*/
|
||||
uint8_t i, j, k;
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
for( k=4 ; k<7 ; k++ )
|
||||
{
|
||||
const FLOAT_T F_i_k = afe_F[i][k];
|
||||
afe_Pdot[i][j] += F_i_k * afe_P[k][j];
|
||||
afe_Pdot[j][i] += afe_P[j][k] * F_i_k;
|
||||
}
|
||||
/*
|
||||
* Compute F*P + P*Ft for the region [0..3][0..3]
|
||||
*/
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<4 ; j++ )
|
||||
for( k=0 ; k<4 ; k++ )
|
||||
{
|
||||
/* The diagonals of A are zero */
|
||||
if( i == k && j == k )
|
||||
continue;
|
||||
if( j == k )
|
||||
afe_Pdot[i][j] += afe_F[i][k] * afe_P[k][j];
|
||||
else
|
||||
if( i == k )
|
||||
afe_Pdot[i][j] += afe_P[i][k] * afe_F[j][k];
|
||||
else
|
||||
afe_Pdot[i][j] += ( afe_F[i][k] * afe_P[k][j] +
|
||||
afe_P[i][k] * afe_F[j][k] );
|
||||
}
|
||||
/* Add in the non-zero parts of Q. The quaternion portions
|
||||
* are all zero, and all the gyros share the same value.
|
||||
*/
|
||||
for( i=4 ; i<7 ; i++ )
|
||||
afe_Pdot[i][i] += afe_Q_gyro;
|
||||
|
||||
/* Compute P = P + Pdot * dt */
|
||||
for( i=0 ; i<7 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
static void run_kalman( const FLOAT_T R_axis, const FLOAT_T error ) {
|
||||
int i, j;
|
||||
|
||||
/* PHt = P * H' */
|
||||
afe_PHt[0] = afe_P[0][0] * afe_H[0] + afe_P[0][1] * afe_H[1] + afe_P[0][2] * afe_H[2] + afe_P[0][3] * afe_H[3];
|
||||
afe_PHt[1] = afe_P[1][0] * afe_H[0] + afe_P[1][1] * afe_H[1] + afe_P[1][2] * afe_H[2] + afe_P[1][3] * afe_H[3];
|
||||
afe_PHt[2] = afe_P[2][0] * afe_H[0] + afe_P[2][1] * afe_H[1] + afe_P[2][2] * afe_H[2] + afe_P[2][3] * afe_H[3];
|
||||
afe_PHt[3] = afe_P[3][0] * afe_H[0] + afe_P[3][1] * afe_H[1] + afe_P[3][2] * afe_H[2] + afe_P[3][3] * afe_H[3];
|
||||
afe_PHt[4] = afe_P[4][0] * afe_H[0] + afe_P[4][1] * afe_H[1] + afe_P[4][2] * afe_H[2] + afe_P[4][3] * afe_H[3];
|
||||
afe_PHt[5] = afe_P[5][0] * afe_H[0] + afe_P[5][1] * afe_H[1] + afe_P[5][2] * afe_H[2] + afe_P[5][3] * afe_H[3];
|
||||
afe_PHt[6] = afe_P[6][0] * afe_H[0] + afe_P[6][1] * afe_H[1] + afe_P[6][2] * afe_H[2] + afe_P[6][3] * afe_H[3];
|
||||
|
||||
/* E = H * PHt + R */
|
||||
afe_E = R_axis;
|
||||
afe_E += afe_H[0] * afe_PHt[0];
|
||||
afe_E += afe_H[1] * afe_PHt[1];
|
||||
afe_E += afe_H[2] * afe_PHt[2];
|
||||
afe_E += afe_H[3] * afe_PHt[3];
|
||||
|
||||
/* Compute the inverse of E */
|
||||
afe_E = 1.0 / afe_E;
|
||||
|
||||
/* Compute K = P * H' * inv(E) */
|
||||
afe_K[0] = afe_PHt[0] * afe_E;
|
||||
afe_K[1] = afe_PHt[1] * afe_E;
|
||||
afe_K[2] = afe_PHt[2] * afe_E;
|
||||
afe_K[3] = afe_PHt[3] * afe_E;
|
||||
afe_K[4] = afe_PHt[4] * afe_E;
|
||||
afe_K[5] = afe_PHt[5] * afe_E;
|
||||
afe_K[6] = afe_PHt[6] * afe_E;
|
||||
|
||||
/* Update our covariance matrix: P = P - K * H * P */
|
||||
|
||||
/* Compute HP = H * P, reusing the PHt array. */
|
||||
afe_PHt[0] = afe_H[0] * afe_P[0][0] + afe_H[1] * afe_P[1][0] + afe_H[2] * afe_P[2][0] + afe_H[3] * afe_P[3][0];
|
||||
afe_PHt[1] = afe_H[0] * afe_P[0][1] + afe_H[1] * afe_P[1][1] + afe_H[2] * afe_P[2][1] + afe_H[3] * afe_P[3][1];
|
||||
afe_PHt[2] = afe_H[0] * afe_P[0][2] + afe_H[1] * afe_P[1][2] + afe_H[2] * afe_P[2][2] + afe_H[3] * afe_P[3][2];
|
||||
afe_PHt[3] = afe_H[0] * afe_P[0][3] + afe_H[1] * afe_P[1][3] + afe_H[2] * afe_P[2][3] + afe_H[3] * afe_P[3][3];
|
||||
afe_PHt[4] = afe_H[0] * afe_P[0][4] + afe_H[1] * afe_P[1][4] + afe_H[2] * afe_P[2][4] + afe_H[3] * afe_P[3][4];
|
||||
afe_PHt[5] = afe_H[0] * afe_P[0][5] + afe_H[1] * afe_P[1][5] + afe_H[2] * afe_P[2][5] + afe_H[3] * afe_P[3][5];
|
||||
afe_PHt[6] = afe_H[0] * afe_P[0][6] + afe_H[1] * afe_P[1][6] + afe_H[2] * afe_P[2][6] + afe_H[3] * afe_P[3][6];
|
||||
|
||||
/* Compute P -= K * HP (aliased to PHt) */
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
for( j=0 ; j<7 ; j++ )
|
||||
afe_P[i][j] -= afe_K[i] * afe_PHt[j];
|
||||
|
||||
/* Update our state: X += K * error */
|
||||
afe_q0 += afe_K[0] * error;
|
||||
afe_q1 += afe_K[1] * error;
|
||||
afe_q2 += afe_K[2] * error;
|
||||
afe_q3 += afe_K[3] * error;
|
||||
booz_ahrs_bp += afe_K[4] * error;
|
||||
booz_ahrs_bq += afe_K[5] * error;
|
||||
booz_ahrs_br += afe_K[6] * error;
|
||||
|
||||
/* normalize quaternion */
|
||||
AFE_NORM_QUAT();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Do the Kalman filter on the acceleration and compass readings.
|
||||
* This is normally a very simple:
|
||||
*
|
||||
* E = H * P * H' + R
|
||||
* K = P * H' * inv(E)
|
||||
* P = P - K * H * P
|
||||
* X = X + K * error
|
||||
*
|
||||
* We notice that P * H' is used twice, so we can cache the
|
||||
* results of it.
|
||||
*
|
||||
* H represents the Jacobian of measurements to states, which we know
|
||||
* to only have the top four rows filled in since the attitude
|
||||
* measurement does not relate to the gyro bias. This allows us to
|
||||
* ignore parts of PHt
|
||||
*
|
||||
* We also only process one axis at a time to avoid having to perform
|
||||
* the 3x3 matrix inversion.
|
||||
*/
|
||||
|
||||
void afe_update_phi( const FLOAT_T* accel ) {
|
||||
AFE_COMPUTE_H_PHI();
|
||||
FLOAT_T accel_phi = afe_phi_of_accel(accel);
|
||||
FLOAT_T err_phi = accel_phi - booz_ahrs_phi;
|
||||
AFE_WARP( err_phi, M_PI);
|
||||
run_kalman( AFE_R_PHI, err_phi );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
void afe_update_theta( const FLOAT_T* accel ) {
|
||||
AFE_COMPUTE_H_THETA();
|
||||
FLOAT_T accel_theta = afe_theta_of_accel(accel);
|
||||
FLOAT_T err_theta = accel_theta - booz_ahrs_theta;
|
||||
AFE_WARP( err_theta, M_PI_2);
|
||||
run_kalman( AFE_R_THETA, err_theta );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
void afe_update_psi( const int16_t* mag ) {
|
||||
AFE_COMPUTE_H_PSI();
|
||||
FLOAT_T mag_psi = afe_psi_of_mag(mag);
|
||||
FLOAT_T err_psi = mag_psi - booz_ahrs_psi;
|
||||
AFE_WARP( err_psi, M_PI);
|
||||
run_kalman( AFE_R_PSI, err_psi );
|
||||
AFE_DCM_OF_QUAT();
|
||||
AFE_EULER_OF_DCM();
|
||||
}
|
||||
|
||||
|
||||
void booz_ahrs_init(void) {
|
||||
booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
|
||||
booz_ahrs_phi = 0.;
|
||||
booz_ahrs_theta = 0.;
|
||||
booz_ahrs_psi = 0.;
|
||||
|
||||
booz_ahrs_p = 0.;
|
||||
booz_ahrs_q = 0.;
|
||||
booz_ahrs_r = 0.;
|
||||
|
||||
booz_ahrs_bp = 0.;
|
||||
booz_ahrs_bq = 0.;
|
||||
booz_ahrs_br = 0.;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Initialize the AHRS state data and covariance matrix.
|
||||
*/
|
||||
void booz_ahrs_start( const FLOAT_T* accel, const FLOAT_T* gyro, const int16_t *mag ) {
|
||||
|
||||
/* F and P will be updated only on the non zero locations */
|
||||
memset( (void*) afe_F, 0, sizeof( afe_F ) );
|
||||
memset( (void*) afe_P, 0, sizeof( afe_P ) );
|
||||
int i;
|
||||
for( i=0 ; i<4 ; i++ )
|
||||
afe_P[i][i] = 1.;
|
||||
for( i=4 ; i<7 ; i++ )
|
||||
afe_P[i][i] = .5;
|
||||
|
||||
/* assume vehicle is still, so initial bias are gyro readings */
|
||||
booz_ahrs_bp = gyro[0];
|
||||
booz_ahrs_bq = gyro[1];
|
||||
booz_ahrs_br = gyro[2];
|
||||
|
||||
booz_ahrs_phi = afe_phi_of_accel(accel);
|
||||
booz_ahrs_theta = afe_theta_of_accel(accel);
|
||||
booz_ahrs_psi = 0.;
|
||||
|
||||
AFE_QUAT_OF_EULER();
|
||||
AFE_DCM_OF_QUAT();
|
||||
booz_ahrs_psi = afe_psi_of_mag( mag );
|
||||
|
||||
AFE_QUAT_OF_EULER();
|
||||
AFE_DCM_OF_QUAT();
|
||||
|
||||
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
|
||||
|
||||
}
|
||||
|
||||
void booz_ahrs_run(const FLOAT_T* accel, const FLOAT_T* gyro, const int16_t *mag ) {
|
||||
static uint8_t step = 0;
|
||||
afe_predict( gyro );
|
||||
step++; if(step>3) step=0;
|
||||
switch (step) {
|
||||
case 0:
|
||||
afe_update_phi( accel );
|
||||
break;
|
||||
case 1:
|
||||
afe_update_theta( accel );
|
||||
break;
|
||||
case 3:
|
||||
afe_update_psi( mag );
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef AHRS_QUAT_FAST_EKF_H
|
||||
#define AHRS_QUAT_FAST_EKF_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "6dof.h"
|
||||
|
||||
//#define FLOAT_T double
|
||||
#define FLOAT_T float
|
||||
|
||||
/* ekf state : quaternion and gyro biases */
|
||||
extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
|
||||
//extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
|
||||
/* we maintain unbiased rates */
|
||||
//extern FLOAT_T afe_p, afe_q, afe_r;
|
||||
/* we maintain eulers angles */
|
||||
//extern FLOAT_T afe_phi, afe_theta, afe_psi;
|
||||
|
||||
extern FLOAT_T afe_P[7][7]; /* covariance */
|
||||
|
||||
extern void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro );
|
||||
extern void afe_predict( const FLOAT_T* gyro );
|
||||
extern void afe_update_phi( const FLOAT_T* accel);
|
||||
extern void afe_update_theta( const FLOAT_T* accel);
|
||||
extern void afe_update_psi( const int16_t* mag);
|
||||
|
||||
#endif /* AHRS_QUAT_FAST_EKF_H_H */
|
||||
@@ -0,0 +1,172 @@
|
||||
#ifndef AHRS_QUAT_FAST_EKF_UTILS_H
|
||||
#define AHRS_QUAT__FAST_EKF_UTILS_H
|
||||
|
||||
#include "ahrs_quat_fast_ekf.h"
|
||||
|
||||
/*
|
||||
* Compute the five elements of the DCM that we use for our
|
||||
* rotations and Jacobians. This is used by several other functions
|
||||
* to speedup their computations.
|
||||
*/
|
||||
#define AFE_DCM_OF_QUAT() { \
|
||||
afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3); \
|
||||
afe_dcm01 = 2*(afe_q1*afe_q2 + afe_q0*afe_q3); \
|
||||
afe_dcm02 = 2*(afe_q1*afe_q3 - afe_q0*afe_q2); \
|
||||
afe_dcm12 = 2*(afe_q2*afe_q3 + afe_q0*afe_q1); \
|
||||
afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2); \
|
||||
}
|
||||
/*
|
||||
* Compute Euler angles from our DCM.
|
||||
*/
|
||||
#define AFE_PHI_OF_DCM() { booz_ahrs_phi = atan2( afe_dcm12, afe_dcm22 );}
|
||||
#define AFE_THETA_OF_DCM() { booz_ahrs_theta = -asin( afe_dcm02 );}
|
||||
#define AFE_PSI_OF_DCM() { booz_ahrs_psi = atan2( afe_dcm01, afe_dcm00 );}
|
||||
#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()}
|
||||
|
||||
/*
|
||||
* initialise the quaternion from the set of eulers
|
||||
*/
|
||||
#define AFE_QUAT_OF_EULER() { \
|
||||
const FLOAT_T phi2 = booz_ahrs_phi / 2.0; \
|
||||
const FLOAT_T theta2 = booz_ahrs_theta / 2.0; \
|
||||
const FLOAT_T psi2 = booz_ahrs_psi / 2.0; \
|
||||
\
|
||||
const FLOAT_T sinphi2 = sin( phi2 ); \
|
||||
const FLOAT_T cosphi2 = cos( phi2 ); \
|
||||
\
|
||||
const FLOAT_T sintheta2 = sin( theta2 ); \
|
||||
const FLOAT_T costheta2 = cos( theta2 ); \
|
||||
\
|
||||
const FLOAT_T sinpsi2 = sin( psi2 ); \
|
||||
const FLOAT_T cospsi2 = cos( psi2 ); \
|
||||
\
|
||||
afe_q0 = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \
|
||||
afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \
|
||||
afe_q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \
|
||||
afe_q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \
|
||||
}
|
||||
|
||||
/*
|
||||
* normalize quaternion
|
||||
*/
|
||||
#define AFE_NORM_QUAT() { \
|
||||
FLOAT_T mag = afe_q0*afe_q0 + afe_q1*afe_q1 \
|
||||
+ afe_q2*afe_q2 + afe_q3*afe_q3; \
|
||||
mag = sqrt( mag ); \
|
||||
afe_q0 /= mag; \
|
||||
afe_q1 /= mag; \
|
||||
afe_q2 /= mag; \
|
||||
afe_q3 /= mag; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Compute the Jacobian of the measurements to the system states.
|
||||
*/
|
||||
#define AFE_COMPUTE_H_PHI() { \
|
||||
const FLOAT_T phi_err = 2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12); \
|
||||
afe_H[0] = (afe_q1 * afe_dcm22) * phi_err; \
|
||||
afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err; \
|
||||
afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err; \
|
||||
afe_H[3] = (afe_q2 * afe_dcm22) * phi_err; \
|
||||
}
|
||||
|
||||
#define AFE_COMPUTE_H_THETA() { \
|
||||
const FLOAT_T theta_err = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 ); \
|
||||
afe_H[0] = -afe_q2 * theta_err; \
|
||||
afe_H[1] = afe_q3 * theta_err; \
|
||||
afe_H[2] = -afe_q0 * theta_err; \
|
||||
afe_H[3] = afe_q1 * theta_err; \
|
||||
}
|
||||
|
||||
#define AFE_COMPUTE_H_PSI() { \
|
||||
const FLOAT_T psi_err = 2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01); \
|
||||
afe_H[0] = (afe_q3 * afe_dcm00) * psi_err; \
|
||||
afe_H[1] = (afe_q2 * afe_dcm00) * psi_err; \
|
||||
afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err; \
|
||||
afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err; \
|
||||
}
|
||||
|
||||
|
||||
/* convert sensor reading into euler angle measurement */
|
||||
static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
|
||||
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
|
||||
}
|
||||
|
||||
static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
|
||||
FLOAT_T g2 =
|
||||
accel[AXIS_X]*accel[AXIS_X] +
|
||||
accel[AXIS_Y]*accel[AXIS_Y] +
|
||||
accel[AXIS_Z]*accel[AXIS_Z];
|
||||
return -asin( accel[AXIS_X] / sqrt( g2 ) );
|
||||
}
|
||||
/*
|
||||
* The rotation matrix to rotate from NED frame to body frame without
|
||||
* rotating in the yaw axis is:
|
||||
*
|
||||
* [ 1 0 0 ] [ cos(Theta) 0 -sin(Theta) ]
|
||||
* [ 0 cos(Phi) sin(Phi) ] [ 0 1 0 ]
|
||||
* [ 0 -sin(Phi) cos(Phi) ] [ sin(Theta) 0 cos(Theta) ]
|
||||
*
|
||||
* This expands to:
|
||||
*
|
||||
* [ cos(Theta) 0 -sin(Theta) ]
|
||||
* [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)]
|
||||
* [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)]
|
||||
*
|
||||
* However, to untilt the compass reading, we need to use the
|
||||
* transpose of this matrix.
|
||||
*
|
||||
* [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ]
|
||||
* [ 0 cos(Phi) -sin(Phi) ]
|
||||
* [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ]
|
||||
*
|
||||
* Additionally,
|
||||
* since we already have the DCM computed for our current attitude,
|
||||
* we can short cut all of the trig. substituting
|
||||
* in from the definition of euler2quat and quat2euler, we have:
|
||||
*
|
||||
* [ cos(Theta) -dcm12*dcm02 -dcm22*dcm02 ]
|
||||
* [ 0 dcm22 -dcm12 ]
|
||||
* [ dcm02 dcm12 dcm22 ]
|
||||
*
|
||||
*/
|
||||
static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
|
||||
const FLOAT_T ctheta = cos( booz_ahrs_theta );
|
||||
#if 0
|
||||
const FLOAT_T mn = ctheta * mag[0]
|
||||
- (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
|
||||
|
||||
const FLOAT_T me =
|
||||
(afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
|
||||
#else
|
||||
const FLOAT_T stheta = sin( booz_ahrs_theta );
|
||||
const FLOAT_T cphi = cos( booz_ahrs_phi );
|
||||
const FLOAT_T sphi = sin( booz_ahrs_phi );
|
||||
|
||||
const FLOAT_T mn =
|
||||
ctheta* mag[0]+
|
||||
sphi*stheta* mag[1]+
|
||||
cphi*stheta* mag[2];
|
||||
const FLOAT_T me =
|
||||
0* mag[0]+
|
||||
cphi* mag[1]+
|
||||
-sphi* mag[2];
|
||||
#endif
|
||||
|
||||
const FLOAT_T psi = -atan2( me, mn );
|
||||
return psi;
|
||||
}
|
||||
|
||||
#define AFE_WARP(x, b) { \
|
||||
while( x < -b ) \
|
||||
x += 2 * b; \
|
||||
while( x > b ) \
|
||||
x -= 2 * b; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */
|
||||
+13
-4
@@ -1,6 +1,8 @@
|
||||
#ifndef BOOZ_AHRS_H
|
||||
#define BOOZ_AHRS_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#define BOOZ_AHRS_STATUS_UNINIT 0
|
||||
#define BOOZ_AHRS_STATUS_RUNNING 1
|
||||
#define BOOZ_AHRS_STATUS_CRASHED 2
|
||||
@@ -21,8 +23,11 @@ extern float booz_ahrs_br;
|
||||
|
||||
extern void booz_ahrs_init(void);
|
||||
extern void booz_ahrs_start( const float* accel, const float* gyro, const int16_t* mag);
|
||||
extern void booz_ahrs_predict( const float* gyro );
|
||||
extern void booz_ahrs_update( const float* accel, const int16_t* mag );
|
||||
extern void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag);
|
||||
/*
|
||||
extern void booz_ahrs_predict( const float* gyro );
|
||||
extern void booz_ahrs_update( const float* accel, const int16_t* mag );
|
||||
*/
|
||||
|
||||
//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext
|
||||
//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext)
|
||||
@@ -30,11 +35,15 @@ extern void booz_ahrs_update( const float* accel, const int16_t* mag );
|
||||
//#error BOOZ_AHRS_TYPE
|
||||
//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\"
|
||||
|
||||
#define BOOZ_AHRS_MULTITILT 0
|
||||
#define BOOZ_AHRS_EULER 1
|
||||
#define BOOZ_AHRS_MULTITILT 0
|
||||
#define BOOZ_AHRS_EULER 1
|
||||
#define BOOZ_AHRS_QUATERNION 2
|
||||
|
||||
#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
|
||||
#include "multitilt.h"
|
||||
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
|
||||
#include "ahrs_quat_fast_ekf.h"
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* BOOZ_AHRS_H */
|
||||
|
||||
@@ -50,7 +50,7 @@ STATIC_INLINE void booz_filter_main_init( void ) {
|
||||
#endif
|
||||
imu_v3_init();
|
||||
|
||||
multitilt_init();
|
||||
booz_ahrs_init();
|
||||
|
||||
booz_link_mcu_init();
|
||||
|
||||
@@ -68,12 +68,12 @@ STATIC_INLINE void booz_filter_main_event_task( void ) {
|
||||
STATIC_INLINE void booz_filter_main_periodic_task( void ) {
|
||||
/* triger measurements */
|
||||
// ami601_periodic();
|
||||
DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
|
||||
// DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
|
||||
ImuPeriodic();
|
||||
static uint8_t _50hz = 0;
|
||||
_50hz++;
|
||||
if (_50hz > 5) _50hz = 0;
|
||||
switch (_50hz) {
|
||||
static uint8_t _62hz = 0;
|
||||
_62hz++;
|
||||
if (_62hz > 4) _62hz = 0;
|
||||
switch (_62hz) {
|
||||
case 0:
|
||||
booz_filter_telemetry_periodic_task();
|
||||
break;
|
||||
@@ -82,20 +82,19 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
|
||||
}
|
||||
|
||||
static inline void on_imu_event( void ) {
|
||||
switch (mtt_status) {
|
||||
switch (booz_ahrs_status) {
|
||||
|
||||
case MT_STATUS_UNINIT :
|
||||
case BOOZ_AHRS_STATUS_UNINIT :
|
||||
imu_v3_detect_vehicle_still();
|
||||
if (imu_vehicle_still) {
|
||||
ImuUpdateFromAvg();
|
||||
multitilt_start(imu_accel, imu_gyro, imu_mag);
|
||||
booz_ahrs_start(imu_accel, imu_gyro, imu_mag);
|
||||
}
|
||||
break;
|
||||
|
||||
case MT_STATUS_RUNNING :
|
||||
case BOOZ_AHRS_STATUS_RUNNING :
|
||||
// t0 = T0TC;
|
||||
multitilt_predict(imu_gyro_prev);
|
||||
multitilt_update(imu_accel, imu_mag);
|
||||
booz_ahrs_run(imu_accel, imu_gyro_prev, imu_mag);
|
||||
// t1 = T0TC;
|
||||
// diff = t1 - t0;
|
||||
// DOWNLINK_SEND_TIME(&diff);
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
#include "uart.h"
|
||||
|
||||
#include "imu_v3.h"
|
||||
#include "multitilt.h"
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
#include "settings.h"
|
||||
|
||||
@@ -66,14 +66,18 @@
|
||||
|
||||
|
||||
#define PERIODIC_SEND_AHRS_STATE() \
|
||||
DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &mtt_psi, &mtt_psi, \
|
||||
&mtt_bp, &mtt_bq, &mtt_br);
|
||||
DOWNLINK_SEND_AHRS_STATE(&booz_ahrs_phi, &booz_ahrs_theta, &booz_ahrs_psi, &booz_ahrs_psi, \
|
||||
&booz_ahrs_bp, &booz_ahrs_bq, &booz_ahrs_br);
|
||||
|
||||
#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT
|
||||
#define PERIODIC_SEND_AHRS_COV() \
|
||||
DOWNLINK_SEND_AHRS_COV(&mtt_P_phi[0][0], &mtt_P_phi[0][1], \
|
||||
&mtt_P_phi[1][0], &mtt_P_phi[1][1], \
|
||||
&mtt_P_theta[0][0], &mtt_P_theta[0][1], \
|
||||
&mtt_P_theta[1][1]);
|
||||
#elif defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_QUATERNION
|
||||
#define PERIODIC_SEND_AHRS_COV() {}
|
||||
#endif /* BOOZ_AHRS_TYPE */
|
||||
|
||||
#define PERIODIC_SEND_BOOZ_DEBUG() { \
|
||||
float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); \
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include "booz_inter_mcu.h"
|
||||
|
||||
#include "imu_v3.h"
|
||||
#include "multitilt.h"
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
|
||||
struct booz_inter_mcu_state inter_mcu_state;
|
||||
@@ -20,12 +20,12 @@ void inter_mcu_fill_state() {
|
||||
inter_mcu_state.r_rates[AXIS_Q] = imu_gyro_lp[AXIS_Q] * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.r_rates[AXIS_R] = imu_gyro_lp[AXIS_R] * RATE_PI_S/M_PI;
|
||||
#endif
|
||||
inter_mcu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_X] = mtt_phi * ANGLE_PI/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_Y] = mtt_theta* ANGLE_PI/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_Z] = mtt_psi * ANGLE_PI/M_PI;
|
||||
inter_mcu_state.status = mtt_status;
|
||||
inter_mcu_state.f_rates[AXIS_P] = booz_ahrs_p * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_rates[AXIS_Q] = booz_ahrs_q * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_rates[AXIS_R] = booz_ahrs_r * RATE_PI_S/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_X] = booz_ahrs_phi * ANGLE_PI/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_Y] = booz_ahrs_theta* ANGLE_PI/M_PI;
|
||||
inter_mcu_state.f_eulers[AXIS_Z] = booz_ahrs_psi * ANGLE_PI/M_PI;
|
||||
inter_mcu_state.status = booz_ahrs_status;
|
||||
}
|
||||
#endif
|
||||
|
||||
+73
-76
@@ -1,26 +1,12 @@
|
||||
#include "multitilt.h"
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "6dof.h"
|
||||
|
||||
uint8_t mtt_status;
|
||||
|
||||
#define DT 4e-3
|
||||
|
||||
/* attitude */
|
||||
float mtt_phi;
|
||||
float mtt_theta;
|
||||
float mtt_psi;
|
||||
/* unbiased rates */
|
||||
float mtt_p;
|
||||
float mtt_q;
|
||||
float mtt_r;
|
||||
/* gyro biases */
|
||||
float mtt_bp;
|
||||
float mtt_bq;
|
||||
float mtt_br;
|
||||
/* covariance matrix */
|
||||
float mtt_P_phi[2][2];
|
||||
float mtt_P_theta[2][2];
|
||||
@@ -34,6 +20,12 @@ static const float Q_bias = 0.0015;
|
||||
//static const float R_accel = 0.3;
|
||||
static const float R_accel = 0.4;
|
||||
|
||||
static inline void multitilt_update( const float* accel, const int16_t* mag );
|
||||
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
|
||||
static inline void multitilt_predict( const float* gyro );
|
||||
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]);
|
||||
|
||||
|
||||
#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
|
||||
|
||||
static inline float phi_of_accel( const float* accel) {
|
||||
@@ -49,39 +41,39 @@ static inline float theta_of_accel( const float* accel) {
|
||||
}
|
||||
|
||||
static inline float psi_of_mag( const int16_t* mag) {
|
||||
/* untilt magnetometer */
|
||||
const float ctheta = cos( mtt_theta );
|
||||
const float stheta = sin( mtt_theta );
|
||||
const float cphi = cos( mtt_phi );
|
||||
const float sphi = sin( mtt_phi );
|
||||
/* untilt magnetometer */
|
||||
const float ctheta = cos( booz_ahrs_theta );
|
||||
const float stheta = sin( booz_ahrs_theta );
|
||||
const float cphi = cos( booz_ahrs_phi );
|
||||
const float sphi = sin( booz_ahrs_phi );
|
||||
|
||||
const float mn =
|
||||
ctheta* mag[0]+
|
||||
sphi*stheta* mag[1]+
|
||||
cphi*stheta* mag[2];
|
||||
const float me =
|
||||
/* 0* mag[0]+ */
|
||||
cphi* mag[1]+
|
||||
-sphi* mag[2];
|
||||
return -atan2( me, mn );
|
||||
const float mn =
|
||||
ctheta* mag[0]+
|
||||
sphi*stheta* mag[1]+
|
||||
cphi*stheta* mag[2];
|
||||
const float me =
|
||||
/* 0* mag[0]+ */
|
||||
cphi* mag[1]+
|
||||
-sphi* mag[2];
|
||||
return -atan2( me, mn );
|
||||
}
|
||||
|
||||
void multitilt_init(void) {
|
||||
mtt_status = MT_STATUS_UNINIT;
|
||||
mtt_phi = 0.;
|
||||
mtt_theta = 0.;
|
||||
mtt_psi = 0.;
|
||||
void booz_ahrs_init(void) {
|
||||
booz_ahrs_status = BOOZ_AHRS_STATUS_UNINIT;
|
||||
booz_ahrs_phi = 0.;
|
||||
booz_ahrs_theta = 0.;
|
||||
booz_ahrs_psi = 0.;
|
||||
|
||||
mtt_p = 0.;
|
||||
mtt_q = 0.;
|
||||
mtt_r = 0.;
|
||||
booz_ahrs_p = 0.;
|
||||
booz_ahrs_q = 0.;
|
||||
booz_ahrs_r = 0.;
|
||||
|
||||
mtt_bp = 0.;
|
||||
mtt_bq = 0.;
|
||||
mtt_br = 0.;
|
||||
booz_ahrs_bp = 0.;
|
||||
booz_ahrs_bq = 0.;
|
||||
booz_ahrs_br = 0.;
|
||||
}
|
||||
|
||||
void multitilt_start(const float* accel, const float* gyro, const int16_t* mag) {
|
||||
void booz_ahrs_start(const float* accel, const float* gyro, const int16_t* mag) {
|
||||
/* reset covariance matrices */
|
||||
const float cov_init[2][2] = {{1., 0.},
|
||||
{0., 1.}};
|
||||
@@ -90,28 +82,32 @@ void multitilt_start(const float* accel, const float* gyro, const int16_t* mag)
|
||||
memcpy(mtt_P_psi, cov_init, sizeof(cov_init));
|
||||
|
||||
/* initialise state */
|
||||
mtt_p = 0.;
|
||||
mtt_q = 0.;
|
||||
mtt_r = 0.;
|
||||
booz_ahrs_p = 0.;
|
||||
booz_ahrs_q = 0.;
|
||||
booz_ahrs_r = 0.;
|
||||
|
||||
mtt_bp = gyro[AXIS_P];
|
||||
mtt_bq = gyro[AXIS_Q];
|
||||
mtt_br = gyro[AXIS_R];
|
||||
booz_ahrs_bp = gyro[AXIS_P];
|
||||
booz_ahrs_bq = gyro[AXIS_Q];
|
||||
booz_ahrs_br = gyro[AXIS_R];
|
||||
|
||||
const float init_phi = phi_of_accel(accel);
|
||||
const float init_theta = theta_of_accel(accel);
|
||||
#ifndef DISABLE_MAGNETOMETER
|
||||
const float init_psi = psi_of_mag(mag);
|
||||
#endif
|
||||
mtt_phi = init_phi;
|
||||
mtt_theta = init_theta;
|
||||
booz_ahrs_phi = init_phi;
|
||||
booz_ahrs_theta = init_theta;
|
||||
#ifndef DISABLE_MAGNETOMETER
|
||||
mtt_psi = init_psi;
|
||||
booz_ahrs_psi = init_psi;
|
||||
#endif
|
||||
|
||||
mtt_status = MT_STATUS_RUNNING;
|
||||
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
|
||||
}
|
||||
|
||||
void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag) {
|
||||
multitilt_predict(gyro);
|
||||
multitilt_update(accel, mag);
|
||||
}
|
||||
|
||||
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]) {
|
||||
|
||||
@@ -131,32 +127,32 @@ static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]
|
||||
}
|
||||
|
||||
|
||||
void multitilt_predict( const float* gyro ) {
|
||||
static inline void multitilt_predict( const float* gyro ) {
|
||||
/* unbias gyro */
|
||||
mtt_p = gyro[AXIS_P] - mtt_bp;
|
||||
mtt_q = gyro[AXIS_Q] - mtt_bq;
|
||||
mtt_r = gyro[AXIS_R] - mtt_br;
|
||||
booz_ahrs_p = gyro[AXIS_P] - booz_ahrs_bp;
|
||||
booz_ahrs_q = gyro[AXIS_Q] - booz_ahrs_bq;
|
||||
booz_ahrs_r = gyro[AXIS_R] - booz_ahrs_br;
|
||||
|
||||
/* update angles */
|
||||
float s_phi = sin(mtt_phi);
|
||||
float c_phi = cos(mtt_phi);
|
||||
float t_theta = tan(mtt_theta);
|
||||
float s_phi = sin(booz_ahrs_phi);
|
||||
float c_phi = cos(booz_ahrs_phi);
|
||||
float t_theta = tan(booz_ahrs_theta);
|
||||
|
||||
float phi_dot = mtt_p + s_phi*t_theta*mtt_q + c_phi*t_theta*mtt_r;
|
||||
float theta_dot = c_phi*mtt_q - s_phi*mtt_r;
|
||||
mtt_predict_axis(&mtt_phi, phi_dot, mtt_P_phi);
|
||||
mtt_predict_axis(&mtt_theta, theta_dot, mtt_P_theta);
|
||||
float phi_dot = booz_ahrs_p + s_phi*t_theta*booz_ahrs_q + c_phi*t_theta*booz_ahrs_r;
|
||||
float theta_dot = c_phi*booz_ahrs_q - s_phi*booz_ahrs_r;
|
||||
mtt_predict_axis(&booz_ahrs_phi, phi_dot, mtt_P_phi);
|
||||
mtt_predict_axis(&booz_ahrs_theta, theta_dot, mtt_P_theta);
|
||||
|
||||
#ifndef DISABLE_MAGNETOMETER
|
||||
float c_theta = cos(mtt_theta);
|
||||
float psi_dot = s_phi/c_theta*mtt_q + c_phi/c_theta*mtt_r;
|
||||
mtt_predict_axis(&mtt_psi, psi_dot, mtt_P_psi);
|
||||
float c_theta = cos(booz_ahrs_theta);
|
||||
float psi_dot = s_phi/c_theta*booz_ahrs_q + c_phi/c_theta*booz_ahrs_r;
|
||||
mtt_predict_axis(&booz_ahrs_psi, psi_dot, mtt_P_psi);
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float* bias) {
|
||||
static void inline mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias) {
|
||||
const float Pct_0 = _P[0][0];
|
||||
const float Pct_1 = _P[1][0];
|
||||
/* E = C P C' + R */
|
||||
@@ -179,27 +175,28 @@ static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float
|
||||
|
||||
}
|
||||
|
||||
void multitilt_update( const float* accel, const int16_t* mag ) {
|
||||
static inline void multitilt_update( const float* accel, const int16_t* mag ) {
|
||||
|
||||
const float measure_phi = phi_of_accel(accel);
|
||||
float err_phi = measure_phi - mtt_phi;
|
||||
float err_phi = measure_phi - booz_ahrs_phi;
|
||||
WRAP(err_phi, M_PI);
|
||||
MttUpdateAxis(err_phi, mtt_P_phi, &mtt_phi, &mtt_bp);
|
||||
WRAP(mtt_phi, M_PI);
|
||||
mtt_update_axis(err_phi, mtt_P_phi, &booz_ahrs_phi, &booz_ahrs_bp);
|
||||
WRAP(booz_ahrs_phi, M_PI);
|
||||
|
||||
const float measure_theta = theta_of_accel(accel);
|
||||
float err_theta = measure_theta - mtt_theta;
|
||||
float err_theta = measure_theta - booz_ahrs_theta;
|
||||
WRAP(err_theta, M_PI_2);
|
||||
MttUpdateAxis(err_theta, mtt_P_theta, &mtt_theta, &mtt_bq);
|
||||
WRAP(mtt_theta, M_PI_2);
|
||||
mtt_update_axis(err_theta, mtt_P_theta, &booz_ahrs_theta, &booz_ahrs_bq);
|
||||
WRAP(booz_ahrs_theta, M_PI_2);
|
||||
|
||||
#ifndef DISABLE_MAGNETOMETER
|
||||
float measure_psi = psi_of_mag(mag);
|
||||
float err_psi = measure_psi - mtt_psi;
|
||||
float err_psi = measure_psi - booz_ahrs_psi;
|
||||
WRAP(err_psi, M_PI);
|
||||
MttUpdateAxis(err_psi, mtt_P_psi, &mtt_psi, &mtt_br);
|
||||
WRAP(mtt_psi, M_PI);
|
||||
mtt_update_axis(err_psi, mtt_P_psi, &booz_ahrs_psi, &booz_ahrs_br);
|
||||
WRAP(booz_ahrs_psi, M_PI);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -3,31 +3,12 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#define MT_STATUS_UNINIT 0
|
||||
#define MT_STATUS_RUNNING 1
|
||||
#define MT_STATUS_CRASHED 2
|
||||
|
||||
extern uint8_t mtt_status;
|
||||
|
||||
extern float mtt_phi;
|
||||
extern float mtt_p;
|
||||
extern float mtt_bp;
|
||||
extern float mtt_P_phi[2][2];
|
||||
|
||||
extern float mtt_theta;
|
||||
extern float mtt_q;
|
||||
extern float mtt_bq;
|
||||
extern float mtt_P_theta[2][2];
|
||||
|
||||
extern float mtt_psi;
|
||||
extern float mtt_r;
|
||||
extern float mtt_br;
|
||||
extern float mtt_P_psi[2][2];
|
||||
|
||||
extern void multitilt_init(void);
|
||||
extern void multitilt_start( const float* accel, const float* gyro, const int16_t* mag);
|
||||
extern void multitilt_predict( const float* gyro );
|
||||
extern void multitilt_update( const float* accel, const int16_t* mag );
|
||||
|
||||
|
||||
#endif /* MULTITILT_H */
|
||||
|
||||
@@ -163,8 +163,12 @@ BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
|
||||
BOOZ_AB_SRCS += $(AB)/micromag.c $(AB_ARCH)/micromag_hw.c
|
||||
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
|
||||
|
||||
CFLAGS += -DBOOZ_AHRS_TYPE=multitilt
|
||||
BOOZ_AB_SRCS += $(AB)/multitilt.c
|
||||
|
||||
BOOZ_AB_SRCS += $(AB)/booz_ahrs.c
|
||||
#CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT
|
||||
#BOOZ_AB_SRCS += $(AB)/multitilt.c
|
||||
CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_QUATERNION -DEKF_UPDATE_DISCRETE
|
||||
BOOZ_AB_SRCS += $(AB)/ahrs_quat_fast_ekf.c
|
||||
|
||||
|
||||
booz_sim: $(BOOZ_SIM_SRCS) $(BOOZ_AB_SRCS)
|
||||
|
||||
Reference in New Issue
Block a user