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{};