rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Flight/AHRS: Update code to coding conventions.

Browse Source 
find ./flight/AHRS/ \! \( -name '*~' -a -prune \) -type f    | xargs -I{} bash -c 'echo {}; dos2unix {}; gnuindent -npro -kr -i8 -ts8 -sob -ss -ncs -cp1 -il0 {};'

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1707 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-21 19:29:39 +00:00 committed by peabody124
parent 833e8428d2
commit a1a3b0774f
14 changed files with 3883 additions and 2568 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
flight/AHRS/MagOrAccelSensorCal.c
View File

@ -1,144 +1,149 @@
/**
******************************************************************************
*
* @file MagOrAccelSensorCal.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief 3 axis sensor cal from six measurements taken in a constant field.
* Call SixPointInConstFieldCal(FieldMagnitude, X, Y, Z, S, b)
* - FieldMagnitude is the constant field, e.g. 9.81 for accels
* - X, Y, Z are vectors of six measurements from different orientations
* - returns, S[3] and b[3], that are the scale and offsett for the axes
* - i.e. Measurementx = S[0]*Sensorx + b[0]
*
* @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 <math.h>
#include "stdint.h"
//Function Prototypes
int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution);
int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
{
int16_t i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i=0;i<5;i++){
A[i][0] = 2.0 * (x[i+1] - x[i]);
A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
A[i][2] = 2.0 * (y[i+1] - y[i]);
A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
A[i][4] = 2.0 * (z[i+1] - z[i]);
f[i] = x[i]*x[i] - x[i+1]*x[i+1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if ( !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0]; yp = y[0]; zp = z[0];
Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
S[0] = Sx;
b[0] = Sx*c[0];
S[1] = sqrt(c[1]*Sx*Sx);
b[1] = c[2]*Sx*Sx/S[1];
S[2] = sqrt(c[3]*Sx*Sx);
b[2] = c[4]*Sx*Sx/S[2];
return 1;
}
//*****************************************************************
int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution)
{
double fMaxElem;
double fAcc;
int16_t i , j, k, m;
for(k=0; k<(nDim-1); k++) // base row of matrix
{
// search of line with max element
fMaxElem = fabs( pfMatr[k*nDim + k] );
m = k;
for(i=k+1; i<nDim; i++)
{
if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
{
fMaxElem = pfMatr[i*nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if(m != k)
{
for(i=k; i<nDim; i++)
{
fAcc = pfMatr[k*nDim + i];
pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
pfMatr[m*nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!
// triangulation of matrix with coefficients
for(j=(k+1); j<nDim; j++) // current row of matrix
{
fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
for(i=k; i<nDim; i++)
{
pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
}
pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
}
}
for(k=(nDim-1); k>=0; k--)
{
pfSolution[k] = pfVect[k];
for(i=(k+1); i<nDim; i++)
{
pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
}
return 1;
}
/**
******************************************************************************
*
* @file MagOrAccelSensorCal.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief 3 axis sensor cal from six measurements taken in a constant field.
* Call SixPointInConstFieldCal(FieldMagnitude, X, Y, Z, S, b)
* - FieldMagnitude is the constant field, e.g. 9.81 for accels
* - X, Y, Z are vectors of six measurements from different orientations
* - returns, S[3] and b[3], that are the scale and offsett for the axes
* - i.e. Measurementx = S[0]*Sensorx + b[0]
*
* @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 <math.h>
#include "stdint.h"
//Function Prototypes
int16_t SixPointInConstFieldCal(double ConstMag, double x[6], double y[6],
double z[6], double S[3], double b[3]);
int16_t LinearEquationsSolving(int16_t nDim, double *pfMatr,
double *pfVect, double *pfSolution);
int16_t SixPointInConstFieldCal(double ConstMag, double x[6], double y[6],
double z[6], double S[3], double b[3])
{
int16_t i;
double A[5][5];
double f[5], c[5];
double xp, yp, zp, Sx;
// Fill in matrix A -
// write six difference-in-magnitude equations of the form
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
// or in other words
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
for (i = 0; i < 5; i++) {
A[i][0] = 2.0 * (x[i + 1] - x[i]);
A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
A[i][2] = 2.0 * (y[i + 1] - y[i]);
A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
A[i][4] = 2.0 * (z[i + 1] - z[i]);
f[i] = x[i] * x[i] - x[i + 1] * x[i + 1];
}
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
if (!LinearEquationsSolving(5, (double *)A, f, c))
return 0;
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
xp = x[0];
yp = y[0];
zp = z[0];
Sx = sqrt(ConstMag * ConstMag /
(xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp +
2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp +
2 * c[4] * zp + c[4] * c[4] / c[3]));
S[0] = Sx;
b[0] = Sx * c[0];
S[1] = sqrt(c[1] * Sx * Sx);
b[1] = c[2] * Sx * Sx / S[1];
S[2] = sqrt(c[3] * Sx * Sx);
b[2] = c[4] * Sx * Sx / S[2];
return 1;
}
//*****************************************************************
int16_t LinearEquationsSolving(int16_t nDim, double *pfMatr,
double *pfVect, double *pfSolution)
{
double fMaxElem;
double fAcc;
int16_t i, j, k, m;
for (k = 0; k < (nDim - 1); k++) // base row of matrix
{
// search of line with max element
fMaxElem = fabs(pfMatr[k * nDim + k]);
m = k;
for (i = k + 1; i < nDim; i++) {
if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
fMaxElem = pfMatr[i * nDim + k];
m = i;
}
}
// permutation of base line (index k) and max element line(index m)
if (m != k) {
for (i = k; i < nDim; i++) {
fAcc = pfMatr[k * nDim + i];
pfMatr[k * nDim + i] =
pfMatr[m * nDim + i];
pfMatr[m * nDim + i] = fAcc;
}
fAcc = pfVect[k];
pfVect[k] = pfVect[m];
pfVect[m] = fAcc;
}
if (pfMatr[k * nDim + k] == 0.)
return 0; // needs improvement !!!
// triangulation of matrix with coefficients
for (j = (k + 1); j < nDim; j++) // current row of matrix
{
fAcc =
-pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
for (i = k; i < nDim; i++) {
pfMatr[j * nDim + i] =
pfMatr[j * nDim + i] +
fAcc * pfMatr[k * nDim + i];
}
pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
}
}
for (k = (nDim - 1); k >= 0; k--) {
pfSolution[k] = pfVect[k];
for (i = (k + 1); i < nDim; i++) {
pfSolution[k] -=
(pfMatr[k * nDim + i] * pfSolution[i]);
}
pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
}
return 1;
}

1931
flight/AHRS/ahrs.c
View File

@ -1,855 +1,1076 @@
/**
******************************************************************************
* @addtogroup AHRS AHRS Control
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @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
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
#include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"
volatile enum algorithms ahrs_algorithm;
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#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
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
/**
* @addtogroup AHRS_Definitions
* @{
*/
// Currently analog acquistion hard coded at 480 Hz
#define ADC_RATE (4*480)
#define EKF_RATE (ADC_RATE / adc_oversampling)
#define VDD 3.3 /* supply voltage for ADC */
#define FULL_RANGE 4096 /* 12 bit ADC */
#define ACCEL_RANGE 2 /* adjustable by FS input */
#define ACCEL_GRAVITY 9.81 /* m s^-1 */
#define ACCEL_SENSITIVITY ( VDD / 5 )
#define ACCEL_SCALE ( (VDD / FULL_RANGE) / ACCEL_SENSITIVITY * 2 / ACCEL_RANGE * ACCEL_GRAVITY )
#define ACCEL_OFFSET -2048
#define GYRO_SENSITIVITY ( 2.0 / 1000 ) /* 2 mV / (deg s^-1) */
#define RAD_PER_DEGREE ( M_PI / 180 )
#define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE )
#define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */
#define MAX_IDLE_COUNT 65e3
/**
* @}
*/
/**
* @addtogroup AHRS_Local Local Variables
* @{
*/
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
};
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
};
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
uint16_t xy;
uint16_t z;
} temp;
};
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
};
struct altitude_sensor {
float altitude;
bool updated;
};
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
};
struct mag_sensor mag_data;
volatile struct accel_sensor accel_data;
volatile struct gyro_sensor gyro_data;
volatile struct altitude_sensor altitude_data;
struct gps_sensor gps_data;
volatile struct attitude_solution attitude_data;
/**
* @}
*/
/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void converge_insgps();
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Accelerometer variance after filter from OP or calibrate_sensors
float accel_var[3] = {1,1,1};
//! Gyro variance after filter from OP or calibrate sensors
float gyro_var[3] = {1,1,1};
//! Accelerometer scale after calibration
float accel_scale[3] = {ACCEL_SCALE, ACCEL_SCALE, ACCEL_SCALE};
//! Gyro scale after calibration
float gyro_scale[3] = {GYRO_SCALE, GYRO_SCALE, GYRO_SCALE};
//! Magnetometer variance from OP or calibrate sensors
float mag_var[3] = {1,1,1};
//! Accelerometer bias from OP or calibrate sensors
int16_t accel_bias[3] = {ACCEL_OFFSET, ACCEL_OFFSET, ACCEL_OFFSET};
//! Gyroscope bias term from OP or calibrate sensors
int16_t gyro_bias[3] = {0,0,0};
//! Magnetometer bias (direction) from OP or calibrate sensors
int16_t mag_bias[3] = {0,0,0};
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! Home location in ECEF coordinates
double BaseECEF[3] = {0, 0, 0};
//! Rotation matrix from LLA to Rne
float Rne[3][3];
//! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE;
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 25;
/**
* @}
*/
/**
* @brief AHRS Main function
*/
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = {0,0,0};
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config(adc_oversampling);
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy ((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
/* SPI link to master */
PIOS_SPI_Init();
lfsm_init();
ahrs_state = AHRS_IDLE;
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
if(ahrs_algorithm == INSGPS_Algo) {
// compute a data point and initialize INS
downsample_data();
converge_insgps();
}
#ifdef DUMP_RAW
int previous_conversion;
while(1) {
int result;
uint8_t framing[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
while( ahrs_state != AHRS_DATA_READY );
ahrs_state = AHRS_PROCESSING;
if(total_conversion_blocks != previous_conversion+1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &valid_data_buffer[0], ADC_OVERSAMPLE * ADC_CONTINUOUS_CHANNELS * sizeof(valid_data_buffer[0]));
if(result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while (1) {
// Alive signal
if((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
if(calibration_pending)
{
calibrate_sensors();
calibration_pending = FALSE;
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.updated = 1;
}
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while ( ahrs_state != AHRS_DATA_READY );
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/***************** SEND BACK SOME RAW DATA ************************/
// 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];
accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7];
if(ahrs_algorithm == INSGPS_Algo)
{
/******************** INS ALGORITHM **************************/
// format data for INS algo
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,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -(mag_data.raw.axis[1] - mag_bias[1]);
mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
INSStatePrediction(gyro, accel, 1 / (float) EKF_RATE);
process_spi_request();
INSCovariancePrediction(1 / (float) EKF_RATE);
if ( gps_data.updated && gps_data.quality == 1)
{
// Compute velocity from Heading and groundspeed
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar(0.004);
if(gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,vel,altitude_data.altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
}
else if(gps_data.quality != -1 && mag_data.updated == 1) {
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection(vel,altitude_data.altitude);
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
}
else if( ahrs_algorithm == SIMPLE_Algo )
{
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI;
rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI;
rpy[0] = atan2(accel_data.filtered.y,accel_data.filtered.z) * 180 / M_PI;
RPY2Quaternion(rpy,q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 1000), (int16_t)(accel_data.filtered.y * 1000), (int16_t)(accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 1000), (int16_t)(gyro_data.filtered.y * 1000), (int16_t)(gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 1000), (int16_t)(Nav.q[1] * 1000), (int16_t)(Nav.q[2] * 1000), (int16_t)(Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] = {15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0};
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &mag_data, sizeof(mag_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gps_data, sizeof(gps_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data, sizeof(accel_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data, sizeof(gyro_data));
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &Q, sizeof(float)*NUMX*NUMX);
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &K, sizeof(float)*NUMX*NUMV);
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &X, sizeof(float)*NUMX*NUMX);
if(result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
}
return 0;
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
int32_t accel_raw[3], gyro_raw[3];
uint16_t i;
// Get the Y data. Third byte in. Convert to m/s
accel_raw[0] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[0] += ( valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] + accel_bias[1] ) * fir_coeffs[i];
accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[adc_oversampling] * accel_scale[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_raw[1] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[1] += ( valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] + accel_bias[0] ) * fir_coeffs[i];
accel_data.filtered.x = (float) accel_raw[1] / (float) fir_coeffs[adc_oversampling] * accel_scale[0];
// Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0;
for( i = 0; i < adc_oversampling; i++ )
accel_raw[2] += ( valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] + accel_bias[2] ) * fir_coeffs[i];
accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[adc_oversampling] * accel_scale[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[0] += ( valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] + gyro_bias[0] ) * fir_coeffs[i];
gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[adc_oversampling] * gyro_scale[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[1] += ( valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] + gyro_bias[1] ) * fir_coeffs[i];
gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[adc_oversampling] * gyro_scale[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0;
for( i = 0; i < adc_oversampling; i++ )
gyro_raw[2] += ( valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] + gyro_bias[2] ) * fir_coeffs[i];
gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[adc_oversampling] * gyro_scale[2];
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*/
void calibrate_sensors() {
int i;
int16_t mag_raw[3] = {0,0,0};
// local biases for noise analysis
float accel_bias[3], gyro_bias[3], mag_bias[3];
// run few loops to get mean
gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
mag_bias[0] = mag_bias[1] = mag_bias[2] = 0;
for(i = 0; i < 50; i++) {
while( ahrs_state != AHRS_DATA_READY );
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_bias[0] += gyro_data.filtered.x;
gyro_bias[1] += gyro_data.filtered.y;
gyro_bias[2] += gyro_data.filtered.z;
accel_bias[0] += accel_data.filtered.x;
accel_bias[1] += accel_data.filtered.y;
accel_bias[2] += accel_data.filtered.z;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_raw);
#endif
mag_bias[0] += mag_raw[0];
mag_bias[1] += mag_raw[1];
mag_bias[2] += mag_raw[2];
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_bias[0] /= i;
gyro_bias[1] /= i;
gyro_bias[2] /= i;
accel_bias[0] /= i;
accel_bias[1] /= i;
accel_bias[2] /= i;
mag_bias[0] /= i;
mag_bias[1] /= i;
mag_bias[2] /= i;
// more iterations for variance
accel_var[0] = accel_var[1] = accel_var[2] = 0;
gyro_var[0] = gyro_var[1] = gyro_var[2] = 0;
mag_var[0] = mag_var[1] = mag_var[2] = 0;
for(i = 0; i < 500; i++)
{
while( ahrs_state != AHRS_DATA_READY );
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_var[0] += (gyro_data.filtered.x - gyro_bias[0]) * (gyro_data.filtered.x - gyro_bias[0]);
gyro_var[1] += (gyro_data.filtered.y - gyro_bias[1]) * (gyro_data.filtered.y - gyro_bias[1]);
gyro_var[2] += (gyro_data.filtered.z - gyro_bias[2]) * (gyro_data.filtered.z - gyro_bias[2]);
accel_var[0] += (accel_data.filtered.x - accel_bias[0]) * (accel_data.filtered.x - accel_bias[0]);
accel_var[1] += (accel_data.filtered.y - accel_bias[1]) * (accel_data.filtered.y - accel_bias[1]);
accel_var[2] += (accel_data.filtered.z - accel_bias[2]) * (accel_data.filtered.z - accel_bias[2]);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_raw);
#endif
mag_var[0] += (mag_raw[0] - mag_bias[0]) * (mag_raw[0] - mag_bias[0]);
mag_var[1] += (mag_raw[1] - mag_bias[1]) * (mag_raw[1] - mag_bias[1]);
mag_var[2] += (mag_raw[2] - mag_bias[2]) * (mag_raw[2] - mag_bias[2]);
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_var[0] /= i;
gyro_var[1] /= i;
gyro_var[2] /= i;
accel_var[0] /= i;
accel_var[1] /= i;
accel_var[2] /= i;
mag_var[0] /= i;
mag_var[1] /= i;
mag_var[2] /= i;
float mag_length2 = mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] + mag_bias[2] * mag_bias[2];
mag_var[0] = mag_var[0] / mag_length2;
mag_var[1] = mag_var[1] / mag_length2;
mag_var[2] = mag_var[2] / mag_length2;
if(ahrs_algorithm == INSGPS_Algo)
converge_insgps();
}
/**
* @brief Quickly initialize INS assuming stationary and gravity is down
*
* Currently this is done iteratively but I'm sure it can be directly computed
* when I sit down and work it out
*/
void converge_insgps()
{
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {0, 0, 0};
INSGPSInit();
INSSetAccelVar(accel_var);
INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
float temp_var[3] = {10, 10, 10};
INSSetGyroVar(temp_var); // ignore gyro's
accel[0] = accel_data.filtered.x;
accel[1] = accel_data.filtered.y;
accel[2] = accel_data.filtered.z;
// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.
for(int i = 0; i < 50; i++)
{
// This should be done directly but I'm too dumb.
float rpy[3];
Quaternion2RPY(Nav.q, rpy);
rpy[1] = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI;
rpy[0] = -atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
// Get magnetic readings
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
#endif
mag[0] = -mag_data.raw.axis[1];
mag[1] = -mag_data.raw.axis[0];
mag[2] = -mag_data.raw.axis[2];
RPY2Quaternion(rpy,Nav.q);
INSStatePrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
INSCovariancePrediction(1 / (float) EKF_RATE);
FullCorrection(mag,pos,vel,BaroAlt);
process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
}
INSSetGyroVar(gyro_var);
}
/**
* @addtogroup AHRS_SPI SPI Messaging
* @{
* @brief SPI protocol handling requests for data from OP mainboard
*/
static struct opahrs_msg_v1 link_tx_v1;
static struct opahrs_msg_v1 link_rx_v1;
static struct opahrs_msg_v1 user_rx_v1;
static struct opahrs_msg_v1 user_tx_v1;
void process_spi_request(void)
{
bool msg_to_process = FALSE;
PIOS_IRQ_Disable();
/* Figure out if we're in an interesting stable state */
switch (lfsm_get_state()) {
case LFSM_STATE_USER_BUSY:
msg_to_process = TRUE;
break;
case LFSM_STATE_INACTIVE:
/* Queue up a receive buffer */
lfsm_user_set_rx_v1 (&user_rx_v1);
lfsm_user_done ();
break;
case LFSM_STATE_STOPPED:
/* Get things going */
lfsm_set_link_proto_v1 (&link_tx_v1, &link_rx_v1);
break;
default:
/* Not a stable state */
break;
}
PIOS_IRQ_Enable();
if (!msg_to_process) {
/* Nothing to do */
return;
}
switch (user_rx_v1.payload.user.t) {
case OPAHRS_MSG_V1_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V1_REQ_SERIAL:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SERIAL);
PIOS_SYS_SerialNumberGet((char *)&(user_tx_v1.payload.user.v.rsp.serial.serial_bcd));
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ALGORITHM:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ALGORITHM);
ahrs_algorithm = user_rx_v1.payload.user.v.req.algorithm.algorithm;
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_NORTH:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_NORTH);
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_CALIBRATION:
if(user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_MEASURE) {
calibration_pending = TRUE;
} else if (user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_SET) {
accel_var[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0];
accel_var[1] = user_rx_v1.payload.user.v.req.calibration.accel_var[1];
accel_var[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
gyro_bias[0] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[0];
gyro_bias[1] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[1];
gyro_bias[2] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[2];
gyro_var[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
gyro_var[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
gyro_var[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
mag_var[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
mag_var[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
mag_var[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
INSSetAccelVar(accel_var);
float gyro_bias_ins[3] = {0,0,0};
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
}
if(user_rx_v1.payload.user.v.req.calibration.measure_var != AHRS_ECHO) {
/* if echoing don't set anything */
accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
accel_scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
accel_scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
accel_scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
mag_bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
mag_bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
mag_bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
}
// echo back the values used
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_CALIBRATION);
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] = accel_var[0];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] = accel_var[1];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] = accel_var[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] = gyro_bias[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] = gyro_bias[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] = gyro_bias[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_var[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_var[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_var[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_var[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_var[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_var[2];
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDERAW);
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x = mag_data.raw.axis[0];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y = mag_data.raw.axis[1];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.z = mag_data.raw.axis[2];
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x = gyro_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y = gyro_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z = gyro_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.x = gyro_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.y = gyro_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.z = gyro_data.filtered.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp = gyro_data.temp.xy;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp = gyro_data.temp.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.x = accel_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.y = accel_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.z = accel_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.x = accel_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.y = accel_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.z = accel_data.filtered.z;
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_UPDATE:
// process incoming data
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_UPDATE);
if(user_rx_v1.payload.user.v.req.update.barometer.updated)
{
altitude_data.altitude = user_rx_v1.payload.user.v.req.update.barometer.altitude;
altitude_data.updated = user_rx_v1.payload.user.v.req.update.barometer.updated;
}
if(user_rx_v1.payload.user.v.req.update.gps.updated)
{
gps_data.updated = true;
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
gps_data.groundspeed = user_rx_v1.payload.user.v.req.update.gps.groundspeed;
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
}
// send out attitude/position estimate
user_tx_v1.payload.user.v.rsp.update.quaternion.q1 = attitude_data.quaternion.q1;
user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2;
user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3;
user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4;
// TODO: separate this from INSGPS
user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0];
user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1];
user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2];
user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0];
user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1];
user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];
// compute the idle fraction
user_tx_v1.payload.user.v.rsp.update.load = ((float) running_counts / (float) (idle_counts+running_counts)) * 100;
user_tx_v1.payload.user.v.rsp.update.idle_time = idle_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.run_time = running_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.dropped_updates = ekf_too_slow;
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
default:
break;
}
/* Finished processing the received message, requeue it */
lfsm_user_set_rx_v1 (&user_rx_v1);
lfsm_user_done ();
}
/**
* @}
*/
/**
******************************************************************************
* @addtogroup AHRS AHRS Control
* @brief The AHRS Modules perform
*
* @{
* @addtogroup AHRS_Main
* @brief Main function which does the hardware dependent stuff
* @{
*
*
* @file ahrs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief INSGPS Test Program
* @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
*/
/* OpenPilot Includes */
#include "ahrs.h"
#include "ahrs_adc.h"
#include "ahrs_timer.h"
#include "pios_opahrs_proto.h"
#include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h"
#include "CoordinateConversions.h"
volatile enum algorithms ahrs_algorithm;
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#ifdef DUMP_EKF
#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
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
/**
* @addtogroup AHRS_Definitions
* @{
*/
// Currently analog acquistion hard coded at 480 Hz
#define ADC_RATE (4*480)
#define EKF_RATE (ADC_RATE / adc_oversampling)
#define VDD 3.3 /* supply voltage for ADC */
#define FULL_RANGE 4096 /* 12 bit ADC */
#define ACCEL_RANGE 2 /* adjustable by FS input */
#define ACCEL_GRAVITY 9.81 /* m s^-1 */
#define ACCEL_SENSITIVITY ( VDD / 5 )
#define ACCEL_SCALE ( (VDD / FULL_RANGE) / ACCEL_SENSITIVITY * 2 / ACCEL_RANGE * ACCEL_GRAVITY )
#define ACCEL_OFFSET -2048
#define GYRO_SENSITIVITY ( 2.0 / 1000 ) /* 2 mV / (deg s^-1) */
#define RAD_PER_DEGREE ( M_PI / 180 )
#define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE )
#define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */
#define MAX_IDLE_COUNT 65e3
/**
* @}
*/
/**
* @addtogroup AHRS_Local Local Variables
* @{
*/
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
};
struct accel_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
};
struct gyro_sensor {
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} raw;
struct {
float x;
float y;
float z;
} filtered;
struct {
uint16_t xy;
uint16_t z;
} temp;
};
struct attitude_solution {
struct {
float q1;
float q2;
float q3;
float q4;
} quaternion;
};
struct altitude_sensor {
float altitude;
bool updated;
};
struct gps_sensor {
float NED[3];
float heading;
float groundspeed;
float quality;
bool updated;
};
struct mag_sensor mag_data;
volatile struct accel_sensor accel_data;
volatile struct gyro_sensor gyro_data;
volatile struct altitude_sensor altitude_data;
struct gps_sensor gps_data;
volatile struct attitude_solution attitude_data;
/**
* @}
*/
/* Function Prototypes */
void process_spi_request(void);
void downsample_data(void);
void calibrate_sensors(void);
void converge_insgps();
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
volatile uint32_t idle_counts;
volatile uint32_t running_counts;
uint32_t counter_val;
/**
* @addtogroup AHRS_Global_Data AHRS Global Data
* @{
* Public data. Used by both EKF and the sender
*/
//! Accelerometer variance after filter from OP or calibrate_sensors
float accel_var[3] = { 1, 1, 1 };
//! Gyro variance after filter from OP or calibrate sensors
float gyro_var[3] = { 1, 1, 1 };
//! Accelerometer scale after calibration
float accel_scale[3] = { ACCEL_SCALE, ACCEL_SCALE, ACCEL_SCALE };
//! Gyro scale after calibration
float gyro_scale[3] = { GYRO_SCALE, GYRO_SCALE, GYRO_SCALE };
//! Magnetometer variance from OP or calibrate sensors
float mag_var[3] = { 1, 1, 1 };
//! Accelerometer bias from OP or calibrate sensors
int16_t accel_bias[3] = { ACCEL_OFFSET, ACCEL_OFFSET, ACCEL_OFFSET };
//! Gyroscope bias term from OP or calibrate sensors
int16_t gyro_bias[3] = { 0, 0, 0 };
//! Magnetometer bias (direction) from OP or calibrate sensors
int16_t mag_bias[3] = { 0, 0, 0 };
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
//! Home location in ECEF coordinates
double BaseECEF[3] = { 0, 0, 0 };
//! Rotation matrix from LLA to Rne
float Rne[3][3];
//! Indicates the communications are requesting a calibration
uint8_t calibration_pending = FALSE;
//! The oversampling rate, ekf is 2k / this
static uint8_t adc_oversampling = 25;
/**
* @}
*/
/**
* @brief AHRS Main function
*/
int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = { 0, 0, 0 };
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Delay system */
PIOS_DELAY_Init();
/* Communication system */
PIOS_COM_Init();
/* ADC system */
AHRS_ADC_Config(adc_oversampling);
/* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0);
SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
/* Magnetic sensor system */
PIOS_I2C_Init();
PIOS_HMC5843_Init();
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
#endif
/* SPI link to master */
PIOS_SPI_Init();
lfsm_init();
ahrs_state = AHRS_IDLE;
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
if (ahrs_algorithm == INSGPS_Algo) {
// compute a data point and initialize INS
downsample_data();
converge_insgps();
}
#ifdef DUMP_RAW
int previous_conversion;
while (1) {
int result;
uint8_t framing[16] =
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15 };
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
if (total_conversion_blocks != previous_conversion + 1)
PIOS_LED_On(LED1); // not keeping up
else
PIOS_LED_Off(LED1);
previous_conversion = total_conversion_blocks;
downsample_data();
ahrs_state = AHRS_IDLE;;
// Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & valid_data_buffer[0],
ADC_OVERSAMPLE *
ADC_CONTINUOUS_CHANNELS *
sizeof(valid_data_buffer[0]));
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
}
#endif
timer_start();
/******************* Main EKF loop ****************************/
while (1) {
// Alive signal
if ((total_conversion_blocks % 100) == 0)
PIOS_LED_Toggle(LED1);
if (calibration_pending) {
calibrate_sensors();
calibration_pending = FALSE;
}
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.updated = 1;
}
#endif
// Delay for valid data
counter_val = timer_count();
running_counts = counter_val - last_counter_idle_end;
last_counter_idle_start = counter_val;
while (ahrs_state != AHRS_DATA_READY) ;
counter_val = timer_count();
idle_counts = counter_val - last_counter_idle_start;
last_counter_idle_end = counter_val;
ahrs_state = AHRS_PROCESSING;
downsample_data();
/***************** SEND BACK SOME RAW DATA ************************/
// 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];
accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7];
if (ahrs_algorithm == INSGPS_Algo) {
/******************** INS ALGORITHM **************************/
// format data for INS algo
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,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -(mag_data.raw.axis[1] - mag_bias[1]);
mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
INSStatePrediction(gyro, accel,
1 / (float)EKF_RATE);
process_spi_request();
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && gps_data.quality == 1) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_data.groundspeed *
cos(gps_data.heading * M_PI / 180);
vel[1] =
gps_data.groundspeed *
sin(gps_data.heading * M_PI / 180);
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar(0.004);
if (gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED,
vel,
altitude_data.
altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,
vel,
altitude_data.
altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
} else if (gps_data.quality != -1
&& mag_data.updated == 1) {
MagCorrection(mag); // only trust mags if outdoors
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection(vel,
altitude_data.altitude);
// MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3];
} else if (ahrs_algorithm == SIMPLE_Algo) {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from accel. */
rpy[2] =
atan2((mag_data.raw.axis[0]),
(-1 * mag_data.raw.axis[1])) * 180 /
M_PI;
rpy[1] =
atan2(accel_data.filtered.x,
accel_data.filtered.z) * 180 / M_PI;
rpy[0] =
atan2(accel_data.filtered.y,
accel_data.filtered.z) * 180 / M_PI;
RPY2Quaternion(rpy, q);
attitude_data.quaternion.q1 = q[0];
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
#ifdef DUMP_FRIENDLY
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "b: %d\r\n",
total_conversion_blocks);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"a: %d %d %d\r\n",
(int16_t) (accel_data.filtered.x * 1000),
(int16_t) (accel_data.filtered.y * 1000),
(int16_t) (accel_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX, "g: %d %d %d\r\n",
(int16_t) (gyro_data.filtered.x * 1000),
(int16_t) (gyro_data.filtered.y * 1000),
(int16_t) (gyro_data.filtered.z * 1000));
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,"m: %d %d %d\r\n",
mag_data.raw.axis[0],
mag_data.raw.axis[1],
mag_data.raw.axis[2]);
PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_AUX,
"q: %d %d %d %d\r\n",
(int16_t) (Nav.q[0] * 1000),
(int16_t) (Nav.q[1] * 1000),
(int16_t) (Nav.q[2] * 1000),
(int16_t) (Nav.q[3] * 1000));
#endif
#ifdef DUMP_EKF
uint8_t framing[16] =
{ 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
0 };
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
// Dump raw buffer
int8_t result;
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header
result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & mag_data,
sizeof(mag_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gps_data,
sizeof(gps_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & accel_data,
sizeof(accel_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX,
(uint8_t *) & gyro_data,
sizeof(gyro_data));
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Q,
sizeof(float) * NUMX * NUMX);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & K,
sizeof(float) * NUMX * NUMV);
result +=
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X,
sizeof(float) * NUMX * NUMX);
if (result == 0)
PIOS_LED_Off(LED1);
else {
PIOS_LED_On(LED1);
}
#endif
}
return 0;
}
/**
* @brief Downsample the analog data
* @return none
*
* Tried to make as much of the filtering fixed point when possible. Need to account
* for offset for each sample before the multiplication if filter not a boxcar. Could
* precompute fixed offset as sum[fir_coeffs[i]] * ACCEL_OFFSET. Puts data into global
* data structures @ref accel_data and @ref gyro_data.
*
* The accel_data values are converted into a coordinate system where X is forwards along
* the fuselage, Y is along right the wing, and Z is down.
*/
void downsample_data()
{
int32_t accel_raw[3], gyro_raw[3];
uint16_t i;
// Get the Y data. Third byte in. Convert to m/s
accel_raw[0] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_raw[0] +=
(valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] +
accel_bias[1]) * fir_coeffs[i];
accel_data.filtered.y =
(float)accel_raw[0] / (float)fir_coeffs[adc_oversampling] *
accel_scale[1];
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_raw[1] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_raw[1] +=
(valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] +
accel_bias[0]) * fir_coeffs[i];
accel_data.filtered.x =
(float)accel_raw[1] / (float)fir_coeffs[adc_oversampling] *
accel_scale[0];
// Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0;
for (i = 0; i < adc_oversampling; i++)
accel_raw[2] +=
(valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] +
accel_bias[2]) * fir_coeffs[i];
accel_data.filtered.z =
-(float)accel_raw[2] / (float)fir_coeffs[adc_oversampling] *
accel_scale[2];
// Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_raw[0] +=
(valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] +
gyro_bias[0]) * fir_coeffs[i];
gyro_data.filtered.x =
(float)gyro_raw[0] / (float)fir_coeffs[adc_oversampling] *
gyro_scale[0];
// Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_raw[1] +=
(valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] +
gyro_bias[1]) * fir_coeffs[i];
gyro_data.filtered.y =
(float)gyro_raw[1] / (float)fir_coeffs[adc_oversampling] *
gyro_scale[1];
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0;
for (i = 0; i < adc_oversampling; i++)
gyro_raw[2] +=
(valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] +
gyro_bias[2]) * fir_coeffs[i];
gyro_data.filtered.z =
(float)gyro_raw[2] / (float)fir_coeffs[adc_oversampling] *
gyro_scale[2];
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors
* @returns None
*
* All data is stored in global structures. This function should be called from OP when
* aircraft is in stable state and then the data stored to SD card.
*/
void calibrate_sensors()
{
int i;
int16_t mag_raw[3] = { 0, 0, 0 };
// local biases for noise analysis
float accel_bias[3], gyro_bias[3], mag_bias[3];
// run few loops to get mean
gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
mag_bias[0] = mag_bias[1] = mag_bias[2] = 0;
for (i = 0; i < 50; i++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_bias[0] += gyro_data.filtered.x;
gyro_bias[1] += gyro_data.filtered.y;
gyro_bias[2] += gyro_data.filtered.z;
accel_bias[0] += accel_data.filtered.x;
accel_bias[1] += accel_data.filtered.y;
accel_bias[2] += accel_data.filtered.z;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_raw);
#endif
mag_bias[0] += mag_raw[0];
mag_bias[1] += mag_raw[1];
mag_bias[2] += mag_raw[2];
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_bias[0] /= i;
gyro_bias[1] /= i;
gyro_bias[2] /= i;
accel_bias[0] /= i;
accel_bias[1] /= i;
accel_bias[2] /= i;
mag_bias[0] /= i;
mag_bias[1] /= i;
mag_bias[2] /= i;
// more iterations for variance
accel_var[0] = accel_var[1] = accel_var[2] = 0;
gyro_var[0] = gyro_var[1] = gyro_var[2] = 0;
mag_var[0] = mag_var[1] = mag_var[2] = 0;
for (i = 0; i < 500; i++) {
while (ahrs_state != AHRS_DATA_READY) ;
ahrs_state = AHRS_PROCESSING;
downsample_data();
gyro_var[0] +=
(gyro_data.filtered.x -
gyro_bias[0]) * (gyro_data.filtered.x - gyro_bias[0]);
gyro_var[1] +=
(gyro_data.filtered.y -
gyro_bias[1]) * (gyro_data.filtered.y - gyro_bias[1]);
gyro_var[2] +=
(gyro_data.filtered.z -
gyro_bias[2]) * (gyro_data.filtered.z - gyro_bias[2]);
accel_var[0] +=
(accel_data.filtered.x -
accel_bias[0]) * (accel_data.filtered.x -
accel_bias[0]);
accel_var[1] +=
(accel_data.filtered.y -
accel_bias[1]) * (accel_data.filtered.y -
accel_bias[1]);
accel_var[2] +=
(accel_data.filtered.z -
accel_bias[2]) * (accel_data.filtered.z -
accel_bias[2]);
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_raw);
#endif
mag_var[0] +=
(mag_raw[0] - mag_bias[0]) * (mag_raw[0] -
mag_bias[0]);
mag_var[1] +=
(mag_raw[1] - mag_bias[1]) * (mag_raw[1] -
mag_bias[1]);
mag_var[2] +=
(mag_raw[2] - mag_bias[2]) * (mag_raw[2] -
mag_bias[2]);
ahrs_state = AHRS_IDLE;
process_spi_request();
}
gyro_var[0] /= i;
gyro_var[1] /= i;
gyro_var[2] /= i;
accel_var[0] /= i;
accel_var[1] /= i;
accel_var[2] /= i;
mag_var[0] /= i;
mag_var[1] /= i;
mag_var[2] /= i;
float mag_length2 =
mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] +
mag_bias[2] * mag_bias[2];
mag_var[0] = mag_var[0] / mag_length2;
mag_var[1] = mag_var[1] / mag_length2;
mag_var[2] = mag_var[2] / mag_length2;
if (ahrs_algorithm == INSGPS_Algo)
converge_insgps();
}
/**
* @brief Quickly initialize INS assuming stationary and gravity is down
*
* Currently this is done iteratively but I'm sure it can be directly computed
* when I sit down and work it out
*/
void converge_insgps()
{
float pos[3] = { 0, 0, 0 }, vel[3] = {
0, 0, 0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {
0, 0, 0};
INSGPSInit();
INSSetAccelVar(accel_var);
INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
float temp_var[3] = { 10, 10, 10 };
INSSetGyroVar(temp_var); // ignore gyro's
accel[0] = accel_data.filtered.x;
accel[1] = accel_data.filtered.y;
accel[2] = accel_data.filtered.z;
// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.
for (int i = 0; i < 50; i++) {
// This should be done directly but I'm too dumb.
float rpy[3];
Quaternion2RPY(Nav.q, rpy);
rpy[1] =
-atan2(accel_data.filtered.x,
accel_data.filtered.z) * 180 / M_PI;
rpy[0] =
-atan2(accel_data.filtered.y,
accel_data.filtered.z) * 180 / M_PI;
// Get magnetic readings
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
#endif
mag[0] = -mag_data.raw.axis[1];
mag[1] = -mag_data.raw.axis[0];
mag[2] = -mag_data.raw.axis[2];
RPY2Quaternion(rpy, Nav.q);
INSStatePrediction(temp_gyro, accel, 1 / (float)EKF_RATE);
INSCovariancePrediction(1 / (float)EKF_RATE);
FullCorrection(mag, pos, vel, BaroAlt);
process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
}
INSSetGyroVar(gyro_var);
}
/**
* @addtogroup AHRS_SPI SPI Messaging
* @{
* @brief SPI protocol handling requests for data from OP mainboard
*/
static struct opahrs_msg_v1 link_tx_v1;
static struct opahrs_msg_v1 link_rx_v1;
static struct opahrs_msg_v1 user_rx_v1;
static struct opahrs_msg_v1 user_tx_v1;
void process_spi_request(void)
{
bool msg_to_process = FALSE;
PIOS_IRQ_Disable();
/* Figure out if we're in an interesting stable state */
switch (lfsm_get_state()) {
case LFSM_STATE_USER_BUSY:
msg_to_process = TRUE;
break;
case LFSM_STATE_INACTIVE:
/* Queue up a receive buffer */
lfsm_user_set_rx_v1(&user_rx_v1);
lfsm_user_done();
break;
case LFSM_STATE_STOPPED:
/* Get things going */
lfsm_set_link_proto_v1(&link_tx_v1, &link_rx_v1);
break;
default:
/* Not a stable state */
break;
}
PIOS_IRQ_Enable();
if (!msg_to_process) {
/* Nothing to do */
return;
}
switch (user_rx_v1.payload.user.t) {
case OPAHRS_MSG_V1_REQ_RESET:
PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.
reset_delay_in_ms);
PIOS_SYS_Reset();
break;
case OPAHRS_MSG_V1_REQ_SERIAL:
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_SERIAL);
PIOS_SYS_SerialNumberGet((char *)
&(user_tx_v1.payload.user.v.rsp.
serial.serial_bcd));
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ALGORITHM:
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_ALGORITHM);
ahrs_algorithm =
user_rx_v1.payload.user.v.req.algorithm.algorithm;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_NORTH:
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_NORTH);
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_CALIBRATION:
if (user_rx_v1.payload.user.v.req.calibration.
measure_var == AHRS_MEASURE) {
calibration_pending = TRUE;
} else if (user_rx_v1.payload.user.v.req.calibration.
measure_var == AHRS_SET) {
accel_var[0] =
user_rx_v1.payload.user.v.req.calibration.
accel_var[0];
accel_var[1] =
user_rx_v1.payload.user.v.req.calibration.
accel_var[1];
accel_var[2] =
user_rx_v1.payload.user.v.req.calibration.
accel_var[2];
gyro_bias[0] =
user_rx_v1.payload.user.v.req.calibration.
gyro_bias[0];
gyro_bias[1] =
user_rx_v1.payload.user.v.req.calibration.
gyro_bias[1];
gyro_bias[2] =
user_rx_v1.payload.user.v.req.calibration.
gyro_bias[2];
gyro_var[0] =
user_rx_v1.payload.user.v.req.calibration.
gyro_var[0];
gyro_var[1] =
user_rx_v1.payload.user.v.req.calibration.
gyro_var[1];
gyro_var[2] =
user_rx_v1.payload.user.v.req.calibration.
gyro_var[2];
mag_var[0] =
user_rx_v1.payload.user.v.req.calibration.
mag_var[0];
mag_var[1] =
user_rx_v1.payload.user.v.req.calibration.
mag_var[1];
mag_var[2] =
user_rx_v1.payload.user.v.req.calibration.
mag_var[2];
INSSetAccelVar(accel_var);
float gyro_bias_ins[3] = { 0, 0, 0 };
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
}
if (user_rx_v1.payload.user.v.req.calibration.
measure_var != AHRS_ECHO) {
/* if echoing don't set anything */
accel_bias[0] =
user_rx_v1.payload.user.v.req.calibration.
accel_bias[0];
accel_bias[1] =
user_rx_v1.payload.user.v.req.calibration.
accel_bias[1];
accel_bias[2] =
user_rx_v1.payload.user.v.req.calibration.
accel_bias[2];
accel_scale[0] =
user_rx_v1.payload.user.v.req.calibration.
accel_scale[0];
accel_scale[1] =
user_rx_v1.payload.user.v.req.calibration.
accel_scale[1];
accel_scale[2] =
user_rx_v1.payload.user.v.req.calibration.
accel_scale[2];
gyro_scale[0] =
user_rx_v1.payload.user.v.req.calibration.
gyro_scale[0];
gyro_scale[1] =
user_rx_v1.payload.user.v.req.calibration.
gyro_scale[1];
gyro_scale[2] =
user_rx_v1.payload.user.v.req.calibration.
gyro_scale[2];
mag_bias[0] =
user_rx_v1.payload.user.v.req.calibration.
mag_bias[0];
mag_bias[1] =
user_rx_v1.payload.user.v.req.calibration.
mag_bias[1];
mag_bias[2] =
user_rx_v1.payload.user.v.req.calibration.
mag_bias[2];
}
// echo back the values used
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_CALIBRATION);
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] =
accel_var[0];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] =
accel_var[1];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] =
accel_var[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] =
gyro_bias[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] =
gyro_bias[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] =
gyro_bias[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] =
gyro_var[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] =
gyro_var[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] =
gyro_var[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] =
mag_var[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] =
mag_var[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] =
mag_var[2];
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_ATTITUDERAW);
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x =
mag_data.raw.axis[0];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y =
mag_data.raw.axis[1];
user_tx_v1.payload.user.v.rsp.attituderaw.mags.z =
mag_data.raw.axis[2];
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x =
gyro_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y =
gyro_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z =
gyro_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
x = gyro_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
y = gyro_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.
z = gyro_data.filtered.z;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp =
gyro_data.temp.xy;
user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp =
gyro_data.temp.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.x =
accel_data.raw.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.y =
accel_data.raw.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels.z =
accel_data.raw.z;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
x = accel_data.filtered.x;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
y = accel_data.filtered.y;
user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.
z = accel_data.filtered.z;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_UPDATE:
// process incoming data
opahrs_msg_v1_init_user_tx(&user_tx_v1,
OPAHRS_MSG_V1_RSP_UPDATE);
if (user_rx_v1.payload.user.v.req.update.barometer.updated) {
altitude_data.altitude =
user_rx_v1.payload.user.v.req.update.barometer.
altitude;
altitude_data.updated =
user_rx_v1.payload.user.v.req.update.barometer.
updated;
}
if (user_rx_v1.payload.user.v.req.update.gps.updated) {
gps_data.updated = true;
gps_data.NED[0] =
user_rx_v1.payload.user.v.req.update.gps.
NED[0];
gps_data.NED[1] =
user_rx_v1.payload.user.v.req.update.gps.
NED[1];
gps_data.NED[2] =
user_rx_v1.payload.user.v.req.update.gps.
NED[2];
gps_data.heading =
user_rx_v1.payload.user.v.req.update.gps.
heading;
gps_data.groundspeed =
user_rx_v1.payload.user.v.req.update.gps.
groundspeed;
gps_data.quality =
user_rx_v1.payload.user.v.req.update.gps.
quality;
}
// send out attitude/position estimate
user_tx_v1.payload.user.v.rsp.update.quaternion.q1 =
attitude_data.quaternion.q1;
user_tx_v1.payload.user.v.rsp.update.quaternion.q2 =
attitude_data.quaternion.q2;
user_tx_v1.payload.user.v.rsp.update.quaternion.q3 =
attitude_data.quaternion.q3;
user_tx_v1.payload.user.v.rsp.update.quaternion.q4 =
attitude_data.quaternion.q4;
// TODO: separate this from INSGPS
user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0];
user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1];
user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2];
user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0];
user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1];
user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2];
// compute the idle fraction
user_tx_v1.payload.user.v.rsp.update.load =
((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
user_tx_v1.payload.user.v.rsp.update.idle_time =
idle_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.run_time =
running_counts / (TIMER_RATE / 10000);
user_tx_v1.payload.user.v.rsp.update.dropped_updates =
ekf_too_slow;
lfsm_user_set_tx_v1(&user_tx_v1);
break;
default:
break;
}
/* Finished processing the received message, requeue it */
lfsm_user_set_rx_v1(&user_rx_v1);
lfsm_user_done();
}
/**
* @}
*/

