diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 1cf6bb3f04..010fbb6fd8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -686,7 +686,7 @@ void Plane::update_flight_mode(void) if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } - if (failsafe.ch3_failsafe && g.short_fs_action == 2) { + if (failsafe.rc_failsafe && g.short_fs_action == 2) { // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 1b458983ad..29a21725f5 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -49,7 +49,7 @@ bool Plane::stick_mixing_enabled(void) } } - if (failsafe.ch3_failsafe && g.short_fs_action == 2) { + if (failsafe.rc_failsafe && g.short_fs_action == 2) { // don't do stick mixing in FBWA glide mode return false; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c3a41f67b4..e59c8b4035 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -323,7 +323,7 @@ private: struct { // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold // RC receiver should be set up to output a low throttle value when signal is lost - uint8_t ch3_failsafe:1; + uint8_t rc_failsafe:1; // has the saved mode for failsafe been set? uint8_t saved_mode_set:1; @@ -341,14 +341,14 @@ private: // Used for failsafe based on loss of RC signal or GCS signal int16_t state; - // number of low ch3 values - uint8_t ch3_counter; + // number of low throttle values + uint8_t throttle_counter; // the time when the last HEARTBEAT message arrived from a GCS uint32_t last_heartbeat_ms; // 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 short_timer_ms; uint32_t last_valid_rc_ms; diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 5696c78610..b98fe3e234 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -9,8 +9,8 @@ void Plane::read_control_switch() // If we get this value we do not want to change modes. if(switchPosition == 255) return; - if (failsafe.ch3_failsafe || failsafe.ch3_counter > 0) { - // when we are in ch3_failsafe mode then RC input is not + if (failsafe.rc_failsafe || failsafe.throttle_counter > 0) { + // when we are in rc_failsafe mode then RC input is not // working, and we need to ignore the mode switch channel return; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 310fea818a..3d13138ecc 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -4,7 +4,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re { // This is how to handle a short loss of control signal failsafe. failsafe.state = fstype; - failsafe.ch3_timer_ms = millis(); + failsafe.short_timer_ms = millis(); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason); switch(control_mode) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 15470b5761..3785c2a4d3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1064,7 +1064,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void) // get pilot desired climb rate in cm/s float QuadPlane::get_pilot_desired_climb_rate_cms(void) { - if (plane.failsafe.ch3_failsafe || plane.failsafe.ch3_counter > 0) { + if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) { // descend at 0.5m/s for now return -50; } @@ -1461,8 +1461,8 @@ void QuadPlane::update(void) // disable throttle_wait when throttle rises above 10% if (throttle_wait && (plane.channel_throttle->get_control_in() > 10 || - plane.failsafe.ch3_failsafe || - plane.failsafe.ch3_counter>0)) { + plane.failsafe.rc_failsafe || + plane.failsafe.throttle_counter>0)) { throttle_wait = false; } diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 329dceac46..9f2f663e76 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -176,7 +176,7 @@ void Plane::read_radio() return; } - if(!failsafe.ch3_failsafe) + if(!failsafe.rc_failsafe) { failsafe.AFS_last_valid_rc_ms = millis(); } @@ -264,27 +264,27 @@ void Plane::control_failsafe(uint16_t pwm) if (rc_failsafe_active()) { // we detect a failsafe from radio // throttle has dropped below the mark - failsafe.ch3_counter++; - if (failsafe.ch3_counter == 10) { + failsafe.throttle_counter++; + if (failsafe.throttle_counter == 10) { gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on %u", (unsigned)pwm); - failsafe.ch3_failsafe = true; + failsafe.rc_failsafe = true; AP_Notify::flags.failsafe_radio = true; } - if (failsafe.ch3_counter > 10) { - failsafe.ch3_counter = 10; + if (failsafe.throttle_counter > 10) { + failsafe.throttle_counter = 10; } - }else if(failsafe.ch3_counter > 0) { + }else if(failsafe.throttle_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; + failsafe.throttle_counter--; + if (failsafe.throttle_counter > 3) { + failsafe.throttle_counter = 3; } - if (failsafe.ch3_counter == 1) { + if (failsafe.throttle_counter == 1) { gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off %u", (unsigned)pwm); - } else if(failsafe.ch3_counter == 0) { - failsafe.ch3_failsafe = false; + } else if(failsafe.throttle_counter == 0) { + failsafe.rc_failsafe = false; AP_Notify::flags.failsafe_radio = false; } } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index eb52cb6047..9878e344e0 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -359,7 +359,7 @@ void Plane::set_servos_controlled(void) control_mode == ACRO || control_mode == FLY_BY_WIRE_A || control_mode == AUTOTUNE) && - !failsafe.ch3_counter) { + !failsafe.throttle_counter) { // manual pass through of throttle while in FBWA or // STABILIZE mode with THR_PASS_STAB set SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz()); @@ -392,7 +392,7 @@ void Plane::set_servos_flaps(void) // work out any manual flap input RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1); - if (flapin != nullptr && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) { + if (flapin != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) { flapin->input(); manual_flap_percent = flapin->percent_input(); } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index f6f093e6d8..ad2fd78326 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -482,9 +482,9 @@ void Plane::check_long_failsafe() uint32_t radio_timeout_ms = failsafe.last_valid_rc_ms; if (failsafe.state == FAILSAFE_SHORT) { // time is relative to when short failsafe enabled - radio_timeout_ms = failsafe.ch3_timer_ms; + radio_timeout_ms = failsafe.short_timer_ms; } - if (failsafe.ch3_failsafe && + if (failsafe.rc_failsafe && (tnow - radio_timeout_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE); } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO && @@ -511,7 +511,7 @@ void Plane::check_long_failsafe() (tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) { failsafe_long_off_event(MODE_REASON_GCS_FAILSAFE); } else if (failsafe.state == FAILSAFE_LONG && - !failsafe.ch3_failsafe) { + !failsafe.rc_failsafe) { failsafe_long_off_event(MODE_REASON_RADIO_FAILSAFE); } } @@ -524,14 +524,14 @@ void Plane::check_short_failsafe() if (g.short_fs_action != SHORT_FS_ACTION_DISABLED && failsafe.state == FAILSAFE_NONE && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { - // The condition is checked and the flag ch3_failsafe is set in radio.cpp - if(failsafe.ch3_failsafe) { + // The condition is checked and the flag rc_failsafe is set in radio.cpp + if(failsafe.rc_failsafe) { failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE); } } if(failsafe.state == FAILSAFE_SHORT) { - if(!failsafe.ch3_failsafe || g.short_fs_action == SHORT_FS_ACTION_DISABLED) { + if(!failsafe.rc_failsafe || g.short_fs_action == SHORT_FS_ACTION_DISABLED) { failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE); } }