Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Arming: cache AP_HAL_millis

Browse Source
This commit is contained in:
Peter Barker 2018-06-16 08:46:31 +10:00 committed by Francisco Ferreira
parent 410e72f83c
commit 4055256878
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Arming/AP_Arming.cpp
View File

@ -176,6 +176,7 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
} }
const Vector3f &prime_accel_vec = ins.get_accel(); const Vector3f &prime_accel_vec = ins.get_accel();
const uint32_t now = AP_HAL::millis();
for(uint8_t i=0; i<ins.get_accel_count(); i++) { for(uint8_t i=0; i<ins.get_accel_count(); i++) {
if (!ins.use_accel(i)) { if (!ins.use_accel(i)) {
continue; continue;
@ -198,9 +199,9 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
vec_diff.z *= 0.5f; vec_diff.z *= 0.5f;
if (vec_diff.length() <= threshold) { if (vec_diff.length() <= threshold) {
last_accel_pass_ms[i] = AP_HAL::millis(); last_accel_pass_ms[i] = now;
} }
if (AP_HAL::millis() - last_accel_pass_ms[i] > 10000) { if (now - last_accel_pass_ms[i] > 10000) {
return false; return false;
} }
} }
@ -215,6 +216,7 @@ bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
} }
const Vector3f &prime_gyro_vec = ins.get_gyro(); const Vector3f &prime_gyro_vec = ins.get_gyro();
const uint32_t now = AP_HAL::millis();
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)) { if (!ins.use_gyro(i)) {
continue; continue;
@ -225,9 +227,9 @@ bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins)
// allow for up to 5 degrees/s difference. Pass if it has // allow for up to 5 degrees/s difference. Pass if it has
// been OK in last 10 seconds // been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) { if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms[i] = AP_HAL::millis(); last_gyro_pass_ms[i] = now;
} }
if (AP_HAL::millis() - last_gyro_pass_ms[i] > 10000) { if (now - last_gyro_pass_ms[i] > 10000) {
return false; return false;
} }
} }