Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use ins_checks from AP_Arming

Browse Source 
Functionality changes:
 - gyros and accels only have to be consistent in last 10 seconds to pass
 - ins.use_accel() is honoured when checking for consistency
 - ins.use_gyro() is honoured when checking for consistency
 - threshold is trippled rather than doubled for accel cal checks
 - checks are reordered
This commit is contained in:
Peter Barker 2016-08-17 13:14:56 +10:00 committed by Randy Mackay
parent bd6ffc025e
commit f3a31b988a
3 changed files with 8 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
ArduCopter/arming_checks.cpp
View File

@ -212,93 +212,20 @@ bool AP_Arming_Copter::fence_checks(bool display_failure)
bool AP_Arming_Copter::ins_checks(bool display_failure)
{
// check INS
const AP_InertialSensor &ins = _ins; // avoid code churn
bool ret = AP_Arming::ins_checks(display_failure);
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
if (!ins.accel_calibrated_ok_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated");
}
return false;
}
// check accels are healthy
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy");
}
return false;
}
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
// check all accelerometers point in roughly same direction
if (ins.get_accel_count() > 1) {
const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<ins.get_accel_count(); i++) {
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF;
if (i >= 2) {
/*
* for boards with 3 IMUs we only use the first two
* in the EKF. Allow for larger accel discrepancy
* for IMU3 as it may be running at a different temperature
*/
threshold *= 2;
}
// EKF is less sensitive to Z-axis error
vec_diff.z *= 0.5f;
if (vec_diff.length() > threshold) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers");
}
return false;
}
}
}
// check gyros are healthy
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy");
}
return false;
}
// check all gyros are consistent
if (ins.get_gyro_count() > 1) {
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
// get rotation rate difference between gyro #i and primary gyro
Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros");
}
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(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling");
}
return false;
ret = false;
}
}
return true;
return ret;
}
bool AP_Arming_Copter::board_voltage_checks(bool display_failure)

4
ArduCopter/arming_checks.h
View File

@ -28,11 +28,13 @@ protected:
bool pre_arm_proximity_check(bool display_failure);
bool arm_checks(bool display_failure, bool arming_from_gcs);
// NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override;
// NOTE! the following check functions *DO NOT* call into AP_Arming!
bool gps_checks(bool display_failure);
bool fence_checks(bool display_failure);
bool compass_checks(bool display_failure);
bool ins_checks(bool display_failure) override;
bool board_voltage_checks(bool display_failure);
bool parameter_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);

10
ArduCopter/config.h
View File

@ -188,16 +188,6 @@
# define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters
#endif
// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers
#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF
#define PREARM_MAX_ACCEL_VECTOR_DIFF 0.70f // pre arm accel check will fail if primary and backup accelerometer vectors differ by 0.7m/s/s
#endif
// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros
#ifndef PREARM_MAX_GYRO_VECTOR_DIFF
#define PREARM_MAX_GYRO_VECTOR_DIFF 0.0873f // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.0873 rad/sec (=5deg/sec)
#endif
//////////////////////////////////////////////////////////////////////////////
// EKF Failsafe
#ifndef FS_EKF_ACTION_DEFAULT