From 64bcebb87a1411acde0d25de595f75e9ca052bdb Mon Sep 17 00:00:00 2001 From: peabody124 Date: Tue, 17 Aug 2010 05:32:07 +0000 Subject: [PATCH] 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 --- flight/AHRS/Makefile | 1 + flight/AHRS/ahrs.c | 74 ++++++++++--------- flight/AHRS/inc/insgps.h | 1 + flight/AHRS/insgps.c | 6 ++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 4 + 5 files changed, 53 insertions(+), 33 deletions(-) diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile index ef1c37070..2b20c56fe 100644 --- a/flight/AHRS/Makefile +++ b/flight/AHRS/Makefile @@ -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 diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 51cd9d485..bebba6d44 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.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; - INSSetMagVar(scaled_mag_var); + 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,46 +337,40 @@ 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 ) MagCorrection(mag); 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.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; } diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 49b9b5ce4..71a936c58 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -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); diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 5b40091e0..0e33a1895 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -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) { diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index b5db430b7..90be51ad3 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -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 = ""; }; 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 = ""; }; @@ -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 */,