Browse Source

Plane: Rename rc failsafe state members

mission-4.1.18
Michael du Breuil 7 years ago committed by Tom Pittenger
parent
commit
d52f5155f0
  1. 2
      ArduPlane/ArduPlane.cpp
  2. 2
      ArduPlane/Attitude.cpp
  3. 8
      ArduPlane/Plane.h
  4. 4
      ArduPlane/control_modes.cpp
  5. 2
      ArduPlane/events.cpp
  6. 6
      ArduPlane/quadplane.cpp
  7. 26
      ArduPlane/radio.cpp
  8. 4
      ArduPlane/servos.cpp
  9. 12
      ArduPlane/system.cpp

2
ArduPlane/ArduPlane.cpp

@ -686,7 +686,7 @@ void Plane::update_flight_mode(void) @@ -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;

2
ArduPlane/Attitude.cpp

@ -49,7 +49,7 @@ bool Plane::stick_mixing_enabled(void) @@ -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;
}

8
ArduPlane/Plane.h

@ -323,7 +323,7 @@ private: @@ -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: @@ -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;

4
ArduPlane/control_modes.cpp

@ -9,8 +9,8 @@ void Plane::read_control_switch() @@ -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;
}

2
ArduPlane/events.cpp

@ -4,7 +4,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re @@ -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)
{

6
ArduPlane/quadplane.cpp

@ -1064,7 +1064,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void) @@ -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) @@ -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;
}

26
ArduPlane/radio.cpp

@ -176,7 +176,7 @@ void Plane::read_radio() @@ -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) @@ -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;
}
}

4
ArduPlane/servos.cpp

@ -359,7 +359,7 @@ void Plane::set_servos_controlled(void) @@ -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) @@ -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();
}

12
ArduPlane/system.cpp

@ -482,9 +482,9 @@ void Plane::check_long_failsafe() @@ -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() @@ -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() @@ -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);
}
}

Loading…
Cancel
Save