diff --git a/msg/ArmingCheckReply.msg b/msg/ArmingCheckReply.msg new file mode 100644 index 0000000000..375fb5269d --- /dev/null +++ b/msg/ArmingCheckReply.msg @@ -0,0 +1,34 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 request_id +uint8 registration_id + +uint8 HEALTH_COMPONENT_INDEX_NONE = 0 +uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 + +uint8 health_component_index # HEALTH_COMPONENT_INDEX_* +bool health_component_is_present +bool health_component_warning +bool health_component_error + +bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run + +uint8 num_events + +Event[5] events + +# Mode requirements +bool mode_req_angular_velocity +bool mode_req_attitude +bool mode_req_local_alt +bool mode_req_local_position +bool mode_req_local_position_relaxed +bool mode_req_global_position +bool mode_req_mission +bool mode_req_home_position +bool mode_req_prevent_arming +bool mode_req_manual_control + + +uint8 ORB_QUEUE_LENGTH = 4 + diff --git a/msg/ArmingCheckRequest.msg b/msg/ArmingCheckRequest.msg new file mode 100644 index 0000000000..cdbb2b5bfe --- /dev/null +++ b/msg/ArmingCheckRequest.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +# broadcast message to request all registered arming checks to be reported + +uint8 request_id + + diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e2cf088979..ba54dd798f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -49,6 +49,8 @@ set(msg_files Airspeed.msg AirspeedValidated.msg AirspeedWind.msg + ArmingCheckReply.msg + ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg BatteryStatus.msg ButtonEvent.msg @@ -161,6 +163,8 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg + RegisterExtComponentReply.msg + RegisterExtComponentRequest.msg Rpm.msg RtlTimeEstimate.msg SatelliteInfo.msg @@ -198,6 +202,7 @@ set(msg_files UavcanParameterValue.msg UlogStream.msg UlogStreamAck.msg + UnregisterExtComponent.msg VehicleAcceleration.msg VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg diff --git a/msg/RegisterExtComponentReply.msg b/msg/RegisterExtComponentReply.msg new file mode 100644 index 0000000000..0223af711a --- /dev/null +++ b/msg/RegisterExtComponentReply.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID from the request +char[25] name # name from the request + +uint16 px4_ros2_api_version + +bool success +int8 arming_check_id # arming check registration ID (-1 if invalid) +int8 mode_id # assigned mode ID (-1 if invalid) +int8 mode_executor_id # assigned mode executor ID (-1 if invalid) + +uint8 ORB_QUEUE_LENGTH = 2 + diff --git a/msg/RegisterExtComponentRequest.msg b/msg/RegisterExtComponentRequest.msg new file mode 100644 index 0000000000..46ab0cb0a1 --- /dev/null +++ b/msg/RegisterExtComponentRequest.msg @@ -0,0 +1,21 @@ +# Request to register an external component +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID, set this to a random value +char[25] name # either the requested mode name, or component name + +uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change. + +uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION + +# Components to be registered +bool register_arming_check +bool register_mode # registering a mode also requires arming_check to be set +bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor) + +bool enable_replace_internal_mode # set to true if an internal mode should be replaced +uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_* +bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor) + + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/UnregisterExtComponent.msg b/msg/UnregisterExtComponent.msg new file mode 100644 index 0000000000..fc4754ded6 --- /dev/null +++ b/msg/UnregisterExtComponent.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +char[25] name # either the mode name, or component name + +int8 arming_check_id # arming check registration ID (-1 if not registered) +int8 mode_id # assigned mode ID (-1 if not registered) +int8 mode_executor_id # assigned mode executor ID (-1 if not registered) + + + diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 9ded909545..80043d5deb 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -174,8 +174,10 @@ uint32 command # Command ID uint8 target_system # System which should execute the command uint8 target_component # Component which should execute the command, 0 for all components uint8 source_system # System sending the command -uint8 source_component # Component sending the command +uint16 source_component # Component / mode executor sending the command uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) bool from_external -# TOPICS vehicle_command gimbal_v1_command +uint16 COMPONENT_MODE_EXECUTOR_START = 1000 + +# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor diff --git a/msg/VehicleCommandAck.msg b/msg/VehicleCommandAck.msg index ef43256af5..6f54fa4631 100644 --- a/msg/VehicleCommandAck.msg +++ b/msg/VehicleCommandAck.msg @@ -28,6 +28,6 @@ uint8 result # Command result uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. uint8 target_system -uint8 target_component +uint16 target_component # Target component / mode executor bool from_external # Indicates if the command came from an external source diff --git a/msg/VehicleControlMode.msg b/msg/VehicleControlMode.msg index 92324cf03f..e12f98dfde 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/VehicleControlMode.msg @@ -15,3 +15,8 @@ bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled bool flag_control_allocation_enabled # true if control allocation is enabled + +# TODO: use dedicated topic for external requests +uint8 source_id # Mode ID (nav_state) + +# TOPICS vehicle_control_mode config_control_setpoints diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index bf545ea02b..24bba10c95 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -52,7 +52,17 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter -uint8 NAVIGATION_STATE_MAX = 23 +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) # Bitmask of detected failures uint16 failure_detector_status diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index da5c2ade56..cf8ff346f7 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -110,6 +110,38 @@ "4194304": { "name": "vtol_takeoff", "description": "VTOL Takeoff" + }, + "8388608": { + "name": "external1", + "description": "External 1" + }, + "16777216": { + "name": "external2", + "description": "External 2" + }, + "33554432": { + "name": "external3", + "description": "External 3" + }, + "67108864": { + "name": "external4", + "description": "External 4" + }, + "134217728": { + "name": "external5", + "description": "External 5" + }, + "268435456": { + "name": "external6", + "description": "External 6" + }, + "536870912": { + "name": "external7", + "description": "External 7" + }, + "1073741824": { + "name": "external8", + "description": "External 8" } } }, @@ -532,6 +564,38 @@ "name": "auto_vtol_takeoff", "description": "Vtol Takeoff" }, + "16": { + "name": "external1", + "description": "External 1" + }, + "17": { + "name": "external2", + "description": "External 2" + }, + "18": { + "name": "external3", + "description": "External 3" + }, + "19": { + "name": "external4", + "description": "External 4" + }, + "20": { + "name": "external5", + "description": "External 5" + }, + "21": { + "name": "external6", + "description": "External 6" + }, + "22": { + "name": "external7", + "description": "External 7" + }, + "23": { + "name": "external8", + "description": "External 8" + }, "255": { "name": "unknown", "description": "[Unknown]" @@ -705,7 +769,15 @@ "19": [134479872], "20": [151257088], "21": [16973824], - "22": [168034304] + "22": [168034304], + "23": [184811520], + "24": [201588736], + "25": [218365952], + "26": [235143168], + "27": [251920384], + "28": [268697600], + "29": [285474816], + "30": [302252032] } }, "supported_protocols": [ "health_and_arming_check" ] diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 8e37de579b..4be4afc81b 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -53,6 +53,7 @@ px4_add_module( factory_calibration_storage.cpp gyro_calibration.cpp HomePosition.cpp + ModeManagement.cpp level_calibration.cpp lm_fit.cpp mag_calibration.cpp diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f5bdf73378..63322ecb6e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -407,6 +407,10 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND); + } else if (!strcmp(argv[1], "ext1")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_EXTERNAL1); + } else { PX4_ERR("argument %s unsupported.", argv[1]); } @@ -475,8 +479,9 @@ int Commander::print_status() { PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed"); PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]); - PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]); + PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]); PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no"); + _mode_management.printStatus(); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); return 0; @@ -727,7 +732,7 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -807,6 +812,10 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); + break; + default: main_ret = TRANSITION_DENIED; mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t"); @@ -848,7 +857,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) { - if (_user_mode_intention.change(desired_nav_state)) { + if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) { main_ret = TRANSITION_CHANGED; } else { @@ -969,7 +978,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { /* switch to RTL which ends the mission */ - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) { mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t"); events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch"); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -983,7 +992,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -997,7 +1006,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* ok, home set, use it to take off */ - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1011,7 +1020,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) { mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t"); events::send(events::ID("commander_landing_current_pos"), events::Log::Info, "Landing at current position"); @@ -1025,7 +1034,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) { mavlink_log_info(&_mavlink_log_pub, "Precision landing\t"); events::send(events::ID("commander_landing_prec_land"), events::Log::Info, "Landing using precision landing"); @@ -1049,7 +1058,7 @@ Commander::handle_command(const vehicle_command_s &cmd) if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { // switch to AUTO_MISSION and ARM - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd)) && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1088,7 +1097,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { // for fixed wings the behavior of orbit is the same as loiter - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) { main_ret = TRANSITION_CHANGED; } else { @@ -1097,7 +1106,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { // Switch to orbit state and let the orbit task handle the command further - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) { main_ret = TRANSITION_CHANGED; } else { @@ -1445,6 +1454,43 @@ Commander::handle_command(const vehicle_command_s &cmd) return true; } +ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd) +{ + return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor : + ModeChangeSource::User; +} + +void Commander::handleCommandsFromModeExecutors() +{ + if (_vehicle_command_mode_executor_sub.updated()) { + const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation(); + vehicle_command_s cmd; + + if (_vehicle_command_mode_executor_sub.copy(&cmd)) { + if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation, + _vehicle_command_mode_executor_sub.get_last_generation()); + } + + // For commands from mode executors, we check if it is in charge and then publish it on the official + // command topic + const int mode_executor_in_charge = _mode_management.modeExecutorInCharge(); + + // source_system is set to the mode executor + if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) { + cmd.source_system = _vehicle_status.system_id; + cmd.timestamp = hrt_absolute_time(); + _vehicle_command_pub.publish(cmd); + + } else { + cmd.source_system = _vehicle_status.system_id; + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge); + } + } + } +} + unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) { if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { @@ -1569,7 +1615,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_SWITCH_MODE: - if (!_user_mode_intention.change(action_request.mode, true)) { + if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) { printRejectMode(action_request.mode); } @@ -1719,6 +1765,8 @@ void Commander::run() _status_changed = true; } + modeManagementUpdate(); + const hrt_abstime now = hrt_absolute_time(); const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe(); @@ -1741,6 +1789,8 @@ void Commander::run() } // handle commands last, as the system needs to be updated to handle them + handleCommandsFromModeExecutors(); + if (_vehicle_command_sub.updated()) { // got command const unsigned last_generation = _vehicle_command_sub.get_last_generation(); @@ -1883,7 +1933,8 @@ void Commander::checkForMissionUpdate() if (isArmed() && !_vehicle_land_detected.landed && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) - && mission_result.finished) { + && mission_result.finished + && _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) { if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) { @@ -2209,13 +2260,16 @@ bool Commander::handleModeIntentionAndFailsafe() // Force intended mode if changed by the failsafe state machine if (state.user_intended_mode != updated_user_intented_mode) { - _user_mode_intention.change(updated_user_intented_mode, false, true); + _user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true); _user_mode_intention.getHadModeChangeAndClear(); } // Handle failsafe action - _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); - _vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get()); + _vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(), + false); + _vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction( + _failsafe.selectedAction(), _user_mode_intention.get())); + _vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge(); // Set this in sync with nav_state switch (_failsafe.selectedAction()) { case FailsafeBase::Action::Disarm: @@ -2257,6 +2311,21 @@ void Commander::checkAndInformReadyForTakeoff() #endif // CONFIG_ARCH_BOARD_PX4_SITL } +void Commander::modeManagementUpdate() +{ + ModeManagement::UpdateRequest mode_management_update{}; + _mode_management.update(isArmed(), _vehicle_status.nav_state_user_intention, + _failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update); + + if (!isArmed() && mode_management_update.change_user_intended_nav_state) { + _user_mode_intention.change(mode_management_update.user_intended_nav_state); + } + + if (mode_management_update.control_setpoint_update) { + _status_changed = true; + } +} + void Commander::control_status_leds(bool changed, const uint8_t battery_warning) { switch (blink_msg_state()) { @@ -2406,8 +2475,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) void Commander::updateControlMode() { _vehicle_control_mode = {}; - mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state, + + mode_util::getVehicleControlMode(_vehicle_status.nav_state, _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); + _mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode); + + _vehicle_control_mode.flag_armed = isArmed(); + _vehicle_control_mode.flag_multicopter_position_control_enabled = + (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_vehicle_control_mode.flag_control_altitude_enabled + || _vehicle_control_mode.flag_control_climb_rate_enabled + || _vehicle_control_mode.flag_control_position_enabled + || _vehicle_control_mode.flag_control_velocity_enabled + || _vehicle_control_mode.flag_control_acceleration_enabled); _vehicle_control_mode.timestamp = hrt_absolute_time(); _vehicle_control_mode_pub.publish(_vehicle_control_mode); } @@ -2757,7 +2837,7 @@ void Commander::manualControlCheck() if (override_enabled) { // If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) { + if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) { tune_positive(true); mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t"); events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks"); @@ -2774,7 +2854,7 @@ void Commander::manualControlCheck() // if there's never been a mode change force position control as initial state if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) { - _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true); + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true); } } } @@ -2836,7 +2916,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 5ead8e8b27..8d49c7842e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -38,6 +38,7 @@ #include "failure_detector/FailureDetector.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include "HomePosition.hpp" +#include "ModeManagement.hpp" #include "MulticopterThrowLaunch/MulticopterThrowLaunch.hpp" #include "Safety.hpp" #include "UserModeIntention.hpp" @@ -124,6 +125,7 @@ public: private: bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); } + static ModeChangeSource getSourceFromCommand(const vehicle_command_s &cmd); void answer_command(const vehicle_command_s &cmd, uint8_t result); @@ -194,6 +196,10 @@ private: void checkAndInformReadyForTakeoff(); + void handleCommandsFromModeExecutors(); + + void modeManagementUpdate(); + enum class PrearmedMode { DISABLED = 0, SAFETY_BUTTON = 1, @@ -216,8 +222,13 @@ private: HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; MulticopterThrowLaunch _multicopter_throw_launch{this}; Safety _safety{}; - UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; WorkerThread _worker_thread{}; + ModeManagement _mode_management{ +#ifndef CONSTRAINED_FLASH + _health_and_arming_checks.externalChecks() +#endif + }; + UserModeIntention _user_mode_intention {this, _vehicle_status, _health_and_arming_checks, &_mode_management}; const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()}; HomePosition _home_position{_failsafe_flags}; @@ -281,6 +292,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_command_mode_executor_sub{ORB_ID(vehicle_command_mode_executor)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; @@ -300,6 +312,7 @@ private: uORB::Publication _actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::Publication _vehicle_status_pub{ORB_ID(vehicle_status)}; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index b62c2805dc..a4e1873be9 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -65,6 +65,7 @@ px4_add_library(health_and_arming_checks checks/vtolCheck.cpp checks/windCheck.cpp + checks/externalChecks.cpp ) add_dependencies(health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index ad9943e21f..678d26636b 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve return header; } +bool Report::addExternalEvent(const event_s &event, NavModes modes) +{ + unsigned args_size = sizeof(event.arguments); + + // trim 0's + while (args_size > 0 && event.arguments[args_size - 1] == '\0') { + --args_size; + } + + unsigned total_size = sizeof(EventBufferHeader) + args_size; + + if (total_size > sizeof(_event_buffer) - _next_buffer_idx) { + _buffer_overflowed = true; + return false; + } + + events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))}; + memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size); + addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size); + return true; +} + + + NavModes Report::reportedModes(NavModes required_modes) { // Make sure optional checks are still shown in the UI diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index b0fb2b5dd8..9c16c6203d 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -249,8 +249,8 @@ public: void clearArmingBits(NavModes modes); /** - * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the - * mode is being run. + * Clear can_run bits for certain modes. This will prevent mode switching. + * For failsafe use the mode requirements instead, which then will clear the can_run bits. * @param modes affected modes */ void clearCanRunBits(NavModes modes); @@ -259,6 +259,8 @@ public: const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; } bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); } + + bool addExternalEvent(const event_s &event, NavModes modes); private: /** @@ -307,6 +309,7 @@ private: NavModes getModeGroup(uint8_t nav_state) const; friend class HealthAndArmingChecks; + friend class ExternalChecks; FRIEND_TEST(ReporterTest, basic_no_checks); FRIEND_TEST(ReporterTest, basic_fail_all_modes); FRIEND_TEST(ReporterTest, arming_checks_mode_category); diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index f98ebb2c46..b349475593 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -69,6 +69,7 @@ #include "checks/vtolCheck.hpp" #include "checks/offboardCheck.hpp" #include "checks/openDroneIDCheck.hpp" +#include "checks/externalChecks.hpp" class HealthAndArmingChecks : public ModuleParams { @@ -101,6 +102,10 @@ public: const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; } +#ifndef CONSTRAINED_FLASH + ExternalChecks &externalChecks() { return _external_checks; } +#endif + protected: void updateParams() override; private: @@ -143,8 +148,14 @@ private: RcAndDataLinkChecks _rc_and_data_link_checks; VtolChecks _vtol_checks; OffboardChecks _offboard_checks; +#ifndef CONSTRAINED_FLASH + ExternalChecks _external_checks; +#endif - HealthAndArmingCheckBase *_checks[31] = { + HealthAndArmingCheckBase *_checks[40] = { +#ifndef CONSTRAINED_FLASH + &_external_checks, +#endif &_accelerometer_checks, &_airspeed_checks, &_arm_permission_checks, @@ -161,7 +172,7 @@ private: &_home_position_checks, &_mission_checks, &_offboard_checks, // must be after _estimator_checks - &_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks + &_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks, _external_checks &_open_drone_id_checks, &_parachute_checks, &_power_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp new file mode 100644 index 0000000000..1d476b1d24 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -0,0 +1,335 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "externalChecks.hpp" + +static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits) +{ + if (requirement_set) { + bits |= 1u << nav_state; + } + + if (replaces_nav_state != -1) { + if (requirement_set) { + bits |= 1u << replaces_nav_state; + + } else { + bits &= ~(1u << replaces_nav_state); + } + } +} + +int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state) +{ + int free_registration_index = -1; + + for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { + if (!registrationValid(i)) { + free_registration_index = i; + break; + } + } + + if (free_registration_index != -1) { + _active_registrations_mask |= 1 << free_registration_index; + _registrations[free_registration_index].nav_mode_id = nav_mode_id; + _registrations[free_registration_index].replaces_nav_state = replaces_nav_state; + _registrations[free_registration_index].num_no_response = 0; + _registrations[free_registration_index].unresponsive = false; + _registrations[free_registration_index].total_num_unresponsive = 0; + + if (!_registrations[free_registration_index].reply) { + _registrations[free_registration_index].reply = new arming_check_reply_s(); + } + } + + return free_registration_index; +} + +bool ExternalChecks::removeRegistration(int registration_id, int8_t nav_mode_id) +{ + if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) { + return false; + } + + if (registrationValid(registration_id)) { + if (_registrations[registration_id].nav_mode_id == nav_mode_id) { + _active_registrations_mask &= ~(1u << registration_id); + return true; + } + } + + PX4_ERR("trying to remove inactive external check"); + return false; +} + +bool ExternalChecks::isUnresponsive(int registration_id) +{ + if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) { + return false; + } + + if (registrationValid(registration_id)) { + return _registrations[registration_id].unresponsive; + } + + return false; +} + + +void ExternalChecks::checkAndReport(const Context &context, Report &reporter) +{ + checkNonRegisteredModes(context, reporter); + + if (_active_registrations_mask == 0) { + return; + } + + NavModes unresponsive_modes{NavModes::None}; + + for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) { + if (!registrationValid(reg_idx) || !_registrations[reg_idx].reply) { + continue; + } + + arming_check_reply_s &reply = *_registrations[reg_idx].reply; + + int8_t nav_mode_id = _registrations[reply.registration_id].nav_mode_id; + + if (_registrations[reply.registration_id].unresponsive) { + + if (nav_mode_id != -1) { + unresponsive_modes = unresponsive_modes | reporter.getModeGroup(nav_mode_id); + setOrClearRequirementBits(true, nav_mode_id, -1, reporter.failsafeFlags().mode_req_other); + } + + } else { + NavModes modes; + + // We distinguish between two cases: + // - external navigation mode: in that case we set the single arming can_run bit for the mode + // - generic external arming check: set all arming bits + if (nav_mode_id == -1) { + modes = NavModes::All; + + } else { + modes = reporter.getModeGroup(nav_mode_id); + + int8_t replaces_nav_state = _registrations[reply.registration_id].replaces_nav_state; + + if (replaces_nav_state != -1) { + modes = modes | reporter.getModeGroup(replaces_nav_state); + // Also clear the arming bits for the replaced mode, as the user intention is always set to the + // replaced mode. + // We only have to clear the bits, as for the internal/replaced mode, the bits are not cleared yet. + } + + if (!reply.can_arm_and_run) { + setOrClearRequirementBits(true, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_other); + } + + // Mode requirements + // A replacement mode will also replace the mode requirements of the internal/replaced mode + setOrClearRequirementBits(reply.mode_req_angular_velocity, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_angular_velocity); + setOrClearRequirementBits(reply.mode_req_attitude, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_attitude); + setOrClearRequirementBits(reply.mode_req_local_alt, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_local_alt); + setOrClearRequirementBits(reply.mode_req_local_position, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_local_position); + setOrClearRequirementBits(reply.mode_req_local_position_relaxed, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_local_position_relaxed); + setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_global_position); + setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_mission); + setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_home_position); + setOrClearRequirementBits(reply.mode_req_prevent_arming, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_prevent_arming); + setOrClearRequirementBits(reply.mode_req_manual_control, nav_mode_id, replaces_nav_state, + reporter.failsafeFlags().mode_req_manual_control); + } + + if (!reply.can_arm_and_run) { + reporter.clearArmingBits(modes); + } + + if (reply.health_component_index > 0) { + reporter.setHealth((health_component_t)(1ull << reply.health_component_index), + reply.health_component_is_present, reply.health_component_warning, + reply.health_component_error); + } + + for (int i = 0; i < reply.num_events; ++i) { + // set the modes, which is the first argument + memcpy(reply.events[i].arguments, &modes, sizeof(modes)); + + reporter.addExternalEvent(reply.events[i], modes); + } + } + } + + if (unresponsive_modes != NavModes::None) { + /* EVENT + * @description + * The application running the mode might have crashed or the CPU load is too high. + */ + reporter.armingCheckFailure(unresponsive_modes, health_component_t::system, + events::ID("check_external_modes_unresponsive"), + events::Log::Critical, "Mode is unresponsive"); + } + +} + +void ExternalChecks::update() +{ + if (_active_registrations_mask == 0) { + return; + } + + const hrt_abstime now = hrt_absolute_time(); + + // Check for incoming replies + arming_check_reply_s reply; + int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH; + + while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) { + if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id) + && _current_request_id == reply.request_id) { + _reply_received_mask |= 1u << reply.registration_id; + _registrations[reply.registration_id].num_no_response = 0; + + // Prevent toggling between unresponsive & responsive state + if (_registrations[reply.registration_id].total_num_unresponsive <= 3) { + _registrations[reply.registration_id].unresponsive = false; + } + + if (_registrations[reply.registration_id].reply) { + *_registrations[reply.registration_id].reply = reply; + } + +// PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events); + } + } + + if (_last_update > 0) { + if (_reply_received_mask == _active_registrations_mask) { // Got all responses + // Nothing to do + } else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout + _had_timeout = true; + unsigned no_reply = _active_registrations_mask & ~_reply_received_mask; + + for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { + if ((1u << i) & no_reply) { + if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) { + // Clear immediately if not a mode + if (_registrations[i].nav_mode_id == -1) { + removeRegistration(i, -1); + PX4_WARN("No response from %i, removing", i); + + } else { + _registrations[i].unresponsive = true; + + if (_registrations[i].total_num_unresponsive < 100) { + ++_registrations[i].total_num_unresponsive; + } + + PX4_WARN("No response from %i, flagging unresponsive", i); + } + } + } + } + } + } + + // Start a new request? + if (now > _last_update + UPDATE_INTERVAL) { + _reply_received_mask = 0; + _last_update = now; + _had_timeout = false; + + // Request the state from all registered components + arming_check_request_s request{}; + request.request_id = ++_current_request_id; + request.timestamp = hrt_absolute_time(); + _arming_check_request_pub.publish(request); + } +} + +void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state) +{ + _first_external_nav_state = first_external_nav_state; + _last_external_nav_state = last_external_nav_state; +} + +void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const +{ + // Clear the arming bits for all non-registered external modes. + // But only report if one of them is selected, so we don't need to generate the extra event in most cases. + bool report_mode_not_available = false; + + for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state; + ++external_nav_state) { + bool found = false; + + for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) { + if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) { + found = true; + break; + } + } + + if (!found) { + if (external_nav_state == context.status().nav_state_user_intention) { + report_mode_not_available = true; + } + + reporter.clearArmingBits(reporter.getModeGroup(external_nav_state)); + setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other); + } + } + + if (report_mode_not_available) { + /* EVENT + * @description + * The application running the mode is not started. + */ + reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention), + health_component_t::system, + events::ID("check_external_modes_unavailable"), + events::Log::Error, "Mode is not registered"); + } +} + diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp new file mode 100644 index 0000000000..b4ee24cba6 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" +#include +#include +#include +#include + +static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) + health_component_t::avoidance, "enum definition missmatch"); + +class ExternalChecks : public HealthAndArmingCheckBase +{ +public: + static constexpr int MAX_NUM_REGISTRATIONS = 8; + + ExternalChecks() = default; + ~ExternalChecks() = default; + + void setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state); + + void checkAndReport(const Context &context, Report &reporter) override; + + bool hasFreeRegistrations() const { return _active_registrations_mask != (1u << MAX_NUM_REGISTRATIONS) - 1; } + /** + * Add registration + * @param nav_mode_id associated mode, -1 if none + * @param replaces_nav_state replaced mode, -1 if none + * @return registration id, or -1 + */ + int addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state); + bool removeRegistration(int registration_id, int8_t nav_mode_id); + void update(); + + bool isUnresponsive(int registration_id); + +private: + static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms; + static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; + static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval"); + static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL + + void checkNonRegisteredModes(const Context &context, Report &reporter) const; + + bool registrationValid(int reg_idx) const { return ((1u << reg_idx) & _active_registrations_mask) != 0; } + + struct Registration { + ~Registration() { delete reply; } + + int8_t nav_mode_id{-1}; ///< associated mode, -1 if none + int8_t replaces_nav_state{-1}; + + uint8_t num_no_response{0}; + bool unresponsive{false}; + uint8_t total_num_unresponsive{0}; + arming_check_reply_s *reply{nullptr}; + }; + + unsigned _active_registrations_mask{0}; + Registration _registrations[MAX_NUM_REGISTRATIONS] {}; + + uint8_t _first_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX; + uint8_t _last_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX; + + // Current requests (async updates) + hrt_abstime _last_update{0}; + unsigned _reply_received_mask{0}; + bool _had_timeout{false}; + + uint8_t _current_request_id{0}; + + uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)}; + + uORB::Publication _arming_check_request_pub{ORB_ID(arming_check_request)}; +}; diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp new file mode 100644 index 0000000000..3182787abe --- /dev/null +++ b/src/modules/commander/ModeManagement.cpp @@ -0,0 +1,471 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef CONSTRAINED_FLASH + +#include "ModeManagement.hpp" + +#include + +bool ModeExecutors::hasFreeExecutors() const +{ + for (int i = 0; i < MAX_NUM; ++i) { + if (!_mode_executors[i].valid) { + return true; + } + } + + return false; +} + +int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor) +{ + for (int i = 0; i < MAX_NUM; ++i) { + if (!_mode_executors[i].valid) { + _mode_executors[i] = executor; + _mode_executors[i].valid = true; + return i + FIRST_EXECUTOR_ID; + } + } + + PX4_ERR("logic error"); + return -1; +} + +void ModeExecutors::removeExecutor(int id) +{ + if (valid(id)) { + _mode_executors[id - FIRST_EXECUTOR_ID].valid = false; + } +} + +void ModeExecutors::printStatus(int executor_in_charge) const +{ + for (int i = 0; i < MAX_NUM; ++i) { + if (_mode_executors[i].valid) { + int executor_id = i + FIRST_EXECUTOR_ID; + PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state, + executor_id == executor_in_charge ? "yes" : "no"); + + } + } +} + +bool Modes::hasFreeExternalModes() const +{ + for (int i = 0; i < MAX_NUM; ++i) { + if (!_modes[i].valid) { + return true; + } + } + + return false; +} + +uint8_t Modes::addExternalMode(const Modes::Mode &mode) +{ + for (int i = 0; i < MAX_NUM; ++i) { + if (!_modes[i].valid) { + _modes[i] = mode; + _modes[i].valid = true; + return i + FIRST_EXTERNAL_NAV_STATE; + } + } + + PX4_ERR("logic error"); + return -1; +} + +bool Modes::removeExternalMode(uint8_t nav_state, const char *name) +{ + if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) { + _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false; + return true; + } + + PX4_ERR("trying to remove invalid mode %s", name); + return false; +} + +void Modes::printStatus() const +{ + for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { + if (valid(i)) { + const Modes::Mode &cur_mode = mode(i); + PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i, + cur_mode.name); + + if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE + && cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) { + PX4_INFO(" Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]); + } + } + } +} + +ModeManagement::ModeManagement(ExternalChecks &external_checks) + : _external_checks(external_checks) +{ + _external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE); +} + +void ModeManagement::checkNewRegistrations(UpdateRequest &update_request) +{ + register_ext_component_request_s request; + int max_updates = 5; + + while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request) + && --max_updates >= 0) { + request.name[sizeof(request.name) - 1] = '\0'; + PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id, + request.register_arming_check, request.register_mode, request.register_mode_executor); + register_ext_component_reply_s reply{}; + reply.mode_executor_id = -1; + reply.mode_id = -1; + reply.arming_check_id = -1; + static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch"); + memcpy(reply.name, request.name, sizeof(request.name)); + reply.request_id = request.request_id; + reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION; + + // validate + bool request_valid = true; + + if (request.register_mode_executor && !request.register_mode) { + request_valid = false; + } + + if (request.register_mode && !request.register_arming_check) { + request_valid = false; + } + + reply.success = false; + + if (request_valid) { + // check free space + reply.success = true; + + if (request.register_arming_check && !_external_checks.hasFreeRegistrations()) { + PX4_WARN("No free slots for arming checks"); + reply.success = false; + } + + if (request.register_mode) { + if (!_modes.hasFreeExternalModes()) { + PX4_WARN("No free slots for modes"); + reply.success = false; + + } else if (request.enable_replace_internal_mode) { + // Check if another one already replaces the same mode + for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { + if (_modes.valid(i)) { + const Modes::Mode &cur_mode = _modes.mode(i); + + if (cur_mode.replaces_nav_state == request.replace_internal_mode) { + // TODO: we could add priorities and allow the highest priority to do the replacement + PX4_ERR("Trying to replace an already replaced mode (%i)", request.replace_internal_mode); + reply.success = false; + } + } + } + } + } + + if (request.register_mode_executor && !_mode_executors.hasFreeExecutors()) { + PX4_WARN("No free slots for executors"); + reply.success = false; + } + + // register component(s) + if (reply.success) { + int nav_mode_id = -1; + + if (request.register_mode) { + Modes::Mode mode{}; + strncpy(mode.name, request.name, sizeof(mode.name)); + + if (request.enable_replace_internal_mode) { + mode.replaces_nav_state = request.replace_internal_mode; + } + + nav_mode_id = _modes.addExternalMode(mode); + reply.mode_id = nav_mode_id; + } + + if (request.register_mode_executor) { + ModeExecutors::ModeExecutor executor{}; + executor.owned_nav_state = nav_mode_id; + int registration_id = _mode_executors.addExecutor(executor); + + if (nav_mode_id != -1) { + _modes.mode(nav_mode_id).mode_executor_registration_id = registration_id; + } + + reply.mode_executor_id = registration_id; + } + + if (request.register_arming_check) { + int8_t replace_nav_state = request.enable_replace_internal_mode ? request.replace_internal_mode : -1; + int registration_id = _external_checks.addRegistration(nav_mode_id, replace_nav_state); + + if (nav_mode_id != -1) { + _modes.mode(nav_mode_id).arming_check_registration_id = registration_id; + } + + reply.arming_check_id = registration_id; + } + + // Activate the mode? + if (request.register_mode_executor && request.activate_mode_immediately && nav_mode_id != -1) { + update_request.change_user_intended_nav_state = true; + update_request.user_intended_nav_state = nav_mode_id; + } + } + } + + reply.timestamp = hrt_absolute_time(); + _register_ext_component_reply_pub.publish(reply); + } +} + +void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request) +{ + unregister_ext_component_s request; + int max_updates = 5; + + while (!update_request.change_user_intended_nav_state && _unregister_ext_component_sub.update(&request) + && --max_updates >= 0) { + request.name[sizeof(request.name) - 1] = '\0'; + PX4_DEBUG("got unregistration request: %s arming: %i mode: %i executor: %i", request.name, + (int)request.arming_check_id, (int)request.mode_id, (int)request.mode_executor_id); + + if (request.arming_check_id != -1) { + _external_checks.removeRegistration(request.arming_check_id, request.mode_id); + } + + if (request.mode_id != -1) { + if (_modes.removeExternalMode(request.mode_id, request.name)) { + removeModeExecutor(request.mode_executor_id); + // else: if the mode was already removed (due to a timeout), the executor was also removed already + } + + // If the removed mode is currently active, switch to Hold + if (user_intended_nav_state == request.mode_id) { + update_request.change_user_intended_nav_state = true; + update_request.user_intended_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + } + } +} + +void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, + UpdateRequest &update_request) +{ + _failsafe_action_active = failsafe_action_active; + _external_checks.update(); + + bool allow_update_while_armed = false; +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + // For simulation, allow registering modes while armed for developer convenience + allow_update_while_armed = true; +#endif + + if (armed && !allow_update_while_armed) { + // Reject registration requests + register_ext_component_request_s request; + + if (_register_ext_component_request_sub.update(&request)) { + PX4_ERR("Not accepting registration requests while armed"); + register_ext_component_reply_s reply{}; + reply.success = false; + static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch"); + memcpy(reply.name, request.name, sizeof(request.name)); + reply.request_id = request.request_id; + reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION; + reply.timestamp = hrt_absolute_time(); + _register_ext_component_reply_pub.publish(reply); + } + + } else { + // Check for unresponsive modes + for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { + if (_modes.valid(i)) { + const Modes::Mode &mode = _modes.mode(i); + + // Remove only if not currently selected + if (user_intended_nav_state != i && _external_checks.isUnresponsive(mode.arming_check_registration_id)) { + PX4_DEBUG("Removing unresponsive mode %i", i); + _external_checks.removeRegistration(mode.arming_check_registration_id, i); + removeModeExecutor(mode.mode_executor_registration_id); + _modes.removeExternalMode(i, mode.name); + } + } + } + + // As we're disarmed we can use the user intended mode, as no failsafe will be active + checkNewRegistrations(update_request); + checkUnregistrations(user_intended_nav_state, update_request); + } + + update_request.control_setpoint_update = checkConfigControlSetpointUpdates(); +} + +void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) +{ + // Update mode executor in charge + int mode_executor_for_intended_nav_state = -1; + + if (_modes.valid(user_intended_nav_state)) { + mode_executor_for_intended_nav_state = _modes.mode(user_intended_nav_state).mode_executor_registration_id; + } + + if (mode_executor_for_intended_nav_state == -1) { + // Not an owned mode: check source + if (source == ModeChangeSource::User) { + // Give control to the pilot + _mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID; + } + + } else { + // Switched into an owned mode: put executor in charge + _mode_executor_in_charge = mode_executor_for_intended_nav_state; + } +} + +uint8_t ModeManagement::getNavStateReplacementIfValid(uint8_t nav_state, bool report_error) +{ + for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { + if (_modes.valid(i)) { + Modes::Mode &mode = _modes.mode(i); + + if (mode.replaces_nav_state == nav_state) { + if (_external_checks.isUnresponsive(mode.arming_check_registration_id)) { + if (!mode.unresponsive_reported && report_error) { + mode.unresponsive_reported = true; + events::send(events::ID("commander_mode_fallback_internal"), events::Log::Critical, + "External mode is unresponsive, falling back to internal"); + } + + return nav_state; + + } else { + return i; + } + } + } + } + + return nav_state; +} + +uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state) +{ + if (_modes.valid(nav_state)) { + const Modes::Mode &mode = _modes.mode(nav_state); + + if (mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) { + return mode.replaces_nav_state; + } + } + + return nav_state; +} + +void ModeManagement::removeModeExecutor(int mode_executor_id) +{ + if (mode_executor_id == -1) { + return; + } + + if (_mode_executor_in_charge == mode_executor_id) { + _mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID; + } + + _mode_executors.removeExecutor(mode_executor_id); +} + +int ModeManagement::modeExecutorInCharge() const +{ + if (_failsafe_action_active) { + return ModeExecutors::AUTOPILOT_EXECUTOR_ID; + } + + return _mode_executor_in_charge; +} + +bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const +{ + bool ret = false; + + if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) { + if (_modes.valid(nav_state)) { + control_mode = _modes.mode(nav_state).config_control_setpoint; + ret = true; + + } else { + Modes::Mode::setControlModeDefaults(control_mode); + } + } + + return ret; +} + +void ModeManagement::printStatus() const +{ + _modes.printStatus(); + _mode_executors.printStatus(modeExecutorInCharge()); +} + +bool ModeManagement::checkConfigControlSetpointUpdates() +{ + bool had_update = false; + vehicle_control_mode_s config_control_setpoint; + int max_updates = 5; + + while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) { + if (_modes.valid(config_control_setpoint.source_id)) { + _modes.mode(config_control_setpoint.source_id).config_control_setpoint = config_control_setpoint; + had_update = true; + + } else { + if (!_invalid_mode_printed) { + PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id); + _invalid_mode_printed = true; + } + } + } + + return had_update; +} + +#endif /* CONSTRAINED_FLASH */ diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp new file mode 100644 index 0000000000..3c5038abaa --- /dev/null +++ b/src/modules/commander/ModeManagement.hpp @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "ModeUtil/ui.hpp" +#include "UserModeIntention.hpp" +#include "HealthAndArmingChecks/checks/externalChecks.hpp" + +class ModeExecutors +{ +public: + static constexpr int AUTOPILOT_EXECUTOR_ID = 0; + static constexpr int FIRST_EXECUTOR_ID = 1; + static constexpr int MAX_NUM = 5; + + struct ModeExecutor { + uint8_t owned_nav_state{}; + bool valid{false}; + }; + + void printStatus(int executor_in_charge) const; + + bool valid(int id) const { return id >= FIRST_EXECUTOR_ID && id < FIRST_EXECUTOR_ID + MAX_NUM && _mode_executors[id - FIRST_EXECUTOR_ID].valid; } + const ModeExecutor &executor(int id) const { return _mode_executors[id - FIRST_EXECUTOR_ID]; } + ModeExecutor &executor(int id) { return _mode_executors[id - FIRST_EXECUTOR_ID]; } + + bool hasFreeExecutors() const; + int addExecutor(const ModeExecutor &executor); + void removeExecutor(int id); +private: + ModeExecutor _mode_executors[MAX_NUM] {}; +}; + +class Modes +{ +public: + static constexpr uint8_t FIRST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; + static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8; + static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1; + + struct Mode { + Mode() + { + // Set defaults for control mode + setControlModeDefaults(config_control_setpoint); + } + static void setControlModeDefaults(vehicle_control_mode_s &config_control_setpoint_) + { + config_control_setpoint_.flag_control_position_enabled = true; + config_control_setpoint_.flag_control_velocity_enabled = true; + config_control_setpoint_.flag_control_altitude_enabled = true; + config_control_setpoint_.flag_control_climb_rate_enabled = true; + config_control_setpoint_.flag_control_acceleration_enabled = true; + config_control_setpoint_.flag_control_rates_enabled = true; + config_control_setpoint_.flag_control_attitude_enabled = true; + config_control_setpoint_.flag_control_allocation_enabled = true; + } + + static constexpr uint8_t REPLACES_NAV_STATE_NONE = 0xff; + + char name[sizeof(register_ext_component_request_s::name)] {}; + bool valid{false}; + uint8_t replaces_nav_state{REPLACES_NAV_STATE_NONE}; + bool unresponsive_reported{false}; + int arming_check_registration_id{-1}; + int mode_executor_registration_id{-1}; + vehicle_control_mode_s config_control_setpoint{}; + }; + + void printStatus() const; + + bool valid(uint8_t nav_state) const { return nav_state >= FIRST_EXTERNAL_NAV_STATE && nav_state <= LAST_EXTERNAL_NAV_STATE && _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid; } + Mode &mode(uint8_t nav_state) { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; } + const Mode &mode(uint8_t nav_state) const { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; } + + bool hasFreeExternalModes() const; + uint8_t addExternalMode(const Mode &mode); + bool removeExternalMode(uint8_t nav_state, const char *name); + +private: + Mode _modes[MAX_NUM] {}; +}; + + +#ifndef CONSTRAINED_FLASH + +class ModeManagement : public ModeChangeHandler +{ +public: + ModeManagement(ExternalChecks &external_checks); + ~ModeManagement() = default; + + struct UpdateRequest { + bool change_user_intended_nav_state{false}; + uint8_t user_intended_nav_state{}; + bool control_setpoint_update{false}; + }; + + void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request); + + /** + * Mode executor ID for who is currently in charge (and can send commands etc). + * This is ModeExecutors::AUTOPILOT_EXECUTOR_ID if no executor is in charge currently. + */ + int modeExecutorInCharge() const; + + void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override; + uint8_t getReplacedModeIfAny(uint8_t nav_state) override; + + uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true); + + bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const; + + void printStatus() const; + + +private: + bool checkConfigControlSetpointUpdates(); + void checkNewRegistrations(UpdateRequest &update_request); + void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request); + + void removeModeExecutor(int mode_executor_id); + + uORB::Subscription _config_control_setpoints_sub{ORB_ID(config_control_setpoints)}; + uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)}; + uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)}; + uORB::Publication _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)}; + + ExternalChecks &_external_checks; + ModeExecutors _mode_executors; + Modes _modes; + + bool _failsafe_action_active{false}; + int _mode_executor_in_charge{ModeExecutors::AUTOPILOT_EXECUTOR_ID}; + + bool _invalid_mode_printed{false}; +}; + +#else /* CONSTRAINED_FLASH */ + +class ModeManagement : public ModeChangeHandler +{ +public: + ModeManagement() = default; + ~ModeManagement() = default; + + struct UpdateRequest { + bool change_user_intended_nav_state{false}; + uint8_t user_intended_nav_state{}; + bool control_setpoint_update{false}; + }; + + void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request) {} + + int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; } + + void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {} + uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; } + + uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; } + + bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const { return false; } + + void printStatus() const {} + + void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const + { + valid_nav_state_mask = mode_util::getValidNavStates(); + can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION); + } + + void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { } + +private: +}; + +#endif /* CONSTRAINED_FLASH */ diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index a8b12edc50..468fdc9f1b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -42,11 +42,10 @@ static bool stabilization_required(uint8_t vehicle_type) return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; } -void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, +void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, vehicle_control_mode_s &vehicle_control_mode) { - vehicle_control_mode.flag_armed = armed; vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default switch (nav_state) { @@ -163,17 +162,11 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_velocity_enabled = true; break; + // vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement default: break; } - vehicle_control_mode.flag_multicopter_position_control_enabled = - (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) - && (vehicle_control_mode.flag_control_altitude_enabled - || vehicle_control_mode.flag_control_climb_rate_enabled - || vehicle_control_mode.flag_control_position_enabled - || vehicle_control_mode.flag_control_velocity_enabled - || vehicle_control_mode.flag_control_acceleration_enabled); } } // namespace mode_util diff --git a/src/modules/commander/ModeUtil/control_mode.hpp b/src/modules/commander/ModeUtil/control_mode.hpp index 9b2375424e..46e10fbb37 100644 --- a/src/modules/commander/ModeUtil/control_mode.hpp +++ b/src/modules/commander/ModeUtil/control_mode.hpp @@ -41,7 +41,7 @@ namespace mode_util { -void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, +void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, vehicle_control_mode_s &vehicle_control_mode); diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index 5f9928c856..8f44a96d55 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/src/modules/commander/ModeUtil/conversions.hpp @@ -75,9 +75,25 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit; case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return navigation_mode_t::external1; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return navigation_mode_t::external2; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return navigation_mode_t::external3; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return navigation_mode_t::external4; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return navigation_mode_t::external5; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return navigation_mode_t::external6; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8; } - static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "code requires update"); + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update"); return navigation_mode_t::unknown; } @@ -104,7 +120,16 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "AUTO_LAND", "AUTO_FOLLOW_TARGET", "AUTO_PRECLAND", - "ORBIT" + "ORBIT", + "AUTO_VTOL_TAKEOFF", + "EXTERNAL1", + "EXTERNAL2", + "EXTERNAL3", + "EXTERNAL4", + "EXTERNAL5", + "EXTERNAL6", + "EXTERNAL7", + "EXTERNAL8", }; } // namespace mode_util diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 1fc08417d5..f067275ea6 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -170,8 +170,9 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt); + // NAVIGATION_STATE_EXTERNALx: handled outside - static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements"); + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements"); } } // namespace mode_util diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 62ab871fe8..9bb3d08407 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -35,14 +35,22 @@ #include "UserModeIntention.hpp" UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status, - const HealthAndArmingChecks &health_and_arming_checks) - : ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks) + const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler) + : ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks), + _handler(handler) { } -bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force) +bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback, + bool force) { _ever_had_mode_change = true; + _had_mode_change = true; + + if (_handler) { + // If a replacement mode is selected, select the internal one instead. The replacement will be selected after. + user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state); + } // Always allow mode change while disarmed bool always_allow = force || !isArmed(); @@ -68,6 +76,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) { _nav_state_after_disarming = user_intended_nav_state; } + + if (_handler) { + _handler->onUserIntendedNavStateChange(source, user_intended_nav_state); + } } return allow_change; diff --git a/src/modules/commander/UserModeIntention.hpp b/src/modules/commander/UserModeIntention.hpp index 3c18a6ff73..1292c95317 100644 --- a/src/modules/commander/UserModeIntention.hpp +++ b/src/modules/commander/UserModeIntention.hpp @@ -37,21 +37,42 @@ #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" #include +enum class ModeChangeSource { + User, ///< RC or MAVLink + ModeExecutor, +}; + +class ModeChangeHandler +{ +public: + virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0; + + /** + * Get the replaced (internal) mode for a given (external) mode + * @param nav_state + * @return nav_state or the mode that nav_state replaces + */ + virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0; +}; + + class UserModeIntention : ModuleParams { public: UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status, - const HealthAndArmingChecks &health_and_arming_checks); + const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler); ~UserModeIntention() = default; /** * Change the user intended mode * @param user_intended_nav_state new mode + * @param source calling reason * @param allow_fallback allow to fallback to a lower mode if current mode cannot run * @param force always set if true * @return true if successfully set (also if unchanged) */ - bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false); + bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User, + bool allow_fallback = false, bool force = false); uint8_t get() const { return _user_intented_nav_state; } @@ -72,6 +93,7 @@ private: const vehicle_status_s &_vehicle_status; const HealthAndArmingChecks &_health_and_arming_checks; + ModeChangeHandler *const _handler{nullptr}; uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b3d43652dd..f73a493c5b 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -63,7 +63,15 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, - PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF + PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, + PX4_CUSTOM_SUB_MODE_EXTERNAL1, + PX4_CUSTOM_SUB_MODE_EXTERNAL2, + PX4_CUSTOM_SUB_MODE_EXTERNAL3, + PX4_CUSTOM_SUB_MODE_EXTERNAL4, + PX4_CUSTOM_SUB_MODE_EXTERNAL5, + PX4_CUSTOM_SUB_MODE_EXTERNAL6, + PX4_CUSTOM_SUB_MODE_EXTERNAL7, + PX4_CUSTOM_SUB_MODE_EXTERNAL8, }; enum PX4_CUSTOM_SUB_MODE_POSCTL { @@ -171,6 +179,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF; break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL2; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL3; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL4; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL5; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL6; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL7; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8; + break; } return custom_mode; diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 6e95db946b..4ce6a078e3 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -68,7 +68,8 @@ MavlinkCommandSender::~MavlinkCommandSender() int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel) { // commands > uint16 are PX4 internal only - if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START + || command.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) { return 0; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 63ce041bb1..8a10a40c36 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2357,7 +2357,8 @@ Mavlink::task_main(int argc, char *argv[]) if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START - && is_target_known) { + && is_target_known + && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) { mavlink_command_ack_t msg{}; msg.result = command_ack.result; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index b09de9cc70..abdec2c246 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -5,12 +5,24 @@ ##### publications: + - topic: /fmu/out/register_ext_component_reply + type: px4_msgs::msg::RegisterExtComponentReply + + - topic: /fmu/out/arming_check_request + type: px4_msgs::msg::ArmingCheckRequest + + - topic: /fmu/out/mode_completed + type: px4_msgs::msg::ModeCompleted + - topic: /fmu/out/collision_constraints type: px4_msgs::msg::CollisionConstraints - topic: /fmu/out/failsafe_flags type: px4_msgs::msg::FailsafeFlags + - topic: /fmu/out/manual_control_setpoint + type: px4_msgs::msg::ManualControlSetpoint + - topic: /fmu/out/position_setpoint_triplet type: px4_msgs::msg::PositionSetpointTriplet @@ -29,6 +41,9 @@ publications: - topic: /fmu/out/vehicle_control_mode type: px4_msgs::msg::VehicleControlMode + - topic: /fmu/out/vehicle_command_ack + type: px4_msgs::msg::VehicleCommandAck + - topic: /fmu/out/vehicle_global_position type: px4_msgs::msg::VehicleGlobalPosition @@ -48,6 +63,20 @@ publications: type: px4_msgs::msg::VehicleTrajectoryWaypoint subscriptions: + - topic: /fmu/in/register_ext_component_request + type: px4_msgs::msg::RegisterExtComponentRequest + + - topic: /fmu/in/unregister_ext_component + type: px4_msgs::msg::UnregisterExtComponent + + - topic: /fmu/in/arming_check_reply + type: px4_msgs::msg::ArmingCheckReply + + - topic: /fmu/in/mode_completed + type: px4_msgs::msg::ModeCompleted + + - topic: /fmu/in/config_control_setpoints + type: px4_msgs::msg::VehicleControlMode - topic: /fmu/in/offboard_control_mode type: px4_msgs::msg::OffboardControlMode @@ -79,6 +108,12 @@ subscriptions: - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry + - topic: /fmu/in/vehicle_command + type: px4_msgs::msg::VehicleCommand + + - topic: /fmu/in/vehicle_command_mode_executor + type: px4_msgs::msg::VehicleCommand + - topic: /fmu/in/vehicle_trajectory_bezier type: px4_msgs::msg::VehicleTrajectoryBezier