dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

manual_control: implement (dis)arming via command

This commit is contained in:
Julian Oes 2021-04-15 14:08:26 +02:00 committed by Matthias Grob
parent 8876af9150
commit 1c15cc11d8
3 changed files with 88 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/modules/commander/Commander.cpp
View File

@ -921,6 +921,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(roundf(cmd.param2)) == 21196);
const bool cmd_from_manual_stick = (static_cast<int>(roundf(cmd.param3)) == 1);
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
if (cmd_arms) {
@ -948,8 +957,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
}
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
// Flick to in-air restore first if this comes from an onboard system and from IO
if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
&& cmd_from_io && cmd_arms) {
@ -964,7 +971,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
arming_res = arm(arm_disarm_reason_t::command_external);
} else {
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
if (cmd_from_manual_stick) {
arming_res = arm(arm_disarm_reason_t::rc_stick, !forced);
} else {
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
}
}
} else {
@ -972,7 +983,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
arming_res = disarm(arm_disarm_reason_t::command_external);
} else {
arming_res = disarm(arm_disarm_reason_t::command_internal);
if (cmd_from_manual_stick) {
arming_res = disarm(arm_disarm_reason_t::rc_stick);
} else {
arming_res = disarm(arm_disarm_reason_t::command_internal);
}
}
}
@ -2383,26 +2398,6 @@ Commander::run()
_status.rc_signal_lost = false;
const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
if (rc_arming_enabled) {
if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
disarm(arm_disarm_reason_t::rc_stick);
}
if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
arm(arm_disarm_reason_t::rc_stick);
} else {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first\t");
events::send(events::ID("commander_arming_denied_not_manual"), {events::Log::Error, events::LogInternal::Info},
"Not arming! Switch to a manual mode first");
tune_negative(true);
}
}
}
// abort autonomous mode and switch to position mode if sticks are moved significantly
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on

65
src/modules/manual_control/ManualControl.cpp
View File

@ -34,6 +34,7 @@
#include "ManualControl.hpp"
#include <drivers/drv_hrt.h>
#include <uORB/topics/vehicle_command.h>
namespace manual_control
{
@ -91,12 +92,17 @@ void ManualControl::Run()
if (_manual_control_input_subs[i].update(&manual_control_input)) {
found_at_least_one = true;
_selector.update_manual_control_input(now, manual_control_input, i);
}
}
bool switches_updated = false;
manual_control_switches_s manual_control_switches;
if (_manual_control_switches_sub.update(&manual_control_switches)) {
switches_updated = true;
}
if (!found_at_least_one) {
_selector.update_time_only(now);
}
@ -114,6 +120,21 @@ void ManualControl::Run()
_selector.setpoint().arm_gesture = _stick_arm_hysteresis.get_state();
_selector.setpoint().disarm_gesture = _stick_disarm_hysteresis.get_state();
if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) {
_previous_arm_gesture = true;
send_arm_command();
} else if (!_selector.setpoint().arm_gesture) {
_previous_arm_gesture = false;
}
if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) {
_previous_disarm_gesture = true;
send_disarm_command();
} else if (!_selector.setpoint().disarm_gesture) {
_previous_disarm_gesture = false;
}
// user wants override
//const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
@ -129,18 +150,26 @@ void ManualControl::Run()
//_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved);
// TODO: at least 3 samples in a time
if (switches_updated) {
// Only use switches if current source is RC as well.
if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) {
// TODO: handle buttons
}
}
_selector.setpoint().timestamp = now;
_manual_control_setpoint_pub.publish(_selector.setpoint());
_manual_control_input_subs[_selector.instance()].registerCallback();
if (_last_selected_input != _selector.instance()) {
PX4_INFO("selected manual_control_input changed %d -> %d", _last_selected_input, _selector.instance());
_last_selected_input = _selector.instance();
}
_manual_control_input_subs[_selector.instance()].registerCallback();
_manual_control_switches_sub.registerCallback();
} else {
_last_selected_input = -1;
@ -156,6 +185,34 @@ void ManualControl::Run()
perf_end(_loop_perf);
}
void ManualControl::send_arm_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
command.param1 = 1.0;
command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::send_disarm_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
command.param1 = 0.0;
command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
int ManualControl::task_spawn(int argc, char *argv[])
{
ManualControl *instance = new ManualControl();

8
src/modules/manual_control/ManualControl.hpp
View File

@ -41,6 +41,7 @@
#include <lib/hysteresis/hysteresis.h>
#include <lib/perf/perf_counter.h>
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Publication.hpp>
@ -84,6 +85,9 @@ private:
void Run() override;
void send_arm_command();
void send_disarm_command();
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@ -92,6 +96,7 @@ private:
{this, ORB_ID(manual_control_input), 1},
{this, ORB_ID(manual_control_input), 2},
};
uORB::SubscriptionCallbackWorkItem _manual_control_switches_sub{this, ORB_ID(manual_control_switches)};
systemlib::Hysteresis _stick_arm_hysteresis{false};
systemlib::Hysteresis _stick_disarm_hysteresis{false};
@ -100,6 +105,9 @@ private:
bool _published_invalid_once{false};
int _last_selected_input{-1};
bool _previous_arm_gesture{false};
bool _previous_disarm_gesture{false};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};