151
flight/AHRS/ahrs_adc.c
View File

@ -34,24 +34,26 @@
#include "ahrs_adc.h"
// Remap the ADC DMA handler to this one
void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler")));
void DMA1_Channel1_IRQHandler()
__attribute__ ((alias("AHRS_ADC_DMA_Handler")));
//! Where the raw data is stored
volatile int16_t raw_data_buffer[MAX_SAMPLES]; // Double buffer that DMA just used
volatile int16_t raw_data_buffer[MAX_SAMPLES]; // Double buffer that DMA just used
//! Swapped by interrupt handler to achieve double buffering
volatile int16_t * valid_data_buffer;
volatile int16_t *valid_data_buffer;
volatile int32_t total_conversion_blocks;
volatile int32_t ekf_too_slow;
volatile uint8_t adc_oversample;
volatile states ahrs_state;
/* Local Variables */
static GPIO_TypeDef* ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS;
static GPIO_TypeDef *ADC_GPIO_PORT[PIOS_ADC_NUM_PINS] = PIOS_ADC_PORTS;
static const uint32_t ADC_GPIO_PIN[PIOS_ADC_NUM_PINS] = PIOS_ADC_PINS;
static const uint32_t ADC_CHANNEL[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNELS;
static ADC_TypeDef* ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING;
static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_MAPPING;
static ADC_TypeDef *ADC_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_MAPPING;
static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] =
PIOS_ADC_CHANNEL_MAPPING;
/**
* @brief Initialise the ADC Peripheral
@ -64,38 +66,42 @@ static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_
*/
uint8_t AHRS_ADC_Config(int32_t adc_oversample)
{
int32_t i;
ADC_DeInit(ADC1);
ADC_DeInit(ADC2);
/* Setup analog pins */
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
/* Enable each ADC pin in the array */
for(i = 0; i < PIOS_ADC_NUM_PINS; i++) {
for (i = 0; i < PIOS_ADC_NUM_PINS; i++) {
GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i];
GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure);
}
/* Enable ADC clocks */
PIOS_ADC_CLOCK_FUNCTION;
/* Map channels to conversion slots depending on the channel selection mask */
for(i = 0; i < PIOS_ADC_NUM_PINS; i++) {
ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], ADC_CHANNEL_MAPPING[i], PIOS_ADC_SAMPLE_TIME);
for (i = 0; i < PIOS_ADC_NUM_PINS; i++) {
ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i],
ADC_CHANNEL_MAPPING[i],
PIOS_ADC_SAMPLE_TIME);
}
#if (PIOS_ADC_USE_TEMP_SENSOR)
ADC_TempSensorVrefintCmd(ENABLE);
ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_14, PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, PIOS_ADC_SAMPLE_TIME);
ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_14,
PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL,
PIOS_ADC_SAMPLE_TIME);
#endif
// TODO: update ADC to continuous sampling, configure the sampling rate
// TODO: update ADC to continuous sampling, configure the sampling rate
/* Configure ADCs */
ADC_InitTypeDef ADC_InitStructure;
ADC_StructInit(&ADC_InitStructure);
@ -104,76 +110,81 @@ uint8_t AHRS_ADC_Config(int32_t adc_oversample)
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfChannel = ((PIOS_ADC_NUM_CHANNELS + 1) >> 1);
ADC_InitStructure.ADC_NbrOfChannel =
((PIOS_ADC_NUM_CHANNELS + 1) >> 1);
ADC_Init(ADC1, &ADC_InitStructure);
#if (PIOS_ADC_USE_ADC2)
ADC_Init(ADC2, &ADC_InitStructure);
/* Enable ADC2 external trigger conversion (to synch with ADC1) */
ADC_ExternalTrigConvCmd(ADC2, ENABLE);
#endif
RCC_ADCCLKConfig(PIOS_ADC_ADCCLK);
RCC_PCLK2Config(RCC_HCLK_Div16);
/* Enable ADC1->DMA request */
ADC_DMACmd(ADC1, ENABLE);
/* ADC1 calibration */
ADC_Cmd(ADC1, ENABLE);
ADC_ResetCalibration(ADC1);
while(ADC_GetResetCalibrationStatus(ADC1));
while (ADC_GetResetCalibrationStatus(ADC1)) ;
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1));
while (ADC_GetCalibrationStatus(ADC1)) ;
#if (PIOS_ADC_USE_ADC2)
/* ADC2 calibration */
ADC_Cmd(ADC2, ENABLE);
ADC_ResetCalibration(ADC2);
while(ADC_GetResetCalibrationStatus(ADC2));
while (ADC_GetResetCalibrationStatus(ADC2)) ;
ADC_StartCalibration(ADC2);
while(ADC_GetCalibrationStatus(ADC2));
while (ADC_GetCalibrationStatus(ADC2)) ;
#endif
/* Enable DMA1 clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
/* Configure DMA1 channel 1 to fetch data from ADC result register */
DMA_InitTypeDef DMA_InitStructure;
DMA_StructInit(&DMA_InitStructure);
DMA_DeInit(DMA1_Channel1);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&raw_data_buffer[0];
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr =
(uint32_t) & raw_data_buffer[0];
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
/* We are double buffering half words from the ADC. Make buffer appropriately sized */
DMA_InitStructure.DMA_BufferSize = (PIOS_ADC_NUM_CHANNELS * adc_oversample * 2) >> 1;
/* We are double buffering half words from the ADC. Make buffer appropriately sized */
DMA_InitStructure.DMA_BufferSize =
(PIOS_ADC_NUM_CHANNELS * adc_oversample * 2) >> 1;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
/* Note: We read ADC1 and ADC2 in parallel making a word read, also hence the half buffer size */
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
/* Note: We read ADC1 and ADC2 in parallel making a word read, also hence the half buffer size */
DMA_InitStructure.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Word;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &DMA_InitStructure);
DMA_Cmd(DMA1_Channel1, ENABLE);
/* Trigger interrupt when for half conversions too to indicate double buffer */
DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE);
DMA_ITConfig(DMA1_Channel1, DMA_IT_HT, ENABLE);
/* Configure and enable DMA interrupt */
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_ADC_IRQ_PRIO;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =
PIOS_ADC_IRQ_PRIO;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Finally start initial conversion */
ADC_SoftwareStartConvCmd(ADC1, ENABLE);
return 1;
}
@ -186,31 +197,33 @@ uint8_t AHRS_ADC_Config(int32_t adc_oversample)
* running in the background. Keep an eye on the ekf_too_slow variable to make sure
* it's keeping up.
*/
void AHRS_ADC_DMA_Handler(void)
void AHRS_ADC_DMA_Handler(void)
{
if ( ahrs_state == AHRS_IDLE )
{
// Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features)
if( DMA_GetFlagStatus( DMA1_IT_TC1 ) ) // whole double buffer filled
valid_data_buffer = &raw_data_buffer[ 1 * PIOS_ADC_NUM_CHANNELS * adc_oversample ];
else if ( DMA_GetFlagStatus(DMA1_IT_HT1) )
valid_data_buffer = &raw_data_buffer[ 0 * PIOS_ADC_NUM_CHANNELS * adc_oversample ];
else {
// lets cause a seg fault and catch whatever is going on
valid_data_buffer = 0;
}
ahrs_state = AHRS_DATA_READY;
}
else {
// Track how many times an interrupt occurred before EKF finished processing
ekf_too_slow++;
}
total_conversion_blocks++;
// Clear all interrupt from DMA 1 - regardless if buffer swapped
DMA_ClearFlag( DMA1_IT_GL1 );
if (ahrs_state == AHRS_IDLE) {
// Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features)
if (DMA_GetFlagStatus(DMA1_IT_TC1)) // whole double buffer filled
valid_data_buffer =
&raw_data_buffer[1 * PIOS_ADC_NUM_CHANNELS *
adc_oversample];
else if (DMA_GetFlagStatus(DMA1_IT_HT1))
valid_data_buffer =
&raw_data_buffer[0 * PIOS_ADC_NUM_CHANNELS *
adc_oversample];
else {
// lets cause a seg fault and catch whatever is going on
valid_data_buffer = 0;
}
ahrs_state = AHRS_DATA_READY;
} else {
// Track how many times an interrupt occurred before EKF finished processing
ekf_too_slow++;
}
total_conversion_blocks++;
// Clear all interrupt from DMA 1 - regardless if buffer swapped
DMA_ClearFlag(DMA1_IT_GL1);
}

