mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 07:13:56 -04:00
Plane: Rename rc failsafe state members
This commit is contained in:
parent
8058fdf093
commit
d52f5155f0
@ -686,7 +686,7 @@ void Plane::update_flight_mode(void)
|
|||||||
if (fly_inverted()) {
|
if (fly_inverted()) {
|
||||||
nav_pitch_cd = -nav_pitch_cd;
|
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
|
// FBWA failsafe glide
|
||||||
nav_roll_cd = 0;
|
nav_roll_cd = 0;
|
||||||
nav_pitch_cd = 0;
|
nav_pitch_cd = 0;
|
||||||
|
@ -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
|
// don't do stick mixing in FBWA glide mode
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -323,7 +323,7 @@ private:
|
|||||||
struct {
|
struct {
|
||||||
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
|
// 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
|
// 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?
|
// has the saved mode for failsafe been set?
|
||||||
uint8_t saved_mode_set:1;
|
uint8_t saved_mode_set:1;
|
||||||
@ -341,14 +341,14 @@ private:
|
|||||||
// Used for failsafe based on loss of RC signal or GCS signal
|
// Used for failsafe based on loss of RC signal or GCS signal
|
||||||
int16_t state;
|
int16_t state;
|
||||||
|
|
||||||
// number of low ch3 values
|
// number of low throttle values
|
||||||
uint8_t ch3_counter;
|
uint8_t throttle_counter;
|
||||||
|
|
||||||
// the time when the last HEARTBEAT message arrived from a GCS
|
// the time when the last HEARTBEAT message arrived from a GCS
|
||||||
uint32_t last_heartbeat_ms;
|
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
|
// 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;
|
uint32_t last_valid_rc_ms;
|
||||||
|
|
||||||
|
@ -9,8 +9,8 @@ void Plane::read_control_switch()
|
|||||||
// If we get this value we do not want to change modes.
|
// If we get this value we do not want to change modes.
|
||||||
if(switchPosition == 255) return;
|
if(switchPosition == 255) return;
|
||||||
|
|
||||||
if (failsafe.ch3_failsafe || failsafe.ch3_counter > 0) {
|
if (failsafe.rc_failsafe || failsafe.throttle_counter > 0) {
|
||||||
// when we are in ch3_failsafe mode then RC input is not
|
// when we are in rc_failsafe mode then RC input is not
|
||||||
// working, and we need to ignore the mode switch channel
|
// working, and we need to ignore the mode switch channel
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -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.
|
// This is how to handle a short loss of control signal failsafe.
|
||||||
failsafe.state = fstype;
|
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);
|
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason);
|
||||||
switch(control_mode)
|
switch(control_mode)
|
||||||
{
|
{
|
||||||
|
@ -1064,7 +1064,7 @@ float QuadPlane::get_desired_yaw_rate_cds(void)
|
|||||||
// get pilot desired climb rate in cm/s
|
// get pilot desired climb rate in cm/s
|
||||||
float QuadPlane::get_pilot_desired_climb_rate_cms(void)
|
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
|
// descend at 0.5m/s for now
|
||||||
return -50;
|
return -50;
|
||||||
}
|
}
|
||||||
@ -1461,8 +1461,8 @@ void QuadPlane::update(void)
|
|||||||
// disable throttle_wait when throttle rises above 10%
|
// disable throttle_wait when throttle rises above 10%
|
||||||
if (throttle_wait &&
|
if (throttle_wait &&
|
||||||
(plane.channel_throttle->get_control_in() > 10 ||
|
(plane.channel_throttle->get_control_in() > 10 ||
|
||||||
plane.failsafe.ch3_failsafe ||
|
plane.failsafe.rc_failsafe ||
|
||||||
plane.failsafe.ch3_counter>0)) {
|
plane.failsafe.throttle_counter>0)) {
|
||||||
throttle_wait = false;
|
throttle_wait = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -176,7 +176,7 @@ void Plane::read_radio()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(!failsafe.ch3_failsafe)
|
if(!failsafe.rc_failsafe)
|
||||||
{
|
{
|
||||||
failsafe.AFS_last_valid_rc_ms = millis();
|
failsafe.AFS_last_valid_rc_ms = millis();
|
||||||
}
|
}
|
||||||
@ -264,27 +264,27 @@ void Plane::control_failsafe(uint16_t pwm)
|
|||||||
if (rc_failsafe_active()) {
|
if (rc_failsafe_active()) {
|
||||||
// we detect a failsafe from radio
|
// we detect a failsafe from radio
|
||||||
// throttle has dropped below the mark
|
// throttle has dropped below the mark
|
||||||
failsafe.ch3_counter++;
|
failsafe.throttle_counter++;
|
||||||
if (failsafe.ch3_counter == 10) {
|
if (failsafe.throttle_counter == 10) {
|
||||||
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on %u", (unsigned)pwm);
|
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;
|
AP_Notify::flags.failsafe_radio = true;
|
||||||
}
|
}
|
||||||
if (failsafe.ch3_counter > 10) {
|
if (failsafe.throttle_counter > 10) {
|
||||||
failsafe.ch3_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
|
// we are no longer in failsafe condition
|
||||||
// but we need to recover quickly
|
// but we need to recover quickly
|
||||||
failsafe.ch3_counter--;
|
failsafe.throttle_counter--;
|
||||||
if (failsafe.ch3_counter > 3) {
|
if (failsafe.throttle_counter > 3) {
|
||||||
failsafe.ch3_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);
|
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off %u", (unsigned)pwm);
|
||||||
} else if(failsafe.ch3_counter == 0) {
|
} else if(failsafe.throttle_counter == 0) {
|
||||||
failsafe.ch3_failsafe = false;
|
failsafe.rc_failsafe = false;
|
||||||
AP_Notify::flags.failsafe_radio = false;
|
AP_Notify::flags.failsafe_radio = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -359,7 +359,7 @@ void Plane::set_servos_controlled(void)
|
|||||||
control_mode == ACRO ||
|
control_mode == ACRO ||
|
||||||
control_mode == FLY_BY_WIRE_A ||
|
control_mode == FLY_BY_WIRE_A ||
|
||||||
control_mode == AUTOTUNE) &&
|
control_mode == AUTOTUNE) &&
|
||||||
!failsafe.ch3_counter) {
|
!failsafe.throttle_counter) {
|
||||||
// manual pass through of throttle while in FBWA or
|
// manual pass through of throttle while in FBWA or
|
||||||
// STABILIZE mode with THR_PASS_STAB set
|
// STABILIZE mode with THR_PASS_STAB set
|
||||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in_zero_dz());
|
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
|
// work out any manual flap input
|
||||||
RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1);
|
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();
|
flapin->input();
|
||||||
manual_flap_percent = flapin->percent_input();
|
manual_flap_percent = flapin->percent_input();
|
||||||
}
|
}
|
||||||
|
@ -482,9 +482,9 @@ void Plane::check_long_failsafe()
|
|||||||
uint32_t radio_timeout_ms = failsafe.last_valid_rc_ms;
|
uint32_t radio_timeout_ms = failsafe.last_valid_rc_ms;
|
||||||
if (failsafe.state == FAILSAFE_SHORT) {
|
if (failsafe.state == FAILSAFE_SHORT) {
|
||||||
// time is relative to when short failsafe enabled
|
// 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) {
|
(tnow - radio_timeout_ms) > g.long_fs_timeout*1000) {
|
||||||
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
|
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
|
||||||
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO &&
|
} 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) {
|
(tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) {
|
||||||
failsafe_long_off_event(MODE_REASON_GCS_FAILSAFE);
|
failsafe_long_off_event(MODE_REASON_GCS_FAILSAFE);
|
||||||
} else if (failsafe.state == FAILSAFE_LONG &&
|
} else if (failsafe.state == FAILSAFE_LONG &&
|
||||||
!failsafe.ch3_failsafe) {
|
!failsafe.rc_failsafe) {
|
||||||
failsafe_long_off_event(MODE_REASON_RADIO_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 &&
|
if (g.short_fs_action != SHORT_FS_ACTION_DISABLED &&
|
||||||
failsafe.state == FAILSAFE_NONE &&
|
failsafe.state == FAILSAFE_NONE &&
|
||||||
flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
||||||
// The condition is checked and the flag ch3_failsafe is set in radio.cpp
|
// The condition is checked and the flag rc_failsafe is set in radio.cpp
|
||||||
if(failsafe.ch3_failsafe) {
|
if(failsafe.rc_failsafe) {
|
||||||
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
|
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if(failsafe.state == FAILSAFE_SHORT) {
|
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);
|
failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user