Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: added constraint to how large local accelerometer offset corrections can become

This commit is contained in:
Randy Mackay 2013-01-10 16:56:21 +09:00 committed by Andrew Tridgell
parent 21b6c78d12
commit 109b1069d8
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -53,9 +53,6 @@ void AP_InertialNav::init()
void AP_InertialNav::save_params()
{
Vector3f accel_corr = accel_correction.get();
accel_corr.x = constrain(accel_corr.x,-100,100);
accel_corr.y = constrain(accel_corr.y,-100,100);
accel_corr.z = constrain(accel_corr.z,-100,100);
accel_correction.set_and_save(accel_corr);
}
@ -385,6 +382,13 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
accel_corr.x += accel_ef_z_correction * dcm.c.x;
accel_corr.y += accel_ef_z_correction * dcm.c.y;
accel_corr.z += accel_ef_z_correction * dcm.c.z;
// ensure corrections are reasonable
accel_corr.x = constrain(accel_corr.x,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
accel_corr.y = constrain(accel_corr.y,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
accel_corr.z = constrain(accel_corr.z,-AP_INTERTIALNAV_ACCEL_CORR_MAX,AP_INTERTIALNAV_ACCEL_CORR_MAX);
// set the parameter to include the corrections
accel_correction.set(accel_corr);
// correct velocity

2
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -12,6 +12,8 @@
#define AP_INTERTIALNAV_TC_XY 3.0 // default time constant for complementary filter's X & Y axis
#define AP_INTERTIALNAV_TC_Z 7.0 // default time constant for complementary filter's Z axis
#define AP_INTERTIALNAV_ACCEL_CORR_MAX 100.0 // max allowed accelerometer offset correction
// #defines to control how often historical accel based positions are saved
// so they can later be compared to laggy gps readings
#define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10