diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 8211afa500..104bff6348 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -37,9 +37,9 @@ const AP_Param::GroupInfo AP_Arming::var_info[] PROGMEM = { // @Param: CHECK // @DisplayName: Arm Checks to Peform (bitmask) // @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed befor allowing arming. The default is no checks, allowing arming at any time. You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and manual RC control you would set ARMING_CHECK to 72. - // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS,16:INS,32:Parameters,64:Manual RC Trasmitter,128:Board voltage,256:Battery Level + // @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS,16:INS,32:Parameters,64:Manual RC Trasmitter,128:Board voltage,256:Battery Level,512:Airspeed // @User: Advanced - AP_GROUPINFO("CHECK", 2, AP_Arming, checks_to_perform, 0), + AP_GROUPINFO("CHECK", 2, AP_Arming, checks_to_perform, ARMING_CHECK_ALL), AP_GROUPEND }; @@ -88,6 +88,79 @@ bool AP_Arming::barometer_checks(bool report) return true; } +bool AP_Arming::airspeed_checks(bool report) +{ + if ((checks_to_perform & ARMING_CHECK_ALL) || + (checks_to_perform & ARMING_CHECK_AIRSPEED)) { + const AP_Airspeed *airspeed = ahrs.get_airspeed(); + if (airspeed == NULL) { + // not an airspeed capable vehicle + return true; + } + if (airspeed->enabled() && airspeed->use() && !airspeed->healthy()) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: airspeed not healthy")); + } + return false; + } + } + + return true; +} + +bool AP_Arming::ins_checks(bool report) +{ + if ((checks_to_perform & ARMING_CHECK_ALL) || + (checks_to_perform & ARMING_CHECK_INS)) { + const AP_InertialSensor &ins = ahrs.get_ins(); + if (! ins.get_gyro_health_all() || + ! ins.gyro_calibrated_ok_all() || + ! ins.get_accel_health_all()) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy!")); + } + return false; + } +#if INS_MAX_INSTANCES > 1 + // 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 0.3f) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers")); + } + return false; + } + } + } + + // check all gyros are giving consistent readings + if (ins.get_gyro_count() > 1) { + const Vector3f &prime_gyro_vec = ins.get_gyro(); + for(uint8_t i=0; i radians(5)) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros")); + } + return false; + } + } + } +#endif + } + + return true; +} + bool AP_Arming::compass_checks(bool report) { if ((checks_to_perform) & ARMING_CHECK_ALL || @@ -106,6 +179,30 @@ bool AP_Arming::compass_checks(bool report) } return false; } + + // check for unreasonable compass offsets + Vector3f offsets = _compass.get_offsets(); + if (offsets.length() > 600) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high")); + } + return false; + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 +# define COMPASS_MAGFIELD_EXPECTED 330 // pre arm will fail if mag field > 544 or < 115 +#else // PX4, SITL +#define COMPASS_MAGFIELD_EXPECTED 530 // pre arm will fail if mag field > 874 or < 185 +#endif + + // check for unreasonable mag field length + float mag_field = _compass.get_field().length(); + if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) { + if (report) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field")); + } + return false; + } } @@ -184,32 +281,23 @@ bool AP_Arming::manual_transmitter_checks(bool report) bool AP_Arming::pre_arm_checks(bool report) { + bool ret = true; if (armed || require == NONE) { // if we are already armed or don't need any arming checks // then skip the checks return true; } - if (! hardware_safety_check(report)) - return false; + ret &= hardware_safety_check(report); + ret &= barometer_checks(report); + ret &= ins_checks(report); + ret &= compass_checks(report); + ret &= gps_checks(report); + ret &= battery_checks(report); + ret &= airspeed_checks(report); + ret &= manual_transmitter_checks(report); - if (! barometer_checks(report)) - return false; - - if (! compass_checks(report)) - return false; - - if (! gps_checks(report)) - return false; - - if (! battery_checks(report)) - return false; - - if (! manual_transmitter_checks(report)) - return false; - - //all checks passed, allow arming! - return true; + return ret; } //returns true if arming occured successfully diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 451aa1a14b..30c159b642 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -21,6 +21,7 @@ public: ARMING_CHECK_RC = 0x0040, ARMING_CHECK_VOLTAGE = 0x0080, ARMING_CHECK_BATTERY = 0x0100, + ARMING_CHECK_AIRSPEED = 0x0200, }; enum ArmingMethod { @@ -75,6 +76,10 @@ private: bool barometer_checks(bool report); + bool airspeed_checks(bool report); + + bool ins_checks(bool report); + bool compass_checks(bool report); bool gps_checks(bool report);