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

AP_AHRS: removed use of "blended" accel values 

calculate a single EF accel based on primary IMU
This commit is contained in:
Andrew Tridgell 2022-07-27 10:21:01 +10:00
parent 924759510d
commit f80ba734c8
7 changed files with 38 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -438,11 +438,7 @@ void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estim
_gyro_estimate = results.gyro_estimate;
_gyro_drift = results.gyro_drift;
// copy earth-frame accelerometer estimates:
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_ef_ekf[i] = results.accel_ef[i];
}
_accel_ef_ekf_blended = results.accel_ef_blended;
_accel_ef = results.accel_ef;
_accel_bias = results.accel_bias;
update_cd_values();
@ -507,9 +503,10 @@ void AP_AHRS::update_EKF2(void)
update_trig();
// Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const AP_InertialSensor &_ins = AP::ins();
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_primary_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel();
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
@ -517,30 +514,18 @@ void AP_AHRS::update_EKF2(void)
EKF2.getGyroBias(_gyro_drift);
_gyro_drift = -_gyro_drift;
// calculate corrected gyro estimate for get_gyro()
if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) {
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins.get_gyro();
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
}
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift;
// get z accel bias estimate from active EKF (this is usually for the primary IMU)
float &abias = _accel_bias.z;
EKF2.getAccelZBias(abias);
// This EKF is currently using primary_imu, and abias applies to only that IMU
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i == primary_imu) {
accel.z -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
Vector3f accel = _ins.get_accel(primary_accel);
accel.z -= abias;
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
nav_filter_status filt_state;
EKF2.getFilterStatus(filt_state);
update_notify_from_filter_status(filt_state);
@ -587,6 +572,8 @@ void AP_AHRS::update_EKF3(void)
// Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
const uint8_t primary_gyro = primary_imu>=0?primary_imu:_ins.get_primary_gyro();
const uint8_t primary_accel = primary_imu>=0?primary_imu:_ins.get_primary_accel();
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
@ -594,32 +581,18 @@ void AP_AHRS::update_EKF3(void)
EKF3.getGyroBias(-1,_gyro_drift);
_gyro_drift = -_gyro_drift;
// calculate corrected gyro estimate for get_gyro()
if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) {
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins.get_gyro();
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
}
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift;
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
Vector3f &abias = _accel_bias;
EKF3.getAccelBias(-1,abias);
// This EKF uses the primary IMU
// Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i == primary_imu) {
accel -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
// use the primary IMU for accel earth frame
Vector3f accel = _ins.get_accel(primary_accel);
accel -= abias;
_accel_ef = _dcm_matrix * accel;
nav_filter_status filt_state;
EKF3.getFilterStatus(filt_state);
update_notify_from_filter_status(filt_state);
@ -654,12 +627,8 @@ void AP_AHRS::update_SITL(void)
_gyro_estimate = _ins.get_gyro();
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
const Vector3f &accel = _ins.get_accel(i);
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
const Vector3f &accel = _ins.get_accel();
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
#if HAL_NAVEKF3_AVAILABLE
@ -709,27 +678,12 @@ void AP_AHRS::update_external(void)
_gyro_estimate = AP::externalAHRS().get_gyro();
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
Vector3f accel = AP::externalAHRS().get_accel();
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
Vector3f accel = AP::externalAHRS().get_accel();
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
}
#endif // HAL_EXTERNAL_AHRS_ENABLED
// accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS::get_accel_ef(uint8_t i) const
{
return _accel_ef_ekf[i];
}
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS::get_accel_ef_blended(void) const
{
return _accel_ef_ekf_blended;
}
void AP_AHRS::reset()
{
// support locked access functions to AHRS data
@ -3025,12 +2979,6 @@ uint8_t AP_AHRS::get_primary_IMU_index() const
return imu;
}
// get earth-frame accel vector for primary IMU
const Vector3f &AP_AHRS::get_accel_ef() const
{
return get_accel_ef(get_primary_accel_index());
}
// return the index of the primary core or -1 if no primary core selected
int8_t AP_AHRS::get_primary_core_index() const
{

11
libraries/AP_AHRS/AP_AHRS.h
View File

@ -193,15 +193,13 @@ public:
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void);
const Vector3f &get_accel_ef(uint8_t i) const;
const Vector3f &get_accel_ef() const;
const Vector3f &get_accel_ef() const {
return _accel_ef;
}
// Retrieves the corrected NED delta velocity in use by the inertial navigation
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const;
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &get_accel_ef_blended() const;
// set the EKF's origin location in 10e7 degrees. This should only
// be called when the EKF has no absolute position reference (i.e. GPS)
// from which to decide the origin on its own
@ -710,8 +708,7 @@ private:
Vector3f _gyro_drift;
Vector3f _gyro_estimate;
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
Vector3f _accel_ef_ekf_blended;
Vector3f _accel_ef;
Vector3f _accel_bias;
const uint16_t startup_delay_ms = 1000;

