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

Copter: add EKF attitude arming check

Browse Source
This commit is contained in:
Randy Mackay 2015-09-17 17:24:26 +09:00
parent f3d4b20a80
commit 57c5840f0d
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -842,6 +842,7 @@ private:
bool pre_arm_checks(bool display_failure);
void pre_arm_rc_checks();
bool pre_arm_gps_checks(bool display_failure);
bool pre_arm_ekf_attitude_check();
bool arm_checks(bool display_failure, bool arming_from_gcs);
void init_disarm_motors();
void motors_output();

24
ArduCopter/motors.cpp
View File

@ -468,6 +468,14 @@ bool Copter::pre_arm_checks(bool display_failure)
}
}
#endif
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: gyros still settling"));
}
return false;
}
}
#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
@ -696,6 +704,15 @@ bool Copter::pre_arm_gps_checks(bool display_failure)
return true;
}
// check ekf attitude is acceptable
bool Copter::pre_arm_ekf_attitude_check()
{
// get ekf filter status
nav_filter_status filt_status = inertial_nav.get_filter_status();
return filt_status.flags.attitude;
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
@ -720,6 +737,13 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
}
return false;
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: gyros still settling"));
}
return false;
}
}
// always check if inertial nav has started and is ready