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

AP_InertialSensor: limit delta-time from INS 

limit delta-time to 2x average time

DEBUG CODE - DO NOT MERGE
This commit is contained in:
Andrew Tridgell 2017-12-04 16:33:36 +11:00
parent a0af4af5b5
commit 39c206ca03
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -628,6 +628,11 @@ AP_InertialSensor::init(uint16_t sample_rate)
_sample_rate = sample_rate;
_loop_delta_t = 1.0f / sample_rate;
// we don't allow deltat values greater than 10x the normal loop
// time to be exposed outside of INS. Large deltat values can
// cause divergence of state estimators
_loop_delta_t_max = 10 * _loop_delta_t;
if (_gyro_count == 0 && _accel_count == 0) {
_start_backends();
}
@ -1461,10 +1466,14 @@ bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity)
*/
float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
{
float ret;
if (_delta_velocity_valid[i]) {
return _delta_velocity_dt[i];
ret = _delta_velocity_dt[i];
} else {
ret = get_delta_time();
}
return get_delta_time();
ret = MIN(ret, _loop_delta_t_max);
return ret;
}
/*
@ -1472,10 +1481,14 @@ float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
*/
float AP_InertialSensor::get_delta_angle_dt(uint8_t i) const
{
float ret;
if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
return _delta_angle_dt[i];
ret = _delta_angle_dt[i];
} else {
ret = get_delta_time();
}
return get_delta_time();
ret = MIN(ret, _loop_delta_t_max);
return ret;
}

3
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -173,7 +173,7 @@ public:
/* get_delta_time returns the time period in seconds
* overwhich the sensor data was collected
*/
float get_delta_time() const { return _delta_time; }
float get_delta_time() const { return MIN(_delta_time, _loop_delta_t_max); }
// return the maximum gyro drift rate in radians/s/s. This
// depends on what gyro chips are being used
@ -378,6 +378,7 @@ private:
// the selected sample rate
uint16_t _sample_rate;
float _loop_delta_t;
float _loop_delta_t_max;
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES];