rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

AHRS: In indoor mode correct with a high variance 0 for position to keep it

Browse Source 
bounded.  Velocity variance still comes from AHRSCalibration.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2186 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-12-04 17:34:18 +00:00 committed by peabody124
parent a04e33ccc6
commit 0679e8d472
1 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/AHRS/ahrs.c
View File

@ -167,6 +167,11 @@ void ins_outdoor_update()
sensors |= HORIZ_SENSORS | POS_SENSORS; sensors |= HORIZ_SENSORS | POS_SENSORS;
} }
/* When doing outdoor update grab this variance */
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
INSSetPosVelVar(cal.pos_var, cal.vel_var);
/* /*
* When using gps need to make sure that barometer is brought into NED frame * When using gps need to make sure that barometer is brought into NED frame
* we should try and see if the altitude from the home location is good enough * we should try and see if the altitude from the home location is good enough
@ -252,7 +257,11 @@ void ins_indoor_update()
if(indoor_delay > INSGPS_GPS_TIMEOUT) if(indoor_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(vel,vel); INSPosVelReset(vel,vel);
else else
sensors = HORIZ_SENSORS | VERT_SENSORS; sensors = HORIZ_SENSORS | VERT_SENSORS | POS_SENSORS;
AHRSCalibrationData cal;
AHRSCalibrationGet(&cal);
INSSetPosVelVar(10, cal.vel_var);
if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { if(mag_data.updated && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
sensors |= MAG_SENSORS; sensors |= MAG_SENSORS;
@ -268,7 +277,7 @@ void ins_indoor_update()
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself * although probably should occur within INS itself
*/ */
INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS); INSCorrection(mag_data.scaled.axis, vel, vel, altitude_data.altitude, sensors | HORIZ_SENSORS | VERT_SENSORS);
} }
/** /**
@ -894,6 +903,7 @@ void calibration_callback(AhrsObjHandle obj)
{ {
AHRSCalibrationData cal; AHRSCalibrationData cal;
AHRSCalibrationGet(&cal); AHRSCalibrationGet(&cal);
if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){ if(cal.measure_var == AHRSCALIBRATION_MEASURE_VAR_SET){
for(int ct=0; ct<3; ct++) for(int ct=0; ct<3; ct++)
{ {