Search in topic

Автопилот Arduplane - открытое ПО

AP_AHRS_DCM::drift_correction(float deltat)
{
Matrix3f temp_dcm = _dcm_matrix;
Vector3f velocity;
uint32_t last_correction_time;

// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();

// apply trim
temp_dcm.rotateXY(_trim);

// rotate accelerometer values into the earth frame
_accel_ef = temp_dcm * _ins.get_accel();

// integrate the accel vector in the earth frame between GPS readings
_ra_sum += _accel_ef * deltat;

// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;

если нет жпс, или 3д фикса или спутников меньше чем минимум для коррекции (указывается в списке параметров)
if (!have_gps() ||
_gps->status() < GPS::GPS_OK_FIX_3D ||
_gps->num_sats < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
//
// As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2f) {
// not enough time has accumulated
return;
}
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;

// add in wind estimate
velocity += _wind;

last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
} else {

if (_gps->last_fix_time == _ra_sum_start) {
// we don’t have a new GPS fix - nothing more to do
return;
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down());
last_correction_time = _gps->last_fix_time;
if (_have_gps_lock == false) {
// if we didn’t have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;

// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}

if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps->latitude;
_last_lng = _gps->longitude;
_position_offset_north = 0;
_position_offset_east = 0;

// once we have a single GPS lock, we can update using
// dead-reckoning from then on
_have_position = true;
} else {
// update dead-reckoning position estimate
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
}

// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}

// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);

bool using_gps_corrections = false;
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
}
using_gps_corrections = true;
}

// calculate the error term in earth frame.
_ra_sum /= (_ra_deltat * GRAVITY_MSS);

// get the delayed ra_sum to match the GPS lag
Vector3f GA_b;
if (using_gps_corrections) {
GA_b = ra_delayed(_ra_sum);
} else {
GA_b = _ra_sum;
}
GA_b.normalize();
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}

Vector3f error = GA_b % GA_e;

#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;

// equation 10
float tilt = pythagorous2(GA_e.x, GA_e.y);

// equation 11
float theta = atan2f(GA_b.y, GA_b.x);

// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);

// step 6
error = GA_b % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION

// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (use_compass()) {
if (have_gps() && gps_gain == 1.0f) {
error.z *= sinf(fabsf(roll));
} else {
error.z = 0;
}
}

// if ins is unhealthy then stop attitude drift correction and
// hope the gyros are OK for a while. Just slowly reduce _omega_P
// to prevent previous bad accels from throwing us off
if (!_ins.healthy()) {
error.zero();
} else {
// convert the error term to body frame
error = _dcm_matrix.mul_transpose(error);
}

if (error.is_nan() || error.is_inf()) {
// don’t allow bad values
check_matrix();
return;
}

_error_rp_sum += error.length();
_error_rp_count++;

// base the P gain on the spin rate
float spin_rate = _omega.length();

// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp;
if (_flags.fast_ground_gains) {
_omega_P *= 8;
}

if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
_gps->ground_speed_cm < GPS_SPEED_MIN &&
_ins.get_accel().x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
_omega_P *= 0.5f;
}

// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}

if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don’t cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}

// zero our accumulator ready for the next GPS step
_ra_sum.zero();
_ra_deltat = 0;
_ra_sum_start = last_correction_time;

// remember the velocity for next time
_last_velocity = velocity;

if (_have_gps_lock && _flags.fly_forward) {
// update wind estimate
estimate_wind(velocity);
}
}