Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS_NavEKF: overload get_accel_ef and get_accel_ef_blended functions

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-31 15:07:18 -07:00 committed by Randy Mackay
parent 4975cefd84
commit 9261dfdefb
2 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -118,8 +118,36 @@ void AP_AHRS_NavEKF::update(void)
_gyro_estimate /= healthy_count; _gyro_estimate /= healthy_count;
} }
_gyro_estimate += _gyro_bias; _gyro_estimate += _gyro_bias;
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * _ins.get_accel(i);
} }
} }
// update _accel_ef_ekf_blended
EKF.getAccelNED(_accel_ef_ekf_blended);
}
}
}
// accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const
{
if(!using_EKF()) {
return AP_AHRS_DCM::get_accel_ef(i);
}
return _accel_ef_ekf[i];
}
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const
{
if(!using_EKF()) {
return AP_AHRS_DCM::get_accel_ef_blended();
}
return _accel_ef_ekf_blended;
} }
void AP_AHRS_NavEKF::reset(bool recover_eulers) void AP_AHRS_NavEKF::reset(bool recover_eulers)

8
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -87,6 +87,12 @@ public:
// EKF has a better ground speed vector estimate // EKF has a better ground speed vector estimate
Vector2f groundspeed_vector(void); Vector2f groundspeed_vector(void);
const Vector3f &get_accel_ef(uint8_t i) const;
const Vector3f &get_accel_ef() const { return get_accel_ef(_ins.get_primary_accel()); };
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &get_accel_ef_blended(void) const;
// set home location // set home location
void set_home(const Location &loc); void set_home(const Location &loc);
@ -112,6 +118,8 @@ private:
Vector3f _dcm_attitude; Vector3f _dcm_attitude;
Vector3f _gyro_bias; Vector3f _gyro_bias;
Vector3f _gyro_estimate; Vector3f _gyro_estimate;
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
Vector3f _accel_ef_ekf_blended;
const uint16_t startup_delay_ms; const uint16_t startup_delay_ms;
uint32_t start_time_ms; uint32_t start_time_ms;
}; };