901
flight/AHRS/ahrs_fsm.c
View File

@ -8,29 +8,29 @@
#include "pios.h"
struct lfsm_context {
enum lfsm_state curr_state;
enum opahrs_msg_link_state link_state;
enum opahrs_msg_type user_payload_type;
uint32_t user_payload_len;
enum lfsm_state curr_state;
enum opahrs_msg_link_state link_state;
enum opahrs_msg_type user_payload_type;
uint32_t user_payload_len;
uint32_t errors;
uint32_t errors;
uint8_t * rx;
uint8_t * tx;
uint8_t *rx;
uint8_t *tx;
uint8_t * link_rx;
uint8_t * link_tx;
uint8_t *link_rx;
uint8_t *link_tx;
uint8_t * user_rx;
uint8_t * user_tx;
uint8_t *user_rx;
uint8_t *user_tx;
struct lfsm_link_stats stats;
struct lfsm_link_stats stats;
};
static struct lfsm_context context = { 0 };
static void lfsm_update_link_tx (struct lfsm_context * context);
static void lfsm_init_rx (struct lfsm_context * context);
static void lfsm_update_link_tx(struct lfsm_context *context);
static void lfsm_init_rx(struct lfsm_context *context);
/*
*
@ -39,332 +39,392 @@ static void lfsm_init_rx (struct lfsm_context * context);
*/
struct lfsm_transition {
void (*entry_fn)(struct lfsm_context * context);
enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
void (*entry_fn) (struct lfsm_context * context);
enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS];
};
static void go_faulted(struct lfsm_context * context);
static void go_stopped(struct lfsm_context * context);
static void go_stopping(struct lfsm_context * context);
static void go_inactive(struct lfsm_context * context);
static void go_user_busy(struct lfsm_context * context);
static void go_user_busy_rx_pending(struct lfsm_context * context);
static void go_user_busy_tx_pending(struct lfsm_context * context);
static void go_user_busy_rxtx_pending(struct lfsm_context * context);
static void go_user_rx_pending(struct lfsm_context * context);
static void go_user_tx_pending(struct lfsm_context * context);
static void go_user_rxtx_pending(struct lfsm_context * context);
static void go_user_rx_active(struct lfsm_context * context);
static void go_user_tx_active(struct lfsm_context * context);
static void go_user_rxtx_active(struct lfsm_context * context);
static void go_faulted(struct lfsm_context *context);
static void go_stopped(struct lfsm_context *context);
static void go_stopping(struct lfsm_context *context);
static void go_inactive(struct lfsm_context *context);
static void go_user_busy(struct lfsm_context *context);
static void go_user_busy_rx_pending(struct lfsm_context *context);
static void go_user_busy_tx_pending(struct lfsm_context *context);
static void go_user_busy_rxtx_pending(struct lfsm_context *context);
static void go_user_rx_pending(struct lfsm_context *context);
static void go_user_tx_pending(struct lfsm_context *context);
static void go_user_rxtx_pending(struct lfsm_context *context);
static void go_user_rx_active(struct lfsm_context *context);
static void go_user_tx_active(struct lfsm_context *context);
static void go_user_rxtx_active(struct lfsm_context *context);
const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES] = {
[LFSM_STATE_FAULTED] = {
.entry_fn = go_faulted,
},
[LFSM_STATE_STOPPED] = {
.entry_fn = go_stopped,
.next_state = {
[LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_STOPPING] = {
.entry_fn = go_stopping,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_INACTIVE] = {
.entry_fn = go_inactive,
.next_state = {
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_BUSY] = {
.entry_fn = go_user_busy,
.next_state = {
[LFSM_EVENT_STOP] = LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY,
},
},
[LFSM_STATE_USER_BUSY_RX_PENDING] = {
.entry_fn = go_user_busy_rx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_TX_PENDING] = {
.entry_fn = go_user_busy_tx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
.entry_fn = go_user_busy_rxtx_pending,
.next_state = {
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
},
},
[LFSM_STATE_USER_RX_PENDING] = {
.entry_fn = go_user_rx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_PENDING] = {
.entry_fn = go_user_tx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
},
},
[LFSM_STATE_USER_RXTX_PENDING] = {
.entry_fn = go_user_rxtx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
},
},
[LFSM_STATE_USER_RX_ACTIVE] = {
.entry_fn = go_user_rx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_ACTIVE] = {
.entry_fn = go_user_tx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_RXTX_ACTIVE] = {
.entry_fn = go_user_rxtx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES]
= {
[LFSM_STATE_FAULTED] = {
.entry_fn = go_faulted,
},
[LFSM_STATE_STOPPED] = {
.entry_fn = go_stopped,
.next_state = {
[LFSM_EVENT_INIT_LINK] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_STOPPING] = {
.entry_fn = go_stopping,
.next_state = {
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_STOPPED,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_STOPPED,
},
},
[LFSM_STATE_INACTIVE] = {
.entry_fn = go_inactive,
.next_state = {
[LFSM_EVENT_STOP] =
LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] =
LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] =
LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_BUSY] = {
.entry_fn = go_user_busy,
.next_state = {
[LFSM_EVENT_STOP] =
LFSM_STATE_STOPPING,
[LFSM_EVENT_USER_SET_RX] =
LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_USER_SET_TX] =
LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_USER_DONE] =
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_LINK] =
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_USER] =
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] =
LFSM_STATE_USER_BUSY,
},
},
[LFSM_STATE_USER_BUSY_RX_PENDING] = {
.entry_fn =
go_user_busy_rx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_TX_PENDING] = {
.entry_fn =
go_user_busy_tx_pending,
.next_state = {
[LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING,
},
},
[LFSM_STATE_USER_BUSY_RXTX_PENDING] = {
.entry_fn =
go_user_busy_rxtx_pending,
.next_state = {
[LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING,
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING,
},
},
[LFSM_STATE_USER_RX_PENDING] = {
.entry_fn = go_user_rx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_PENDING] = {
.entry_fn = go_user_tx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_TX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE,
},
},
[LFSM_STATE_USER_RXTX_PENDING] = {
.entry_fn = go_user_rxtx_pending,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE,
},
},
[LFSM_STATE_USER_RX_ACTIVE] = {
.entry_fn = go_user_rx_active,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
[LFSM_STATE_USER_TX_ACTIVE] = {
.entry_fn = go_user_tx_active,
.next_state = {
[LFSM_EVENT_RX_LINK]
=
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_USER]
=
LFSM_STATE_INACTIVE,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE,
},
},
[LFSM_STATE_USER_RXTX_ACTIVE] = {
.entry_fn = go_user_rxtx_active,
.next_state = {
[LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE,
[LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY,
[LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE,
},
},
};
/*
* FSM State Entry Functions
*/
static void go_faulted(struct lfsm_context * context)
static void go_faulted(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(0);
PIOS_DEBUG_Assert(0);
}
static void go_stopped(struct lfsm_context * context)
static void go_stopped(struct lfsm_context *context)
{
#if 0
PIOS_SPI_Stop(PIOS_SPI_OP);
PIOS_SPI_Stop(PIOS_SPI_OP);
#endif
}
static void go_stopping(struct lfsm_context * context)
static void go_stopping(struct lfsm_context *context)
{
context->link_tx = NULL;
context->tx = NULL;
context->link_tx = NULL;
context->tx = NULL;
}
static void go_inactive(struct lfsm_context * context)
static void go_inactive(struct lfsm_context *context)
{
context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE;
lfsm_update_link_tx(context);
context->user_rx = NULL;
context->user_tx = NULL;
context->user_rx = NULL;
context->user_tx = NULL;
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy(struct lfsm_context * context)
static void go_user_busy(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->user_rx = NULL;
context->user_tx = NULL;
context->user_rx = NULL;
context->user_tx = NULL;
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_rx_pending(struct lfsm_context * context)
static void go_user_busy_rx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_tx_pending(struct lfsm_context * context)
static void go_user_busy_tx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_busy_rxtx_pending(struct lfsm_context * context)
static void go_user_busy_rxtx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rx_pending(struct lfsm_context * context)
static void go_user_rx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_tx_pending(struct lfsm_context * context)
static void go_user_tx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rxtx_pending(struct lfsm_context * context)
static void go_user_rxtx_pending(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
lfsm_update_link_tx(context);
context->rx = context->link_rx;
context->tx = context->link_tx;
context->rx = context->link_rx;
context->tx = context->link_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rx_active(struct lfsm_context * context)
static void go_user_rx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
context->rx = context->user_rx;
context->tx = context->link_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
context->rx = context->user_rx;
context->tx = context->link_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
lfsm_update_link_tx(context);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_update_link_tx(context);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_tx_active(struct lfsm_context * context)
static void go_user_tx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
context->rx = context->link_rx;
context->tx = context->user_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_BUSY;
context->rx = context->link_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
static void go_user_rxtx_active(struct lfsm_context * context)
static void go_user_rxtx_active(struct lfsm_context *context)
{
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
/* Sanity checks */
PIOS_DEBUG_Assert(context->user_rx);
PIOS_DEBUG_Assert(context->user_tx);
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
context->rx = context->user_rx;
context->tx = context->user_tx;
context->link_state = OPAHRS_MSG_LINK_STATE_READY;
context->rx = context->user_rx;
context->tx = context->user_tx;
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback);
lfsm_init_rx(context);
PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx,
context->user_payload_len,
lfsm_irq_callback);
}
/*
@ -373,52 +433,60 @@ static void go_user_rxtx_active(struct lfsm_context * context)
*
*/
static void lfsm_update_link_tx_v0 (struct opahrs_msg_v0 * msg, enum opahrs_msg_link_state state, uint16_t errors)
static void lfsm_update_link_tx_v0(struct opahrs_msg_v0 *msg,
enum opahrs_msg_link_state state,
uint16_t errors)
{
opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx_v1 (struct opahrs_msg_v1 * msg, enum opahrs_msg_link_state state, uint16_t errors)
static void lfsm_update_link_tx_v1(struct opahrs_msg_v1 *msg,
enum opahrs_msg_link_state state,
uint16_t errors)
{
opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP);
msg->payload.link.state = state;
msg->payload.link.errors = errors;
msg->payload.link.state = state;
msg->payload.link.errors = errors;
}
static void lfsm_update_link_tx (struct lfsm_context * context)
static void lfsm_update_link_tx(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(context->link_tx);
PIOS_DEBUG_Assert(context->link_tx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
lfsm_update_link_tx_v0 ((struct opahrs_msg_v0 *)context->link_tx, context->link_state, context->errors);
break;
case OPAHRS_MSG_TYPE_USER_V1:
lfsm_update_link_tx_v1 ((struct opahrs_msg_v1 *)context->link_tx, context->link_state, context->errors);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
lfsm_update_link_tx_v0((struct opahrs_msg_v0 *)context->
link_tx, context->link_state,
context->errors);
break;
case OPAHRS_MSG_TYPE_USER_V1:
lfsm_update_link_tx_v1((struct opahrs_msg_v1 *)context->
link_tx, context->link_state,
context->errors);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
static void lfsm_init_rx (struct lfsm_context * context)
static void lfsm_init_rx(struct lfsm_context *context)
{
PIOS_DEBUG_Assert(context->rx);
PIOS_DEBUG_Assert(context->rx);
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
opahrs_msg_v0_init_rx ((struct opahrs_msg_v0 *)context->rx);
break;
case OPAHRS_MSG_TYPE_USER_V1:
opahrs_msg_v1_init_rx ((struct opahrs_msg_v1 *)context->rx);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
switch (context->user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
opahrs_msg_v0_init_rx((struct opahrs_msg_v0 *)context->rx);
break;
case OPAHRS_MSG_TYPE_USER_V1:
opahrs_msg_v1_init_rx((struct opahrs_msg_v1 *)context->rx);
break;
case OPAHRS_MSG_TYPE_LINK:
PIOS_DEBUG_Assert(0);
}
}
/*
@ -429,118 +497,127 @@ static void lfsm_init_rx (struct lfsm_context * context)
void lfsm_inject_event(enum lfsm_event event)
{
PIOS_IRQ_Disable();
PIOS_IRQ_Disable();
/*
* Move to the next state
*
* This is done prior to calling the new state's entry function to
* guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was.
*/
context.curr_state = lfsm_transitions[context.curr_state].next_state[event];
/*
* Move to the next state
*
* This is done prior to calling the new state's entry function to
* guarantee that the entry function never depends on the previous
* state. This way, it cannot ever know what the previous state was.
*/
context.curr_state =
lfsm_transitions[context.curr_state].next_state[event];
/* Call the entry function (if any) for the next state. */
if (lfsm_transitions[context.curr_state].entry_fn) {
lfsm_transitions[context.curr_state].entry_fn(&context);
}
PIOS_IRQ_Enable();
/* Call the entry function (if any) for the next state. */
if (lfsm_transitions[context.curr_state].entry_fn) {
lfsm_transitions[context.curr_state].entry_fn(&context);
}
PIOS_IRQ_Enable();
}
void lfsm_init(void)
{
context.curr_state = LFSM_STATE_STOPPED;
go_stopped(&context);
context.curr_state = LFSM_STATE_STOPPED;
go_stopped(&context);
}
void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx)
void lfsm_set_link_proto_v0(struct opahrs_msg_v0 *link_tx,
struct opahrs_msg_v0 *link_rx)
{
PIOS_DEBUG_Assert(link_tx);
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *)link_tx;
context.link_rx = (uint8_t *)link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
context.user_payload_len = sizeof(*link_tx);
context.link_tx = (uint8_t *) link_tx;
context.link_rx = (uint8_t *) link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors);
lfsm_update_link_tx_v0(link_tx, context.link_state,
context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx)
void lfsm_set_link_proto_v1(struct opahrs_msg_v1 *link_tx,
struct opahrs_msg_v1 *link_rx)
{
PIOS_DEBUG_Assert(link_tx);
PIOS_DEBUG_Assert(link_tx);
context.link_tx = (uint8_t *)link_tx;
context.link_rx = (uint8_t *)link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
context.user_payload_len = sizeof(*link_tx);
context.link_tx = (uint8_t *) link_tx;
context.link_rx = (uint8_t *) link_rx;
context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1;
context.user_payload_len = sizeof(*link_tx);
lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors);
lfsm_update_link_tx_v1(link_tx, context.link_state,
context.errors);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
lfsm_inject_event(LFSM_EVENT_INIT_LINK);
}
void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx)
void lfsm_user_set_tx_v0(struct opahrs_msg_v0 *user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
context.user_tx = (uint8_t *)user_tx;
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V0);
context.user_tx = (uint8_t *) user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx)
void lfsm_user_set_rx_v0(struct opahrs_msg_v0 *user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0);
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V0);
context.user_rx = (uint8_t *)user_rx;
context.user_rx = (uint8_t *) user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx)
void lfsm_user_set_tx_v1(struct opahrs_msg_v1 *user_tx)
{
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
PIOS_DEBUG_Assert(user_tx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V1);
context.user_tx = (uint8_t *)user_tx;
context.user_tx = (uint8_t *) user_tx;
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
lfsm_inject_event(LFSM_EVENT_USER_SET_TX);
}
void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx)
void lfsm_user_set_rx_v1(struct opahrs_msg_v1 *user_rx)
{
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1);
PIOS_DEBUG_Assert(user_rx);
PIOS_DEBUG_Assert(context.user_payload_type ==
OPAHRS_MSG_TYPE_USER_V1);
context.user_rx = (uint8_t *)user_rx;
context.user_rx = (uint8_t *) user_rx;
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
lfsm_inject_event(LFSM_EVENT_USER_SET_RX);
}
void lfsm_user_done (void)
void lfsm_user_done(void)
{
lfsm_inject_event(LFSM_EVENT_USER_DONE);
lfsm_inject_event(LFSM_EVENT_USER_DONE);
}
void lfsm_stop (void)
void lfsm_stop(void)
{
lfsm_inject_event(LFSM_EVENT_STOP);
lfsm_inject_event(LFSM_EVENT_STOP);
}
void lfsm_get_link_stats (struct lfsm_link_stats * stats)
void lfsm_get_link_stats(struct lfsm_link_stats *stats)
{
PIOS_DEBUG_Assert(stats);
PIOS_DEBUG_Assert(stats);
*stats = context.stats;
*stats = context.stats;
}
enum lfsm_state lfsm_get_state (void)
enum lfsm_state lfsm_get_state(void)
{
return context.curr_state;
return context.curr_state;
}
/*
@ -551,70 +628,70 @@ enum lfsm_state lfsm_get_state (void)
void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val)
{
if (!crc_ok) {
context.stats.rx_badcrc++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
if (!crc_ok) {
context.stats.rx_badcrc++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
if (!context.rx) {
/* No way to know what we just received, assume invalid */
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
if (!context.rx) {
/* No way to know what we just received, assume invalid */
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Recover the head and tail pointers from the message */
struct opahrs_msg_link_head * head = NULL;
struct opahrs_msg_link_tail * tail = NULL;
/* Recover the head and tail pointers from the message */
struct opahrs_msg_link_head *head = NULL;
struct opahrs_msg_link_tail *tail = NULL;
switch (context.user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
head = &((struct opahrs_msg_v0 *)context.rx)->head;
tail = &((struct opahrs_msg_v0 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_USER_V1:
head = &((struct opahrs_msg_v1 *)context.rx)->head;
tail = &((struct opahrs_msg_v1 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_LINK:
/* Should never be rx'ing before the link protocol version is known */
PIOS_DEBUG_Assert(0);
break;
}
switch (context.user_payload_type) {
case OPAHRS_MSG_TYPE_USER_V0:
head = &((struct opahrs_msg_v0 *)context.rx)->head;
tail = &((struct opahrs_msg_v0 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_USER_V1:
head = &((struct opahrs_msg_v1 *)context.rx)->head;
tail = &((struct opahrs_msg_v1 *)context.rx)->tail;
break;
case OPAHRS_MSG_TYPE_LINK:
/* Should never be rx'ing before the link protocol version is known */
PIOS_DEBUG_Assert(0);
break;
}
/* Check for bad magic */
if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) ||
(tail->magic != OPAHRS_MSG_MAGIC_TAIL)) {
if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
context.stats.rx_badmagic_head++;
}
if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
context.stats.rx_badmagic_tail++;
}
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Check for bad magic */
if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) ||
(tail->magic != OPAHRS_MSG_MAGIC_TAIL)) {
if (head->magic != OPAHRS_MSG_MAGIC_HEAD) {
context.stats.rx_badmagic_head++;
}
if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) {
context.stats.rx_badmagic_tail++;
}
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
return;
}
/* Good magic, find out what type of payload we've got */
switch (head->type) {
case OPAHRS_MSG_TYPE_LINK:
context.stats.rx_link++;
lfsm_inject_event (LFSM_EVENT_RX_LINK);
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
if (head->type == context.user_payload_type) {
context.stats.rx_user++;
lfsm_inject_event (LFSM_EVENT_RX_USER);
} else {
/* Mismatched user payload type */
context.stats.rx_badver++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
}
break;
default:
/* Unidentifiable payload type */
context.stats.rx_badtype++;
lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN);
}
/* Good magic, find out what type of payload we've got */
switch (head->type) {
case OPAHRS_MSG_TYPE_LINK:
context.stats.rx_link++;
lfsm_inject_event(LFSM_EVENT_RX_LINK);
break;
case OPAHRS_MSG_TYPE_USER_V0:
case OPAHRS_MSG_TYPE_USER_V1:
if (head->type == context.user_payload_type) {
context.stats.rx_user++;
lfsm_inject_event(LFSM_EVENT_RX_USER);
} else {
/* Mismatched user payload type */
context.stats.rx_badver++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
}
break;
default:
/* Unidentifiable payload type */
context.stats.rx_badtype++;
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
}
}