3
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -55,8 +55,7 @@ public:
Matrix3f dcm_matrix;
Vector3f gyro_estimate;
Vector3f gyro_drift;
Vector3f accel_ef[INS_MAX_INSTANCES]; // must be INS_MAX_INSTANCES
Vector3f accel_ef_blended;
Vector3f accel_ef;
Vector3f accel_bias;
};

17
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -122,10 +122,7 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)
results.dcm_matrix = _body_dcm_matrix;
results.gyro_estimate = _omega;
results.gyro_drift = _omega_I;
const uint8_t to_copy = MIN(sizeof(results.accel_ef), sizeof(_accel_ef));
memcpy(results.accel_ef, _accel_ef, to_copy);
results.accel_ef_blended = _accel_ef_blended;
results.accel_ef = _accel_ef;
}
/*
@ -397,7 +394,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
float
AP_AHRS_DCM::_yaw_gain(void) const
{
const float VdotEFmag = _accel_ef[_active_accel_instance].xy().length();
const float VdotEFmag = _accel_ef.xy().length();
if (VdotEFmag <= 4.0f) {
return 0.2f*(4.5f - VdotEFmag);
@ -654,9 +651,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
const AP_InertialSensor &_ins = AP::ins();
// rotate accelerometer values into the earth frame
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.use_accel(i) && healthy_count < 2) {
if (_ins.use_accel(i)) {
/*
by using get_delta_velocity() instead of get_accel() the
accel value is sampled over the right time delta for
@ -666,16 +662,15 @@ AP_AHRS_DCM::drift_correction(float deltat)
float delta_velocity_dt;
_ins.get_delta_velocity(i, delta_velocity, delta_velocity_dt);
if (delta_velocity_dt > 0) {
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
Vector3f accel_ef = _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;
_ra_sum[i] += accel_ef * deltat;
}
healthy_count++;
}
}
// set _accel_ef_blended based on filtered accel
_accel_ef_blended = _dcm_matrix * _ins.get_accel();
_accel_ef = _dcm_matrix * _ins.get_accel();
// keep a sum of the deltat values, so we know how much time
// we have integrated over

3
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -144,8 +144,7 @@ private:
static constexpr float _ki_yaw = 0.01f;
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;
Vector3f _accel_ef;
// Methods
void matrix_update(void);

2
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -158,7 +158,7 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
yaw : degrees(get_gyro().z),
yaw_out : motors.get_yaw()+motors.get_yaw_ff(),
control_accel : (float)accel_target.z,
accel : (float)(-(get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel : (float)(-(get_accel_ef().z + GRAVITY_MSS) * 100.0f),
accel_out : motors.get_throttle()
};
AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -141,8 +141,8 @@ public:
return ahrs.groundspeed();
}
const Vector3f &get_accel_ef_blended(void) const {
return ahrs.get_accel_ef_blended();
const Vector3f &get_accel_ef(void) const {
return ahrs.get_accel_ef();
}
uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {