From e828ba4288f96225dd3ff4e694a6dc84a0a256ef Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 15 Jun 2021 11:50:30 +0200 Subject: [PATCH] commander: send parachute command on termination (#17564) * commander: send parachute command on termination This sends the DO_PARACHUTE command to parachute component. * commander: fix lying comments and printf * commander: use one flag for termination triggered This merges the duplicate flags _flight_termination_triggered and _flight_flight_termination_printed. * commander: correct variable name * commander: always send tune with parachute * commander: fix target_component for parachute cmd The previous changes were wrong in that all commands were now sent to the parachute component which doesn't make any sense. Of course only the parachute command should be sent there. --- msg/vehicle_command.msg | 4 +++ src/modules/commander/Commander.cpp | 55 ++++++++++++++++++++--------- src/modules/commander/Commander.hpp | 3 +- 3 files changed, 45 insertions(+), 17 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 089934f103..6d69141068 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -120,6 +120,10 @@ uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1 # Continuous zoom up/down unt uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2 # Zoom value as proportion of full camera range uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3 # Zoom to a focal length +uint8 PARACHUTE_ACTION_DISABLE = 0 +uint8 PARACHUTE_ACTION_ENABLE = 1 +uint8 PARACHUTE_ACTION_RELEASE = 2 + uint8 FAILURE_UNIT_SENSOR_GYRO = 0 uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 uint8 FAILURE_UNIT_SENSOR_MAG = 2 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3cf8a473c2..10cabc1547 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -915,6 +915,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd.param1 > 1.5f) { + // Test termination command triggers lockdown but not actual termination. if (!_lockdown_triggered) { _armed.lockdown = true; _lockdown_triggered = true; @@ -922,16 +923,12 @@ Commander::handle_command(const vehicle_command_s &cmd) } } else if (cmd.param1 > 0.5f) { - //XXX update state machine? + // Trigger real termination. if (!_flight_termination_triggered) { _armed.force_failsafe = true; _flight_termination_triggered = true; PX4_WARN("forcing failsafe (termination)"); - } - - if ((int)cmd.param2 <= 0) { - /* reset all commanded failure modes */ - PX4_WARN("reset all non-flighttermination failsafe commands"); + send_parachute_command(); } } else { @@ -2170,9 +2167,15 @@ Commander::run() case (geofence_result_s::GF_ACTION_TERMINATE) : { PX4_WARN("Flight termination because of geofence"); - mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); - _armed.force_failsafe = true; - _status_changed = true; + + if (!_flight_termination_triggered && !_lockdown_triggered) { + _flight_termination_triggered = true; + mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); + _armed.force_failsafe = true; + _status_changed = true; + send_parachute_command(); + } + break; } } @@ -2220,12 +2223,13 @@ Commander::run() if (_armed.armed && _mission_result_sub.get().flight_termination && !_status_flags.circuit_breaker_flight_termination_disabled) { - _armed.force_failsafe = true; - _status_changed = true; - if (!_flight_termination_printed) { + if (!_flight_termination_triggered && !_lockdown_triggered) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated"); - _flight_termination_printed = true; + _flight_termination_triggered = true; + _armed.force_failsafe = true; + _status_changed = true; + send_parachute_command(); } if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { @@ -2474,9 +2478,9 @@ Commander::run() if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) { - const bool is_second_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); + const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); - if (is_second_after_takeoff && !_lockdown_triggered) { + if (is_right_after_takeoff && !_lockdown_triggered) { // This handles the case where something fails during the early takeoff phase _armed.lockdown = true; _lockdown_triggered = true; @@ -2488,7 +2492,7 @@ Commander::run() _armed.force_failsafe = true; _flight_termination_triggered = true; mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight"); - set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); + send_parachute_command(); } } } @@ -4056,6 +4060,25 @@ void Commander::esc_status_check() _last_esc_status_updated = esc_status.timestamp; } +void Commander::send_parachute_command() +{ + vehicle_command_s vcmd{}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE; + vcmd.param1 = static_cast(vehicle_command_s::PARACHUTE_ACTION_RELEASE); + + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = vehicle_status_sub.get().system_id; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd.timestamp = hrt_absolute_time(); + vcmd_pub.publish(vcmd); + + set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); +} + int Commander::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index c95313a22f..6b696295da 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -187,6 +187,8 @@ private: bool stabilization_required(); + void send_parachute_command(); + DEFINE_PARAMETERS( (ParamInt) _param_nav_dll_act, @@ -373,7 +375,6 @@ private: bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag bool _have_taken_off_since_arming{false}; bool _should_set_home_on_takeoff{true}; - bool _flight_termination_printed{false}; bool _system_power_usb_connected{false}; cpuload_s _cpuload{};