zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: ARMING_CHECK made into bitmask 

Allows arming checks to be individually enabled or disabled for baro,
compass, GPS, INS, parameters, RC and board voltage
master
Randy Mackay 11 years ago
parent
fe822ba0b6
commit
f0f6e85b39
4 changed files with 139 additions and 92 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      ArduCopter/AP_State.pde
  2. 6
      ArduCopter/Parameters.pde
  3. 10
      ArduCopter/defines.h
  4. 208
      ArduCopter/motors.pde

7
ArduCopter/AP_State.pde
Unescape View File

@ -131,3 +131,10 @@ void set_pre_arm_check(bool b) @@ -131,3 +131,10 @@ void set_pre_arm_check(bool b)
}
}
void set_pre_arm_rc_check(bool b)
{
if(ap.pre_arm_rc_check != b) {
ap.pre_arm_rc_check = b;
}
}

6
ArduCopter/Parameters.pde
Unescape View File

@ -397,10 +397,10 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -397,10 +397,10 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: ARMING_CHECK
// @DisplayName: Arming check
// @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer and compass
// @Values: 0:Disabled, 1:Enabled
// @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS
// @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Parameters, -65:Skip RC, 127:Skip Voltage
// @User: Standard
GSCALAR(arming_check_enabled, "ARMING_CHECK", 1),
GSCALAR(arming_check_enabled, "ARMING_CHECK", ARMING_CHECK_ALL),
// @Param: ANGLE_MAX
// @DisplayName: Angle Max

10
ArduCopter/defines.h
Unescape View File

@ -467,5 +467,15 @@ enum ap_message { @@ -467,5 +467,15 @@ enum ap_message {
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00
#define ARMING_CHECK_ALL 0x01
#define ARMING_CHECK_BARO 0x02
#define ARMING_CHECK_COMPASS 0x04
#define ARMING_CHECK_GPS 0x08
#define ARMING_CHECK_INS 0x10
#define ARMING_CHECK_PARAMETERS 0x20
#define ARMING_CHECK_RC 0x40
#define ARMING_CHECK_VOLTAGE 0x80
#endif // _DEFINES_H

208
ArduCopter/motors.pde
Unescape View File

@ -211,13 +211,14 @@ static void init_arm_motors() @@ -211,13 +211,14 @@ static void init_arm_motors()
static void pre_arm_checks(bool display_failure)
{
// exit immediately if we've already successfully performed the pre-arm check
if( ap.pre_arm_check ) {
if (ap.pre_arm_check) {
return;
}
// succeed if pre arm checks are disabled
if(!g.arming_check_enabled) {
if(g.arming_check_enabled == ARMING_CHECK_NONE) {
set_pre_arm_check(true);
set_pre_arm_rc_check(true);
return;
}
@ -230,117 +231,135 @@ static void pre_arm_checks(bool display_failure) @@ -230,117 +231,135 @@ static void pre_arm_checks(bool display_failure)
return;
}
// pre-arm check to ensure ch7 and ch8 have different functions
if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same"));
// check Baro
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_BARO)) {
// barometer health check
if(!barometer.healthy) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
}
return;
}
return;
}
// check accelerometers have been calibrated
if(!ins.calibrated()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
// check Compass
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_COMPASS)) {
// check the compass is healthy
if(!compass.healthy) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy"));
}
return;
}
return;
}
// check accels and gyros are healthy
if(!ins.healthy()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy"));
// check compass learning is on or offsets have been set
Vector3f offsets = compass.get_offsets();
if(!compass._learn && offsets.length() == 0) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated"));
}
return;
}
return;
}
// check the compass is healthy
if(!compass.healthy) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy"));
// check for unreasonable compass offsets
if(offsets.length() > 500) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high"));
}
return;
}
return;
}
// check compass learning is on or offsets have been set
Vector3f offsets = compass.get_offsets();
if(!compass._learn && offsets.length() == 0) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated"));
// check for unreasonable mag field length
float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
}
return;
}
return;
}
// check for unreasonable compass offsets
if(offsets.length() > 500) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high"));
// check GPS
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
// check gps is ok if required - note this same check is repeated again in arm_checks
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
}
return;
}
return;
}
// check for unreasonable mag field length
float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
#if AC_FENCE == ENABLED
// check fence is initialised
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks())) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
}
return;
}
return;
#endif
}
// barometer health check
if(!barometer.healthy) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
// check INS
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
if(!ins.calibrated()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
}
return;
}
return;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks())) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
// check accels and gyros are healthy
if(!ins.healthy()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy"));
}
return;
}
return;
}
#endif
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
// check board voltage
if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage"));
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_VOLTAGE)) {
if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage"));
}
return;
}
return;
}
#endif
// failsafe parameter checks
if (g.failsafe_throttle) {
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
if (g.rc_3.radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
// check various parameter values
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) {
// ensure ch7 and ch8 have different functions
if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same"));
}
return;
}
}
// lean angle parameter check
if (g.angle_max < 1000 || g.angle_max > 8000) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
// failsafe parameter checks
if (g.failsafe_throttle) {
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
if (g.rc_3.radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE"));
}
return;
}
}
return;
}
// check gps is ok if required - note this same check is repeated again in arm_checks
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
// lean angle parameter check
if (g.angle_max < 1000 || g.angle_max > 8000) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
}
return;
}
return;
}
// if we've gotten this far then pre arm checks have completed
@ -355,6 +374,12 @@ static void pre_arm_rc_checks() @@ -355,6 +374,12 @@ static void pre_arm_rc_checks()
return;
}
// set rc-checks to success if RC checks are disabled
if ((g.arming_check_enabled != ARMING_CHECK_ALL) && !(g.arming_check_enabled & ARMING_CHECK_RC)) {
set_pre_arm_rc_check(true);
return;
}
// check if radio has been calibrated
if(!g.rc_3.radio_min.load() && !g.rc_3.radio_max.load()) {
return;
@ -371,7 +396,7 @@ static void pre_arm_rc_checks() @@ -371,7 +396,7 @@ static void pre_arm_rc_checks()
}
// if we've gotten this far rc is ok
ap.pre_arm_rc_check = true;
set_pre_arm_rc_check(true);
}
// performs pre_arm gps related checks and returns true if passed
@ -393,24 +418,29 @@ static bool pre_arm_gps_checks() @@ -393,24 +418,29 @@ static bool pre_arm_gps_checks()
static bool arm_checks(bool display_failure)
{
// succeed if arming checks are disabled
if (!g.arming_check_enabled) {
if (g.arming_check_enabled == ARMING_CHECK_NONE) {
return true;
}
// check throttle is above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS"));
// check gps is ok if required - note this same check is also done in pre-arm checks
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
}
return false;
}
return false;
}
// check gps is ok if required - note this same check is also done in pre-arm checks
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
// check parameters
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) {
// check throttle is above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS"));
}
return false;
}
return false;
}
// check if safety switch has been pushed

Write Preview
Loading…
Cancel
Save