Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

Use mode_request for RC mode switching

Browse Source
This commit is contained in:
Matthias Grob
2021-10-19 15:06:17 +02:00
parent f8e4846851
commit 052e29267d
7 changed files with 44 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
+1
View File
@@ -109,6 +109,7 @@ set(msg_files
mavlink_log.msg
mission.msg
mission_result.msg
mode_request.msg
mount_orientation.msg
multirotor_motor_limits.msg
navigator_mission_item.msg
msg/mode_request.msg
+7
View File
@@ -0,0 +1,7 @@
uint64 timestamp # time since system start (microseconds)
uint8 mode # what mode is requested according to commander_state.MAIN_STATE_
uint8 source # how the request was triggered
uint8 SOURCE_RC_MODE_SLOT = 0
uint8 SOURCE_RC_SWITCH = 1
src/modules/commander/Commander.cpp
+8
View File
@@ -2512,6 +2512,14 @@ Commander::run()
}
}
while (_mode_request_sub.updated()) {
mode_request_s mode_request;
if (_mode_request_sub.copy(&mode_request)) {
main_state_transition(_status, mode_request.mode, _status_flags, _internal_state);
}
}
/* Check for failure detector status */
if (_failure_detector.update(_status, _vehicle_control_mode)) {
_status.failure_detector_status = _failure_detector.getStatus().value;
src/modules/commander/Commander.hpp
+2
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/mode_request.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
@@ -399,6 +400,7 @@ private:
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _mode_request_sub {ORB_ID(mode_request)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
src/modules/logger/logged_topics.cpp
+1
View File
@@ -73,6 +73,7 @@ void LoggedTopics::add_default_topics()
add_topic("manual_control_setpoint", 200);
add_topic("manual_control_switches");
add_topic("mission_result");
add_topic("mode_request");
add_topic("navigator_mission_item");
add_topic("offboard_control_mode", 100);
add_topic("onboard_computer_status", 10);
src/modules/manual_control/ManualControl.cpp
+21 -152
View File
@@ -156,7 +156,7 @@ void ManualControl::Run()
if (_selector.setpoint().chosen_input.data_source == manual_control_input_s::SOURCE_RC) {
if (_previous_switches_initialized) {
if (switches.mode_slot != _previous_switches.mode_slot) {
evaluate_mode_slot(switches.mode_slot);
evaluateModeSlot(switches.mode_slot);
}
if (_param_com_arm_swisbtn.get()) {
@@ -182,28 +182,28 @@ void ManualControl::Run()
if (switches.return_switch != _previous_switches.return_switch) {
if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_rtl_command();
sendModeRequest(commander_state_s::MAIN_STATE_AUTO_RTL, mode_request_s::SOURCE_RC_SWITCH);
} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
evaluateModeSlot(switches.mode_slot);
}
}
if (switches.loiter_switch != _previous_switches.loiter_switch) {
if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_loiter_command();
sendModeRequest(commander_state_s::MAIN_STATE_AUTO_LOITER, mode_request_s::SOURCE_RC_SWITCH);
} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
evaluateModeSlot(switches.mode_slot);
}
}
if (switches.offboard_switch != _previous_switches.offboard_switch) {
if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_offboard_command();
sendModeRequest(commander_state_s::MAIN_STATE_OFFBOARD, mode_request_s::SOURCE_RC_SWITCH);
} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
evaluateModeSlot(switches.mode_slot);
}
}
@@ -238,7 +238,7 @@ void ManualControl::Run()
} else {
// Send an initial command to switch to the mode requested by RC
evaluate_mode_slot(switches.mode_slot);
evaluateModeSlot(switches.mode_slot);
}
_previous_switches_initialized = true;
@@ -246,7 +246,6 @@ void ManualControl::Run()
} else {
_previous_switches_initialized = false;
_last_mode_slot_flt = -1;
}
}
@@ -285,135 +284,49 @@ void ManualControl::Run()
perf_end(_loop_perf);
}
void ManualControl::evaluate_mode_slot(uint8_t mode_slot)
void ManualControl::evaluateModeSlot(uint8_t mode_slot)
{
switch (mode_slot) {
case manual_control_switches_s::MODE_SLOT_NONE:
_last_mode_slot_flt = -1;
break;
case manual_control_switches_s::MODE_SLOT_1:
_last_mode_slot_flt = _param_fltmode_1.get();
sendModeRequest(_param_fltmode_1.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
case manual_control_switches_s::MODE_SLOT_2:
_last_mode_slot_flt = _param_fltmode_2.get();
sendModeRequest(_param_fltmode_2.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
case manual_control_switches_s::MODE_SLOT_3:
_last_mode_slot_flt = _param_fltmode_3.get();
sendModeRequest(_param_fltmode_3.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
case manual_control_switches_s::MODE_SLOT_4:
_last_mode_slot_flt = _param_fltmode_4.get();
sendModeRequest(_param_fltmode_4.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
case manual_control_switches_s::MODE_SLOT_5:
_last_mode_slot_flt = _param_fltmode_5.get();
sendModeRequest(_param_fltmode_5.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
case manual_control_switches_s::MODE_SLOT_6:
_last_mode_slot_flt = _param_fltmode_6.get();
sendModeRequest(_param_fltmode_6.get(), mode_request_s::SOURCE_RC_MODE_SLOT);
break;
default:
_last_mode_slot_flt = -1;
PX4_WARN("mode slot overflow");
break;
}
send_mode_command(_last_mode_slot_flt);
}
void ManualControl::send_mode_command(int32_t commander_main_state)
void ManualControl::sendModeRequest(uint8_t mode, uint8_t source)
{
if (commander_main_state == -1) {
// Not assigned.
return;
}
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
switch (commander_main_state) {
case commander_state_s::MAIN_STATE_MANUAL:
command.param2 = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case commander_state_s::MAIN_STATE_ALTCTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case commander_state_s::MAIN_STATE_POSCTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL;
command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL;
break;
case commander_state_s::MAIN_STATE_AUTO_MISSION:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case commander_state_s::MAIN_STATE_AUTO_LOITER:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case commander_state_s::MAIN_STATE_AUTO_RTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case commander_state_s::MAIN_STATE_ACRO:
command.param2 = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case commander_state_s::MAIN_STATE_OFFBOARD:
command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case commander_state_s::MAIN_STATE_STAB:
command.param2 = PX4_CUSTOM_MAIN_MODE_STABILIZED;
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case commander_state_s::MAIN_STATE_AUTO_LAND:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
// FALLTHROUGH
case commander_state_s::MAIN_STATE_ORBIT:
PX4_WARN("Unhandled main_state");
return;
case commander_state_s::MAIN_STATE_MAX:
// FALLTHROUGH
default:
PX4_WARN("Unknown main_state");
return;
}
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);
mode_request_s mode_request{};
mode_request.mode = mode;
mode_request.source = source;
mode_request.timestamp = hrt_absolute_time();
_mode_request_pub.publish(mode_request);
}
void ManualControl::sendArmRequest(int8_t action, int8_t source)
@@ -425,50 +338,6 @@ void ManualControl::sendArmRequest(int8_t action, int8_t source)
_arm_request_pub.publish(arm_request);
}
void ManualControl::send_rtl_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
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::send_loiter_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
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::send_offboard_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
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{};
src/modules/manual_control/ManualControl.hpp
+4 -6
View File
@@ -44,6 +44,7 @@
#include <uORB/topics/manual_control_input.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mode_request.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
@@ -119,17 +120,15 @@ private:
void Run() override;
void evaluate_mode_slot(uint8_t mode_slot);
void send_mode_command(int32_t commander_main_state);
void evaluateModeSlot(uint8_t mode_slot);
void sendModeRequest(uint8_t mode, uint8_t source);
void sendArmRequest(int8_t action, int8_t source);
void send_rtl_command();
void send_loiter_command();
void send_offboard_command();
void publish_landing_gear(int8_t action);
void send_vtol_transition_command(uint8_t action);
uORB::Publication<arm_request_s> _arm_request_pub{ORB_ID(arm_request)};
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::Publication<mode_request_s> _mode_request_pub{ORB_ID(mode_request)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] {
@@ -153,7 +152,6 @@ private:
manual_control_switches_s _previous_switches{};
bool _previous_switches_initialized{false};
int32_t _last_mode_slot_flt{-1};
hrt_abstime _last_time{0};