Copter: continuously reevaluate rc calibration checks

Stop "latching" calibration checks - if an RC radio's calibration
changes after it passes once, these patches allow the rc calibraiton
checks to then fail.
This commit is contained in:
Peter Barker 2017-08-20 16:24:33 +10:00 committed by Randy Mackay
parent f5d91f63d2
commit b0c7766197
6 changed files with 8 additions and 33 deletions

View File

@ -70,7 +70,6 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
// succeed if pre arm checks are disabled
if (checks_to_perform == ARMING_CHECK_NONE) {
set_pre_arm_check(true);
set_pre_arm_rc_check(true);
return true;
}
@ -87,13 +86,6 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
& pilot_throttle_checks(display_failure);
}
bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
{
// pre-arm rc checks a prerequisite
pre_arm_rc_checks(display_failure);
return copter.ap.pre_arm_rc_check;
}
bool AP_Arming_Copter::barometer_checks(bool display_failure)
{
if (!AP_Arming::barometer_checks(display_failure)) {
@ -322,14 +314,8 @@ bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
return true;
}
// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag
void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure)
bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
{
// exit immediately if we've already successfully performed the pre-arm rc check
if (copter.ap.pre_arm_rc_check) {
return;
}
const RC_Channel *channels[] = {
copter.channel_roll,
copter.channel_pitch,
@ -338,6 +324,7 @@ void AP_Arming_Copter::pre_arm_rc_checks(const bool display_failure)
};
copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels);
return copter.ap.pre_arm_rc_check;
}
// performs pre_arm gps related checks and returns true if passed
@ -766,10 +753,3 @@ void AP_Arming_Copter::set_pre_arm_check(bool b)
AP_Notify::flags.pre_arm_check = b;
}
}
void AP_Arming_Copter::set_pre_arm_rc_check(bool b)
{
if(copter.ap.pre_arm_rc_check != b) {
copter.ap.pre_arm_rc_check = b;
}
}

View File

@ -17,7 +17,8 @@ public:
void update(void);
bool all_checks_passing(bool arming_from_gcs);
void pre_arm_rc_checks(bool display_failure);
bool rc_calibration_checks(bool display_failure);
protected:
@ -39,10 +40,8 @@ protected:
bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);
bool barometer_checks(bool display_failure);
bool rc_calibration_checks(bool display_failure);
void set_pre_arm_check(bool b);
void set_pre_arm_rc_check(bool b);
enum HomeState home_status() const override;

View File

@ -58,8 +58,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
}
// check if radio is calibrated
arming.pre_arm_rc_checks(true);
if (!ap.pre_arm_rc_check) {
if (!arming.rc_calibration_checks(true)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated");
ap.compass_mot = false;
return MAV_RESULT_TEMPORARILY_REJECTED;

View File

@ -27,8 +27,7 @@ void Copter::esc_calibration_startup_check()
}
// exit immediately if pre-arm rc checks fail
arming.pre_arm_rc_checks(true);
if (!ap.pre_arm_rc_check) {
if (!arming.rc_calibration_checks(true)) {
// clear esc flag for next time
if ((g.esc_calibrate != ESCCAL_NONE) && (g.esc_calibrate != ESCCAL_DISABLED)) {
g.esc_calibrate.set_and_save(ESCCAL_NONE);

View File

@ -81,8 +81,7 @@ bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
}
// check rc has been calibrated
arming.pre_arm_rc_checks(true);
if(check_rc && !ap.pre_arm_rc_check) {
if (check_rc && !arming.rc_calibration_checks(true)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
return false;
}

View File

@ -250,8 +250,7 @@ void Copter::init_ardupilot()
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// enable output to motors
arming.pre_arm_rc_checks(true);
if (ap.pre_arm_rc_check) {
if (arming.rc_calibration_checks(true)) {
enable_motor_output();
}