diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 8e62864f63..16b23dfb8f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -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; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 58baf91fa8..833ef5ff5e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -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 { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index b79b6ac703..4a873800fd 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -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;