Plane: don't trigger RC failsafe until RC has been received for the first time

This commit is contained in:
Iampete1 2023-05-01 00:30:35 +01:00 committed by Andrew Tridgell
parent 4a8eda0589
commit bb80881c10
3 changed files with 44 additions and 2 deletions

View File

@ -42,7 +42,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
}
//are arming checks disabled?
if (checks_to_perform == 0) {
return true;
return mandatory_checks(display_failure);
}
if (hal.util->was_watchdog_armed()) {
// on watchdog reset bypass arming checks to allow for
@ -91,6 +91,8 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
ret = false;
}
ret &= rc_received_if_enabled_check(display_failure);
#if HAL_QUADPLANE_ENABLED
ret &= quadplane_checks(display_failure);
#endif
@ -124,6 +126,19 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
return ret;
}
bool AP_Arming_Plane::mandatory_checks(bool display_failure)
{
bool ret = true;
ret &= rc_received_if_enabled_check(display_failure);
// Call parent class checks
ret &= AP_Arming::mandatory_checks(display_failure);
return ret;
}
#if HAL_QUADPLANE_ENABLED
bool AP_Arming_Plane::quadplane_checks(bool display_failure)
{
@ -411,3 +426,21 @@ bool AP_Arming_Plane::mission_checks(bool report)
#endif
return ret;
}
// Checks rc has been received if it is configured to be used
bool AP_Arming_Plane::rc_received_if_enabled_check(bool display_failure)
{
if (rc().enabled_protocols() == 0) {
// No protocols enabled, will never get RC, don't block arming
return true;
}
// If RC failsafe is enabled we must receive RC before arming
if ((Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) == Plane::ThrFailsafe::Enabled) &&
!(rc().has_had_rc_receiver() || rc().has_had_rc_override())) {
check_failed(display_failure, "Waiting for RC");
return false;
}
return true;
}

View File

@ -29,6 +29,9 @@ public:
void update_soft_armed();
bool get_delay_arming() const { return delay_arming; };
// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(bool display_failure) override;
protected:
bool ins_checks(bool report) override;
bool terrain_database_required() const override;
@ -36,6 +39,9 @@ protected:
bool quadplane_checks(bool display_failure);
bool mission_checks(bool report) override;
// Checks rc has been received if it is configured to be used
bool rc_received_if_enabled_check(bool display_failure);
private:
void change_arm_state(void);

View File

@ -278,7 +278,10 @@ void Plane::control_failsafe()
}
}
if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) {
const bool allow_failsafe_bypass = !arming.is_armed() && !is_flying() && (rc().enabled_protocols() != 0);
const bool has_had_input = rc().has_had_rc_receiver() || rc().has_had_rc_override();
if ((ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) || (allow_failsafe_bypass && !has_had_input)) {
// If not flying and disarmed don't trigger failsafe until RC has been received for the fist time
return;
}