Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Arming: Allow memorizing ICE state #25905

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+29 −4
Open

AP_Arming: Allow memorizing ICE state #25905

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 28 additions & 4 deletions libraries/AP_Arming/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_KDECAN/AP_KDECAN.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_ICEngine/AP_ICEngine.h>

#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_CANManager/AP_CANManager.h>
Expand Down Expand Up @@ -142,7 +143,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Arming options
// @Description: Options that can be applied to change arming behaviour
// @Values: 0:None,1:Disable prearm display,2:Do not send status text on state change
// @Bitmask: 0:None,1:Disable prearm display,2:Do not send status text on state change,3:Use the last passing value for INS consistency check if ICE is running
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

change this to skip INS consistency check if ICE is running

// @User: Advanced
AP_GROUPINFO("OPTIONS", 9, AP_Arming, _arming_options, 0),

Expand Down Expand Up @@ -357,8 +358,31 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
return true;
}

const Vector3f &prime_accel_vec = ins.get_accel();
const uint32_t minimum_time = 10000;
const uint32_t now = AP_HAL::millis();

bool passed_before_ice_start = false;

#if AP_ICENGINE_ENABLED
if (option_enabled (Option::FREEZE_INS_CONSISTENT_WITH_ICE_RUN)) {
const auto *ice = AP::ice();
if (ice != nullptr) {
if (ice->get_state() >= AP_ICEngine::ICE_START_HEIGHT_DELAY) {
// the engine can create a lot of vibrations, if we were valid before it started
// we can keep that state and propegate it forward. If the system is highly unstable
// with temperature this may cause us to miss a bad IMU, but it's going to be masked
// by the engine vibration anyways
if (last_accel_pass_ms == 0 || now - last_accel_pass_ms < minimum_time) {
last_accel_pass_ms = 0;
} else {
passed_before_ice_start = true;
}
}
}
}
#endif

const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<accel_count; i++) {
if (!ins.use_accel(i)) {
continue;
Expand All @@ -380,7 +404,7 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;

if (vec_diff.length() > threshold) {
if (vec_diff.length() > threshold && !passed_before_ice_start) {
// this sensor disagrees with the primary sensor, so
// accels are inconsistent:
last_accel_pass_ms = 0;
Expand All @@ -395,7 +419,7 @@ bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins)
}

// must pass for at least 10 seconds before we're considered consistent:
if (now - last_accel_pass_ms < 10000) {
if (now - last_accel_pass_ms < minimum_time) {
return false;
}

Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Arming/AP_Arming.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,7 @@ class AP_Arming {
enum class Option : int32_t {
DISABLE_PREARM_DISPLAY = (1U << 0),
DISABLE_STATUSTEXT_ON_STATE_CHANGE = (1U << 1),
FREEZE_INS_CONSISTENT_WITH_ICE_RUN = (1U << 2),
};
bool option_enabled(Option option) const {
return (_arming_options & uint32_t(option)) != 0;
Expand Down