diff --git a/msg/arm_request.msg b/msg/arm_request.msg index 426f3ab18c..f97abf319f 100644 --- a/msg/arm_request.msg +++ b/msg/arm_request.msg @@ -3,7 +3,9 @@ uint64 timestamp # time since system start (microseconds) uint8 action # what action is requested uint8 ACTION_DISARM = 0 uint8 ACTION_ARM = 1 -uint8 ACTION_TOGGLE = 2 +uint8 ACTION_TOGGLE_ARMING = 2 +uint8 ACTION_UNKILL = 3 +uint8 ACTION_KILL = 4 uint8 source # how the request was triggered uint8 SOURCE_RC_STICK_GESTURE = 0 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b40bd957d3..61a67956f5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2468,7 +2468,7 @@ Commander::run() case arm_request_s::ACTION_ARM: arm(arm_disarm_reason); break; - case arm_request_s::ACTION_TOGGLE: + case arm_request_s::ACTION_TOGGLE_ARMING: if (_armed.armed) { disarm(arm_disarm_reason); @@ -2476,6 +2476,37 @@ Commander::run() arm(arm_disarm_reason); } + break; + + case arm_request_s::ACTION_UNKILL: + if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _armed.manual_lockdown) { + mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t"); + events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged"); + _status_changed = true; + _armed.manual_lockdown = false; + } + + break; + + case arm_request_s::ACTION_KILL: + if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_armed.manual_lockdown) { + const char kill_switch_string[] = "Kill-switch engaged\t"; + events::LogLevels log_levels{events::Log::Info}; + + if (_land_detector.landed) { + mavlink_log_info(&_mavlink_log_pub, kill_switch_string); + + } else { + mavlink_log_critical(&_mavlink_log_pub, kill_switch_string); + log_levels.external = events::Log::Critical; + } + + events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill-switch engaged"); + + _status_changed = true; + _armed.manual_lockdown = true; + } + break; } } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 229129e4c7..931da999d5 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,6 +53,7 @@ void LoggedTopics::add_default_topics() add_topic("actuator_controls_status_0", 300); add_topic("airspeed", 1000); add_topic("airspeed_validated", 200); + add_topic("arm_request"); add_topic("autotune_attitude_control_status", 100); add_topic("camera_capture"); add_topic("camera_trigger"); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index d451446b91..50050a0758 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -165,7 +165,7 @@ void ManualControl::Run() _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); if (!previous_button_hysteresis && _button_hysteresis.get_state()) { - sendArmRequest(arm_request_s::ACTION_TOGGLE, arm_request_s::SOURCE_RC_BUTTON); + sendArmRequest(arm_request_s::ACTION_TOGGLE_ARMING, arm_request_s::SOURCE_RC_BUTTON); } } else { @@ -209,10 +209,10 @@ void ManualControl::Run() if (switches.kill_switch != _previous_switches.kill_switch) { if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_termination_command(true); + sendArmRequest(arm_request_s::ACTION_KILL, arm_request_s::SOURCE_RC_SWITCH); } else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { - send_termination_command(false); + sendArmRequest(arm_request_s::ACTION_UNKILL, arm_request_s::SOURCE_RC_SWITCH); } } @@ -469,19 +469,6 @@ void ManualControl::send_offboard_command() command_pub.publish(command); } -void ManualControl::send_termination_command(bool should_terminate) -{ - vehicle_command_s command{}; - command.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; - command.param1 = should_terminate ? 1.0f : 0.0f; - command.target_system = _param_mav_sys_id.get(); - command.target_component = _param_mav_comp_id.get(); - - uORB::Publication command_pub{ORB_ID(vehicle_command)}; - command.timestamp = hrt_absolute_time(); - command_pub.publish(command); -} - void ManualControl::publish_landing_gear(int8_t action) { landing_gear_s landing_gear{}; diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 39d6f493b4..e0f4d94237 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -125,7 +125,6 @@ private: void send_rtl_command(); void send_loiter_command(); void send_offboard_command(); - void send_termination_command(bool should_terminate); void publish_landing_gear(int8_t action); void send_vtol_transition_command(uint8_t action);