19
flight/AHRS/ahrs_timer.c
View File

@ -33,28 +33,29 @@
#include "ahrs_timer.h"
void timer_start()
void timer_start()
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_BKP | RCC_APB1Periph_PWR,
ENABLE);
PWR_BackupAccessCmd(ENABLE);
RCC_RTCCLKConfig(RCC_RTCCLKSource_HSE_Div128);
RCC_RTCCLKCmd(ENABLE);
RTC_WaitForLastTask();
RTC_WaitForSynchro();
RTC_WaitForLastTask();
RTC_SetPrescaler(0); // counting at 8e6 / 128
RTC_WaitForLastTask();
RTC_SetCounter(0);
RTC_SetPrescaler(0); // counting at 8e6 / 128
RTC_WaitForLastTask();
RTC_SetCounter(0);
RTC_WaitForLastTask();
}
uint32_t timer_count()
{
return RTC_GetCounter();
return RTC_GetCounter();
}
uint32_t timer_rate()
uint32_t timer_rate()
{
return TIMER_RATE;
}
}

66
flight/AHRS/inc/ahrs.h
View File

@ -1,34 +1,32 @@
/**
******************************************************************************
*
* @file ahrs.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main AHRS header.
* @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 AHRS_H
#define AHRS_H
/* PIOS Includes */
#include <pios.h>
#endif /* AHRS_H */
/**
******************************************************************************
*
* @file ahrs.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main AHRS header.
* @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 AHRS_H
#define AHRS_H
/* PIOS Includes */
#include <pios.h>
#endif /* AHRS_H */

