mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
AHRS: Fixed bug in the downsampling code (used a += a + blah, dumb). Also added initialization of the magnetic flux based on fixed GPS settings (I hope you're near Houston). I will add the GPS communication shortly.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1307 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
ae18a8e89c
commit
64bcebb87a
@ -90,6 +90,7 @@ SRC += pios_board.c
|
||||
SRC += ahrs_fsm.c
|
||||
SRC += insgps.c
|
||||
SRC += CoordinateConversions.c
|
||||
SRC += WorldMagModel.c
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
|
||||
@ -37,6 +37,8 @@
|
||||
#include "pios_opahrs_proto.h"
|
||||
#include "ahrs_fsm.h" /* lfsm_state */
|
||||
#include "insgps.h"
|
||||
#include "CoordinateConversions.h"
|
||||
#include "WorldMagModel.h"
|
||||
|
||||
/**
|
||||
* State of AHRS EKF
|
||||
@ -269,20 +271,30 @@ int main()
|
||||
// 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();
|
||||
|
||||
if(ahrs_algorithm == INSGPS_Algo) {
|
||||
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;
|
||||
float scaled_mag_var[3] = {mag_var[0] / mag_length, mag_var[1] / mag_length, mag_var[2] / mag_length};
|
||||
INSSetMagVar(scaled_mag_var);
|
||||
}
|
||||
|
||||
/******************* World magnetic model *********************/
|
||||
float MagNorth[3];
|
||||
WMM_Initialize(); // Set default values and constants
|
||||
WMM_GetMagVector(29, -95, 18, 8, 17, 2010, MagNorth);
|
||||
// TODO: Get this from first GPS coordinate or whenever we initialize NED frame
|
||||
if(ahrs_algorithm == INSGPS_Algo)
|
||||
{
|
||||
float MagNorthLen = sqrt(MagNorth[0] * MagNorth[0] + MagNorth[1] * MagNorth[1] + MagNorth[2] * MagNorth[2]);
|
||||
float MagNorthScaled[3] = {MagNorth[0] / MagNorthLen, MagNorth[1] / MagNorthLen, MagNorth[2] / MagNorthLen};
|
||||
INSSetMagNorth(MagNorthScaled);
|
||||
}
|
||||
|
||||
/******************* Main EKF loop ****************************/
|
||||
while (1) {
|
||||
// Alive signal
|
||||
@ -314,6 +326,8 @@ int main()
|
||||
if(ahrs_algorithm == INSGPS_Algo)
|
||||
{
|
||||
/******************** INS ALGORITHM **************************/
|
||||
float rpy[3];
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
gyro[1] = gyro_data.filtered.y;
|
||||
@ -323,7 +337,7 @@ int main()
|
||||
accel[2] = accel_data.filtered.z,
|
||||
mag[0] = mag_data.raw.axis[0];
|
||||
mag[1] = mag_data.raw.axis[1];
|
||||
mag[2] = mag_data.raw.axis[2];
|
||||
mag[2] = -mag_data.raw.axis[2];
|
||||
|
||||
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
|
||||
if ( 0 )
|
||||
@ -331,38 +345,32 @@ int main()
|
||||
else
|
||||
FullCorrection(mag,pos,vel,BaroAlt);
|
||||
|
||||
Quaternion2RPY(Nav.q,rpy);
|
||||
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];
|
||||
attitude_data.euler.roll = atan2( (double) 2 * (Nav.q[0] * Nav.q[1] + Nav.q[2] * Nav.q[3]),
|
||||
(double) (1 - 2 * (Nav.q[1] * Nav.q[1] + Nav.q[2] * Nav.q[2])) ) * 180 / M_PI;
|
||||
attitude_data.euler.pitch = asin( (double) 2 * (Nav.q[0] * Nav.q[2] - Nav.q[3] * Nav.q[1] ) ) * 180 / M_PI;
|
||||
attitude_data.euler.yaw = atan2( (double) 2 * (Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
|
||||
(double) (1 - 2 * (Nav.q[2] * Nav.q[2] + Nav.q[3] * Nav.q[3]) ) ) * 180 / M_PI;
|
||||
attitude_data.euler.roll = rpy[0];
|
||||
attitude_data.euler.pitch = rpy[1];
|
||||
attitude_data.euler.yaw = rpy[2];
|
||||
if(attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360;
|
||||
}
|
||||
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. */
|
||||
attitude_data.euler.yaw = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI;
|
||||
attitude_data.euler.pitch = atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
|
||||
attitude_data.euler.roll = -atan2(accel_data.filtered.x,accel_data.filtered.z) * 180 / M_PI;
|
||||
rpy[2] = attitude_data.euler.yaw = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI;
|
||||
rpy[1] = attitude_data.euler.pitch = atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI;
|
||||
rpy[0] = attitude_data.euler.roll = -atan2(accel_data.filtered.x,accel_data.filtered.z) * 180 / M_PI;
|
||||
if (attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360.0;
|
||||
|
||||
float c1 = cos(attitude_data.euler.yaw/2);
|
||||
float s1 = sin(attitude_data.euler.yaw/2);
|
||||
float c2 = cos(attitude_data.euler.pitch/2);
|
||||
float s2 = sin(attitude_data.euler.pitch/2);
|
||||
float c3 = cos(attitude_data.euler.roll/2);
|
||||
float s3 = sin(attitude_data.euler.roll/2);
|
||||
float c1c2 = c1*c2;
|
||||
float s1s2 = s1*s2;
|
||||
attitude_data.quaternion.q1 = c1c2*c3 - s1s2*s3;
|
||||
attitude_data.quaternion.q2 = c1c2*s3 + s1s2*c3;
|
||||
attitude_data.quaternion.q3 = s1*c2*c3 + c1*s2*s3;
|
||||
attitude_data.quaternion.q4 =c1*s2*c3 - s1*c2*s3;
|
||||
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];
|
||||
}
|
||||
|
||||
ahrs_state = AHRS_IDLE;
|
||||
@ -391,37 +399,37 @@ void downsample_data()
|
||||
// Get the X data. Fifth byte in. Convert to m/s
|
||||
accel_raw[0] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
accel_raw[0] = accel_raw[0] + ( valid_data_buffer[0 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_raw[0] += ( valid_data_buffer[0 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_data.filtered.x = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
|
||||
|
||||
// Get the Y data. Third byte in. Convert to m/s
|
||||
accel_raw[1] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
accel_raw[1] = accel_raw[1] + ( valid_data_buffer[2 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_raw[1] += ( valid_data_buffer[2 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_data.filtered.y = (float) accel_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
|
||||
|
||||
// Get the Z data. Third byte in. Convert to m/s
|
||||
accel_raw[2] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
accel_raw[2] = accel_raw[2] + ( valid_data_buffer[4 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_raw[2] += ( valid_data_buffer[4 + (i-1) * ADC_CONTINUOUS_CHANNELS] + ACCEL_OFFSET ) * fir_coeffs[i];
|
||||
accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * ACCEL_SCALE;
|
||||
|
||||
// Get the X gyro data. Seventh byte in. Convert to deg/s.
|
||||
gyro_raw[0] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
gyro_raw[0] += gyro_raw[0] + ( valid_data_buffer[1 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_raw[0] += ( valid_data_buffer[1 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
|
||||
|
||||
// Get the Y gyro data. Second byte in. Convert to deg/s.
|
||||
gyro_raw[1] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
gyro_raw[1] += gyro_raw[1] + ( valid_data_buffer[3 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_raw[1] += ( valid_data_buffer[3 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
|
||||
|
||||
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
|
||||
gyro_raw[2] = 0;
|
||||
for( i = 0; i < ADC_OVERSAMPLE; i++ )
|
||||
gyro_raw[2] += gyro_raw[2] + ( valid_data_buffer[5 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_raw[2] += ( valid_data_buffer[5 + (i-1) * ADC_CONTINUOUS_CHANNELS] + GYRO_OFFSET ) * fir_coeffs[i];
|
||||
gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
|
||||
}
|
||||
|
||||
|
||||
@ -33,6 +33,7 @@
|
||||
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 FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
|
||||
|
||||
@ -107,6 +107,12 @@ void INSSetMagVar(float scaled_mag_var[3])
|
||||
R[8] = scaled_mag_var[2];
|
||||
}
|
||||
|
||||
void INSSetMagNorth(float B[3])
|
||||
{
|
||||
Be[0] = B[0];
|
||||
Be[1] = B[1];
|
||||
Be[2] = B[2];
|
||||
}
|
||||
|
||||
void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
|
||||
{
|
||||
|
||||
@ -60,6 +60,8 @@
|
||||
65A2C81B11E2A33D00D0391E /* pios_sdcard.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_sdcard.c; path = ../../PiOS.posix/posix/pios_sdcard.c; sourceTree = SOURCE_ROOT; };
|
||||
65A2C81C11E2A33D00D0391E /* pios_sys.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_sys.c; path = ../../PiOS.posix/posix/pios_sys.c; sourceTree = SOURCE_ROOT; };
|
||||
65A2C81D11E2A33D00D0391E /* pios_udp.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_udp.c; path = ../../PiOS.posix/posix/pios_udp.c; sourceTree = SOURCE_ROOT; };
|
||||
65B35CFA121A4540003EAD18 /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = CoordinateConversions.c; path = ../../AHRS/CoordinateConversions.c; sourceTree = SOURCE_ROOT; };
|
||||
65B35CFB121A45C6003EAD18 /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = "<group>"; };
|
||||
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs_fsm.c; path = ../../AHRS/ahrs_fsm.c; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
|
||||
@ -521,6 +523,7 @@
|
||||
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65B35CFA121A4540003EAD18 /* CoordinateConversions.c */,
|
||||
6543305B1219868D0063F913 /* WorldMagModel.c */,
|
||||
654330231218E9780063F913 /* insgps.c */,
|
||||
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
|
||||
@ -535,6 +538,7 @@
|
||||
65B7E6AF120DF1E2000C1123 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65B35CFB121A45C6003EAD18 /* CoordinateConversions.h */,
|
||||
6543304F121980300063F913 /* insgps.h */,
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user