Browse Source

AP_AHRS: remove separate calls to get delta-times for vel and ang

c415-sdk
Peter Barker 4 years ago committed by Peter Barker
parent
commit
ea1884f491
  1. 4
      libraries/AP_AHRS/AP_AHRS.h
  2. 7
      libraries/AP_AHRS/AP_AHRS_DCM.cpp
  3. 4
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

4
libraries/AP_AHRS/AP_AHRS.h

@ -545,9 +545,7 @@ public:
// Retrieves the corrected NED delta velocity in use by the inertial navigation // Retrieves the corrected NED delta velocity in use by the inertial navigation
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
ret.zero(); ret.zero();
const AP_InertialSensor &_ins = AP::ins(); AP::ins().get_delta_velocity(ret, dt);
_ins.get_delta_velocity(ret);
dt = _ins.get_delta_velocity_dt();
} }
// create a view // create a view

7
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -150,7 +150,8 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.use_gyro(i) && healthy_count < 2) { if (_ins.use_gyro(i) && healthy_count < 2) {
Vector3f dangle; Vector3f dangle;
if (_ins.get_delta_angle(i, dangle)) { float dangle_dt;
if (_ins.get_delta_angle(i, dangle, dangle_dt)) {
healthy_count++; healthy_count++;
delta_angle += dangle; delta_angle += dangle;
} }
@ -652,8 +653,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
each sensor, which prevents an aliasing effect each sensor, which prevents an aliasing effect
*/ */
Vector3f delta_velocity; Vector3f delta_velocity;
_ins.get_delta_velocity(i, delta_velocity); float delta_velocity_dt;
const float delta_velocity_dt = _ins.get_delta_velocity_dt(i); _ins.get_delta_velocity(i, delta_velocity, delta_velocity_dt);
if (delta_velocity_dt > 0) { if (delta_velocity_dt > 0) {
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt); _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
// integrate the accel vector in the earth frame between GPS readings // integrate the accel vector in the earth frame between GPS readings

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -2011,9 +2011,7 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons
return; return;
} }
ret.zero(); ret.zero();
const AP_InertialSensor &_ins = AP::ins(); AP::ins().get_delta_velocity((uint8_t)imu_idx, ret, dt);
_ins.get_delta_velocity((uint8_t)imu_idx, ret);
dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx);
ret -= accel_bias*dt; ret -= accel_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt; ret.z += GRAVITY_MSS*dt;

Loading…
Cancel
Save