8
flight/AHRS/inc/ahrs_adc.h
View File

@ -32,7 +32,7 @@
*/
#ifndef AHRS_ADC
#define AHRS_ADC
#define AHRS_ADC
#include <pios.h>
@ -42,12 +42,12 @@
uint8_t AHRS_ADC_Config(int32_t adc_oversample);
void AHRS_ADC_DMA_Handler(void);
typedef enum {AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING} states;
typedef enum { AHRS_IDLE, AHRS_DATA_READY, AHRS_PROCESSING } states;
extern volatile states ahrs_state;
extern volatile int16_t * valid_data_buffer;
extern volatile int16_t *valid_data_buffer;
//! Counts how many times the EKF wasn't idle when DMA handler called
extern volatile int32_t total_conversion_blocks;
//! Total number of data blocks converted
extern volatile int32_t ekf_too_slow;
#endif
#endif

187
flight/AHRS/inc/ahrs_fsm.h
View File

@ -1,93 +1,94 @@
/**
******************************************************************************
*
* @file ahrs_fsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @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 AHRS_FSM_H
#define AHRS_FSM_H
#include "pios_opahrs_proto.h"
enum lfsm_state {
LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
LFSM_STATE_STOPPED,
LFSM_STATE_STOPPING,
LFSM_STATE_INACTIVE,
LFSM_STATE_USER_BUSY,
LFSM_STATE_USER_BUSY_RX_PENDING,
LFSM_STATE_USER_BUSY_TX_PENDING,
LFSM_STATE_USER_BUSY_RXTX_PENDING,
LFSM_STATE_USER_RX_PENDING,
LFSM_STATE_USER_TX_PENDING,
LFSM_STATE_USER_RXTX_PENDING,
LFSM_STATE_USER_RX_ACTIVE,
LFSM_STATE_USER_TX_ACTIVE,
LFSM_STATE_USER_RXTX_ACTIVE,
LFSM_STATE_NUM_STATES /* Must be last */
};
enum lfsm_event {
LFSM_EVENT_INIT_LINK,
LFSM_EVENT_STOP,
LFSM_EVENT_USER_SET_RX,
LFSM_EVENT_USER_SET_TX,
LFSM_EVENT_USER_DONE,
LFSM_EVENT_RX_LINK,
LFSM_EVENT_RX_USER,
LFSM_EVENT_RX_UNKNOWN,
LFSM_EVENT_NUM_EVENTS /* Must be last */
};
struct lfsm_link_stats {
uint32_t rx_badcrc;
uint32_t rx_badmagic_head;
uint32_t rx_badmagic_tail;
uint32_t rx_link;
uint32_t rx_user;
uint32_t tx_user;
uint32_t rx_badtype;
uint32_t rx_badver;
};
extern void lfsm_init(void);
extern void lfsm_inject_event(enum lfsm_event event);
extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
extern void lfsm_get_link_stats (struct lfsm_link_stats * stats);
extern enum lfsm_state lfsm_get_state (void);
extern void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx);
extern void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx);
extern void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx);
extern void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx);
extern void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx);
extern void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx);
extern void lfsm_user_done (void);
#endif /* AHRS_FSM_H */
/**
******************************************************************************
*
* @file ahrs_fsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief
* @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 AHRS_FSM_H
#define AHRS_FSM_H
#include "pios_opahrs_proto.h"
enum lfsm_state {
LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */
LFSM_STATE_STOPPED,
LFSM_STATE_STOPPING,
LFSM_STATE_INACTIVE,
LFSM_STATE_USER_BUSY,
LFSM_STATE_USER_BUSY_RX_PENDING,
LFSM_STATE_USER_BUSY_TX_PENDING,
LFSM_STATE_USER_BUSY_RXTX_PENDING,
LFSM_STATE_USER_RX_PENDING,
LFSM_STATE_USER_TX_PENDING,
LFSM_STATE_USER_RXTX_PENDING,
LFSM_STATE_USER_RX_ACTIVE,
LFSM_STATE_USER_TX_ACTIVE,
LFSM_STATE_USER_RXTX_ACTIVE,
LFSM_STATE_NUM_STATES /* Must be last */
};
enum lfsm_event {
LFSM_EVENT_INIT_LINK,
LFSM_EVENT_STOP,
LFSM_EVENT_USER_SET_RX,
LFSM_EVENT_USER_SET_TX,
LFSM_EVENT_USER_DONE,
LFSM_EVENT_RX_LINK,
LFSM_EVENT_RX_USER,
LFSM_EVENT_RX_UNKNOWN,
LFSM_EVENT_NUM_EVENTS /* Must be last */
};
struct lfsm_link_stats {
uint32_t rx_badcrc;
uint32_t rx_badmagic_head;
uint32_t rx_badmagic_tail;
uint32_t rx_link;
uint32_t rx_user;
uint32_t tx_user;
uint32_t rx_badtype;
uint32_t rx_badver;
};
extern void lfsm_init(void);
extern void lfsm_inject_event(enum lfsm_event event);
extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
extern void lfsm_get_link_stats(struct lfsm_link_stats *stats);
extern enum lfsm_state lfsm_get_state(void);
extern void lfsm_set_link_proto_v0(struct opahrs_msg_v0 *link_tx,
struct opahrs_msg_v0 *link_rx);
extern void lfsm_user_set_rx_v0(struct opahrs_msg_v0 *user_rx);
extern void lfsm_user_set_tx_v0(struct opahrs_msg_v0 *user_tx);
extern void lfsm_set_link_proto_v1(struct opahrs_msg_v1 *link_tx,
struct opahrs_msg_v1 *link_rx);
extern void lfsm_user_set_rx_v1(struct opahrs_msg_v1 *user_rx);
extern void lfsm_user_set_tx_v1(struct opahrs_msg_v1 *user_tx);
extern void lfsm_user_done(void);
#endif /* AHRS_FSM_H */

