mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: remove separate calls to get delta-times for vel and ang
This commit is contained in:
parent
5dff32422d
commit
4605870788
|
@ -431,8 +431,8 @@ void NavEKF3_core::readIMUData()
|
|||
imuDataNew.accel_index = accel_index_active;
|
||||
|
||||
// Get delta angle data from primary gyro or primary if not available
|
||||
readDeltaAngle(gyro_index_active, imuDataNew.delAng);
|
||||
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(gyro_index_active),1.0e-4f);
|
||||
readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
|
||||
imuDataNew.delAngDT = MAX(imuDataNew.delAngDT, 1.0e-4f);
|
||||
imuDataNew.gyro_index = gyro_index_active;
|
||||
|
||||
// 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();
|
||||
|
||||
if (ins_index < ins.get_accel_count()) {
|
||||
ins.get_delta_velocity(ins_index,dVel);
|
||||
dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f);
|
||||
ins.get_delta_velocity(ins_index,dVel,dVel_dt);
|
||||
dVel_dt = MAX(dVel_dt,1.0e-4f);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -710,11 +710,11 @@ void NavEKF3_core::readGpsData()
|
|||
|
||||
// read the delta angle and corresponding time interval from the IMU
|
||||
// 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();
|
||||
|
||||
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 false;
|
||||
|
|
|
@ -681,7 +681,7 @@ private:
|
|||
|
||||
// helper functions for readIMUData
|
||||
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
|
||||
void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
|
||||
|
|
Loading…
Reference in New Issue