rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Added a testing function for INS - currently doesn't stabilize for fixed inputs. Fixed two bugs I found in INSGPS (R[10] instead of R[9] initialized and Nav.Pos[2] was set to wrong X element).

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1302 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-16 16:58:20 +00:00 committed by peabody124
parent d2e234b53a
commit e0d63103ec
4 changed files with 97 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
flight/AHRS/ahrs.c
View File

@ -190,8 +190,8 @@ uint32_t total_conversion_blocks = 0;
*/
int main()
{
float gyro[3], accel[3], mag[3];
float gyro[3] = {0,0,0}, accel[3] = {0,0,-9.81}, mag[3] = {100,0,0};
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0;
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
@ -218,7 +218,11 @@ int main()
HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL;
HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2;
HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS;
PIOS_HMC5843_Config(&HMC5843_InitStructure);
PIOS_HMC5843_Config(&HMC5843_InitStructure);
// Get 3 ID bytes
strcpy ((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
/* SPI link to master */
PIOS_SPI_Init();
@ -233,15 +237,19 @@ int main()
fir_coeffs[i] = 1;
fir_coeffs[ADC_OVERSAMPLE] = ADC_OVERSAMPLE;
pos[0] = 0;
pos[1] = 0;
pos[2] = 0;
BaroAlt = 0;
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
// Main loop
while (1) {
// Alive signal
PIOS_LED_Toggle(LED1);
// Get 3 ID bytes
strcpy ((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id);
// Get magnetic readings
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
@ -250,7 +258,9 @@ int main()
ahrs_state = AHRS_PROCESSING;
downsample_data();
/******************** INS ALGORITHM *************************/
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
gyro[1] = gyro_data.filtered.y;
gyro[2] = gyro_data.filtered.z;
@ -261,9 +271,26 @@ int main()
mag[1] = mag_data.raw.axis[1];
mag[2] = mag_data.raw.axis[2];
INSPrediction(gyro, accel, 1 / (double) EKF_RATE);
MagCorrection(mag);
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
if ( 0 )
MagCorrection(mag);
else
FullCorrection(mag,pos,vel,BaroAlt);
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;
if(attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360;
/***************** SEND BACK SOME RAW DATA ************************/
// Hacky - grab one sample from buffer to populate this. Need to send back
// all raw data if it's happening
accel_data.raw.x = valid_data_buffer[0];
@ -279,24 +306,27 @@ int main()
ahrs_state = AHRS_IDLE;
/* 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;
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;
/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
if( 0 ) {
/* 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;
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;
}
process_spi_request();
@ -335,7 +365,7 @@ void downsample_data()
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_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.
gyro_raw[0] = 0;
@ -618,7 +648,7 @@ void AHRS_ADC_DMA_Handler(void)
if( DMA_GetFlagStatus( DMA1_IT_TC1 ) ) // whole double buffer filled
valid_data_buffer = &raw_data_buffer[ ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ];
else if ( DMA_GetFlagStatus(DMA1_IT_HT1) )
valid_data_buffer = &raw_data_buffer[0];
valid_data_buffer = &raw_data_buffer[ ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ];
else {
// lets cause a seg fault and catch whatever is going on
valid_data_buffer = 0;

10
flight/AHRS/insgps.c
View File

@ -76,7 +76,7 @@ void INSGPSInit() //pretty much just a place holder for now
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[10]=1; // High freq altimeter noise variance (m^2)
R[9]=1; // High freq altimeter noise variance (m^2)
}
void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
@ -104,7 +104,7 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[3];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
@ -135,7 +135,7 @@ void MagCorrection(float mag_data[3])
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[3];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
@ -179,7 +179,7 @@ void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[3];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];
@ -214,7 +214,7 @@ void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3])
// Update Nav solution structure
Nav.Pos[0] = X[0];
Nav.Pos[1] = X[1];
Nav.Pos[2] = X[3];
Nav.Pos[2] = X[2];
Nav.Vel[0] = X[3];
Nav.Vel[1] = X[4];
Nav.Vel[2] = X[5];

30
flight/AHRS/test.c Normal file
View File

@ -0,0 +1,30 @@
#include "inc/insgps.h"
#include "stdio.h"
#include "math.h"
extern struct NavStruct Nav;
extern float X[13];
int main()
{
float gyro[3]={2.47,-0.25,7.71}, accel[3]={-1.02,0.70,-10.11}, dT=0.04, mags[3]={-50,-180,-376};
float Pos[3]={0,0,0}, Vel[3]={0,0,0}, BaroAlt=2.66, Speed=4.4, Heading=0;
float yaw;
int i,j;
INSGPSInit();
for(i=0;i<10000000;i++){
INSPrediction(gyro, accel, dT);
//MagCorrection(mags);
FullCorrection(mags,Pos,Vel,BaroAlt);
yaw = atan2( (float) 2 * (Nav.q[0] * Nav.q[3] + Nav.q[1] * Nav.q[2]),
(float) (1 - 2 * (Nav.q[2] * Nav.q[2] + Nav.q[3] * Nav.q[3]) ) ) * 180 / M_PI;
printf("%0.3f ", yaw);
for(j=0; j < 13; j++)
printf("%f ", X[j]);
printf("\r\n");
}
return 0;
}

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

@ -20,6 +20,7 @@
651CF9F1120B700D00EEFD70 /* pios_usb_hid_prop.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_usb_hid_prop.h; sourceTree = "<group>"; };
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; };
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; };
@ -518,6 +519,7 @@
65B7E6AC120DF1CD000C1123 /* AHRS */ = {
isa = PBXGroup;
children = (
654330231218E9780063F913 /* insgps.c */,
65B7E6AD120DF1E2000C1123 /* ahrs_fsm.c */,
65B7E6AE120DF1E2000C1123 /* ahrs.c */,
65B7E6AF120DF1E2000C1123 /* inc */,