示例#1
0
// return true if we should use the compass for yaw correction
bool AP_AHRS_DCM::use_compass(void)
{
    if (!_compass || !_compass->use_for_yaw()) {
        // no compass available
        return false;
    }
    if (!_flags.fly_forward || !have_gps()) {
        // we don't have any alterative to the compass
        return true;
    }
    if (_gps.ground_speed() < GPS_SPEED_MIN) {
        // we are not going fast enough to use the GPS
        return true;
    }

    // if the current yaw differs from the GPS yaw by more than 45
    // degrees and the estimated wind speed is less than 80% of the
    // ground speed, then switch to GPS navigation. This will help
    // prevent flyaways with very bad compass offsets
    int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
    if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
        if (AP_HAL::millis() - _last_consistent_heading > 2000) {
            // start using the GPS for heading if the compass has been
            // inconsistent with the GPS for 2 seconds
            return false;
        }
    } else {
        _last_consistent_heading = AP_HAL::millis();
    }

    // use the compass
    return true;
}
示例#2
0
// return an airspeed estimate if available
bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
{
    bool ret = false;
    if (airspeed_sensor_enabled()) {
        *airspeed_ret = _airspeed->get_airspeed();
        return true;
    }

    if (!_flags.wind_estimation) {
        return false;
    }

    // estimate it via GPS speed and wind
    if (have_gps()) {
        *airspeed_ret = _last_airspeed;
        ret = true;
    }

    if (ret && _wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
        // constrain the airspeed by the ground speed
        // and AHRS_WIND_MAX
        float gnd_speed = _gps.ground_speed();
        float true_airspeed = *airspeed_ret * get_EAS2TAS();
        true_airspeed = constrain_float(true_airspeed,
                                        gnd_speed - _wind_max,
                                        gnd_speed + _wind_max);
        *airspeed_ret = true_airspeed / get_EAS2TAS();
    }
    return ret;
}
示例#3
0
// return an airspeed estimate if available
bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
{
	bool ret = false;
	if (_airspeed && _airspeed->use()) {
		*airspeed_ret = _airspeed->get_airspeed();
		ret = true;
	}

	// estimate it via GPS speed and wind
	if (have_gps()) {
		*airspeed_ret = _last_airspeed;
		ret = true;
	}

	if (ret && _wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) {
		// constrain the airspeed by the ground speed
		// and AHRS_WIND_MAX
		*airspeed_ret = constrain(*airspeed_ret, 
					  _gps->ground_speed*0.01 - _wind_max, 
					  _gps->ground_speed*0.01 + _wind_max);
	}
	return ret;
}
示例#4
0
// return true if we should use the compass for yaw correction
// (超訳) YAW補正にコンパスを使うべきかを返す		// ★チェックOK
bool BC_AHRS::use_compass(void)
{
	if (_compass.use_for_yaw()) {
		// no compass available
		return false;
	}
	if (!have_gps()) {
		// we don't have any alterative to the compass
		return true;
	}
	if (_gps.ground_speed() < GPS_SPEED_MIN) {
		// we are not going fast enough to use the GPS
		return true;
	}
	
	// if the current yaw differs from the GPS yaw by more than 45
	// degrees and the estimated wind speed is less than 80% of the
	// ground speed, then switch to GPS navigation. This will help
	// prevent flyaways with very bad compass offsets
	// (超訳) 現在のYAWとGPSのYAWが45度以上離れていてかつ推定風速が
	//        地上速の80%以下だったらGPSナビゲーションにシフトする
	//        これは異常なコンパスオフセットによりどこかにぶっとんで
	//        いかないようにするため
	int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
	if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
		if (millis() - _last_consistent_heading > 2000) {
			// start using the GPS for heading if the compass has been
			// inconsistent with the GPS for 2 seconds
			return false;
		}
	} else {
		_last_consistent_heading = millis();
	}
	
	// use the compass
	return true;
}
示例#5
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    Vector3f velocity;
    uint32_t last_correction_time;

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

    // rotate accelerometer values into the earth frame
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (_ins.get_accel_health(i)) {
            /*
              by using get_delta_velocity() instead of get_accel() the
              accel value is sampled over the right time delta for
              each sensor, which prevents an aliasing effect
             */
            Vector3f delta_velocity;
            float delta_velocity_dt;
            _ins.get_delta_velocity(i, delta_velocity);
            delta_velocity_dt = _ins.get_delta_velocity_dt(i);
            if (delta_velocity_dt > 0) {
                _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
                // integrate the accel vector in the earth frame between GPS readings
                _ra_sum[i] += _accel_ef[i] * deltat;
            }
        }
    }

    //update _accel_ef_blended
    if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
        float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
        // slew _imu1_weight over one second
        _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
        _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
    } else {
        _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
    }

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

    if (!have_gps() ||
            _gps.status() < AP_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_sensor_enabled()) {
            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 = AP_HAL::millis();
        _have_gps_lock = false;
    } else {
        if (_gps.last_fix_time_ms() == _ra_sum_start) {
            // we don't have a new GPS fix - nothing more to do
            return;
        }
        velocity = _gps.velocity();
        last_correction_time = _gps.last_fix_time_ms();
        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.location().lat;
        _last_lng = _gps.location().lng;
        _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);

    if (_ra_deltat <= 0) {
        // waiting for more data
        return;
    }
    
    bool using_gps_corrections = false;
    float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);

    if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
        float v_scale = gps_gain.get() * ra_scale;
        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
            _last_failure_ms = AP_HAL::millis();
            return;
        }
        using_gps_corrections = true;
    }

    // calculate the error term in earth frame.
    // we do this for each available accelerometer then pick the
    // accelerometer that leads to the smallest error term. This takes
    // advantage of the different sample rates on different
    // accelerometers to dramatically reduce the impact of aliasing
    // due to harmonics of vibrations that match closely the sampling
    // rate of our accelerometers. On the Pixhawk we have the LSM303D
    // running at 800Hz and the MPU6000 running at 1kHz, by combining
    // the two the effects of aliasing are greatly reduced.
    Vector3f error[INS_MAX_INSTANCES];
    float error_dirn[INS_MAX_INSTANCES];
    Vector3f GA_b[INS_MAX_INSTANCES];
    int8_t besti = -1;
    float best_error = 0;
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (!_ins.get_accel_health(i)) {
            // only use healthy sensors
            continue;
        }
        _ra_sum[i] *= ra_scale;

        // get the delayed ra_sum to match the GPS lag
        if (using_gps_corrections) {
            GA_b[i] = ra_delayed(i, _ra_sum[i]);
        } else {
            GA_b[i] = _ra_sum[i];
        }
        if (GA_b[i].is_zero()) {
            // wait for some non-zero acceleration information
            continue;
        }
        GA_b[i].normalize();
        if (GA_b[i].is_inf()) {
            // wait for some non-zero acceleration information
            continue;
        }
        error[i] = GA_b[i] % GA_e;
        // Take dot product to catch case vectors are opposite sign and parallel
        error_dirn[i] = GA_b[i] * GA_e;
        float error_length = error[i].length();
        if (besti == -1 || error_length < best_error) {
            besti = i;
            best_error = error_length;
        }
        // Catch case where orientation is 180 degrees out
        if (error_dirn[besti] < 0.0f) {
            best_error = 1.0f;
        }

    }

    if (besti == -1) {
        // no healthy accelerometers!
        _last_failure_ms = AP_HAL::millis();
        return;
    }

    _active_accel_instance = besti;

