diff --git a/conf/flight_plans/grz.xml b/conf/flight_plans/grz.xml deleted file mode 100644 index 6fa2fed6b7..0000000000 --- a/conf/flight_plans/grz.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/conf/settings/grz.xml b/conf/settings/grz.xml deleted file mode 100644 index 5402bf25cf..0000000000 --- a/conf/settings/grz.xml +++ /dev/null @@ -1,85 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/telemetry/grz.xml b/conf/telemetry/grz.xml deleted file mode 100644 index d6d066aeef..0000000000 --- a/conf/telemetry/grz.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sw/airborne/ahrs.c b/sw/airborne/ahrs.c deleted file mode 100644 index 1f8f4c7d91..0000000000 --- a/sw/airborne/ahrs.c +++ /dev/null @@ -1,1633 +0,0 @@ -/* - * $Id$ - * - * Fast AHRS object almost ready for use on microcontrollers - * - * (c) 2003 Trammell Hudson - * (c) 2005 Jean-Pierre Dumont - * - * 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. -*/ - - -#include -#include -#include "timer_ap.h" -#include "string.h" -#include "adc.h" -#include "ahrs.h" -#include "autopilot.h" -#include "inter_mcu.h" -#include "uart.h" - -#ifdef PNI_MAG -#include "pni.h" -#endif //PNI_MAG - -/* un peu de doc en fancais - ahrs est en repere NED (north, east, down) contairement au reste de paparazzi - les accelerometres doivent donc etre calibrés (cf quadRotorJP.xml) de sorte que - AX = accel[0] = -9.81 lorsque l'imu est posée sur le nez (nez en bas & queue en l'air), - AY = accel[1] = -9.81 lorsque l'imu est posée sur sa tranche droite - AZ = accel[2] = -9.81 lorsque l'imu est posée a l'horizontale sur le ventre (soit normalement) - - Note : Quatre choses à fixer, dans airframe.xml l'affectation des adc, le signe, l'offset (valeur raw à 0g) et le scale - - les gyroscopes doivent etre calibrés de sorte que - P = pqr[0] positif en roulie sur la droite (manche à droite) - Q = pqr[1] positif en tangage positff (tire sur le= manche pour monter) - R = pqr[2] positif lacet à droite - - Note : De meme 4 choses a fixer dans l'xml comme pour les accels - Veuillez aussi fixer le define IMU_GYROS_CONNECTED_TO_AP ou non en fonction de votre configuration -*/ -#ifdef IMU_ANALOG - - -#define C_ONE 1.0 -#define C_TWO 2.0 -#define C_PI ((real_t) 3.14159265358979323846264338327950) -#define C_HALF_PI (C_PI/2) -#define C_TWO_PI (C_PI*2) - - -/* - * dt is our time step. It is important to be close to the actual - * frequency with which ahrs_state_update() is called with the body - * angular rates. - */ - -#define dt 1/15 //0,066666667 //1/15 -#define CONFIG_SPLIT_COVARIANCE -#ifdef CONFIG_SPLIT_COVARIANCE -#define dt_covariance 2 * dt -#else -#define dt_covariance dt -#endif //CONFIG_SLIT_COVARIANCE - -#define AHRS_DEBUG - -/* - * 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 - * - * The actual data segment in the bss is defined in ahrs_data.S - * Most of these are aliases to each other. - */ - - -/* - * The user inputs the acceleration and rotational rates here, then - * calls the update functions. - */ -real_t pqr[3];//rad/s -real_t accel[3];//in G - -/* - * The euler estimate will be updated less frequently than the - * quaternion, but some applications are ok with that. - */ -real_t ahrs_euler[3]; - - -/* - * 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. - */ -static real_t dcm00; -static real_t dcm01; -static real_t dcm02; -static real_t dcm12; -static real_t dcm22; - - -/* - * Covariance matrix and covariance matrix derivative are updated - * every other state step. This is because the covariance should change - * at a rate somewhat slower than the dynamics of the system. - */ -static real_t P[7][7]; -static real_t Pdot[7][7]; -static index_t covariance_state; - - -/* - * A 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. - */ -extern real_t A[4][7]; - -/* - * These Kalman filter variables share space with A when the filter - * is being updated. - */ -extern real_t PCt[7]; -extern real_t K[7]; -extern real_t E; - -/* - * C 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. - * - * Since we compute each axis independently, we only allocate one - * column out of the C matrix at a time. This allows us to reuse the - * matrix. Additionally, this space is shared by Qdot. - */ -extern real_t C[4]; -extern real_t Qdot[4]; - - -/* - * 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. - */ - -#define Q_gyro 0.0075 - -/* - * 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 R_pitch 1.3 * 1.3 -#define R_roll 1.0 * 1.0 -#define R_yaw 2.5 * 2.5 - -//jpdumont adds... (take care on asm ahrs.S) - -/*raw datas - */ -int16_t gyro[3];//direct mean from gyro -int16_t gyro_zero[3];//this datas will by re-initialiazed during startup - -int16_t accel_raw[3];//direct lean from adxl -int16_t accel_raw_zero[3]; - -/* real_t datas - */ - -real_t gyro_scale[3]; -real_t accel_scale[3]; - -/* - * paparazzi adc_buff - */ -static struct adc_buf buf_accelX; -static struct adc_buf buf_accelY; -static struct adc_buf buf_accelZ; - -/* - * only if gyro are connected on ap - */ -#if (defined IMU_GYROS_CONNECTED_TO_AP) && (IMU_GYROS_CONNECTED_TO_AP != 0) -static struct adc_buf buf_gyroP; -static struct adc_buf buf_gyroQ; -static struct adc_buf buf_gyroR; -#endif //IMU_GYROS_CONNECTED_TO_AP - -#ifdef AHRS_DEBUG -char float_buf[12]; -void -put_float(const float f) -{ - uint8_t i; - - - dtostrf( - f, - sizeof( float_buf )-1, - 5, - float_buf - ); - - for( i=0 ; i C_PI ) - roll -= C_TWO_PI; - - run_kalman( R_roll, roll ); -} - - -void -ahrs_pitch_update( - real_t pitch - ) -{ - - // Reuse DCM - //compute_DCM(); - compute_euler_pitch(); - compute_dtheta_dq(); - - /* - * Compute the error in our pitch estimate and measurement. - * Pitch can be between -90 and +90 degrees, so wrap it around - * for the shortest distance. - */ - pitch -= ahrs_euler[1]; - if( pitch < -C_HALF_PI ) - pitch += C_PI; - if( pitch > C_HALF_PI ) - pitch -= C_PI; - - run_kalman( R_pitch, pitch ); - - // We'll normalize our quaternion here only - norm_quat(); -} - -/* - * Call this with an untilted heading - */ -void -ahrs_compass_update( - real_t heading - ) -{ - // Update the DCM since this will require - compute_DCM(); - compute_euler_heading(); - compute_dpsi_dq(); - - /* - * Compute the error in our heading estimate and measurement. - * Heading can be between -180 and +180 degrees, so we wrap - * to find the shortest turn between the two. - */ - heading -= ahrs_euler[2]; - if( heading < -C_PI ) - heading += C_TWO_PI; - if( heading > C_PI ) - heading -= C_TWO_PI; - - run_kalman( R_yaw, heading ); -} - -/* - * 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) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ] - * [ 0 cos(Phi) -sin(Phi) ] - * [ -sin(Theta) sin(Phi)*cos(Theta) cos(Phi)*cos(Theta) ] - * - * However, to untilt the compass reading, but not rotate it, - * we need to use the transpose of this matrix. Additionally, - * since we already have the DCM computed for our current attitude, - * we can short cut all of the trig. Transposing and substituting - * in from the definition of euler2quat and quat2euler, we have: - * - * [ ? -dcm12*dcm02 -dcm22*dcm02 ] - * [ 0 dcm22 -dcm12 ] - * [ dcm02 dcm12 dcm22 ] - * - * As installed in my AHRS, the magnetomer is rotated 180 degrees - * about the Z axis relative to the IMU. Thus, we must negate - * mag[0] and mag[1], while mag[2] is still positive. - */ - -#ifdef PNI_MAG -real_t -untilt_compass( - const int16_t * mag - ) - const real_t ctheta = cos( ahrs_euler[1] ); - -#ifdef AHRS_DEBUG -if (ctheta == 0) - { - uart0_print_string("\nuntilt_compass:div by 0 !\n"); - return 0 ;//prhaps should we return other thing - } -#endif //AHRS_DEBUG - -const real_t mn = ctheta * mag[0] - - (dcm12 * mag[1] + dcm22 * mag[2]) * dcm02 / ctheta; - -const real_t me = - (dcm22 * mag[1] - dcm12 * mag[2]) / ctheta; -#ifdef AHRS_DEBUG -if (mn == 0) - { - uart0_print_string("\nuntilt_compass:atan2(Y,0) !\n"); - return -((me > 0) ? C_HALF_PI : C_HALF_PI); - } -#endif //AHRS_DEBUG -return -atan2( me, -mn ); -} -#else -real_t -untilt_compass( - const int16_t * mag - ) -{ - //Faking Compass - return 0; -} -#endif //PNI_MAG - - -static void -euler2quat( void ) -{ - const real_t phi = ahrs_euler[0] / 2.0; - const real_t theta = ahrs_euler[1] / 2.0; - const real_t psi = ahrs_euler[2] / 2.0; - - const real_t shphi0 = sin( phi ); - const real_t chphi0 = cos( phi ); - - const real_t shtheta0 = sin( theta ); - const real_t chtheta0 = cos( theta ); - - const real_t shpsi0 = sin( psi ); - const real_t chpsi0 = cos( psi ); - - q0 = chphi0 * chtheta0 * chpsi0 + shphi0 * shtheta0 * shpsi0; - q1 = -chphi0 * shtheta0 * shpsi0 + shphi0 * chtheta0 * chpsi0; - q2 = chphi0 * shtheta0 * chpsi0 + shphi0 * chtheta0 * shpsi0; - q3 = chphi0 * chtheta0 * shpsi0 - shphi0 * shtheta0 * chpsi0; -} - -//END OF USING_FP WORK - -/* - * Initialize the AHRS state data and covariance matrix. The user - * should have filled in the pqr and accel vectors before calling this. - * They should also have set ahrs_euler[2] from an untilted compass reading - * and the ahrs_euler[0], ahrs_euler[1] from the accel2roll / accel2pitch values. - */ -void -_ahrs_init( - const int16_t * mag - ) -{ - // index_t i; - - euler2quat(); - compute_DCM(); - - ahrs_euler[2] = untilt_compass( mag ); - - euler2quat(); - compute_DCM(); - - /* Initialize the bias terms so the filter doesn't work as hard */ - bias[0] = pqr[0]; - bias[1] = pqr[1]; - bias[2] = pqr[2]; - - /* - * Setup our covariance matrix - * It is 1 in the diagonal elements for which Q is zero in the - * same elements and vice versa. Thus, only the quaternion - * rows/columns have any entries. - */ - memset( (void*) P, 0, sizeof( P ) ); - /*for( i=0 ; i<4 ; i++ ) - P[i][i] = C_ONE;*/ - P[0][0] = C_ONE; - P[1][1] = C_ONE; - P[2][2] = C_ONE; - P[3][3] = C_ONE; -} - -/* - * Check for a valid reading from the compass. If there is not one yet, - * start a reading. If the last one failed, emit the error and try again. - */ -#ifdef PNI_MAG -void -pni_poll( void ) -{ - if( pni_valid < 0 ) - { - puts( "PNIINV\r\n" ); - pni_valid = 0; - return; - } - - if( pni_valid == 0 ) - { - pni_read_axis(); - return; - } - - /* Valid reading! */ - pni_valid = 0; -} -#endif //PNI_MAG - -//some inline functions -static inline real_t -accel2roll( void ) -{ -#ifdef AHRS_DEBUG - if (accel[2] == 0) - { - uart0_print_string("\naccel2roll:atan2(Y,0) !\n"); - return -((accel[1]>0) ? C_HALF_PI : -C_HALF_PI); - } -#endif //AHRS_DEBUG - return -atan2( accel[1], -accel[2] ); -} - - -static inline real_t -accel2pitch( void ) -{ - // index_t i; - real_t g2 = 0; - - /* Compute the square of the magnitude of the acceleration */ - - /*for( i=0 ; i<3 ; i++ ) - g2 += accel[i] * accel[i];*/ - g2 += accel[0] * accel[0]; - g2 += accel[1] * accel[1]; - g2 += accel[2] * accel[2]; - - real_t tmp = -sqrt(g2); - -#ifdef AHRS_DEBUG - if (tmp == 0) - { - uart0_print_string("\naccel2pitch:div by 0 !\n"); - tmp = -g2; - } -#endif //AHRS_DEBUG - - return -asin( accel[0] / tmp ); -} - - -/** - Here start the Paparazzi englued code - * - TODO : move this code to another file will be good - */ -uint8_t ahrs_state = AHRS_NOT_INITIALIZED; - -static inline void -accel_init( void ) -{ - //accel : paparazzi mean accel buffer init - adc_buf_channel(IMU_ADC_ACCELX, &buf_accelX, DEFAULT_AV_NB_SAMPLE); - adc_buf_channel(IMU_ADC_ACCELY, &buf_accelY, DEFAULT_AV_NB_SAMPLE); - adc_buf_channel(IMU_ADC_ACCELZ, &buf_accelZ, DEFAULT_AV_NB_SAMPLE); - - //Default accel_raw_zeo initialisation - accel_raw_zero[0] = IMU_ADC_ACCELX_ZERO; - accel_raw_zero[1] = IMU_ADC_ACCELY_ZERO; - accel_raw_zero[2] = IMU_ADC_ACCELZ_ZERO; - - //default accel_scale initialisation - accel_scale[0] = IMU_ADC_ACCELX_SCALE; - accel_scale[1] = IMU_ADC_ACCELY_SCALE; - accel_scale[2] = IMU_ADC_ACCELZ_SCALE; -} - -static inline void -gyro_init( void) -{ - //gyro : paparazzi initialization - //nothing todo -#if (defined IMU_GYROS_CONNECTED_TO_AP) && (IMU_GYROS_CONNECTED_TO_AP != 0) - //gyro : paparazzi mean gyro buffer init(only if gyro are connected to ap) - adc_buf_channel(IMU_ADC_ROLL_DOT, &buf_gyroP, DEFAULT_AV_NB_SAMPLE); - adc_buf_channel(IMU_ADC_PITCH_DOT, &buf_gyroQ, DEFAULT_AV_NB_SAMPLE); - adc_buf_channel(IMU_ADC_YAW_DOT, &buf_gyroR, DEFAULT_AV_NB_SAMPLE); -#endif //IMU_GYROS_CONNECTED_TO_AP - - //Default gyro_zero initialisation - gyro_zero[0] = IMU_ADC_ROLL_DOT_ZERO; - gyro_zero[1] = IMU_ADC_PITCH_DOT_ZERO; - gyro_zero[2] = IMU_ADC_YAW_DOT_ZERO; - - //Default gyro_scale Initialisation - gyro_scale[0] = IMU_ADC_ROLL_DOT_SCALE; - gyro_scale[1] = IMU_ADC_PITCH_DOT_SCALE; - gyro_scale[2] = IMU_ADC_YAW_DOT_SCALE; -} - - -static inline void -pqr_update( void ) -{ -#if (defined IMU_GYROS_CONNECTED_TO_AP) && (IMU_GYROS_CONNECTED_TO_AP != 0) - ahrs_gyro_update(); - - pqr[0] = IMU_ADC_ROLL_DOT_SIGN ((real_t)(gyro[0] - gyro_zero[0])) * gyro_scale[0]; - pqr[1] = IMU_ADC_PITCH_DOT_SIGN ((real_t)(gyro[1] - gyro_zero[1])) * gyro_scale[1]; - pqr[2] = IMU_ADC_YAW_DOT_SIGN ((real_t)(gyro[2] - gyro_zero[2])) * gyro_scale[2]; -#else - //ahrs_gyro_update is called from the mainloop.c when fbw gyro comming if gyro are connected to fbw - //data are signed or not and unoffseted so ? - //TODO: do it as you feel - pqr[0] = /*IMU_ADC_ROLL_DOT_SIGN*/ ((real_t)(gyro[0] /*- gyro_zero[0]*/)) * gyro_scale[0]; - pqr[1] = /*IMU_ADC_PITCH_DOT_SIGN*/ ((real_t)(gyro[1] /*- gyro_zero[1]*/)) * gyro_scale[1]; - pqr[2] = /*IMU_ADC_YAW_DOT_SIGN*/ ((real_t)(gyro[2] /*- gyro_zero[2]*/)) * gyro_scale[2]; -#endif // IMU_GYROS_CONNECTED_TO_AP -} - - -void -roll_update( void ) -{ - //Geeting ax ay az from this adc buffer mean - accel_raw[1] = buf_accelY.sum/buf_accelY.av_nb_sample; - accel_raw[2] = buf_accelZ.sum/buf_accelZ.av_nb_sample; - - //accel[0] is not needed for roll_update. - accel[1] = IMU_ADC_ACCELY_SIGN ((real_t)(accel_raw[1] - accel_raw_zero[1])) * accel_scale[1]; - accel[2] = IMU_ADC_ACCELZ_SIGN ((real_t)(accel_raw[2] - accel_raw_zero[2])) * accel_scale[2]; - - ahrs_euler[0] = accel2roll(); - -} - - -static inline void -pitch_update( void ) -{ - //Geeting ax ay az from this adc buffer mean - accel_raw[0] = buf_accelX.sum/buf_accelX.av_nb_sample; - accel_raw[1] = buf_accelY.sum/buf_accelY.av_nb_sample; - accel_raw[2] = buf_accelZ.sum/buf_accelZ.av_nb_sample; - - accel[0] = IMU_ADC_ACCELX_SIGN ((real_t)(accel_raw[0] - accel_raw_zero[0])) * accel_scale[0]; - accel[1] = IMU_ADC_ACCELY_SIGN ((real_t)(accel_raw[1] - accel_raw_zero[1])) * accel_scale[1]; - accel[2] = IMU_ADC_ACCELZ_SIGN ((real_t)(accel_raw[2] - accel_raw_zero[2])) * accel_scale[2]; - - ahrs_euler[1] = accel2pitch(); -} - - - -static inline void -compass_update( void ) -{ -#ifdef PNI_MAG - index_t i; - - /* Swap the sensor readings to rotate front to back */ - pni_values[0] = -pni_values[0]; - pni_values[1] = -pni_values[1]; - ahrs_euler[2] = untilt_compass( pni_values ); -#else - //faking the Compass - ahrs_euler[2] = 0; -#endif //PNI_MAG -} - -#define reset() ((void(*)(void))0)() - - -//Exported Function to paparazzi - -static inline void -zero_calibration( uint8_t reset ) -{ - static int16_t dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN; - static int16_t gyro_min[3],gyro_max[3]; - uint8_t i; - -#ifdef AHRS_DEBUG - //init-algo - uart0_print_string(" dec = "); - uart0_print_hex16(dec); - uart0_transmit('\n'); -#endif //AHRS_DEBUG - if (reset) - dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN; - -#if (defined IMU_GYROS_CONNECTED_TO_AP) && (IMU_GYROS_CONNECTED_TO_AP != 0) - ahrs_gyro_update();//only if gyros are connected to the ap -#endif //IMU_GYROS_CONNECTED_TO_AP - - if (dec == IMU_INIT_EULER_DOT_NB_SAMPLES_MIN) - { - for(i=0;i<3;i++) - gyro_min[i] = gyro_max[i] = gyro[i];//<--from_fbw.euler_dot[i]; - dec--; - return; - } - - if (dec) - { - //Saving min and max - for(i=0;i<3;i++) - { - //gyro[i] = from_fbw.euler_dot[i];//already done - gyro_min[i] = (gyro_min[i] < gyro[i])? gyro_min[i] : gyro[i]; - gyro_max[i] = (gyro_max[i] > gyro[i])? gyro_max[i] : gyro[i]; - } - - //testing variance - for(i=0;i<3;i++) - { - if ((gyro_max[i]- gyro_min[i]) > IMU_INIT_EULER_DOT_VARIANCE_MAX) - { - //re-init algo - dec = IMU_INIT_EULER_DOT_NB_SAMPLES_MIN; - return; - } - } - dec--; - } - else - { - //entering in the end of calibration ;-) - - //we take the middle for pqr - //normally pqr should be normalized and scaled by the fbw mcu - //but offset is determined here and with kalman - for(i=0;i<3;i++) - gyro_zero[i] = (gyro_min[i] + gyro_max[i])/2; - -#ifdef AHRS_DEBUG - uart0_print_string("gyro_zero : "); - uart0_print_hex16(gyro_zero[0]); - uart0_transmit(','); - uart0_print_hex16(gyro_zero[1]); - uart0_transmit(','); - uart0_print_hex16(gyro_zero[2]); - uart0_transmit('\n'); -#endif //AHRS_DEBUG - - //fixe here accel_raw_zero - accel_raw_zero[0] = buf_accelX.sum/buf_accelX.av_nb_sample; - accel_raw_zero[1] = buf_accelY.sum/buf_accelY.av_nb_sample; - //accel_raw_zero[2] = buf_accelZ.sum/buf_accelZ.av_nb_sample + IMU_ADC_ACCELZ_RAW_RANGE/2; - -#ifdef AHRS_DEBUG - uart0_print_string("accel_raw_zero : "); - uart0_print_hex16(accel_raw_zero[0]); - uart0_transmit(','); - uart0_print_hex16(accel_raw_zero[1]); - uart0_transmit(','); - uart0_print_hex16(accel_raw_zero[2]); - uart0_transmit('\n'); -#endif //AHRS_DEBUG - - pqr_update(); - roll_update(); - pitch_update(); - - //Enf of ahrs_init ;-) -#ifdef PNI_MAG - pni_poll(); - _ahrs_init( pni_values ); -#else - _ahrs_init( NULL ); -#endif //PNI_MAG - ahrs_state = AHRS_RUNNING; - } - -} - - -//AHRS EXPORTED FUNCTION -void ahrs_init(uint8_t do_zero_calibration) -{ - //fp_test();return; - if (ahrs_state == AHRS_IMU_CALIBRATION) - return; - - if(ahrs_state == AHRS_NOT_INITIALIZED) - { -#ifdef PNI_MAG - pni_init(); -#endif //PNI_MAG - accel_init(); - gyro_init(); - } - - if(do_zero_calibration) - { - ahrs_state = AHRS_IMU_CALIBRATION; - zero_calibration(TRUE); - //end of ahrs_init is done in zero_calibration when calib is done - } - else - { - pqr_update(); - roll_update(); - pitch_update(); - - //Enf of ahrs_init ;-) -#ifdef PNI_MAG - pni_poll(); - _ahrs_init( pni_values ); -#else - _ahrs_init( NULL ); -#endif //PNI_MAG - ahrs_state = AHRS_RUNNING; - } - - -} - -void ahrs_gyro_update( void ) -{ -#if (defined IMU_GYROS_CONNECTED_TO_AP) && (IMU_GYROS_CONNECTED_TO_AP != 0) - gyro[0] = buf_gyroP.sum/buf_gyroP.av_nb_sample; - gyro[1] = buf_gyroQ.sum/buf_gyroQ.av_nb_sample; - gyro[2] = buf_gyroR.sum/buf_gyroR.av_nb_sample; -#else - //taking data from fbw - gyro[0] = from_fbw.euler_dot[0]; - gyro[1] = from_fbw.euler_dot[1]; - gyro[2] = from_fbw.euler_dot[2]; -#endif //IMU_GYROS_CONNECTED_TO_AP -} - -void ahrs_update() -{ - static uint8_t step = 0; - -#ifdef AHRS_DEBUG - /*if (ahrs_state == AHRS_RUNNING) - uart0_transmit('R'); - else if (ahrs_state == AHRS_IMU_CALIBRATION) - uart0_transmit('C'); - else if (ahrs_state == AHRS_NOT_INITIALIZED) - uart0_transmit('N');*/ - - uint16_t t1 = TCNT2;//timer_now(); -#endif //AHRS_DEBUG - - - if (ahrs_state != AHRS_RUNNING) - { - if (ahrs_state == AHRS_IMU_CALIBRATION) - zero_calibration(FALSE); - return; - } - - if( isnan( q0 ) ) - { - uart0_print_string( "\nFilter NaN! Reset!\n" ); - //reset(); - } - switch(step) - { - case 0: - //this one takes les than 6.2 ms - pqr_update(); - ahrs_state_update(); - break; - case 1: - //this one takes les than 8.6 ms (atan2) - roll_update(); - ahrs_roll_update( ahrs_euler[0] ); - break; - case 2: - //this one takes les than 6.4 ms (asin) - pitch_update(); - ahrs_pitch_update( ahrs_euler[1] ); - break; - case 3: - //this one takes les than 6.9 ms -#ifdef PNI_MAG - pni_poll();//perhaps should I call this more often - //Updating Compass - compass_update(); -#else - //Fucking Compass - ahrs_euler[2] = 0; -#endif //PNI_MAG - ahrs_compass_update( ahrs_euler[2] ); - break; - } - step = (step<3) ? step+1 : 0; - -#ifdef AHRS_DEBUG - uint16_t t2 = TCNT2;//timer_now(); - uint16_t t3 = t2 > t1 ? t2 - t1 : t1 - t2; - float tms= t3; - tms *= 0.064f; - switch(step) - { - case 0: - uart0_print_string("\nEuler = "); - put_float(ahrs_euler[0]); - put_float(ahrs_euler[1]); - put_float(ahrs_euler[2]); - uart0_print_string("in"); - put_float(tms); - break; - case 1: - case 2: - uart0_print_string(" +"); - put_float(tms); - break; - case 3: - put_float(tms); - uart0_print_string(" ms"); - } -#endif //AHRS_DEBUG - -} - -#endif //IMU_ANALOG diff --git a/sw/airborne/ahrs.h b/sw/airborne/ahrs.h deleted file mode 100644 index d2c396d002..0000000000 --- a/sw/airborne/ahrs.h +++ /dev/null @@ -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 - * (c) 2005 Jean-Pierre Dumont - * - * 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 - -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_ diff --git a/sw/airborne/ahrs_new.c b/sw/airborne/ahrs_new.c deleted file mode 100644 index 8d2b848e68..0000000000 --- a/sw/airborne/ahrs_new.c +++ /dev/null @@ -1,525 +0,0 @@ - -#include "std.h" -#include "ahrs_new.h" -#include "ahrs_utils.h" - -#include -#include -#include - -#include "frames.h" - -/* - * dt is our time step. It is important to be close to the actual - * frequency with which ahrs_state_update() is called with the body - * angular rates. - */ -#define dt 0.015625 -#define dt_covariance dt - - -/* - * 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 - */ - -float q0, q1, q2, q3; -float bias_p, bias_q, bias_r; - -/* - * We maintain eulers. - */ -float ahrs_phi, ahrs_theta, ahrs_psi; - -/* - * We maintain unbiased rates. - */ -float ahrs_p, ahrs_q, ahrs_r; - -/* - * 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. - */ -static float dcm00; -static float dcm01; -static float dcm02; -static float dcm12; -static float dcm22; - -/* - * Covariance matrix and covariance matrix derivative are updated - * every other state step. This is because the covariance should change - * at a rate somewhat slower than the dynamics of the system. - */ -float P[7][7]; -static float Pdot[7][7]; - -/* - * A 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 A[4][7]; - -/* - * Kalman filter variables. - */ -float PCt[7]; -float K[7]; -float E; - -/* - * C 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. - * - * Since we compute each axis independently, we only allocate one - * column out of the C matrix at a time. This allows us to reuse the - * matrix. - */ -float C[4]; -float Qdot[4]; - - -/* - * 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. - */ -//#define Q_gyro 0.0075 -/* I measured about 0.009 rad/s noise */ -#define 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 R_pitch 1.3 * 1.3 -#define R_roll 1.3 * 1.3 -#define R_yaw 2.5 * 2.5 - -/* - * These are the rows and columns of A that relate the derivative - * of the quaternion to the quaternion. It is actual the omega - * cross matrix of the body rates in this case. - * - * Wxq is the quaternion omega matrix: - * - * [ 0, -p, -q, -r ] - * 1/2 * [ p, 0, r, -q ] - * [ q, -r, 0, p ] - * [ r, q, -p, 0 ] - * - * Call compute_A_quat() before calling compute_A_bias - */ -static inline void compute_A_quat( const float* gyro ) -{ - /* Unbias our gyro values */ - ahrs_p = gyro[0] - bias_p; - ahrs_q = gyro[1] - bias_q; - ahrs_r = gyro[2] - bias_r; - - /* Zero our A matrix since it is shared with K */ - memset( (void*) A, 0, sizeof( A ) ); - - /* Fill in Wxq(pqr) into A */ - /* A[0][0] = A[1][1] = A[2][2] = A[3][3] = 0; */ - A[1][0] = A[2][3] = ahrs_p * 0.5; - A[2][0] = A[3][1] = ahrs_q * 0.5; - A[3][0] = A[1][2] = ahrs_r * 0.5; - - A[0][1] = A[3][2] = -A[1][0]; - A[0][2] = A[1][3] = -A[2][0]; - A[0][3] = A[2][1] = -A[3][0]; -} - -/* - * Finish filling in the terms of the Jacobian matrix A. It has already - * had the terms that relate the derivative of the quaternion to the - * quaternion; this is now the terms that relate the derivative of the - * quaternion to the gyro bias. As mentioned above, the gyro bias - * derivative is zero, so there is no need to fill in the bottom rows. - * - * Since the function for Q_dot = quatW( pqr - gyro_bias ) * Q, - * we can compute these terms: - * - * [ q1 q2 q3 ] - * [ -q0 q3 -q2 ] * 0.5 - * [ -q3 -q0 q1 ] - * [ q2 -q1 -q0 ] - */ -static inline void -compute_A_bias( void ) -{ - A[0][4] = A[2][6] = q1 * 0.5; - A[0][5] = A[3][4] = q2 * 0.5; - A[0][6] = A[1][5] = q3 * 0.5; - - A[1][4] = A[2][5] = A[3][6] = q0 * -0.5; - A[3][5] = -A[0][4]; - A[1][6] = -A[0][5]; - A[2][4] = -A[0][6]; -} - - -/* - * Typically the covariance update equation is: - * - * P_dot = A*P + P*A_transpose + Q - * - * The first of these zeros P_dot, computes part of (A*P + P*A_tranpose) - * and adds in the parts of Q that are non-zero. Note that we know that - * A has three zero rows at the bottom, so we do not include those in our - * math. - * - * The second part of the covariance update computes the inner portion - * of Pdot, the 4x4 region that corresponds to the quaternion. It also - * updates the covariance matrix P. - */ -static void -covariance_update( void ) -{ - index_t i; - index_t j; - index_t k; - - memset( Pdot, 0, sizeof(Pdot) ); - - /* Finish the computation of A */ - compute_A_bias(); - - /* - * Compute the A*P+P*At for the bottom rows of P and A_tranpose - */ - for( i=0 ; i<4 ; i++ ) - for( j=0 ; j<7 ; j++ ) - for( k=4 ; k<7 ; k++ ) - { - const float A_i_k = A[i][k]; - - Pdot[i][j] += A_i_k * P[k][j]; - Pdot[j][i] += P[j][k] * A_i_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++ ) - Pdot[i][i] += Q_gyro; - - /* - * Compute A*P + P*At 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 ) - Pdot[i][j] += A[i][k] * P[k][j]; - else - if( i == k ) - Pdot[i][j] += P[i][k] * A[j][k]; - else - Pdot[i][j] += A[i][k] * P[k][j] - + P[i][k] * A[j][k]; - } - - /* Compute P = P + Pdot * dt */ - for( i=0 ; i<7 ; i++ ) - for( j=0 ; j<7 ; j++ ) - P[i][j] += Pdot[i][j] * dt_covariance; -} - - -/* - * Call ahrs_state_update every dt seconds with the raw body frame angular - * rates. It updates the attitude state estimate via this function: - * - * Q_dot = Wxq(pqr) * Q - * - * Since A 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 ] - */ -void ahrs_predict( const float* gyro ) { - - compute_A_quat( gyro ); - memset( Qdot, 0, sizeof(Qdot) ); - - /* Compute Q_dot = Wxq(pqr) * Q (storing temp in C) */ - Qdot[0] = A[0][1] * q1 + A[0][2] * q2 + A[0][3] * q3; - Qdot[1] = A[1][0] * q0 + A[1][2] * q2 + A[1][3] * q3; - Qdot[2] = A[2][0] * q0 + A[2][1] * q1 + A[2][3] * q3; - Qdot[3] = A[3][0] * q0 + A[3][1] * q1 + A[3][2] * q2; - - /* Compute Q = Q + Q_dot * dt */ - q0 += Qdot[0] * dt; - q1 += Qdot[1] * dt; - q2 += Qdot[2] * dt; - q3 += Qdot[3] * dt; - - /* - * We would normally renormalize our quaternion, but we will - * allow it to drift until the next kalman update. - */ - norm_quat(); - - covariance_update(); -} - -/* - * Translate our quaternion attitude estimate into Euler angles. - * This is expensive, so don't do it often. You must have already - * computed the DCM for the quaternion before calling. - */ -static inline void -compute_euler_roll( void ) -{ - ahrs_phi = atan2( dcm12, dcm22 ); -} - -static inline void -compute_euler_pitch( void ) -{ - ahrs_theta = -asin( dcm02 ); -} - -static inline void -compute_euler_heading( void ) -{ - ahrs_psi = atan2( dcm01, dcm00 ); -} - - -/* - * The Kalman filter will share space with A, since it will be - * recomputed each timestep. - */ -static void -run_kalman( - const float R_axis, - const float error - ) -{ - index_t i; - index_t j; - index_t k; - - memset( (void*) PCt, 0, sizeof( PCt ) ); - - /* Compute PCt = P * C_tranpose */ - for( i=0 ; i<7 ; i++ ) - for( k=0 ; k<4 ; k++ ) - PCt[i] += P[i][k] * C[k]; - - /* Compute E = C * PCt + R */ - E = R_axis; - for( i=0 ; i<4 ; i++ ) - E += C[i] * PCt[i]; - - /* Compute the inverse of E */ - E = 1.0 / E; - - /* Compute K = P * C_tranpose * inv(E) */ - for( i=0 ; i<7 ; i++ ) - K[i] = PCt[i] * E; - - /* Update our covariance matrix: P = P - K * C * P */ - - /* Compute CP = C * P, reusing the PCt array. */ - memset( (void*) PCt, 0, sizeof( PCt ) ); - for( j=0 ; j<7 ; j++ ) - for( k=0 ; k<4 ; k++ ) - PCt[j] += C[k] * P[k][j]; - - /* Compute P -= K * CP (aliased to PCt) */ - for( i=0 ; i<7 ; i++ ) - for( j=0 ; j<7 ; j++ ) - P[i][j] -= K[i] * PCt[j]; - - /* Update our state: X += K * error */ -#if 0 - for( i=0 ; i<7 ; i++ ) - X[i] += K[i] * error; -#else - q0 += K[0] * error; - q1 += K[1] * error; - q2 += K[2] * error; - q3 += K[3] * error; - bias_p += K[4] * error; - bias_q += K[5] * error; - bias_r += K[6] * error; - -/* Bound(bias_p, -0.1, 0.1); */ -/* Bound(bias_q, -0.1, 0.1); */ -/* Bound(bias_r, -0.1, 0.1); */ -#endif - - /* - * We would normally normalize our quaternion here, but - * instead we will allow our caller to do it - */ - // norm_quat(); -} - - -/* - * Do the Kalman filter on the acceleration and compass readings. - * This is normally a very simple: - * - * E = C * P * C_tranpose + R - * K = P * C_tranpose * inv(E) - * P = P - K * C * P - * X = X + K * error - * - * However, this would take forever. Notably, the inv(E) routine - * might be very time consuming, even for the 3x3 matrix that results - * from our three measurements. - * - * We notice that P * C_tranpose is used twice, so we can cache the - * results of it. - * - * C 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 PCt that are not - * - * We also only process one axis at a time to avoid having to perform - * the 3x3 matrix inversion. - */ - -void -ahrs_roll_update( const float* accel ) { - float accel_roll = ahrs_roll_of_accel(accel); - // Reuse the DCM and A matrix from the compass computations - DCM_of_quat(); - compute_euler_roll(); - compute_dphi_dq(); - - /* - * Compute the error in our roll estimate and measurement. - * This can be between -180 and +180 degrees. - */ - accel_roll -= ahrs_phi; - if( accel_roll < -M_PI ) - accel_roll += 2 * M_PI; - if( accel_roll > M_PI ) - accel_roll -= 2 * M_PI; - - run_kalman( R_roll, accel_roll ); - norm_quat(); -} - -void -ahrs_pitch_update( const float* accel ) { - float accel_pitch = ahrs_pitch_of_accel(accel); - // Reuse DCM - DCM_of_quat(); - compute_euler_pitch(); - compute_dtheta_dq(); - - /* - * Compute the error in our pitch estimate and measurement. - * Pitch can be between -90 and +90 degrees, so wrap it around - * for the shortest distance. - */ - accel_pitch -= ahrs_theta; - if( accel_pitch < -M_PI_2 ) - accel_pitch += M_PI; - if( accel_pitch > M_PI_2 ) - accel_pitch -= M_PI; - - run_kalman( R_pitch, accel_pitch ); - - // We'll normalize our quaternion here only - norm_quat(); -} - -void -ahrs_yaw_update( const int16_t* mag ) { - - float mag_yaw = ahrs_yaw_of_mag(mag); - // Update the DCM since this will require - DCM_of_quat(); - compute_euler_heading(); - compute_dpsi_dq(); - - /* - * Compute the error in our heading estimate and measurement. - * Heading can be between -180 and +180 degrees, so we wrap - * to find the shortest turn between the two. - */ - mag_yaw -= ahrs_psi; - if( mag_yaw < -M_PI ) - mag_yaw += 2 * M_PI; - if( mag_yaw > M_PI ) - mag_yaw -= 2 * M_PI; - - run_kalman( R_yaw, mag_yaw ); - norm_quat(); -} - -/* - * Initialize the AHRS state data and covariance matrix. The user - * should have filled in the pqr and accel vectors before calling this. - * They should also have set euler[2] from an untilted compass reading - * and the euler[0], euler[1] from the accel2roll / accel2pitch values. - */ -void ahrs_init( const int16_t *mag, const float* accel, const float* gyro ) { - - ahrs_phi = ahrs_roll_of_accel(accel); - ahrs_theta = ahrs_pitch_of_accel(accel); - ahrs_psi = 0.; - - quat_of_eulers(); - DCM_of_quat(); - ahrs_psi = ahrs_yaw_of_mag( mag ); - - // imu_mag[AXIS_X] = 100; imu_mag[AXIS_Y] = 0; imu_mag[AXIS_Z] = 0; - - index_t i; - - - quat_of_eulers(); - DCM_of_quat(); - - /* Initialize the bias terms so the filter doesn't work as hard */ - bias_p = gyro[0]; - bias_q = gyro[1]; - bias_r = gyro[2]; - - /* - * Setup our covariance matrix - * It is 1 in the diagonal elements for which Q is zero in the - * same elements and vice versa. Thus, only the quaternion - * rows/columns have any entries. - */ - memset( (void*) P, 0, sizeof( P ) ); - for( i=0 ; i<4 ; i++ ) - P[i][i] = 1; -} diff --git a/sw/airborne/ahrs_new.h b/sw/airborne/ahrs_new.h deleted file mode 100644 index 0813466aae..0000000000 --- a/sw/airborne/ahrs_new.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef AHRS_H -#define AHRS_H - - -#include - -#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 */ diff --git a/sw/airborne/ahrs_quat_fast_ekf.c b/sw/airborne/ahrs_quat_fast_ekf.c new file mode 100644 index 0000000000..7f671949aa --- /dev/null +++ b/sw/airborne/ahrs_quat_fast_ekf.c @@ -0,0 +1,439 @@ +//#include "ahrs_quat_fast_ekf.h" +#include "booz_ahrs.h" + +#include +#include +#include + +/* 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; + } +} diff --git a/sw/airborne/ahrs_quat_fast_ekf.h b/sw/airborne/ahrs_quat_fast_ekf.h new file mode 100644 index 0000000000..d290b28bbc --- /dev/null +++ b/sw/airborne/ahrs_quat_fast_ekf.h @@ -0,0 +1,26 @@ +#ifndef AHRS_QUAT_FAST_EKF_H +#define AHRS_QUAT_FAST_EKF_H + +#include +#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 */ diff --git a/sw/airborne/ahrs_quat_fast_ekf_utils.h b/sw/airborne/ahrs_quat_fast_ekf_utils.h new file mode 100644 index 0000000000..c9dd352707 --- /dev/null +++ b/sw/airborne/ahrs_quat_fast_ekf_utils.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 */ diff --git a/sw/airborne/booz_ahrs.h b/sw/airborne/booz_ahrs.h index 32053ae990..540e25ced3 100644 --- a/sw/airborne/booz_ahrs.h +++ b/sw/airborne/booz_ahrs.h @@ -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 */ diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c index fab8ff30ce..8b6fe608ca 100644 --- a/sw/airborne/booz_filter_main.c +++ b/sw/airborne/booz_filter_main.c @@ -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); diff --git a/sw/airborne/booz_filter_telemetry.h b/sw/airborne/booz_filter_telemetry.h index 1ecf13e81f..1d5920e098 100644 --- a/sw/airborne/booz_filter_telemetry.h +++ b/sw/airborne/booz_filter_telemetry.h @@ -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]); \ diff --git a/sw/airborne/booz_inter_mcu.c b/sw/airborne/booz_inter_mcu.c index b45c3888db..b08cf6ae4c 100644 --- a/sw/airborne/booz_inter_mcu.c +++ b/sw/airborne/booz_inter_mcu.c @@ -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 diff --git a/sw/airborne/multitilt.c b/sw/airborne/multitilt.c index ff7c23e7d5..2a8120ed5c 100644 --- a/sw/airborne/multitilt.c +++ b/sw/airborne/multitilt.c @@ -1,26 +1,12 @@ -#include "multitilt.h" +#include "booz_ahrs.h" #include #include #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 + } diff --git a/sw/airborne/multitilt.h b/sw/airborne/multitilt.h index d9eba7463a..b719c04145 100644 --- a/sw/airborne/multitilt.h +++ b/sw/airborne/multitilt.h @@ -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 */ diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 2431522771..dc096062dc 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -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)