Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed throttle failsafe for FS_SHORT_ACTN=3

Browse Source 
this is a replacement for #12043
many thanks to @Jaaaky for finding this issue
This commit is contained in:
Andrew Tridgell 2019-08-26 12:57:39 +10:00
parent 66dc36ef88
commit 296ce5a409
2 changed files with 22 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -937,6 +937,7 @@ private:
int16_t rudder_input(void);
void control_failsafe();
bool trim_radio();
bool rc_throttle_value_ok(void) const;
bool rc_failsafe_active(void) const;
void read_rangefinder(void);
void read_airspeed(void);

29
ArduPlane/radio.cpp
View File

@ -182,12 +182,14 @@ void Plane::read_radio()
return;
}
if(!failsafe.rc_failsafe)
if (!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
if (rc_throttle_value_ok()) {
failsafe.last_valid_rc_ms = millis();
}
if (control_mode == &mode_training) {
// in training mode we don't want to use a deadzone, as we
@ -366,21 +368,32 @@ bool Plane::trim_radio()
return true;
}
/*
check if throttle value is within allowed range
*/
bool Plane::rc_throttle_value_ok(void) const
{
if (!g.throttle_fs_enabled) {
return true;
}
if (channel_throttle->get_reverse()) {
return channel_throttle->get_radio_in() < g.throttle_fs_value;
}
return channel_throttle->get_radio_in() > g.throttle_fs_value;
}
/*
return true if throttle level is below throttle failsafe threshold
or RC input is invalid
*/
bool Plane::rc_failsafe_active(void) const
{
if (!g.throttle_fs_enabled) {
return false;
if (!rc_throttle_value_ok()) {
return true;
}
if (millis() - failsafe.last_valid_rc_ms > 1000) {
// we haven't had a valid RC frame for 1 seconds
return true;
}
if (channel_throttle->get_reverse()) {
return channel_throttle->get_radio_in() >= g.throttle_fs_value;
}
return channel_throttle->get_radio_in() <= g.throttle_fs_value;
return false;
}