Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: failsafe.manual_control -> failsafe.pilot_input 

reset pilot input failsafe timer when RC_CHANNELS_OVERRIDE is received
This commit is contained in:
Jacob Walser 2017-04-14 15:58:54 -04:00
parent 8d1b6ef8a1
commit b7de1eb88a
9 changed files with 24 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -210,7 +210,7 @@ void Sub::fast_loop()
void Sub::fifty_hz_loop()
{
// check pilot input failsafe
failsafe_manual_control_check();
failsafe_pilot_input_check();
failsafe_crash_check();

2
ArduSub/Attitude.cpp
View File

@ -97,7 +97,7 @@ float Sub::get_look_ahead_yaw()
float Sub::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if (failsafe.manual_control) {
if (failsafe.pilot_input) {
return 0.0f;
}

8
ArduSub/GCS_Mavlink.cpp
View File

@ -34,7 +34,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.manual_control || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
if (failsafe.pilot_input || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
system_status = MAV_STATE_CRITICAL;
}
@ -1023,7 +1023,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons);
sub.failsafe.last_manual_control_ms = AP_HAL::millis();
sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;
@ -1047,9 +1047,9 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
sub.failsafe.rc_override_active = hal.rcin->set_overrides(v, 8);
hal.rcin->set_overrides(v, 8);
sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;

2
ArduSub/Sub.cpp
View File

@ -92,7 +92,7 @@ Sub::Sub(void) :
failsafe.last_heartbeat_ms = 0;
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
failsafe.manual_control = true;
failsafe.pilot_input = true;
#endif
}

7
ArduSub/Sub.h
View File

@ -259,8 +259,7 @@ private:
// Failsafe
struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t manual_control : 1; // 1 // A status flag for the radio failsafe
uint8_t pilot_input : 1; // true if pilot input failsafe is active, handles things like joystick being disconnected during operation
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
@ -273,7 +272,7 @@ private:
uint32_t last_gcs_warn_ms;
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
uint32_t last_manual_control_ms; // last time MANUAL_CONTROL message arrived from GCS
uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs
@ -595,7 +594,7 @@ private:
void failsafe_ekf_check(void);
void failsafe_battery_check(void);
void failsafe_gcs_check();
void failsafe_manual_control_check(void);
void failsafe_pilot_input_check(void);
void set_neutral_controls(void);
void failsafe_terrain_check();
void failsafe_terrain_set_status(bool data_ok);

6
ArduSub/control_auto.cpp
View File

@ -125,7 +125,7 @@ void Sub::auto_wp_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -211,7 +211,7 @@ void Sub::auto_spline_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -398,7 +398,7 @@ void Sub::auto_loiter_run()
// accept pilot input of yaw
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}

6
ArduSub/control_guided.cpp
View File

@ -288,7 +288,7 @@ void Sub::guided_pos_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -339,7 +339,7 @@ void Sub::guided_vel_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
@ -395,7 +395,7 @@ void Sub::guided_posvel_control_run()
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.manual_control) {
if (!failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {

13
ArduSub/failsafe.cpp
View File

@ -164,17 +164,16 @@ void Sub::failsafe_battery_check(void)
}
}
// MANUAL_CONTROL failsafe check
// Make sure that we are receiving MANUAL_CONTROL at an appropriate interval
void Sub::failsafe_manual_control_check()
// Make sure that we are receiving pilot input at an appropriate interval
void Sub::failsafe_pilot_input_check()
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
uint32_t tnow = AP_HAL::millis();
// Require at least 0.5 Hz update
if (tnow > failsafe.last_manual_control_ms + 2000) {
if (!failsafe.manual_control) {
failsafe.manual_control = true;
if (tnow > failsafe.last_pilot_input_ms + 2000) {
if (!failsafe.pilot_input) {
failsafe.pilot_input = true;
set_neutral_controls();
init_disarm_motors();
Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
@ -183,7 +182,7 @@ void Sub::failsafe_manual_control_check()
return;
}
failsafe.manual_control = false;
failsafe.pilot_input = false;
#endif
}

5
ArduSub/joystick.cpp
View File

@ -117,8 +117,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
y_last = y;
z_last = z;
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
hal.rcin->set_overrides(channels, 10);
}
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
@ -570,5 +569,5 @@ void Sub::set_neutral_controls()
channels[4] = 0xffff; // Leave mode switch where it was
failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
hal.rcin->set_overrides(channels, 10);
}