diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile index 44345dda9..4b1d19afe 100644 --- a/flight/AHRS/Makefile +++ b/flight/AHRS/Makefile @@ -88,6 +88,7 @@ CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3 SRC = ahrs.c SRC += pios_board.c SRC += ahrs_fsm.c +SRC += insgps.c ## PIOS Hardware (STM32F10x) SRC += $(PIOSSTM32F10X)/pios_sys.c diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 0f59d57a1..57462bd8e 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -36,6 +36,7 @@ #include "ahrs.h" #include "pios_opahrs_proto.h" #include "ahrs_fsm.h" /* lfsm_state */ +#include "insgps.h" /** * State of AHRS EKF @@ -189,6 +190,8 @@ uint32_t total_conversion_blocks = 0; */ int main() { + float gyro[3], accel[3], mag[3]; + /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); @@ -222,6 +225,8 @@ int main() lfsm_init(); + INSGPSInit(); + ahrs_state = AHRS_IDLE;; /* Use simple averaging filter for now */ for (int i = 0; i < ADC_OVERSAMPLE; i++) @@ -230,7 +235,6 @@ int main() // Main loop while (1) { - // Alive signal PIOS_LED_Toggle(LED1); @@ -247,6 +251,19 @@ int main() downsample_data(); + gyro[0] = gyro_data.filtered.x; + gyro[1] = gyro_data.filtered.y; + gyro[2] = gyro_data.filtered.z; + accel[0] = accel_data.filtered.x, + accel[1] = accel_data.filtered.y, + accel[2] = accel_data.filtered.z, + mag[0] = mag_data.raw.axis[0]; + mag[1] = mag_data.raw.axis[1]; + mag[2] = mag_data.raw.axis[2]; + + INSPrediction(gyro, accel, 1 / (double) EKF_RATE); + MagCorrection(mag); + // Hacky - grab one sample from buffer to populate this. Need to send back // all raw data if it's happening accel_data.raw.x = valid_data_buffer[0]; @@ -531,7 +548,8 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) ADC_ExternalTrigConvCmd(ADC2, ENABLE); #endif - //RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); + RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); + RCC_PCLK2Config(RCC_HCLK_Div16); /* Enable ADC1->DMA request */ ADC_DMACmd(ADC1, ENABLE); diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h new file mode 100644 index 000000000..dd978fc46 --- /dev/null +++ b/flight/AHRS/inc/insgps.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file insgps.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the INSGPS exposed functionality. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef INSGPS_H_ +#define INSGPS_H_ + + // Exposed Function Prototypes + void INSGPSInit(); + void INSPrediction(float gyro_data[3], float accel_data[3], float dT); + void MagCorrection(float mag_data[3]); + void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt); + void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]); + + // Nav structure containing current solution + struct NavStruct { + float Pos[3]; // Position in meters and relative to a local NED frame + float Vel[3]; // Velocity in meters and in NED + float q[4]; // unit quaternion rotation relative to NED + } Nav; + + // constants + #define MagSensors 0x1C0 + #define FullSensors 0x3FF + #define GndSpeedAndMagSensors 0x1D8 + +#endif /* EKF_H_ */ diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c new file mode 100644 index 000000000..052759892 --- /dev/null +++ b/flight/AHRS/insgps.c @@ -0,0 +1,461 @@ +/** + ****************************************************************************** + * + * @file insgps.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief An INS/GPS algorithm implemented with an EKF. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "insgps.h" +#include +#include + +// constants/macros/typdefs +#define NUMX 13 // number of states, X is the state vector +#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector + +// Private functions +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]); +void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], float Y[NUMV], + float P[NUMX][NUMX], float X[NUMX], uint16_t SensorsUsed); +void RungeKutta(float X[NUMX],float U[NUMU], float dT); +void StateEq(float X[NUMX],float U[NUMU],float Xdot[NUMX]); +void LinearizeFG(float X[NUMX],float U[NUMU], float F[NUMX][NUMX], float G[NUMX][NUMW]); +void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); +void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); + +// Private variables +float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices + // global to init to zero and maintain zero elements +float Be[3]; // local magnetic unit vector in NED frame +float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector +float Q[NUMW], R[NUMV]; // input noise and measurement noise variances + + +// ************* Exposed Functions **************** +// ************************************************* +void INSGPSInit() //pretty much just a place holder for now +{ + Be[0]=1; Be[1]=0; Be[2]=0; // local magnetic unit vector + + P[0][0]=P[1][1]=P[2][2]=25; // initial position variance (m^2) + P[3][3]=P[4][4]=P[5][5]=5; // initial velocity variance (m/s)^2 + P[6][6]=P[7][7]=P[8][8]=P[9][9]=1e-5; // initial quaternion variance + P[10][10]=P[11][11]=P[12][12]=1e-6; // initial gyro bias variance (rad/s)^2 + + X[0]=X[1]=X[2]=X[3]=X[4]=X[5]=0; // initial pos and vel (m) + X[6]=1; X[7]=X[8]=X[9]=0; // initial quaternion (level and North) (m/s) + X[10]=X[11]=X[12]=0; // initial gyro bias (rad/s) + + Q[0]=Q[1]=Q[2]=50e-8; // gyro noise variance (rad/s)^2 + Q[3]=Q[4]=Q[5]=0.01; // accelerometer noise variance (m/s^2)^2 + Q[6]=Q[7]=Q[8]=2e-10; // gyro bias random walk variance (rad/s^2)^2 + + R[0]=R[1]=0.004; // High freq GPS horizontal position noise variance (m^2) + R[2]=0.036; // High freq GPS vertical position noise variance (m^2) + R[3]=R[4]=0.004; // High freq GPS horizontal velocity noise variance (m/s)^2 + R[5]=0; // High freq GPS vertical velocity noise variance (m/s)^2 + R[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance + R[10]=1; // High freq altimeter noise variance (m^2) +} + +void INSPrediction(float gyro_data[3], float accel_data[3], float dT) +{ + float U[6]; + float qmag; + + // rate gyro inputs in units of rad/s + U[0]=gyro_data[0]; + U[1]=gyro_data[1]; + U[2]=gyro_data[2]; + + // accelerometer inputs in units of m/s + U[3]=accel_data[0]; + U[4]=accel_data[1]; + U[5]=accel_data[2]; + + // EKF prediction step + LinearizeFG(X,U,F,G); + RungeKutta(X,U,dT); + qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]); + X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag; + CovariancePrediction(F,G,Q,dT,P); + + // Update Nav solution structure + Nav.Pos[0] = X[0]; + Nav.Pos[1] = X[1]; + Nav.Pos[2] = X[3]; + Nav.Vel[0] = X[3]; + Nav.Vel[1] = X[4]; + Nav.Vel[2] = X[5]; + Nav.q[0] = X[6]; + Nav.q[1] = X[7]; + Nav.q[2] = X[8]; + Nav.q[3] = X[9]; +} + +void MagCorrection(float mag_data[3]) +{ + float Z[10], Y[10]; + float Bmag, qmag; + + // magnetometer data in any units (use unit vector) and in body frame + Bmag = sqrt(mag_data[0]*mag_data[0] + mag_data[1]*mag_data[1] + mag_data[2]*mag_data[2]); + Z[6] = mag_data[0]/Bmag; + Z[7] = mag_data[1]/Bmag; + Z[8] = mag_data[2]/Bmag; + + // EKF correction step + LinearizeH(X,Be,H); + MeasurementEq(X,Be,Y); + SerialUpdate(H,R,Z,Y,P,X,MagSensors); + qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]); + X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag; + + // Update Nav solution structure + Nav.Pos[0] = X[0]; + Nav.Pos[1] = X[1]; + Nav.Pos[2] = X[3]; + Nav.Vel[0] = X[3]; + Nav.Vel[1] = X[4]; + Nav.Vel[2] = X[5]; + Nav.q[0] = X[6]; + Nav.q[1] = X[7]; + Nav.q[2] = X[8]; + Nav.q[3] = X[9]; +} + +void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt) +{ + float Z[10], Y[10]; + float Bmag, qmag; + + // GPS Position in meters and in local NED frame + Z[0]=Pos[0]; + Z[1]=Pos[1]; + Z[2]=Pos[2]; + + // GPS Velocity in meters and in local NED frame + Z[3]=Vel[0]; + Z[4]=Vel[1]; + Z[5]=Vel[2]; + + // magnetometer data in any units (use unit vector) and in body frame + Bmag = sqrt(mag_data[0]*mag_data[0] + mag_data[1]*mag_data[1] + mag_data[2]*mag_data[2]); + Z[6] = mag_data[0]/Bmag; + Z[7] = mag_data[1]/Bmag; + Z[8] = mag_data[2]/Bmag; + + // barometric altimeter in meters and in local NED frame + Z[9] = BaroAlt; + + // EKF correction step + LinearizeH(X,Be,H); + MeasurementEq(X,Be,Y); + SerialUpdate(H,R,Z,Y,P,X,FullSensors); + qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]); + X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag; + + // Update Nav solution structure + Nav.Pos[0] = X[0]; + Nav.Pos[1] = X[1]; + Nav.Pos[2] = X[3]; + Nav.Vel[0] = X[3]; + Nav.Vel[1] = X[4]; + Nav.Vel[2] = X[5]; + Nav.q[0] = X[6]; + Nav.q[1] = X[7]; + Nav.q[2] = X[8]; + Nav.q[3] = X[9]; +} + +void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]) +{ + float Z[10], Y[10]; + float Bmag, qmag; + + // Ground Speed in m/s and Heading in rad + Z[3] = Speed*cos((double)Heading); + Z[4] = Speed*sin((double)Heading); + + // magnetometer data in any units (use unit vector) and in body frame + Bmag = sqrt(mag_data[0]*mag_data[0] + mag_data[1]*mag_data[1] + mag_data[2]*mag_data[2]); + Z[6] = mag_data[0]/Bmag; + Z[7] = mag_data[1]/Bmag; + Z[8] = mag_data[2]/Bmag; + + // EKF correction step + LinearizeH(X,Be,H); + MeasurementEq(X,Be,Y); + SerialUpdate(H,R,Z,Y,P,X,GndSpeedAndMagSensors); + qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]); + X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag; + + // Update Nav solution structure + Nav.Pos[0] = X[0]; + Nav.Pos[1] = X[1]; + Nav.Pos[2] = X[3]; + Nav.Vel[0] = X[3]; + Nav.Vel[1] = X[4]; + Nav.Vel[2] = X[5]; + Nav.q[0] = X[6]; + Nav.q[1] = X[7]; + Nav.q[2] = X[8]; + Nav.q[3] = X[9]; +} + +// ************* CovariancePrediction ************* +// Does the prediction step of the Kalman filter for the covariance matrix +// Output, Pnew, overwrites P, the input covariance +// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' +// Q is the discrete time covariance of process noise +// Q is vector of the diagonal for a square matrix with +// dimensions equal to the number of disturbance noise variables +// Could be much more efficient using the sparse, block structure of F and G +// ************************************************ + +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]){ + float Dummy[NUMX][NUMX], dTsq; + uint8_t i,j,k; + + // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')] + + dTsq = dT*dT; + + for (i=0;i