#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 = norm(GA_e.x, GA_e.y);

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

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

    // step 6
    error = GA_b[besti] % 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 (AP_AHRS_DCM::use_compass()) {
        if (have_gps() && is_equal(gps_gain.get(), 1.0f)) {
            error[besti].z *= sinf(fabsf(roll));
        } else {
            error[besti].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[besti].zero();
    } else {
        // convert the error term to body frame
        error[besti] = _dcm_matrix.mul_transpose(error[besti]);
    }

    if (error[besti].is_nan() || error[besti].is_inf()) {
        // don't allow bad values
        check_matrix();
        _last_failure_ms = AP_HAL::millis();
        return;
    }

    _error_rp = 0.8f * _error_rp + 0.2f * best_error;

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

    // sanity check _kp value
    if (_kp < AP_AHRS_RP_P_MIN) {
        _kp = AP_AHRS_RP_P_MIN;
    }

    // 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[besti] * _P_gain(spin_rate) * _kp;
    if (use_fast_gains()) {
        _omega_P *= 8;
    }

    if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
            _gps.ground_speed() < 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[besti] * _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
    memset(&_ra_sum[0], 0, sizeof(_ra_sum));
    _ra_deltat = 0;
    _ra_sum_start = last_correction_time;

    // remember the velocity for next time
    _last_velocity = velocity;
}
示例#6
0
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
    bool new_value = false;
    float yaw_error;
    float yaw_deltat;

    if (AP_AHRS_DCM::use_compass()) {
        /*
          we are using compass for yaw
         */
        if (_compass->last_update_usec() != _compass_last_update) {
            yaw_deltat = (_compass->last_update_usec() - _compass_last_update) * 1.0e-6f;
            _compass_last_update = _compass->last_update_usec();
            // we force an additional compass read()
            // here. This has the effect of throwing away
            // the first compass value, which can be bad
            if (!_flags.have_initial_yaw && _compass->read()) {
                float heading = _compass->calculate_heading(_dcm_matrix);
                _dcm_matrix.from_euler(roll, pitch, heading);
                _omega_yaw_P.zero();
                _flags.have_initial_yaw = true;
            }
            new_value = true;
            yaw_error = yaw_error_compass();

            // also update the _gps_last_update, so if we later
            // disable the compass due to significant yaw error we
            // don't suddenly change yaw with a reset
            _gps_last_update = _gps.last_fix_time_ms();
        }
    } else if (_flags.fly_forward && have_gps()) {
        /*
          we are using GPS for yaw
         */
        if (_gps.last_fix_time_ms() != _gps_last_update &&
                _gps.ground_speed() >= GPS_SPEED_MIN) {
            yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
            _gps_last_update = _gps.last_fix_time_ms();
            new_value = true;
            float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
            float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
            yaw_error = sinf(yaw_error_rad);

            /* reset yaw to match GPS heading under any of the
               following 3 conditions:

               1) if we have reached GPS_SPEED_MIN and have never had
               yaw information before

               2) if the last time we got yaw information from the GPS
               is more than 20 seconds ago, which means we may have
               suffered from considerable gyro drift

               3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
               and our yaw error is over 60 degrees, which means very
               poor yaw. This can happen on bungee launch when the
               operator pulls back the plane rapidly enough then on
               release the GPS heading changes very rapidly
            */
            if (!_flags.have_initial_yaw ||
                    yaw_deltat > 20 ||
                    (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
                // reset DCM matrix based on current yaw
                _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
                _omega_yaw_P.zero();
                _flags.have_initial_yaw = true;
                yaw_error = 0;
            }
        }
    }

    if (!new_value) {
        // we don't have any new yaw information
        // slowly decay _omega_yaw_P to cope with loss
        // of our yaw source
        _omega_yaw_P *= 0.97f;
        return;
    }

    // convert the error vector to body frame
    float error_z = _dcm_matrix.c.z * yaw_error;

    // the spin rate changes the P gain, and disables the
    // integration at higher rates
    float spin_rate = _omega.length();

    // sanity check _kp_yaw
    if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
        _kp_yaw = AP_AHRS_YAW_P_MIN;
    }

    // update the proportional control to drag the
    // yaw back to the right value. We use a gain
    // that depends on the spin rate. See the fastRotations.pdf
    // paper from Bill Premerlani
    // We also adjust the gain depending on the rate of change of horizontal velocity which
    // is proportional to how observable the heading is from the acceerations and GPS velocity
    // The accelration derived heading will be more reliable in turns than compass or GPS

    _omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
    if (use_fast_gains()) {
        _omega_yaw_P.z *= 8;
    }

    // don't update the drift term if we lost the yaw reference
    // for more than 2 seconds
    if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        // also add to the I term
        _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
    }

    _error_yaw = 0.8f * _error_yaw + 0.2f * fabsf(yaw_error);
}
示例#7
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    Vector3f velocity;
    uint32_t last_correction_time;

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

    // rotate accelerometer values into the earth frame
    _accel_ef = _dcm_matrix * _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;

    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;
}
示例#8
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
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.rotate(_trim);

    // rotate accelerometer values into the earth frame
    _accel_ef = temp_dcm * _accel_vector;

    // 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;

    if (!have_gps()) {
        // 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.2) {
            // 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;

        // update position delta for get_position()
        _position_offset_north += velocity.x * _ra_deltat;
        _position_offset_east  += velocity.y * _ra_deltat;
    } 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;

        // remember position for get_position()
        _last_lat = _gps->latitude;
        _last_lng = _gps->longitude;
        _position_offset_north = 0;
        _position_offset_east = 0;

        // once we have a single GPS lock, we update using
        // dead-reckoning from then on
        _have_position = true;

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

    /*
     *  The barometer for vertical velocity is only enabled if we got
     *  at least 5 pressure samples for the reading. This ensures we
     *  don't use very noisy climb rate data
     */
    if (_baro_use && _barometer != NULL && _barometer->get_pressure_samples() >= 5) {
        // Z velocity is down
        velocity.z = -_barometer->get_climb_rate();
    }

    // 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;
    float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
    Vector3f vdelta = (velocity - _last_velocity) * v_scale;
    // limit vertical acceleration correction to 0.5 gravities. The
    // barometer sometimes gives crazy acceleration changes. 
    vdelta.z = constrain(vdelta.z, -0.5, 0.5);
    GA_e = Vector3f(0, 0, -1.0) + vdelta;
    GA_e.normalize();
    if (GA_e.is_inf()) {
        // wait for some non-zero acceleration information
        return;
    }

    // calculate the error term in earth frame.
    Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS);
    float length = GA_b.length();
    if (length > 1.0) {
        GA_b /= length;
        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 = atan2(GA_b.y, GA_b.x);

    // equation 12
    Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(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 (_compass && _compass->use_for_yaw()) {
        if (have_gps() && gps_gain == 1.0) {
            error.z *= sin(fabs(roll));
        } else {
            error.z = 0;
        }
    }

    // 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 (_fast_ground_gains) {
        _omega_P *= 8;
    }

    // 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(_omega_I_sum.x, -change_limit, change_limit);
        _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
        _omega_I_sum.z = constrain(_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 && _fly_forward) {
        // update wind estimate
        estimate_wind(velocity);
    }
}
示例#9
0
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
    bool new_value = false;
    float yaw_error;
    float yaw_deltat;

    if (_compass && _compass->use_for_yaw()) {
        if (_compass->last_update != _compass_last_update) {
            yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
            _compass_last_update = _compass->last_update;
            // we force an additional compass read()
            // here. This has the effect of throwing away
            // the first compass value, which can be bad
            if (!_have_initial_yaw && _compass->read()) {
                float heading = _compass->calculate_heading(_dcm_matrix);
                _dcm_matrix.from_euler(roll, pitch, heading);
                _omega_yaw_P.zero();
                _have_initial_yaw = true;
            }
            new_value = true;
            yaw_error = yaw_error_compass();
        }
    } else if (_fly_forward && have_gps()) {
        if (_gps->last_fix_time != _gps_last_update &&
            _gps->ground_speed >= GPS_SPEED_MIN) {
            yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
            _gps_last_update = _gps->last_fix_time;
            if (!_have_initial_yaw) {
                _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
                _omega_yaw_P.zero();
                _have_initial_yaw = true;
            }
            new_value = true;
            yaw_error = yaw_error_gps();
        }
    }

    if (!new_value) {
        // we don't have any new yaw information
        // slowly decay _omega_yaw_P to cope with loss
        // of our yaw source
        _omega_yaw_P *= 0.97;
        return;
    }

    // the yaw error is a vector in earth frame
    Vector3f error = Vector3f(0,0, yaw_error);

    // convert the error vector to body frame
    error = _dcm_matrix.mul_transpose(error);

    // the spin rate changes the P gain, and disables the
    // integration at higher rates
    float spin_rate = _omega.length();

    // update the proportional control to drag the
    // yaw back to the right value. We use a gain
    // that depends on the spin rate. See the fastRotations.pdf
    // paper from Bill Premerlani

    _omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw;
    if (_fast_ground_gains) {
        _omega_yaw_P.z *= 8;
    }

    // don't update the drift term if we lost the yaw reference
    // for more than 2 seconds
    if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        // also add to the I term
        _omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
    }

    _error_yaw_sum += fabs(yaw_error);
    _error_yaw_count++;
}