forked from Archive/PX4-Autopilot
Use arm_request for manual killing
This commit is contained in:
parent
af607e3040
commit
f8e4846851
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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<vehicle_command_s> 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{};
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
Loading…
Reference in New Issue