From fc7c8b4b89f590e7f4778ae0085eaf4742a5c221 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 20 Dec 2017 18:01:27 -0500 Subject: [PATCH] vehicle_status delete engine_failure_cmd --- msg/vehicle_status.msg | 1 - src/modules/commander/commander.cpp | 8 -------- src/modules/commander/state_machine_helper.cpp | 6 +----- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++-------- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 2 +- 5 files changed, 5 insertions(+), 23 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 69eb7b1b64..a60f550aec 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -63,7 +63,6 @@ uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual co bool data_link_lost # datalink to GCS lost uint8 data_link_lost_counter # counts unique data link lost events bool engine_failure # Set to true if an engine failure is detected -bool engine_failure_cmd # Set to true if an engine failure mode is commanded bool mission_failure # Set to true if mission could not continue/finish # see SYS_STATUS mavlink message for the following diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d9b9da5773..3580678077 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -957,17 +957,9 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); - /* param2 is currently used for other failsafe modes */ - status_local->engine_failure_cmd = false; - if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); - - } else if ((int)cmd->param2 == 1) { - /* trigger engine failure mode */ - status_local->engine_failure_cmd = true; - warnx("engine failure mode commanded"); } } else { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3decaf112e..34259fb89c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,11 +668,7 @@ bool set_nav_state(struct vehicle_status_s *status, * - if we have vtol transition failure * - depending on datalink, RC and if the mission is finished */ - /* first look at the commands */ - if (status->engine_failure_cmd) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - - } else if (status_flags->gps_failure) { + if (status_flags->gps_failure) { if (status->is_rotary_wing) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index cedeacfe70..85b56c155c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1125,10 +1125,8 @@ FixedwingAttitudeControl::task_main() } /* throttle passed through if it is finite and if no engine failure was detected */ - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) && - !(_vehicle_status.engine_failure || - _vehicle_status.engine_failure_cmd)) ? - throttle_sp : 0.0f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) + && !_vehicle_status.engine_failure) ? throttle_sp : 0.0f; /* scale effort by battery status */ if (_parameters.bat_scale_en && _battery_status.scale > 0.0f && @@ -1169,10 +1167,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(throttle_sp) && - //!(_vehicle_status.engine_failure || - !_vehicle_status.engine_failure_cmd) ? - throttle_sp : 0.0f; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; } /* diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 8924532a8c..5913d15b86 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1786,7 +1786,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee _reinitialize_tecs = false; } - if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + if (_vehicle_status.engine_failure) { /* Force the slow downwards spiral */ pitch_min_rad = M_DEG_TO_RAD_F * -1.0f; pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;