mirror of https://github.com/ArduPilot/ardupilot
Copter: add vibration check
This commit is contained in:
parent
f2f1dbeccd
commit
ce2cd5d6dc
|
@ -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),
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
@ -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[];
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue