Skip to content

Commit

Permalink
Copter: Make the same string common
Browse files Browse the repository at this point in the history
  • Loading branch information
muramura committed Apr 27, 2024
1 parent 54bfaa4 commit 5a0dbba
Showing 1 changed file with 8 additions and 6 deletions.
14 changes: 8 additions & 6 deletions ArduCopter/AP_Arming.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -477,20 +477,21 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance);
const char *ekf_variance = "EKF %s variance";
if (mag_variance.length() >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF compass variance");
check_failed(display_failure, ekf_variance, "compass");
return false;
}
if (pos_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF position variance");
check_failed(display_failure, ekf_variance, "position");
return false;
}
if (vel_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF velocity variance");
check_failed(display_failure, ekf_variance, "velocity");
return false;
}
if (hgt_variance >= copter.g.fs_ekf_thresh) {
check_failed(display_failure, "EKF height variance");
check_failed(display_failure, ekf_variance, "height");
return false;
}
}
Expand Down Expand Up @@ -607,14 +608,15 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
// check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto
if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) {
// above top of deadband is too always high
const char *high_msg = "%s too high";
if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
check_failed(ARMING_CHECK_RC, true, high_msg, rc_item);
return false;
}
// in manual modes throttle must be at zero
#if FRAME_CONFIG != HELI_FRAME
if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
check_failed(ARMING_CHECK_RC, true, high_msg, rc_item);
return false;
}
#endif
Expand Down

0 comments on commit 5a0dbba

Please sign in to comment.