mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
Behaving reasonably, added calibration code which runs by default on startup and take 6 seconds - this will be called from GCS in future and saved to SD card.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1303 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
e0d63103ec
commit
a23f3c66f4
@ -168,17 +168,35 @@ static struct attitude_solution attitude_data = {
|
||||
|
||||
/* Function Prototypes */
|
||||
void process_spi_request(void);
|
||||
void downsample_data();
|
||||
void downsample_data(void);
|
||||
void calibrate_sensors(void);
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Global_Data AHRS Global Data
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
int16_t fir_coeffs[ADC_OVERSAMPLE+1]; // FIR filter coefficients
|
||||
//! Accelerometer variance after filter from OP or calibrate_sensors
|
||||
float accel_var[3];
|
||||
//! Gyro variance after filter from OP or calibrate sensors
|
||||
float gyro_var[3];
|
||||
//! Magnetometer variance from OP or calibrate sensors
|
||||
float mag_var[3];
|
||||
//! Accelerometer bias from OP or calibrate sensors
|
||||
float accel_bias[3];
|
||||
//! Gyroscope bias term from OP or calibrate sensors
|
||||
float gyro_bias[3];
|
||||
//! Magnetometer bias (direction) from OP or calibrate sensors
|
||||
float mag_bias[3];
|
||||
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
|
||||
int16_t fir_coeffs[ADC_OVERSAMPLE+1];
|
||||
//! Raw buffer that DMA data is dumped into
|
||||
int16_t raw_data_buffer[ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE * 2]; // Double buffer that DMA just used
|
||||
int16_t * valid_data_buffer; // Swapped by interrupt handler to achieve double buffering
|
||||
//! Swapped by interrupt handler to achieve double buffering
|
||||
int16_t * valid_data_buffer;
|
||||
//! Counts how many times the EKF wasn't idle when DMA handler called
|
||||
uint32_t ekf_too_slow = 0;
|
||||
//! Total number of data blocks converted
|
||||
uint32_t total_conversion_blocks = 0;
|
||||
/**
|
||||
* @}
|
||||
@ -228,9 +246,7 @@ int main()
|
||||
PIOS_SPI_Init();
|
||||
|
||||
lfsm_init();
|
||||
|
||||
INSGPSInit();
|
||||
|
||||
|
||||
ahrs_state = AHRS_IDLE;;
|
||||
/* Use simple averaging filter for now */
|
||||
for (int i = 0; i < ADC_OVERSAMPLE; i++)
|
||||
@ -245,7 +261,23 @@ int main()
|
||||
vel[1] = 0;
|
||||
vel[2] = 0;
|
||||
|
||||
// Main loop
|
||||
/******* Get initial estimate of gyro bias term *************/
|
||||
// TODO: There needs to be a calibration mode, then this is received from the SD card
|
||||
// otherwise if we reset in air during a snap, this will be all wrong
|
||||
calibrate_sensors();
|
||||
INSGPSInit();
|
||||
INSSetGyroBias(gyro_bias);
|
||||
INSSetAccelVar(accel_var);
|
||||
INSSetGyroVar(gyro_var);
|
||||
// INS algo wants noise on magnetometer in unit length variance
|
||||
float scaled_mag_var[3];
|
||||
float mag_length = mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] + mag_bias[2] * mag_bias[2];
|
||||
scaled_mag_var[0] = mag_var[0] / mag_length;
|
||||
scaled_mag_var[1] = mag_var[1] / mag_length;
|
||||
scaled_mag_var[2] = mag_var[2] / mag_length;
|
||||
INSSetMagVar(scaled_mag_var);
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while (1) {
|
||||
// Alive signal
|
||||
PIOS_LED_Toggle(LED1);
|
||||
@ -259,7 +291,7 @@ int main()
|
||||
|
||||
downsample_data();
|
||||
|
||||
/******************** INS ALGORITHM *************************/
|
||||
/******************** INS ALGORITHM **************************/
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
@ -386,13 +418,82 @@ void downsample_data()
|
||||
gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @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];
|
||||
|
||||
// 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;
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
mag_bias[0] += mag_raw[0];
|
||||
mag_bias[1] += mag_raw[1];
|
||||
mag_bias[2] += mag_raw[2];
|
||||
|
||||
ahrs_state = AHRS_IDLE;
|
||||
}
|
||||
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]);
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
void dump_spi_message(uint8_t port, const char * prefix, uint8_t * data, uint32_t len)
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -30,6 +30,10 @@
|
||||
// Exposed Function Prototypes
|
||||
void INSGPSInit();
|
||||
void INSPrediction(float gyro_data[3], float accel_data[3], float dT);
|
||||
void INSSetGyroBias(float gyro_bias[3]);
|
||||
void INSSetAccelVar(float accel_var[3]);
|
||||
void INSSetGyroVar(float gyro_var[3]);
|
||||
void INSSetMagVar(float scaled_mag_var[3]);
|
||||
void MagCorrection(float mag_data[3]);
|
||||
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
|
||||
void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]);
|
||||
|
||||
@ -61,7 +61,7 @@ void INSGPSInit() //pretty much just a place holder for now
|
||||
P[0][0]=P[1][1]=P[2][2]=25; // initial position variance (m^2)
|
||||
P[3][3]=P[4][4]=P[5][5]=5; // initial velocity variance (m/s)^2
|
||||
P[6][6]=P[7][7]=P[8][8]=P[9][9]=1e-5; // initial quaternion variance
|
||||
P[10][10]=P[11][11]=P[12][12]=1e-6; // initial gyro bias variance (rad/s)^2
|
||||
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)
|
||||
@ -69,16 +69,45 @@ 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-10; // gyro bias random walk variance (rad/s^2)^2
|
||||
Q[6]=Q[7]=Q[8]=2e-5; // gyro bias random walk variance (rad/s^2)^2
|
||||
|
||||
R[0]=R[1]=0.004; // High freq GPS horizontal position noise variance (m^2)
|
||||
R[2]=0.036; // High freq GPS vertical position noise variance (m^2)
|
||||
R[3]=R[4]=0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
|
||||
R[5]=0; // High freq GPS vertical velocity noise variance (m/s)^2
|
||||
R[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance
|
||||
R[9]=1; // High freq altimeter noise variance (m^2)
|
||||
R[9]=1; // High freq altimeter noise variance (m^2)
|
||||
}
|
||||
|
||||
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 INSPrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
float U[6];
|
||||
|
||||
@ -21,6 +21,8 @@
|
||||
651CF9F2120B700D00EEFD70 /* pios_usb_hid_pwr.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_usb_hid_pwr.h; sourceTree = "<group>"; };
|
||||
651CF9F3120B700D00EEFD70 /* usb_conf.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = usb_conf.h; sourceTree = "<group>"; };
|
||||
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
|
||||
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
|
||||
6543305B1219868D0063F913 /* WorldMagModel.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = WorldMagModel.c; path = ../../AHRS/WorldMagModel.c; sourceTree = SOURCE_ROOT; };
|
||||
65A2C7EF11E2A33D00D0391E /* FreeRTOSConfig.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = FreeRTOSConfig.h; path = ../../PiOS.posix/inc/FreeRTOSConfig.h; sourceTree = SOURCE_ROOT; };
|
||||
65A2C7F011E2A33D00D0391E /* pios_com.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_com.h; path = ../../PiOS.posix/inc/pios_com.h; sourceTree = SOURCE_ROOT; };
|
||||
65A2C7F111E2A33D00D0391E /* pios_com_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = pios_com_priv.h; path = ../../PiOS.posix/inc/pios_com_priv.h; sourceTree = SOURCE_ROOT; };
|
||||
@ -519,6 +521,7 @@
|
||||
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6543305B1219868D0063F913 /* WorldMagModel.c */,
|
||||
654330231218E9780063F913 /* insgps.c */,
|
||||
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
|
||||
65B7E6AE120DF1E2000C1123 /* ahrs.c */,
|
||||
@ -532,6 +535,7 @@
|
||||
65B7E6AF120DF1E2000C1123 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
6543304F121980300063F913 /* insgps.h */,
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
|
||||
65B7E6B3120DF1E2000C1123 /* pios_board.h */,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user