aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Arming: added INS checks and airspeed checks

Browse Source 
this also displays all failing arming checks, not just the first
one. That is more useful for the user
This commit is contained in:
Andrew Tridgell 2015-01-20 11:27:13 +11:00
parent 2235d18d67
commit 850b3b89ea
2 changed files with 114 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
libraries/AP_Arming/AP_Arming.cpp
View File

@ -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<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;
// allow for up to 0.3 m/s/s difference
if (vec_diff.length() > 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<ins.get_gyro_count(); i++) {
// get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference
if (vec_diff.length() > 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

5
libraries/AP_Arming/AP_Arming.h
View File

@ -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);