Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: get_gyro_latest returns latest rates from IMU with drift correction

Browse Source 
Pulling the gyro values from the IMU allows our rate controllers to run before the EKF
This commit is contained in:
Randy Mackay 2017-03-02 20:33:13 +09:00
parent 1bc1e3faeb
commit d3aca5544e
5 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -131,6 +131,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS::get_gyro_latest(void) const
{
uint8_t primary_gyro = get_primary_gyro_index();
return get_ins().get_gyro(primary_gyro) + get_gyro_drift();
}
// return airspeed estimate if available // return airspeed estimate if available
bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const
{ {

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -235,6 +235,9 @@ public:
// return a smoothed and corrected gyro vector // return a smoothed and corrected gyro vector
virtual const Vector3f &get_gyro(void) const = 0; virtual const Vector3f &get_gyro(void) const = 0;
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f get_gyro_latest(void) const;
// return the current estimate of the gyro drift // return the current estimate of the gyro drift
virtual const Vector3f &get_gyro_drift(void) const = 0; virtual const Vector3f &get_gyro_drift(void) const = 0;

2
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1471,7 +1471,7 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
return false; return false;
} }
// get earth-frame accel vector for primary IMU // get the index of the current primary IMU
uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
{ {
int8_t imu = -1; int8_t imu = -1;

7
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -67,3 +67,10 @@ void AP_AHRS_View::update(void)
trig.cos_roll, trig.cos_pitch, trig.cos_yaw, trig.cos_roll, trig.cos_pitch, trig.cos_yaw,
trig.sin_roll, trig.sin_pitch, trig.sin_yaw); trig.sin_roll, trig.sin_pitch, trig.sin_yaw);
} }
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS_View::get_gyro_latest(void) const {
Vector3f gyro_latest = ahrs.get_gyro_latest();
gyro_latest.rotate(rotation);
return gyro_latest;
}

3
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -39,6 +39,9 @@ public:
return gyro; return gyro;
} }
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f get_gyro_latest(void) const;
// return a DCM rotation matrix representing our current // return a DCM rotation matrix representing our current
// attitude in this view // attitude in this view
const Matrix3f &get_rotation_body_to_ned(void) const { const Matrix3f &get_rotation_body_to_ned(void) const {