rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1307 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-17 05:32:07 +00:00 committed by peabody124
parent ae18a8e89c
commit 64bcebb87a
5 changed files with 53 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/AHRS/Makefile
View File

@ -90,6 +90,7 @@ SRC += pios_board.c
SRC += ahrs_fsm.c SRC += ahrs_fsm.c
SRC += insgps.c SRC += insgps.c
SRC += CoordinateConversions.c SRC += CoordinateConversions.c
SRC += WorldMagModel.c
## PIOS Hardware (STM32F10x) ## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c SRC += $(PIOSSTM32F10X)/pios_sys.c

74
flight/AHRS/ahrs.c
View File

@ -37,6 +37,8 @@
#include "pios_opahrs_proto.h" #include "pios_opahrs_proto.h"
#include "ahrs_fsm.h" /* lfsm_state */ #include "ahrs_fsm.h" /* lfsm_state */
#include "insgps.h" #include "insgps.h"
#include "CoordinateConversions.h"
#include "WorldMagModel.h"
/** /**
* State of AHRS EKF * 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 // 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 // otherwise if we reset in air during a snap, this will be all wrong
calibrate_sensors(); calibrate_sensors();
if(ahrs_algorithm == INSGPS_Algo) { if(ahrs_algorithm == INSGPS_Algo) {
INSGPSInit(); INSGPSInit();
INSSetGyroBias(gyro_bias); INSSetGyroBias(gyro_bias);
INSSetAccelVar(accel_var); INSSetAccelVar(accel_var);
INSSetGyroVar(gyro_var); INSSetGyroVar(gyro_var);
// INS algo wants noise on magnetometer in unit length variance // 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]; 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; float scaled_mag_var[3] = {mag_var[0] / mag_length, mag_var[1] / mag_length, mag_var[2] / mag_length};
scaled_mag_var[1] = mag_var[1] / mag_length; INSSetMagVar(scaled_mag_var);
scaled_mag_var[2] = 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 ****************************/ /******************* Main EKF loop ****************************/
while (1) { while (1) {
// Alive signal // Alive signal
@ -314,6 +326,8 @@ int main()
if(ahrs_algorithm == INSGPS_Algo) if(ahrs_algorithm == INSGPS_Algo)
{ {
/******************** INS ALGORITHM **************************/ /******************** INS ALGORITHM **************************/
float rpy[3];
// format data for INS algo // format data for INS algo
gyro[0] = gyro_data.filtered.x; gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y; gyro[1] = gyro_data.filtered.y;
@ -323,46 +337,40 @@ int main()
accel[2] = accel_data.filtered.z, accel[2] = accel_data.filtered.z,
mag[0] = mag_data.raw.axis[0]; mag[0] = mag_data.raw.axis[0];
mag[1] = mag_data.raw.axis[1]; 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); INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
if ( 0 ) if ( 0 )
MagCorrection(mag); MagCorrection(mag);
else else
FullCorrection(mag,pos,vel,BaroAlt); FullCorrection(mag,pos,vel,BaroAlt);
Quaternion2RPY(Nav.q,rpy);
attitude_data.quaternion.q1 = Nav.q[0]; attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1]; attitude_data.quaternion.q2 = Nav.q[1];
attitude_data.quaternion.q3 = Nav.q[2]; attitude_data.quaternion.q3 = Nav.q[2];
attitude_data.quaternion.q4 = Nav.q[3]; 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]), attitude_data.euler.roll = rpy[0];
(double) (1 - 2 * (Nav.q[1] * Nav.q[1] + Nav.q[2] * Nav.q[2])) ) * 180 / M_PI; attitude_data.euler.pitch = rpy[1];
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 = rpy[2];
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;
if(attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360; if(attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360;
} }
else if( ahrs_algorithm == SIMPLE_Algo ) else if( ahrs_algorithm == SIMPLE_Algo )
{ {
float q[4];
float rpy[3];
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
/* Very simple computation of the heading and attitude from 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; rpy[2] = 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; rpy[1] = 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[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; if (attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360.0;
float c1 = cos(attitude_data.euler.yaw/2); RPY2Quaternion(rpy,q);
float s1 = sin(attitude_data.euler.yaw/2); attitude_data.quaternion.q1 = q[0];
float c2 = cos(attitude_data.euler.pitch/2); attitude_data.quaternion.q2 = q[1];
float s2 = sin(attitude_data.euler.pitch/2); attitude_data.quaternion.q3 = q[2];
float c3 = cos(attitude_data.euler.roll/2); attitude_data.quaternion.q4 = q[3];
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;
} }
ahrs_state = AHRS_IDLE; ahrs_state = AHRS_IDLE;
@ -391,37 +399,37 @@ void downsample_data()
// Get the X data. Fifth byte in. Convert to m/s // Get the X data. Fifth byte in. Convert to m/s
accel_raw[0] = 0; accel_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; 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 // Get the Y data. Third byte in. Convert to m/s
accel_raw[1] = 0; accel_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; 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 // Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0; accel_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; 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. // Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0; gyro_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; 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. // Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0; gyro_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; 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. // Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0; gyro_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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; gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * GYRO_SCALE;
} }

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

@ -33,6 +33,7 @@
void INSSetGyroBias(float gyro_bias[3]); void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]); void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]); void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]); void INSSetMagVar(float scaled_mag_var[3]);
void MagCorrection(float mag_data[3]); void MagCorrection(float mag_data[3]);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt); void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);

6
flight/AHRS/insgps.c
View File

@ -107,6 +107,12 @@ void INSSetMagVar(float scaled_mag_var[3])
R[8] = scaled_mag_var[2]; 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) void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
{ {

4
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -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; }; 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; }; 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; }; 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; }; 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; }; 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>"; }; 65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
@ -521,6 +523,7 @@
65B7E6AC120DF1CD000C1123 /* AHRS */ = { 65B7E6AC120DF1CD000C1123 /* AHRS */ = {
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
65B35CFA121A4540003EAD18 /* CoordinateConversions.c */,
6543305B1219868D0063F913 /* WorldMagModel.c */, 6543305B1219868D0063F913 /* WorldMagModel.c */,
654330231218E9780063F913 /* insgps.c */, 654330231218E9780063F913 /* insgps.c */,
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */, 65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
@ -535,6 +538,7 @@
65B7E6AF120DF1E2000C1123 /* inc */ = { 65B7E6AF120DF1E2000C1123 /* inc */ = {
isa = PBXGroup; isa = PBXGroup;
children = ( children = (
65B35CFB121A45C6003EAD18 /* CoordinateConversions.h */,
6543304F121980300063F913 /* insgps.h */, 6543304F121980300063F913 /* insgps.h */,
65B7E6B0120DF1E2000C1123 /* ahrs.h */, 65B7E6B0120DF1E2000C1123 /* ahrs.h */,
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */, 65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,