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

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

This commit is contained in:
Peter Barker 2021-02-12 08:34:38 +11:00 committed by Peter Barker
parent 5dff32422d
commit 4605870788
2 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -431,8 +431,8 @@ void NavEKF3_core::readIMUData()
imuDataNew.accel_index = accel_index_active; imuDataNew.accel_index = accel_index_active;
// Get delta angle data from primary gyro or primary if not available // Get delta angle data from primary gyro or primary if not available
readDeltaAngle(gyro_index_active, imuDataNew.delAng); readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(gyro_index_active),1.0e-4f); imuDataNew.delAngDT = MAX(imuDataNew.delAngDT, 1.0e-4f);
imuDataNew.gyro_index = gyro_index_active; imuDataNew.gyro_index = gyro_index_active;
// Get current time stamp // Get current time stamp
@ -530,8 +530,8 @@ bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
const auto &ins = dal.ins(); const auto &ins = dal.ins();
if (ins_index < ins.get_accel_count()) { if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel); ins.get_delta_velocity(ins_index,dVel,dVel_dt);
dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f); dVel_dt = MAX(dVel_dt,1.0e-4f);
return true; return true;
} }
return false; return false;
@ -710,11 +710,11 @@ void NavEKF3_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU // read the delta angle and corresponding time interval from the IMU
// return false if data is not available // return false if data is not available
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAngDT) {
const auto &ins = dal.ins(); const auto &ins = dal.ins();
if (ins_index < ins.get_gyro_count()) { if (ins_index < ins.get_gyro_count()) {
ins.get_delta_angle(ins_index,dAng); ins.get_delta_angle(ins_index, dAng, dAngDT);
return true; return true;
} }
return false; return false;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -681,7 +681,7 @@ private:
// helper functions for readIMUData // helper functions for readIMUData
bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt);
bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng); bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt);
// helper functions for correcting IMU data // helper functions for correcting IMU data
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index); void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);