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

AP_AHRS: add interface to retrieve corrected delta velocities in NED frame

This commit is contained in:
Jonathan Challinger 2016-08-11 12:58:02 -07:00 committed by Randy Mackay
parent ccc99c772f
commit 25a14fe0dc
3 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_AHRS/AP_AHRS.h
View File

@ -463,6 +463,9 @@ public:
int8_t get_ekf_type(void) const {
return _ekf_type;
}
// Retrieves the corrected NED delta velocity in use by the inertial navigation
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); }
protected:
AHRS_VehicleClass _vehicle_class;

18
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1109,6 +1109,24 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets)
}
}
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
if (ekf_type() == 2) {
uint8_t imu_idx = EKF2.getPrimaryCoreIMUIndex();
float accel_z_bias;
EKF2.getAccelZBias(-1,accel_z_bias);
ret.zero();
_ins.get_delta_velocity(imu_idx, ret);
dt = _ins.get_delta_velocity_dt(imu_idx);
ret.z -= accel_z_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt;
} else {
AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
}
}
// report any reason for why the backend is refusing to initialise
const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const
{

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -126,6 +126,9 @@ public:
const Vector3f &get_accel_ef(uint8_t i) const override;
const Vector3f &get_accel_ef() const override;
// 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(void) const;