2
flight/AHRS/inc/ahrs_timer.h
View File

@ -42,4 +42,4 @@ void timer_start();
uint32_t timer_count();
uint32_t timer_rate();
#endif
#endif

165
flight/AHRS/inc/insgps.h
View File

@ -1,82 +1,83 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @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_
/**
* @addtogroup Constants
* @{
*/
#define POS_SENSORS 0x007
#define HORIZ_SENSORS 0x018
#define VERT_SENSORS 0x020
#define MAG_SENSORS 0x1C0
#define BARO_SENSOR 0x200
#define FULL_SENSORS 0x3FF
/**
* @}
*/
// Exposed Function Prototypes
void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSSetPosVelVar(float PosVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
// 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;
/**
* @}
* @}
*/
#endif /* EKF_H_ */
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @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_
/**
* @addtogroup Constants
* @{
*/
#define POS_SENSORS 0x007
#define HORIZ_SENSORS 0x018
#define VERT_SENSORS 0x020
#define MAG_SENSORS 0x1C0
#define BARO_SENSOR 0x200
#define FULL_SENSORS 0x3FF
/**
* @}
*/
// Exposed Function Prototypes
void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSSetPosVelVar(float PosVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
// 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;
/**
* @}
* @}
*/
#endif /* EKF_H_ */

90
flight/AHRS/inc/pios_config.h
View File

@ -1,46 +1,44 @@
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_HMC5843
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#endif /* PIOS_CONFIG_H */
/**
******************************************************************************
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* - Central compile time config for the project.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_HMC5843
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#endif /* PIOS_CONFIG_H */

2183
flight/AHRS/insgps.c
View File

@ -1,604 +1,1579 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @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 <math.h>
#include <stdint.h>
// 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
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
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
float K[NUMX][NUMV]; // feedback gain matrix
// ************* 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-5; // 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-7; // 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]=100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance
R[9]=.05; // High freq altimeter noise variance (m^2)
}
void INSSetPosVelVar(float PosVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = PosVar;
R[4] = PosVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(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[2];
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 INSCovariancePrediction(float dT)
{
CovariancePrediction(F,G,Q,dT,P);
}
float zeros[3] = {0,0,0};
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt, MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt, HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0], POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt, HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed)
{
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,SensorsUsed);
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[2];
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
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
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<NUMX;i++) // Calculate Dummy = (P/T +F*P)
for (j=0;j<NUMX;j++){
Dummy[i][j] = P[i][j]/dT;
for (k=0;k<NUMX;k++) Dummy[i][j] += F[i][k]*P[k][j];
}
for (i=0;i<NUMX;i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j=i;j<NUMX;j++){ // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j]/dT;
for (k=0;k<NUMX;k++) P[i][j] += Dummy[i][k]*F[j][k]; // P = Dummy/T + Dummy*F'
for (k=0;k<NUMW;k++) P[i][j] += Q[k]*G[i][k]*G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j]*dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], float Q[NUMW], float dT, float P[NUMX][NUMX]){
float D[NUMX][NUMX], T, Tsq;
uint8_t i,j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T=dT;
Tsq = dT*dT;
for (i=0;i<NUMX;i++) // Create a copy of the upper triangular of P
for (j=i;j<NUMX;j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9])*T + D[0][3];
P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9])*T + D[0][4];
P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9])*T + D[0][5];
P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9])*T + D[1][3];
P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9])*T + D[1][4];
P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9])*T + D[1][5];
P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9])*T + D[2][3];
P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9])*T + D[2][4];
P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9])*T + D[2][5];
P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9])*T + D[3][3];
P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9])*T + D[3][4];
P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9])*T + D[3][5];
P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*T + D[3][6];
P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*T + D[3][7];
P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*T + D[3][8];
P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9])*T + D[4][4];
P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9])*T + D[4][5];
P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*T + D[4][6];
P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*T + D[4][7];
P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*T + D[4][8];
P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9])*T + D[5][5];
P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*T + D[5][6];
P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*T + D[5][7];
P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*T + D[5][8];
P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
P[10][10] = Q[6]*Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[11][11] = Q[7]*Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[12][12] = Q[8]*Tsq + D[12][12];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
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){
float HP[NUMX], HPHR, Error;
uint8_t i,j,k,m;
for (m=0;m<NUMV;m++){
if ( SensorsUsed & (0x01<<m)){ // use this sensor for update
for (j=0;j<NUMX;j++){ // Find Hp = H*P
HP[j]=0;
for (k=0;k<NUMX;k++) HP[j] += H[m][k]*P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k=0;k<NUMX;k++) HPHR += HP[k]*H[m][k];
for (k=0;k<NUMX;k++) K[m][k] = HP[k]/HPHR; // find K = HP/HPHR
for (i=0;i<NUMX;i++){ // Find P(m)= P(m-1) + K*HP
for (j=i;j<NUMX;j++) P[i][j]=P[j][i] = P[i][j] - K[m][i]*HP[j];
}
Error = Z[m]-Y[m];
for (i=0;i<NUMX;i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[m][i]*Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX],float U[NUMU], float dT){
float dT2=dT/2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i=0;i<NUMX;i++) Xlast[i] = X[i]; // make a working copy
StateEq(X,U,K1); // k1 = f(x,u)
for (i=0;i<NUMX;i++) X[i] = Xlast[i] + dT2*K1[i];
StateEq(X,U,K2); // k2 = f(x+0.5*dT*k1,u)
for (i=0;i<NUMX;i++) X[i] = Xlast[i] + dT2*K2[i];
StateEq(X,U,K3); // k3 = f(x+0.5*dT*k2,u)
for (i=0;i<NUMX;i++) X[i] = Xlast[i] + dT*K3[i];
StateEq(X,U,K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i=0;i<NUMX;i++) X[i] = Xlast[i] + dT*(K1[i]+2*K2[i]+2*K3[i]+K4[i])/6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX],float U[NUMU],float Xdot[NUMX]){
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax=U[3]; ay=U[4]; az=U[5]; // NO BIAS STATES ON ACCELS
wx=U[0]-X[10]; wy=U[1]-X[11]; wz=U[2]-X[12]; // subtract the biases on gyros
q0=X[6]; q1=X[7]; q2=X[8]; q3=X[9];
// Pdot = V
Xdot[0]=X[3]; Xdot[1]=X[4]; Xdot[2]=X[5];
// Vdot = Reb*a
Xdot[3]=(q0*q0+q1*q1-q2*q2-q3*q3)*ax + 2*(q1*q2-q0*q3)*ay + 2*(q1*q3+q0*q2)*az;
Xdot[4]=2*(q1*q2+q0*q3)*ax + (q0*q0-q1*q1+q2*q2-q3*q3)*ay + 2*(q2*q3-q0*q1)*az;
Xdot[5]=2*(q1*q3-q0*q2)*ax + 2*(q2*q3+q0*q1)*ay + (q0*q0-q1*q1-q2*q2+q3*q3)*az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1*wx-q2*wy-q3*wz)/2;
Xdot[7] = (q0*wx-q3*wy+q2*wz)/2;
Xdot[8] = (q3*wx+q0*wy-q1*wz)/2;
Xdot[9] = (-q2*wx+q1*wy+q0*wz)/2;
// best guess is that bias stays constant
Xdot[10]=Xdot[11]=Xdot[12]=0;
}
void LinearizeFG(float X[NUMX],float U[NUMU], float F[NUMX][NUMX], float G[NUMX][NUMW]){
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax=U[3]; ay=U[4]; az=U[5]; // NO BIAS STATES ON ACCELS
wx=U[0]-X[10]; wy=U[1]-X[11]; wz=U[2]-X[12]; // subtract the biases on gyros
q0=X[6]; q1=X[7]; q2=X[8]; q3=X[9];
// Pdot = V
F[0][3]=F[1][4]=F[2][5]=1;
// dVdot/dq
F[3][6]=2*(q0*ax-q3*ay+q2*az); F[3][7]=2*(q1*ax+q2*ay+q3*az); F[3][8]=2*(-q2*ax+q1*ay+q0*az); F[3][9]=2*(-q3*ax-q0*ay+q1*az);
F[4][6]=2*(q3*ax+q0*ay-q1*az); F[4][7]=2*(q2*ax-q1*ay-q0*az); F[4][8]=2*(q1*ax+q2*ay+q3*az); F[4][9]=2*(q0*ax-q3*ay+q2*az);
F[5][6]=2*(-q2*ax+q1*ay+q0*az); F[5][7]=2*(q3*ax+q0*ay-q1*az); F[5][8]=2*(-q0*ax+q3*ay-q2*az); F[5][9]=2*(q1*ax+q2*ay+q3*az);
// dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6]=0; F[6][7]=-wx/2; F[6][8]=-wy/2; F[6][9]=-wz/2;
F[7][6]=wx/2; F[7][7]=0; F[7][8]=wz/2; F[7][9]=-wy/2;
F[8][6]=wy/2; F[8][7]=-wz/2; F[8][8]=0; F[8][9]=wx/2;
F[9][6]=wz/2; F[9][7]=wy/2; F[9][8]=-wx/2; F[9][9]=0;
// dqdot/dwbias
F[6][10]=q1/2; F[6][11]=q2/2; F[6][12]=q3/2;
F[7][10]=-q0/2; F[7][11]=q3/2; F[7][12]=-q2/2;
F[8][10]=-q3/2; F[8][11]=-q0/2; F[8][12]=q1/2;
F[9][10]=q2/2; F[9][11]=-q1/2; F[9][12]=-q0/2;
// dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
G[4][3]=-2*(q1*q2+q0*q3); G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dnw
G[6][0]=q1/2; G[6][1]=q2/2; G[6][2]=q3/2;
G[7][0]=-q0/2; G[7][1]=q3/2; G[7][2]=-q2/2;
G[8][0]=-q3/2; G[8][1]=-q0/2; G[8][2]=q1/2;
G[9][0]=q2/2; G[9][1]=-q1/2; G[9][2]=-q0/2;
// dwbias = random walk noise
G[10][6]=G[11][7]=G[12][8]=1;
// dabias = random walk noise
// G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]){
float q0, q1, q2, q3;
q0=X[6]; q1=X[7]; q2=X[8]; q3=X[9];
// first six outputs are P and V
Y[0]=X[0]; Y[1]=X[1]; Y[2]=X[2]; Y[3]=X[3]; Y[4]=X[4]; Y[5]=X[5];
// Bb=Rbe*Be
Y[6]=(q0*q0+q1*q1-q2*q2-q3*q3)*Be[0] + 2*(q1*q2+q0*q3)*Be[1] + 2*(q1*q3-q0*q2)*Be[2];
Y[7]=2*(q1*q2-q0*q3)*Be[0] + (q0*q0-q1*q1+q2*q2-q3*q3)*Be[1] + 2*(q2*q3+q0*q1)*Be[2];
Y[8]=2*(q1*q3+q0*q2)*Be[0] + 2*(q2*q3-q0*q1)*Be[1] + (q0*q0-q1*q1-q2*q2+q3*q3)*Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]){
float q0, q1, q2, q3;
q0=X[6]; q1=X[7]; q2=X[8]; q3=X[9];
// dP/dP=I;
H[0][0]=H[1][1]=H[2][2]=1;
// dV/dV=I;
H[3][3]=H[4][4]=H[5][5]=1;
// dBb/dq
H[6][6]=2*(q0*Be[0]+q3*Be[1]-q2*Be[2]); H[6][7]=2*(q1*Be[0]+q2*Be[1]+q3*Be[2]); H[6][8]=2*(-q2*Be[0]+q1*Be[1]-q0*Be[2]); H[6][9]=2*(-q3*Be[0]+q0*Be[1]+q1*Be[2]);
H[7][6]=2*(-q3*Be[0]+q0*Be[1]+q1*Be[2]); H[7][7]=2*(q2*Be[0]-q1*Be[1]+q0*Be[2]); H[7][8]=2*(q1*Be[0]+q2*Be[1]+q3*Be[2]); H[7][9]=2*(-q0*Be[0]-q3*Be[1]+q2*Be[2]);
H[8][6]=2*(q2*Be[0]-q1*Be[1]+q0*Be[2]); H[8][7]=2*(q3*Be[0]-q0*Be[1]-q1*Be[2]); H[8][8]=2*(q0*Be[0]+q3*Be[1]-q2*Be[2]); H[8][9]=2*(q1*Be[0]+q2*Be[1]+q3*Be[2]);
// dAlt/dPz = -1
H[9][2]=-1;
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @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 <math.h>
#include <stdint.h>
// 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
#if defined(GENERAL_COV)
// This might trick people so I have a note here. There is a slower but bigger version of the
// code here but won't fit when debugging disabled (requires -Os)
#define COVARIANCE_PREDICTION_GENERAL
#endif
// Private functions
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed);
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
float K[NUMX][NUMV]; // feedback gain matrix
// ************* 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-5; // 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-7; // 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] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSSetPosVelVar(float PosVar)
{
R[0] = PosVar;
R[1] = PosVar;
R[2] = PosVar;
R[3] = PosVar;
R[4] = PosVar;
// R[5] = PosVar; // Don't change vertical velocity, not measured
}
void INSSetGyroBias(float gyro_bias[3])
{
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSSetAccelVar(float accel_var[3])
{
Q[3] = accel_var[0];
Q[4] = accel_var[1];
Q[5] = accel_var[2];
}
void INSSetGyroVar(float gyro_var[3])
{
Q[0] = gyro_var[0];
Q[1] = gyro_var[1];
Q[2] = gyro_var[2];
}
void INSSetMagVar(float scaled_mag_var[3])
{
R[6] = scaled_mag_var[0];
R[7] = scaled_mag_var[1];
R[8] = scaled_mag_var[2];
}
void INSSetMagNorth(float B[3])
{
Be[0] = B[0];
Be[1] = B[1];
Be[2] = B[2];
}
void INSStatePrediction(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[2];
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 INSCovariancePrediction(float dT)
{
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
void MagCorrection(float mag_data[3])
{
INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
}
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, zeros, Vel, BaroAlt,
MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
}
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
{
INSCorrection(mag_data, Pos, Vel, zeros[0],
POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
}
void VelBaroCorrection(float Vel[3], float BaroAlt)
{
INSCorrection(zeros, zeros, Vel, BaroAlt,
HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
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, SensorsUsed);
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[2];
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
// The General Method is very inefficient,not taking advantage of the sparse F and G
// The first Method is very specific to this implementation
// ************************************************
#ifdef COVARIANCE_PREDICTION_GENERAL
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 < NUMX; i++) // Calculate Dummy = (P/T +F*P)
for (j = 0; j < NUMX; j++) {
Dummy[i][j] = P[i][j] / dT;
for (k = 0; k < NUMX; k++)
Dummy[i][j] += F[i][k] * P[k][j];
}
for (i = 0; i < NUMX; i++) // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
P[i][j] = Dummy[i][j] / dT;
for (k = 0; k < NUMX; k++)
P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F'
for (k = 0; k < NUMW; k++)
P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G'
P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular;
}
}
#else
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
float D[NUMX][NUMX], T, Tsq;
uint8_t i, j;
// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
T = dT;
Tsq = dT * dT;
for (i = 0; i < NUMX; i++) // Create a copy of the upper triangular of P
for (j = i; j < NUMX; j++)
D[i][j] = P[i][j];
// Brute force calculation of the elements of P
P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
P[0][1] = P[1][0] =
D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
P[0][2] = P[2][0] =
D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
P[0][3] = P[3][0] =
(F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
F[3][7] * D[0][7] +
F[3][8] * D[0][8] +
F[3][9] * D[0][9]) * T + D[0][3];
P[0][4] = P[4][0] =
(F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
F[4][7] * D[0][7] +
F[4][8] * D[0][8] +
F[4][9] * D[0][9]) * T + D[0][4];
P[0][5] = P[5][0] =
(F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
F[5][7] * D[0][7] +
F[5][8] * D[0][8] +
F[5][9] * D[0][9]) * T + D[0][5];
P[0][6] = P[6][0] =
(F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
F[6][10] * D[3][10] + F[6][11] * D[3][11] +
F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
F[6][8] * D[0][8] +
F[6][9] * D[0][9] +
F[6][10] * D[0][10] +
F[6][11] * D[0][11] +
F[6][12] * D[0][12]) * T +
D[0][6];
P[0][7] = P[7][0] =
(F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
F[7][10] * D[3][10] + F[7][11] * D[3][11] +
F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
F[7][8] * D[0][8] +
F[7][9] * D[0][9] +
F[7][10] * D[0][10] +
F[7][11] * D[0][11] +
F[7][12] * D[0][12]) * T +
D[0][7];
P[0][8] = P[8][0] =
(F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
F[8][10] * D[3][10] + F[8][11] * D[3][11] +
F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
F[8][7] * D[0][7] +
F[8][9] * D[0][9] +
F[8][10] * D[0][10] +
F[8][11] * D[0][11] +
F[8][12] * D[0][12]) * T +
D[0][8];
P[0][9] = P[9][0] =
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[9][10] * D[3][10] + F[9][11] * D[3][11] +
F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
F[9][7] * D[0][7] +
F[9][8] * D[0][8] +
F[9][10] * D[0][10] +
F[9][11] * D[0][11] +
F[9][12] * D[0][12]) * T +
D[0][9];
P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
P[1][2] = P[2][1] =
D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
P[1][3] = P[3][1] =
(F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
F[3][7] * D[1][7] +
F[3][8] * D[1][8] +
F[3][9] * D[1][9]) * T + D[1][3];
P[1][4] = P[4][1] =
(F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
F[4][7] * D[1][7] +
F[4][8] * D[1][8] +
F[4][9] * D[1][9]) * T + D[1][4];
P[1][5] = P[5][1] =
(F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
F[5][7] * D[1][7] +
F[5][8] * D[1][8] +
F[5][9] * D[1][9]) * T + D[1][5];
P[1][6] = P[6][1] =
(F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
F[6][10] * D[4][10] + F[6][11] * D[4][11] +
F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
F[6][8] * D[1][8] +
F[6][9] * D[1][9] +
F[6][10] * D[1][10] +
F[6][11] * D[1][11] +
F[6][12] * D[1][12]) * T +
D[1][6];
P[1][7] = P[7][1] =
(F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
F[7][10] * D[4][10] + F[7][11] * D[4][11] +
F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
F[7][8] * D[1][8] +
F[7][9] * D[1][9] +
F[7][10] * D[1][10] +
F[7][11] * D[1][11] +
F[7][12] * D[1][12]) * T +
D[1][7];
P[1][8] = P[8][1] =
(F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
F[8][10] * D[4][10] + F[8][11] * D[4][11] +
F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
F[8][7] * D[1][7] +
F[8][9] * D[1][9] +
F[8][10] * D[1][10] +
F[8][11] * D[1][11] +
F[8][12] * D[1][12]) * T +
D[1][8];
P[1][9] = P[9][1] =
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[9][10] * D[4][10] + F[9][11] * D[4][11] +
F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
F[9][7] * D[1][7] +
F[9][8] * D[1][8] +
F[9][10] * D[1][10] +
F[9][11] * D[1][11] +
F[9][12] * D[1][12]) * T +
D[1][9];
P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
P[2][3] = P[3][2] =
(F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
F[3][7] * D[2][7] +
F[3][8] * D[2][8] +
F[3][9] * D[2][9]) * T + D[2][3];
P[2][4] = P[4][2] =
(F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
F[4][7] * D[2][7] +
F[4][8] * D[2][8] +
F[4][9] * D[2][9]) * T + D[2][4];
P[2][5] = P[5][2] =
(F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
F[5][7] * D[2][7] +
F[5][8] * D[2][8] +
F[5][9] * D[2][9]) * T + D[2][5];
P[2][6] = P[6][2] =
(F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
F[6][10] * D[5][10] + F[6][11] * D[5][11] +
F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
F[6][8] * D[2][8] +
F[6][9] * D[2][9] +
F[6][10] * D[2][10] +
F[6][11] * D[2][11] +
F[6][12] * D[2][12]) * T +
D[2][6];
P[2][7] = P[7][2] =
(F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
F[7][10] * D[5][10] + F[7][11] * D[5][11] +
F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
F[7][8] * D[2][8] +
F[7][9] * D[2][9] +
F[7][10] * D[2][10] +
F[7][11] * D[2][11] +
F[7][12] * D[2][12]) * T +
D[2][7];
P[2][8] = P[8][2] =
(F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
F[8][10] * D[5][10] + F[8][11] * D[5][11] +
F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
F[8][7] * D[2][7] +
F[8][9] * D[2][9] +
F[8][10] * D[2][10] +
F[8][11] * D[2][11] +
F[8][12] * D[2][12]) * T +
D[2][8];
P[2][9] = P[9][2] =
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[9][10] * D[5][10] + F[9][11] * D[5][11] +
F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
F[9][7] * D[2][7] +
F[9][8] * D[2][8] +
F[9][10] * D[2][10] +
F[9][11] * D[2][11] +
F[9][12] * D[2][12]) * T +
D[2][9];
P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
P[3][3] =
(Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
F[3][6] * D[6][9] +
F[3][7] * D[7][9] +
F[3][8] * D[8][9]) +
F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
P[3][4] = P[4][3] =
(F[4][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
F[4][6] * D[3][6] +
F[3][7] * D[4][7] +
F[4][7] * D[3][7] +
F[3][8] * D[4][8] +
F[4][8] * D[3][8] +
F[3][9] * D[4][9] +
F[4][9] * D[3][9]) * T +
D[3][4];
P[3][5] = P[5][3] =
(F[5][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
F[3][7] * D[6][7] +
F[3][8] * D[6][8] +
F[3][9] * D[6][9]) +
F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
F[5][6] * D[3][6] +
F[3][7] * D[5][7] +
F[5][7] * D[3][7] +
F[3][8] * D[5][8] +
F[5][8] * D[3][8] +
F[3][9] * D[5][9] +
F[5][9] * D[3][9]) * T +
D[3][5];
P[3][6] = P[6][3] =
(F[6][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
F[6][9] * D[3][9] + F[6][10] * D[3][10] +
F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
P[3][7] = P[7][3] =
(F[7][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
F[7][9] * D[3][9] + F[7][10] * D[3][10] +
F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
P[3][8] = P[8][3] =
(F[8][9] *
(F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
F[3][6] * D[6][10] +
F[3][7] * D[7][10] +
F[3][8] * D[8][10]) +
F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
(F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
F[8][9] * D[3][9] + F[8][10] * D[3][10] +
F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
P[3][9] = P[9][3] =
(F[9][10] *
(F[3][9] * D[9][10] + F[3][6] * D[6][10] +
F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
(F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
F[3][9] * D[9][9] + F[9][10] * D[3][10] +
F[9][11] * D[3][11] + F[9][12] * D[3][12] +
F[3][6] * D[6][9] + F[3][7] * D[7][9] +
F[3][8] * D[8][9]) * T + D[3][9];
P[3][10] = P[10][3] =
(F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
F[3][8] * D[8][10]) * T + D[3][10];
P[3][11] = P[11][3] =
(F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
F[3][8] * D[8][11]) * T + D[3][11];
P[3][12] = P[12][3] =
(F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
F[3][8] * D[8][12]) * T + D[3][12];
P[4][4] =
(Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
F[4][6] * D[6][9] +
F[4][7] * D[7][9] +
F[4][8] * D[8][9]) +
F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
P[4][5] = P[5][4] =
(F[5][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
F[4][7] * D[6][7] +
F[4][8] * D[6][8] +
F[4][9] * D[6][9]) +
F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
F[5][6] * D[4][6] +
F[4][7] * D[5][7] +
F[5][7] * D[4][7] +
F[4][8] * D[5][8] +
F[5][8] * D[4][8] +
F[4][9] * D[5][9] +
F[5][9] * D[4][9]) * T +
D[4][5];
P[4][6] = P[6][4] =
(F[6][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
F[6][9] * D[4][9] + F[6][10] * D[4][10] +
F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
P[4][7] = P[7][4] =
(F[7][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
F[7][9] * D[4][9] + F[7][10] * D[4][10] +
F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
P[4][8] = P[8][4] =
(F[8][9] *
(F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
F[4][6] * D[6][10] +
F[4][7] * D[7][10] +
F[4][8] * D[8][10]) +
F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
(F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
F[8][9] * D[4][9] + F[8][10] * D[4][10] +
F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
P[4][9] = P[9][4] =
(F[9][10] *
(F[4][9] * D[9][10] + F[4][6] * D[6][10] +
F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
(F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
F[4][9] * D[9][9] + F[9][10] * D[4][10] +
F[9][11] * D[4][11] + F[9][12] * D[4][12] +
F[4][6] * D[6][9] + F[4][7] * D[7][9] +
F[4][8] * D[8][9]) * T + D[4][9];
P[4][10] = P[10][4] =
(F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
F[4][8] * D[8][10]) * T + D[4][10];
P[4][11] = P[11][4] =
(F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
F[4][8] * D[8][11]) * T + D[4][11];
P[4][12] = P[12][4] =
(F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
F[4][8] * D[8][12]) * T + D[4][12];
P[5][5] =
(Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
F[5][6] * D[6][9] +
F[5][7] * D[7][9] +
F[5][8] * D[8][9]) +
F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
P[5][6] = P[6][5] =
(F[6][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
F[6][9] * D[5][9] + F[6][10] * D[5][10] +
F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
P[5][7] = P[7][5] =
(F[7][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
F[7][9] * D[5][9] + F[7][10] * D[5][10] +
F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
P[5][8] = P[8][5] =
(F[8][9] *
(F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
F[5][6] * D[6][10] +
F[5][7] * D[7][10] +
F[5][8] * D[8][10]) +
F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
(F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
F[8][9] * D[5][9] + F[8][10] * D[5][10] +
F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
P[5][9] = P[9][5] =
(F[9][10] *
(F[5][9] * D[9][10] + F[5][6] * D[6][10] +
F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
(F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
F[5][9] * D[9][9] + F[9][10] * D[5][10] +
F[9][11] * D[5][11] + F[9][12] * D[5][12] +
F[5][6] * D[6][9] + F[5][7] * D[7][9] +
F[5][8] * D[8][9]) * T + D[5][9];
P[5][10] = P[10][5] =
(F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
F[5][8] * D[8][10]) * T + D[5][10];
P[5][11] = P[11][5] =
(F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
F[5][8] * D[8][11]) * T + D[5][11];
P[5][12] = P[12][5] =
(F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
F[5][8] * D[8][12]) * T + D[5][12];
P[6][6] =
(Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
F[6][10] * D[9][10] +
F[6][11] * D[9][11] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) +
F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
(2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
D[6][6];
P[6][7] = P[7][6] =
(F[7][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
F[6][7] * D[7][7] +
F[6][8] * D[7][8] +
F[7][8] * D[6][8] +
F[6][9] * D[7][9] +
F[7][9] * D[6][9] +
F[6][10] * D[7][10] +
F[7][10] * D[6][10] +
F[6][11] * D[7][11] +
F[7][11] * D[6][11] +
F[6][12] * D[7][12] +
F[7][12] * D[6][12]) * T +
D[6][7];
P[6][8] = P[8][6] =
(F[8][9] *
(F[6][9] * D[9][9] + F[6][10] * D[9][10] +
F[6][11] * D[9][11] + F[6][12] * D[9][12] +
F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
F[8][6] * D[6][6] +
F[8][7] * D[6][7] +
F[6][8] * D[8][8] +
F[6][9] * D[8][9] +
F[8][9] * D[6][9] +
F[6][10] * D[8][10] +
F[8][10] * D[6][10] +
F[6][11] * D[8][11] +
F[8][11] * D[6][11] +
F[6][12] * D[8][12] +
F[8][12] * D[6][12]) * T +
D[6][8];
P[6][9] = P[9][6] =
(F[9][10] *
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
F[6][9] * D[6][9] + F[6][10] * D[6][10] +
F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
F[6][9] * D[7][9] + F[6][10] * D[7][10] +
F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
F[6][9] * D[8][9] + F[6][10] * D[8][10] +
F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
F[9][7] * D[6][7] +
F[9][8] * D[6][8] +
F[6][9] * D[9][9] +
F[9][10] * D[6][10] +
F[6][10] * D[9][10] +
F[9][11] * D[6][11] +
F[6][11] * D[9][11] +
F[9][12] * D[6][12] +
F[6][12] * D[9][12] +
F[6][7] * D[7][9] +
F[6][8] * D[8][9]) * T +
D[6][9];
P[6][10] = P[10][6] =
(F[6][9] * D[9][10] + F[6][10] * D[10][10] +
F[6][11] * D[10][11] + F[6][12] * D[10][12] +
F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
P[6][11] = P[11][6] =
(F[6][9] * D[9][11] + F[6][10] * D[10][11] +
F[6][11] * D[11][11] + F[6][12] * D[11][12] +
F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
P[6][12] = P[12][6] =
(F[6][9] * D[9][12] + F[6][10] * D[10][12] +
F[6][11] * D[11][12] + F[6][12] * D[12][12] +
F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
P[7][7] =
(Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
F[7][10] * D[9][10] +
F[7][11] * D[9][11] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) +
F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
(2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
D[7][7];
P[7][8] = P[8][7] =
(F[8][9] *
(F[7][9] * D[9][9] + F[7][10] * D[9][10] +
F[7][11] * D[9][11] + F[7][12] * D[9][12] +
F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
F[8][6] * D[6][7] +
F[8][7] * D[7][7] +
F[7][8] * D[8][8] +
F[7][9] * D[8][9] +
F[8][9] * D[7][9] +
F[7][10] * D[8][10] +
F[8][10] * D[7][10] +
F[7][11] * D[8][11] +
F[8][11] * D[7][11] +
F[7][12] * D[8][12] +
F[8][12] * D[7][12]) * T +
D[7][8];
P[7][9] = P[9][7] =
(F[9][10] *
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
F[7][9] * D[6][9] + F[7][10] * D[6][10] +
F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
F[7][9] * D[7][9] + F[7][10] * D[7][10] +
F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
F[7][9] * D[8][9] + F[7][10] * D[8][10] +
F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
F[9][7] * D[7][7] +
F[9][8] * D[7][8] +
F[7][9] * D[9][9] +
F[9][10] * D[7][10] +
F[7][10] * D[9][10] +
F[9][11] * D[7][11] +
F[7][11] * D[9][11] +
F[9][12] * D[7][12] +
F[7][12] * D[9][12] +
F[7][6] * D[6][9] +
F[7][8] * D[8][9]) * T +
D[7][9];
P[7][10] = P[10][7] =
(F[7][9] * D[9][10] + F[7][10] * D[10][10] +
F[7][11] * D[10][11] + F[7][12] * D[10][12] +
F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
P[7][11] = P[11][7] =
(F[7][9] * D[9][11] + F[7][10] * D[10][11] +
F[7][11] * D[11][11] + F[7][12] * D[11][12] +
F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
P[7][12] = P[12][7] =
(F[7][9] * D[9][12] + F[7][10] * D[10][12] +
F[7][11] * D[11][12] + F[7][12] * D[12][12] +
F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
P[8][8] =
(Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
F[8][10] * D[9][10] +
F[8][11] * D[9][11] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) +
F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
(2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
D[8][8];
P[8][9] = P[9][8] =
(F[9][10] *
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
F[8][9] * D[6][9] + F[8][10] * D[6][10] +
F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
F[8][9] * D[7][9] + F[8][10] * D[7][10] +
F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
F[8][9] * D[8][9] + F[8][10] * D[8][10] +
F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
F[9][7] * D[7][8] +
F[9][8] * D[8][8] +
F[8][9] * D[9][9] +
F[9][10] * D[8][10] +
F[8][10] * D[9][10] +
F[9][11] * D[8][11] +
F[8][11] * D[9][11] +
F[9][12] * D[8][12] +
F[8][12] * D[9][12] +
F[8][6] * D[6][9] +
F[8][7] * D[7][9]) * T +
D[8][9];
P[8][10] = P[10][8] =
(F[8][9] * D[9][10] + F[8][10] * D[10][10] +
F[8][11] * D[10][11] + F[8][12] * D[10][12] +
F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
P[8][11] = P[11][8] =
(F[8][9] * D[9][11] + F[8][10] * D[10][11] +
F[8][11] * D[11][11] + F[8][12] * D[11][12] +
F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
P[8][12] = P[12][8] =
(F[8][9] * D[9][12] + F[8][10] * D[10][12] +
F[8][11] * D[11][12] + F[8][12] * D[12][12] +
F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
P[9][9] =
(Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
F[9][11] * D[10][11] +
F[9][12] * D[10][12] +
F[9][6] * D[6][10] +
F[9][7] * D[7][10] +
F[9][8] * D[8][10]) +
F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
F[9][8] * D[6][8] + F[9][10] * D[6][10] +
F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
F[9][8] * D[7][8] + F[9][10] * D[7][10] +
F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
F[9][8] * D[8][8] + F[9][10] * D[8][10] +
F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
(2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
P[9][10] = P[10][9] =
(F[9][10] * D[10][10] + F[9][11] * D[10][11] +
F[9][12] * D[10][12] + F[9][6] * D[6][10] +
F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
P[9][11] = P[11][9] =
(F[9][10] * D[10][11] + F[9][11] * D[11][11] +
F[9][12] * D[11][12] + F[9][6] * D[6][11] +
F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
P[9][12] = P[12][9] =
(F[9][10] * D[10][12] + F[9][11] * D[11][12] +
F[9][12] * D[12][12] + F[9][6] * D[6][12] +
F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
P[10][10] = Q[6] * Tsq + D[10][10];
P[10][11] = P[11][10] = D[10][11];
P[10][12] = P[12][10] = D[10][12];
P[11][11] = Q[7] * Tsq + D[11][11];
P[11][12] = P[12][11] = D[11][12];
P[12][12] = Q[8] * Tsq + D[12][12];
}
#endif
// ************* SerialUpdate *******************
// Does the update step of the Kalman filter for the covariance and estimate
// Outputs are Xnew & Pnew, and are written over P and X
// Z is actual measurement, Y is predicted measurement
// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
// where K=P*H'*inv[H*P*H'+R]
// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
// i.e. the measurment noises are uncorrelated.
// It therefore uses a serial update that requires no matrix inversion by
// processing the measurements one at a time.
// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
// - or see Simon, "Optimal State Estimation," 1st Ed, p.150
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
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)
{
float HP[NUMX], HPHR, Error;
uint8_t i, j, k, m;
for (m = 0; m < NUMV; m++) {
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
HPHR = R[m]; // Find HPHR = H*P*H' + R
for (k = 0; k < NUMX; k++)
HPHR += HP[k] * H[m][k];
for (k = 0; k < NUMX; k++)
K[m][k] = HP[k] / HPHR; // find K = HP/HPHR
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++)
P[i][j] = P[j][i] =
P[i][j] - K[m][i] * HP[j];
}
Error = Z[m] - Y[m];
for (i = 0; i < NUMX; i++) // Find X(m)= X(m-1) + K*Error
X[i] = X[i] + K[m][i] * Error;
}
}
}
// ************* RungeKutta **********************
// Does a 4th order Runge Kutta numerical integration step
// Output, Xnew, is written over X
// NOTE the algorithm assumes time invariant state equations and
// constant inputs over integration step
// ************************************************
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
Xlast[i] = X[i]; // make a working copy
StateEq(X, U, K1); // k1 = f(x,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K1[i];
StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT2 * K2[i];
StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u)
for (i = 0; i < NUMX; i++)
X[i] = Xlast[i] + dT * K3[i];
StateEq(X, U, K4); // k4 = f(x+dT*k3,u)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
}
// ************* Model Specific Stuff ***************************
// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
//
// State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
// Deterministic Inputs = [AngularVel Accel]
// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
// Inputs to Measurement = [EarthFrameMagField]
//
// Notes: Pos and Vel in earth frame
// AngularVel and Accel in body frame
// MagFields are unit vectors
// Xdot is output of StateEq()
// F and G are outputs of LinearizeFG(), all elements not set should be zero
// y is output of OutputEq()
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
Xdot[0] = X[3];
Xdot[1] = X[4];
Xdot[2] = X[5];
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
}
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels
ax = U[3];
ay = U[4];
az = U[5]; // NO BIAS STATES ON ACCELS
wx = U[0] - X[10];
wy = U[1] - X[11];
wz = U[2] - X[12]; // subtract the biases on gyros
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
// dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
G[3][4] = 2 * (-q1 * q2 + q0 * q3);
G[3][5] = -2 * (q1 * q3 + q0 * q2);
G[4][3] = -2 * (q1 * q2 + q0 * q3);
G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
G[4][5] = 2 * (-q2 * q3 + q0 * q1);
G[5][3] = 2 * (-q1 * q3 + q0 * q2);
G[5][4] = -2 * (q2 * q3 + q0 * q1);
G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
// dabias = random walk noise
// G[13][9]=G[14][10]=G[15][11]=1; // NO BIAS STATES ON ACCELS
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// first six outputs are P and V
Y[0] = X[0];
Y[1] = X[1];
Y[2] = X[2];
Y[3] = X[3];
Y[4] = X[4];
Y[5] = X[5];
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
{
float q0, q1, q2, q3;
q0 = X[6];
q1 = X[7];
q2 = X[8];
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
}
/**
* @}
* @}
*/

414
flight/AHRS/pios_board.c
View File

@ -35,113 +35,126 @@
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_op_irq_handler(void);
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler")));
void DMA1_Channel5_IRQHandler()
__attribute__ ((alias("PIOS_SPI_op_irq_handler")));
void DMA1_Channel4_IRQHandler()
__attribute__ ((alias("PIOS_SPI_op_irq_handler")));
static const struct pios_spi_cfg pios_spi_op_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_op_irq_handler,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
},
.use_crc = TRUE,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.irq = {
.handler = PIOS_SPI_op_irq_handler,
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority =
PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr =
(uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc =
DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize =
DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize =
DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.ssel = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
/*
* Board specific number of devices.
*/
struct pios_spi_dev pios_spi_devs[] = {
{
.cfg = &pios_spi_op_cfg,
},
{
.cfg = &pios_spi_op_cfg,
},
};
uint8_t pios_spi_num_devices = NELEMENTS(pios_spi_devs);
void PIOS_SPI_op_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(PIOS_SPI_OP);
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(PIOS_SPI_OP);
}
#endif /* PIOS_INCLUDE_SPI */
@ -153,46 +166,49 @@ void PIOS_SPI_op_irq_handler(void)
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
void USART3_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.regs = USART3,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl =
USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority =
PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
/*
@ -200,16 +216,16 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
*/
struct pios_usart_dev pios_usart_devs[] = {
#define PIOS_USART_AUX 0
{
.cfg = &pios_usart_aux_cfg,
},
{
.cfg = &pios_usart_aux_cfg,
},
};
uint8_t pios_usart_num_devices = NELEMENTS(pios_usart_devs);
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(PIOS_USART_AUX);
PIOS_USART_IRQ_Handler(PIOS_USART_AUX);
}
#endif /* PIOS_INCLUDE_USART */
@ -227,11 +243,11 @@ void PIOS_USART_aux_irq_handler(void)
extern const struct pios_com_driver pios_usart_com_driver;
struct pios_com_dev pios_com_devs[] = {
#if defined(PIOS_INCLUDE_USART)
{
.id = PIOS_USART_AUX,
.driver = &pios_usart_com_driver,
},
#if defined(PIOS_INCLUDE_USART)
{
.id = PIOS_USART_AUX,
.driver = &pios_usart_com_driver,
},
#endif
};
@ -248,81 +264,83 @@ const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
void PIOS_I2C_main_adapter_ev_irq_handler(void);
void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C1_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
void I2C1_EV_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 200000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
//.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelPreemptionPriority = 2,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
//.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelPreemptionPriority = 2,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 200000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
//.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelPreemptionPriority = 2,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
//.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelPreemptionPriority = 2,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
/*
* Board specific number of devices.
*/
struct pios_i2c_adapter pios_i2c_adapters[] = {
{
.cfg = &pios_i2c_main_adapter_cfg,
},
{
.cfg = &pios_i2c_main_adapter_cfg,
},
};
uint8_t pios_i2c_num_adapters = NELEMENTS(pios_i2c_adapters);
void PIOS_I2C_main_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(PIOS_I2C_MAIN_ADAPTER);
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(PIOS_I2C_MAIN_ADAPTER);
}
void PIOS_I2C_main_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(PIOS_I2C_MAIN_ADAPTER);
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(PIOS_I2C_MAIN_ADAPTER);
}
#endif /* PIOS_INCLUDE_I2C */

41
flight/AHRS/test.c
View File

@ -7,24 +7,31 @@ extern float X[13];
int main()
{
float gyro[3]={2.47,-0.25,7.71}, accel[3]={-1.02,0.70,-10.11}, dT=0.04, mags[3]={-50,-180,-376};
float Pos[3]={0,0,0}, Vel[3]={0,0,0}, BaroAlt=2.66, Speed=4.4, Heading=0;
float yaw;
int i,j;
float gyro[3] = { 2.47, -0.25, 7.71 }, accel[3] = {
-1.02, 0.70, -10.11}, dT = 0.04, mags[3] = {
-50, -180, -376};
float Pos[3] = { 0, 0, 0 }, Vel[3] = {
0, 0, 0}, BaroAlt = 2.66, Speed = 4.4, Heading = 0;
float yaw;
int i, j;
INSGPSInit();
INSGPSInit();
for(i=0;i<10000000;i++){
INSPrediction(gyro, accel, dT);
//MagCorrection(mags);
FullCorrection(mags,Pos,Vel,BaroAlt);
yaw = atan2( (float) 2 * (Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
(float) (1 - 2 * (Nav.q[2] * Nav.q[2] + Nav.q[3] * Nav.q[3]) ) ) * 180 / M_PI;
for (i = 0; i < 10000000; i++) {
INSPrediction(gyro, accel, dT);
//MagCorrection(mags);
FullCorrection(mags, Pos, Vel, BaroAlt);
yaw =
atan2((float)2 *
(Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
(float)(1 -
2 * (Nav.q[2] * Nav.q[2] +
Nav.q[3] * Nav.q[3]))) * 180 / M_PI;
printf("%0.3f ", yaw);
for(j=0; j < 13; j++)
printf("%f ", X[j]);
printf("\r\n");
}
return 0;
printf("%0.3f ", yaw);
for (j = 0; j < 13; j++)
printf("%f ", X[j]);
printf("\r\n");
}
return 0;
}