mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: use millis/micros/panic functions
This commit is contained in:
parent
0b2184b818
commit
e3c317b96b
|
@ -380,7 +380,7 @@ void NavEKF_core::UpdateFilter()
|
||||||
hal.util->perf_begin(_perf_UpdateFilter);
|
hal.util->perf_begin(_perf_UpdateFilter);
|
||||||
|
|
||||||
//get starting time for update step
|
//get starting time for update step
|
||||||
imuSampleTime_ms = hal.scheduler->millis();
|
imuSampleTime_ms = AP_HAL::millis();
|
||||||
|
|
||||||
// read IMU data and convert to delta angles and velocities
|
// read IMU data and convert to delta angles and velocities
|
||||||
readIMUData();
|
readIMUData();
|
||||||
|
@ -3834,7 +3834,7 @@ void NavEKF_core::readIMUData()
|
||||||
}
|
}
|
||||||
|
|
||||||
// the imu sample time is used as a common time reference throughout the filter
|
// the imu sample time is used as a common time reference throughout the filter
|
||||||
imuSampleTime_ms = hal.scheduler->millis();
|
imuSampleTime_ms = AP_HAL::millis();
|
||||||
|
|
||||||
// dual accel mode - require both IMU's to be able to provide delta velocity outputs
|
// dual accel mode - require both IMU's to be able to provide delta velocity outputs
|
||||||
if (ins.use_accel(0) && ins.use_accel(1) && readDeltaVelocity(0, dVelIMU1, dtDelVel1) && readDeltaVelocity(1, dVelIMU2, dtDelVel2)) {
|
if (ins.use_accel(0) && ins.use_accel(1) && readDeltaVelocity(0, dVelIMU1, dtDelVel1) && readDeltaVelocity(1, dVelIMU2, dtDelVel2)) {
|
||||||
|
@ -4389,7 +4389,7 @@ void NavEKF_core::InitialiseVariables()
|
||||||
}
|
}
|
||||||
|
|
||||||
// initialise time stamps
|
// initialise time stamps
|
||||||
imuSampleTime_ms = hal.scheduler->millis();
|
imuSampleTime_ms = AP_HAL::millis();
|
||||||
lastHealthyMagTime_ms = imuSampleTime_ms;
|
lastHealthyMagTime_ms = imuSampleTime_ms;
|
||||||
TASmsecPrev = imuSampleTime_ms;
|
TASmsecPrev = imuSampleTime_ms;
|
||||||
BETAmsecPrev = imuSampleTime_ms;
|
BETAmsecPrev = imuSampleTime_ms;
|
||||||
|
|
|
@ -47,7 +47,7 @@ SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
|
||||||
// run a 9-state EKF used to calculate orientation
|
// run a 9-state EKF used to calculate orientation
|
||||||
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
|
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
|
||||||
{
|
{
|
||||||
imuSampleTime_ms = hal.scheduler->millis();
|
imuSampleTime_ms = AP_HAL::millis();
|
||||||
dtIMU = delta_time;
|
dtIMU = delta_time;
|
||||||
|
|
||||||
// initialise variables and constants
|
// initialise variables and constants
|
||||||
|
@ -882,7 +882,7 @@ void SmallEKF::getQuat(Quaternion &quat) const
|
||||||
// get filter status - true is aligned
|
// get filter status - true is aligned
|
||||||
bool SmallEKF::getStatus() const
|
bool SmallEKF::getStatus() const
|
||||||
{
|
{
|
||||||
float run_time = hal.scheduler->millis() - StartTime_ms;
|
float run_time = AP_HAL::millis() - StartTime_ms;
|
||||||
return YawAligned && (run_time > 30000);
|
return YawAligned && (run_time > 30000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue