void loop()
{
if(started)
{
check_gps();
Check_SMS();
}
}
// update - updates velocities and positions using latest info from ahrs and barometer if new data is available;
void AP_InertialNav::update(float dt)
{
// discard samples where dt is too large
if( dt > 0.1f ) {
return;
}
// decrement ignore error count if required
if (_flags.ignore_error > 0) {
_flags.ignore_error--;
}
// check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
check_baro();
// check if new gps readings have arrived and use them to correct position estimates
check_gps();
Vector3f accel_ef = _ahrs.get_accel_ef();
// remove influence of gravity
accel_ef.z += GRAVITY_MSS;
accel_ef *= 100;
// remove xy if not enabled
if( !_xy_enabled ) {
accel_ef.x = 0;
accel_ef.y = 0;
}
//Convert North-East-Down to North-East-Up
accel_ef.z = -accel_ef.z;
float tmp = _k3_xy * dt;
accel_correction_ef.x += _position_error.x * tmp;
accel_correction_ef.y += _position_error.y * tmp;
accel_correction_ef.z += _position_error.z * _k3_z * dt;
tmp = _k2_xy * dt;
_velocity.x += _position_error.x * tmp;
_velocity.y += _position_error.y * tmp;
_velocity.z += _position_error.z * _k2_z * dt;
tmp = _k1_xy * dt;
_position_correction.x += _position_error.x * tmp;
_position_correction.y += _position_error.y * tmp;
_position_correction.z += _position_error.z * _k1_z * dt;
// calculate velocity increase adding new acceleration from accelerometers
const Vector3f &velocity_increase = (accel_ef + accel_correction_ef) * dt;
// calculate new estimate of position
_position_base += (_velocity + velocity_increase*0.5) * dt;
// update the corrected position estimate
_position = _position_base + _position_correction;
// calculate new velocity
_velocity += velocity_increase;
// store 3rd order estimate (i.e. estimated vertical position) for future use
_hist_position_estimate_z.push_back(_position_base.z);
// store 3rd order estimate (i.e. horizontal position) for future use at 10hz
_historic_xy_counter++;
if( _historic_xy_counter >= AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS ) {
_historic_xy_counter = 0;
_hist_position_estimate_x.push_back(_position_base.x);
_hist_position_estimate_y.push_back(_position_base.y);
}
}
