diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 0ba9691742..239e1f7489 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/mode_request.msg b/msg/mode_request.msg new file mode 100644 index 0000000000..5050d1936b --- /dev/null +++ b/msg/mode_request.msg @@ -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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 61a67956f5..32739ba214 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3805d35dca..c7318bf476 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -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)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 931da999d5..b5e602b935 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 50050a0758..28b3f8542e 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -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 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 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 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 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 e0f4d94237..d4a329d944 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -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_pub{ORB_ID(arm_request)}; uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::Publication _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};