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

INS working on Revo

Browse Source
This commit is contained in:
James Cotton 2012-02-28 02:37:59 -06:00
parent 7e36d086de
commit 9e4651ab80
2 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/Libraries/insgps13state.c
View File

@ -121,7 +121,7 @@ void INSGPSInit() //pretty much just a place holder for now
R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2 R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance
R[9] = .05f; // High freq altimeter noise variance (m^2) R[9] = .005f; // High freq altimeter noise variance (m^2)
} }
void INSResetP(float PDiag[NUMX]) void INSResetP(float PDiag[NUMX])

25
flight/Modules/Attitude/revolution/attitude.c
View File

@ -409,6 +409,7 @@ static int32_t updateAttitudeINSGPS(bool first_run)
BaroAltitudeData baroData; BaroAltitudeData baroData;
GPSPositionData gpsData; GPSPositionData gpsData;
GyrosBiasData gyrosBias; GyrosBiasData gyrosBias;
HomeLocationData home;
static bool mag_updated; static bool mag_updated;
static bool baro_updated; static bool baro_updated;
@ -451,6 +452,7 @@ static int32_t updateAttitudeINSGPS(bool first_run)
MagnetometerGet(&magData); MagnetometerGet(&magData);
BaroAltitudeGet(&baroData); BaroAltitudeGet(&baroData);
GPSPositionGet(&gpsData); GPSPositionGet(&gpsData);
HomeLocationGet(&home);
// Have a minimum requirement for gps usage // Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP >= 3.0f); gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP >= 3.0f);
@ -473,7 +475,8 @@ static int32_t updateAttitudeINSGPS(bool first_run)
// Reset the INS algorithm // Reset the INS algorithm
INSGPSInit(); INSGPSInit();
var[0] = var[1] = var[2] = 5e-3f; var[0] = var[1] = 1e-4f;
var[2] = 1e-1f;
INSSetMagVar(var); INSSetMagVar(var);
var[0] = var[1] = var[2] = 1.5e-5f; var[0] = var[1] = var[2] = 1.5e-5f;
INSSetAccelVar(var); INSSetAccelVar(var);
@ -486,6 +489,10 @@ static int32_t updateAttitudeINSGPS(bool first_run)
rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI; rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI;
rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI; rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI;
RPY2Quaternion(rpy,q); RPY2Quaternion(rpy,q);
/*float Rbe[3][3];
float ge[3] = {0,0,-9.81f};
RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
R2Quaternion(Rbe,q);*/
INSSetState(pos, zeros, q, zeros, zeros); INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag); INSResetP(Pdiag);
@ -504,8 +511,6 @@ static int32_t updateAttitudeINSGPS(bool first_run)
var[0] = var[1] = var[2] = 2.0e-4f; var[0] = var[1] = var[2] = 2.0e-4f;
INSSetGyroVar(var); INSSetGyroVar(var);
HomeLocationData home;
HomeLocationGet(&home);
INSSetMagNorth(home.Be); INSSetMagNorth(home.Be);
GPSPositionData gpsPosition; GPSPositionData gpsPosition;
@ -539,7 +544,7 @@ static int32_t updateAttitudeINSGPS(bool first_run)
} }
init_stage++; init_stage++;
if(init_stage > 500) if(init_stage > 10)
inited = true; inited = true;
ins_last_time = PIOS_DELAY_GetRaw(); ins_last_time = PIOS_DELAY_GetRaw();
@ -588,15 +593,12 @@ static int32_t updateAttitudeINSGPS(bool first_run)
// Advance the covariance estimate // Advance the covariance estimate
INSCovariancePrediction(dT); INSCovariancePrediction(dT);
//if(mag_updated) if(mag_updated)
// sensors |= MAG_SENSORS; sensors |= MAG_SENSORS;
if(baro_updated) if(baro_updated)
sensors |= BARO_SENSOR; sensors |= BARO_SENSOR;
HomeLocationData home;
HomeLocationGet(&home);
INSSetMagNorth(home.Be); INSSetMagNorth(home.Be);
if(gps_updated && outdoor_mode) if(gps_updated && outdoor_mode)
@ -620,12 +622,11 @@ static int32_t updateAttitudeINSGPS(bool first_run)
(float) (home.ECEF[2] / 100.0f)}; (float) (home.ECEF[2] / 100.0f)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED); LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
} else if (!outdoor_mode) { } else if (!outdoor_mode) {
//INSSetPosVelVar(0.1f, 0.1f); INSSetPosVelVar(1e-2f, 1e-2f);
vel[0] = vel[1] = vel[2] = 0; vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0; NED[0] = NED[1] = 0;
NED[2] = baroData.Altitude; NED[2] = baroData.Altitude;
sensors |= HORIZ_POS_SENSORS; sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
//sensors |= HORIZ_SENSORS | VERT_SENSORS | POS_SENSORS;
} }
/* /*