Copter: add vibration check

This commit is contained in:
Randy Mackay 2019-03-21 20:51:35 +11:00 committed by Andrew Tridgell
parent f2f1dbeccd
commit ce2cd5d6dc
6 changed files with 90 additions and 2 deletions

View File

@ -142,6 +142,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(check_vibration, 10, 50),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(standby_update, 100, 75),

View File

@ -333,6 +333,13 @@ private:
uint32_t ekfYawReset_ms;
int8_t ekf_primary_core;
// vibration check
struct {
bool high_vibes; // true while high vibration are detected
uint32_t start_ms; // system time high vibration were last detected
uint32_t clear_ms; // system time high vibrations stopped
} vibration_check;
// GCS selection
GCS_Copter _gcs; // avoid using this; use gcs()
GCS_Copter &gcs() { return _gcs; }
@ -693,6 +700,7 @@ private:
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void check_ekf_reset();
void check_vibration();
// esc_calibration.cpp
void esc_calibration_startup_check();

View File

@ -940,6 +940,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
#endif
// @Param: FS_VIBE_ENABLE
// @DisplayName: Vibration Failsafe enable
// @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
// @Values: 0:Disabled, 1:Enabled
// @User: Standard
AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
AP_GROUPEND
};

View File

@ -609,6 +609,9 @@ public:
// we need a pointer to the mode for the G2 table
void *mode_systemid_ptr;
#endif
// vibration failsafe enable/disable
AP_Int8 fs_vibe_enabled;
};
extern const AP_Param::Info var_info[];

View File

@ -204,3 +204,72 @@ void Copter::check_ekf_reset()
}
#endif
}
// check for high vibrations affecting altitude control
void Copter::check_vibration()
{
uint32_t now = AP_HAL::millis();
// assume checks will succeed
bool checks_succeeded = true;
// check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
Vector3f vel_innovation;
Vector3f pos_innovation;
Vector3f mag_innovation;
float tas_innovation;
float yaw_innovation;
if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
checks_succeeded = false;
}
const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
// check if EKF's NKF4.SH and NK4.SV > 1.0
float position_variance, vel_variance, height_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset)) {
checks_succeeded = false;
}
const bool var_vel_hgt_high = (vel_variance >= 1.0f);
// if no failure
if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || !var_vel_hgt_high) {
if (vibration_check.high_vibes) {
// start clear time
if (vibration_check.clear_ms == 0) {
vibration_check.clear_ms = now;
return;
}
// turn off vibration compensation after 15 seconds
if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user
vibration_check.high_vibes = false;
pos_control->set_vibe_comp(false);
vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
}
}
vibration_check.start_ms = 0;
return;
}
// start timer
if (vibration_check.start_ms == 0) {
vibration_check.start_ms = now;
vibration_check.clear_ms = 0;
return;
}
// check if failure has persisted for at least 1 second
if (now - vibration_check.start_ms > 1000) {
if (!vibration_check.high_vibes) {
// switch ekf to use resistant gains
vibration_check.high_vibes = true;
pos_control->set_vibe_comp(true);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
}
}
}

View File

@ -3,8 +3,8 @@
// read_inertia - read inertia in from accelerometers
void Copter::read_inertia()
{
// inertial altitude estimates
inertial_nav.update();
// inertial altitude estimates. Use barometer climb rate during high vibrations
inertial_nav.update(vibration_check.high_vibes);
// pull position from ahrs
Location loc;