diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile index 125025ddf..c1823e437 100644 --- a/flight/AHRS/Makefile +++ b/flight/AHRS/Makefile @@ -102,7 +102,7 @@ SRC += $(FLIGHTLIB)/ahrs_comm_objects.c SRC += $(BOOT)/ahrs_spi_program_slave.c SRC += $(BOOT)/ahrs_slave_test.c SRC += $(BOOT)/ahrs_spi_program.c -SRC += insgps.c +SRC += insgps13state.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(FLIGHTLIB)/fifo_buffer.c diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 77469e178..4cdc1a9b8 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -365,16 +365,12 @@ void simple_update() { * @brief Output all the important inputs and states of the ekf through serial port */ #ifdef DUMP_EKF -#define NUMX 16 // number of states, X is the state vector -#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector -#define NUMV 10 // number of measurements, v is the measurement noise vector -#define NUMU 7 // 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 + +extern float **P, *X; // covariance matrix and state vector + void print_ekf_binary() { + uint16_t states = ins_get_num_states(); uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; // Dump raw buffer PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16) @@ -388,9 +384,9 @@ void print_ekf_binary() PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:149) - for(uint8_t i = 0; i < NUMX; i++) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (150:213) + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states); // X (86:149) + for(uint8_t i = 0; i < states; i++) + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4); // diag(P) (150:213) PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217) PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221) diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 2b217bbd0..bf4faae4b 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -74,6 +74,8 @@ 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); +uint16_t ins_get_num_states(); + // Nav structure containing current solution struct NavStruct { float Pos[3]; // Position in meters and relative to a local NED frame diff --git a/flight/AHRS/inc/insgps13state.h b/flight/AHRS/inc/insgps13state.h deleted file mode 100644 index b05d8b868..000000000 --- a/flight/AHRS/inc/insgps13state.h +++ /dev/null @@ -1,90 +0,0 @@ -/** - ****************************************************************************** - * @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_ - -#include "stdint.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 INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed); - -void INSResetP(float PDiag[13]); -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]); -void INSSetPosVelVar(float PosVar, float VelVar); -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 INSPosVelReset(float pos[3], float vel[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 - float gyro_bias[3]; -} Nav; - -/** - * @} - * @} - */ - -#endif /* EKF_H_ */ diff --git a/flight/AHRS/insgps13state.c b/flight/AHRS/insgps13state.c index e7cbb0a56..24ffab1db 100644 --- a/flight/AHRS/insgps13state.c +++ b/flight/AHRS/insgps13state.c @@ -29,7 +29,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "insgps13state.h" +#include "insgps.h" #include #include @@ -68,6 +68,12 @@ float K[NUMX][NUMV]; // feedback gain matrix // ************* Exposed Functions **************** // ************************************************* + +uint16_t ins_get_num_states() +{ + return NUMX; +} + void INSGPSInit() //pretty much just a place holder for now { Be[0] = 1; @@ -92,7 +98,7 @@ void INSGPSInit() //pretty much just a place holder for now 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-14; // gyro bias random walk variance (rad/s^2)^2 + Q[6] = Q[7] = Q[8] = 2e-9; // 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) @@ -116,8 +122,9 @@ void INSResetP(float PDiag[NUMX]) } } -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]) +void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]) { + /* Note: accel_bias not used in 13 state INS */ X[0] = pos[0]; X[1] = pos[1]; X[2] = pos[2]; diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps16state.c similarity index 99% rename from flight/AHRS/insgps.c rename to flight/AHRS/insgps16state.c index cd08ca7dd..d1384b213 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps16state.c @@ -68,6 +68,12 @@ float K[NUMX][NUMV]; // feedback gain matrix // ************* Exposed Functions **************** // ************************************************* + +uint16_t ins_get_num_states() +{ + return NUMX; +} + void INSGPSInit() //pretty much just a place holder for now { Be[0] = 1; @@ -92,8 +98,8 @@ void INSGPSInit() //pretty much just a place holder for now 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-14; // gyro bias random walk variance (rad/s^2)^2 - Q[9] = Q[10] = Q[11] = 2e-13; // accel bias random walk variance (m/s^3)^2 + Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2 + Q[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^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)