void Plane::control_failsafe(uint16_t pwm)
{
if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
// control inputs to the trim value and throttle to min
channel_roll->radio_in = channel_roll->radio_trim;
channel_pitch->radio_in = channel_pitch->radio_trim;
channel_rudder->radio_in = channel_rudder->radio_trim;
// note that we don't set channel_throttle->radio_in to radio_trim,
// as that would cause throttle failsafe to not activate
channel_roll->control_in = 0;
channel_pitch->control_in = 0;
channel_rudder->control_in = 0;
channel_throttle->control_in = 0;
}
if(g.throttle_fs_enabled == 0)
return;
if (g.throttle_fs_enabled) {
if (rc_failsafe_active()) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafe.ch3_counter++;
if (failsafe.ch3_counter == 10) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS ON %u", (unsigned)pwm);
failsafe.ch3_failsafe = true;
AP_Notify::flags.failsafe_radio = true;
}
if (failsafe.ch3_counter > 10) {
failsafe.ch3_counter = 10;
}
}else if(failsafe.ch3_counter > 0) {
// we are no longer in failsafe condition
// but we need to recover quickly
failsafe.ch3_counter--;
if (failsafe.ch3_counter > 3) {
failsafe.ch3_counter = 3;
}
if (failsafe.ch3_counter == 1) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "MSG FS OFF %u", (unsigned)pwm);
} else if(failsafe.ch3_counter == 0) {
failsafe.ch3_failsafe = false;
AP_Notify::flags.failsafe_radio = false;
}
}
}
}
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(channel_throttle->control_in > 0) {
hal.scheduler->delay(20);
read_radio();
}
while(1) {
hal.scheduler->delay(20);
read_radio();
if(channel_throttle->control_in > 0) {
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), (int)channel_throttle->control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()) {
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(rc_failsafe_active()) {
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), (int)channel_throttle->radio_in);
print_flight_mode(cliSerial, readSwitch());
cliSerial->println();
fail_test++;
}
if(fail_test > 0) {
return (0);
}
if(cliSerial->available() > 0) {
cliSerial->printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
/*
return true if the current settings and mode should allow for stick mixing
*/
bool Plane::stick_mixing_enabled(void)
{
if (auto_throttle_mode) {
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
geofence_stickmixing() &&
failsafe.state == FAILSAFE_NONE &&
!rc_failsafe_active()) {
// we're in an auto mode, and haven't triggered failsafe
return true;
} else {
return false;
}
}
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
// don't do stick mixing in FBWA glide mode
return false;
}
// non-auto mode. Always do stick mixing
return true;
}
void Plane::control_failsafe()
{
if (millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
// control inputs to the trim value and throttle to min
channel_roll->set_radio_in(channel_roll->get_radio_trim());
channel_pitch->set_radio_in(channel_pitch->get_radio_trim());
channel_rudder->set_radio_in(channel_rudder->get_radio_trim());
// note that we don't set channel_throttle->radio_in to radio_trim,
// as that would cause throttle failsafe to not activate
channel_roll->set_control_in(0);
channel_pitch->set_control_in(0);
channel_rudder->set_control_in(0);
switch (control_mode) {
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND: // throttle is ignored, but reset anyways
case QRTL: // throttle is ignored, but reset anyways
case QAUTOTUNE:
if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DESIRED_GROUND_IDLE) {
// set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
channel_throttle->set_control_in(channel_throttle->get_range() / 2);
break;
}
FALLTHROUGH;
default:
channel_throttle->set_control_in(0);
break;
}
}
if(g.throttle_fs_enabled == 0)
return;
if (g.throttle_fs_enabled) {
if (rc_failsafe_active()) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafe.throttle_counter++;
if (failsafe.throttle_counter == 10) {
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on");
failsafe.rc_failsafe = true;
AP_Notify::flags.failsafe_radio = true;
}
if (failsafe.throttle_counter > 10) {
failsafe.throttle_counter = 10;
}
}else if(failsafe.throttle_counter > 0) {
// we are no longer in failsafe condition
// but we need to recover quickly
failsafe.throttle_counter--;
if (failsafe.throttle_counter > 3) {
failsafe.throttle_counter = 3;
}
if (failsafe.throttle_counter == 1) {
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off");
} else if(failsafe.throttle_counter == 0) {
failsafe.rc_failsafe = false;
AP_Notify::flags.failsafe_radio = false;
}
}
}
}