// correct_with_baro - modifies accelerometer offsets using barometer. dt is time since last baro reading
void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
{
static uint8_t first_reads = 0;
// discard samples where dt is too large
if( dt > 0.5f ) {
return;
}
// discard first 10 reads but perform some initialisation
if( first_reads <= 10 ) {
set_altitude(baro_alt);
first_reads++;
}
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if( _hist_position_estimate_z.is_full() ) {
hist_position_base_z = _hist_position_estimate_z.front();
}else{
hist_position_base_z = _position_base.z;
}
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
}
//-----------------------------------------------------------------------------
void Heightfield::set_altitudes(float altitude)
{
for (uint32_t w = 0; w < m_width; w++)
{
for (uint32_t h = 0; h < m_height; h++)
{
set_altitude(w, h, altitude);
}
}
}
// correct_with_baro - modifies accelerometer offsets using barometer. dt is time since last baro reading
void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
{
static uint8_t first_reads = 0;
// discard samples where dt is too large
if( dt > 0.5f ) {
return;
}
// discard first 10 reads but perform some initialisation
if( first_reads <= 10 ) {
set_altitude(baro_alt);
first_reads++;
}
// sanity check the baro position. Relies on the main code calling Baro_Glitch::check_alt() immediatley after a baro update
if (_baro_glitch.glitching()) {
// failed sanity check so degrate position_error to 10% over 2 seconds (assumes 10hz update rate)
_position_error.z *= 0.89715f;
}else{
// if our internal baro glitching flag (from previous iteration) is true we have just recovered from a glitch
// reset the inertial nav alt to baro alt
if (_flags.baro_glitching) {
set_altitude(baro_alt);
_position_error.z = 0.0f;
}else{
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if (_hist_position_estimate_z.is_full()) {
hist_position_base_z = _hist_position_estimate_z.front();
} else {
hist_position_base_z = _position_base.z;
}
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
}
}
// update our internal record of glitching flag so that we can notice a change
_flags.baro_glitching = _baro_glitch.glitching();
}
