Plane: allow throttle failsafe on no RC input

on PX4 we just stop getting input on loss of RC - we need to consider
this to be "throttle failsafe"
This commit is contained in:
Andrew Tridgell 2013-12-20 09:41:15 +11:00
parent 765bfbfe43
commit 77c6e51887
2 changed files with 15 additions and 4 deletions

View File

@ -408,6 +408,8 @@ static struct {
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t ch3_timer_ms; uint32_t ch3_timer_ms;
uint32_t last_valid_rc_ms;
} failsafe; } failsafe;

View File

@ -111,16 +111,21 @@ static void rudder_arm_check()
Log_Arm_Disarm(); Log_Arm_Disarm();
} }
} }
} else {
} // not at full right rudder
else { //not at full left or right rudder
rudder_arm_timer = 0; rudder_arm_timer = 0;
return;
} }
} }
static void read_radio() static void read_radio()
{ {
if (!hal.rcin->valid_channels()) {
control_failsafe(channel_throttle->radio_in);
return;
}
failsafe.last_valid_rc_ms = hal.scheduler->millis();
elevon.ch1_temp = channel_roll->read(); elevon.ch1_temp = channel_roll->read();
elevon.ch2_temp = channel_pitch->read(); elevon.ch2_temp = channel_pitch->read();
uint16_t pwm_roll, pwm_pitch; uint16_t pwm_roll, pwm_pitch;
@ -289,6 +294,10 @@ static bool throttle_failsafe_level(void)
if (!g.throttle_fs_enabled) { if (!g.throttle_fs_enabled) {
return false; return false;
} }
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 2000) {
// we haven't had a valid RC frame for 2 seconds
return true;
}
if (channel_throttle->get_reverse()) { if (channel_throttle->get_reverse()) {
return channel_throttle->radio_in >= g.throttle_fs_value; return channel_throttle->radio_in >= g.throttle_fs_value;
} }