diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ba54dd798f..646e470a90 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -60,6 +60,7 @@ set(msg_files CellularStatus.msg CollisionConstraints.msg CollisionReport.msg + ConfigOverrides.msg ControlAllocatorStatus.msg Cpuload.msg DatamanRequest.msg diff --git a/msg/ConfigOverrides.msg b/msg/ConfigOverrides.msg new file mode 100644 index 0000000000..9800066b2a --- /dev/null +++ b/msg/ConfigOverrides.msg @@ -0,0 +1,19 @@ +# Configurable overrides by (external) modes or mode executors +uint64 timestamp # time since system start (microseconds) + +bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) + +bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared) +int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout + + +int8 SOURCE_TYPE_MODE = 0 +int8 SOURCE_TYPE_MODE_EXECUTOR = 1 +int8 source_type + +uint8 source_id # ID depending on source_type + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS config_overrides config_overrides_request + diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 24bba10c95..8b79003efe 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -88,8 +88,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 uint8 VEHICLE_TYPE_AIRSHIP = 4 +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* # Link loss bool gcs_connection_lost # datalink to GCS lost diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b2687ea50f..77d75df25e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2200,8 +2200,10 @@ void Commander::handleAutoDisarm() const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) && !_mission_result_sub.get().finished; + const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission + && !_config_overrides.disable_auto_disarm; - if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) { + if (auto_disarm_land_enabled && _have_taken_off_since_arming) { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); @@ -2253,6 +2255,7 @@ bool Commander::handleModeIntentionAndFailsafe() { const uint8_t prev_nav_state = _vehicle_status.nav_state; const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction(); + const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state; FailsafeBase::State state{}; state.armed = isArmed(); @@ -2303,7 +2306,24 @@ bool Commander::handleModeIntentionAndFailsafe() _vehicle_status.nav_state_timestamp = hrt_absolute_time(); } - return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction(); + _mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides); + + // Apply failsafe deferring & get the current state + _failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s); + + if (_failsafe.failsafeDeferred()) { + _vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE; + + } else if (_failsafe.getDeferFailsafes()) { + _vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED; + + } else { + _vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED; + } + + return prev_nav_state != _vehicle_status.nav_state || + prev_failsafe_action != _failsafe.selectedAction() || + prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state; } void Commander::checkAndInformReadyForTakeoff() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 8d49c7842e..874f9a9b14 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -232,6 +232,7 @@ private: const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()}; HomePosition _home_position{_failsafe_flags}; + config_overrides_s _config_overrides{}; Hysteresis _auto_disarm_landed{false}; diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 3182787abe..ba28e550ac 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -338,6 +338,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa } update_request.control_setpoint_update = checkConfigControlSetpointUpdates(); + checkConfigOverrides(); } void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) @@ -446,6 +447,44 @@ void ModeManagement::printStatus() const _mode_executors.printStatus(modeExecutorInCharge()); } +void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) +{ + config_overrides_s current_overrides; + + if (_modes.valid(nav_state)) { + current_overrides = _modes.mode(nav_state).overrides; + + } else { + current_overrides = {}; + } + + // Apply the overrides from executors on top (executors take precedence) + const int executor_in_charge = modeExecutorInCharge(); + + if (_mode_executors.valid(executor_in_charge)) { + const config_overrides_s &executor_overrides = _mode_executors.executor(executor_in_charge).overrides; + + if (executor_overrides.disable_auto_disarm) { + current_overrides.disable_auto_disarm = true; + } + + if (executor_overrides.defer_failsafes) { + current_overrides.defer_failsafes = true; + current_overrides.defer_failsafes_timeout_s = executor_overrides.defer_failsafes_timeout_s; + } + } + + // Publish if changed or at low rate + current_overrides.timestamp = overrides_in_out.timestamp; + + if (memcmp(&overrides_in_out, ¤t_overrides, sizeof(current_overrides)) != 0 + || hrt_elapsed_time(¤t_overrides.timestamp) > 500_ms) { + current_overrides.timestamp = hrt_absolute_time(); + _config_overrides_pub.publish(current_overrides); + overrides_in_out = current_overrides; + } +} + bool ModeManagement::checkConfigControlSetpointUpdates() { bool had_update = false; @@ -468,4 +507,31 @@ bool ModeManagement::checkConfigControlSetpointUpdates() return had_update; } +void ModeManagement::checkConfigOverrides() +{ + config_overrides_s override_request; + int max_updates = config_overrides_s::ORB_QUEUE_LENGTH; + + while (_config_overrides_request_sub.update(&override_request) && --max_updates >= 0) { + switch (override_request.source_type) { + case config_overrides_s::SOURCE_TYPE_MODE_EXECUTOR: + if (_mode_executors.valid(override_request.source_id)) { + ModeExecutors::ModeExecutor &executor = _mode_executors.executor(override_request.source_id); + memcpy(&executor.overrides, &override_request, sizeof(executor.overrides)); + static_assert(sizeof(executor.overrides) == sizeof(override_request), "size mismatch"); + } + + break; + + case config_overrides_s::SOURCE_TYPE_MODE: + if (_modes.valid(override_request.source_id)) { + Modes::Mode &mode = _modes.mode(override_request.source_id); + memcpy(&mode.overrides, &override_request, sizeof(mode.overrides)); + } + + break; + } + } +} + #endif /* CONSTRAINED_FLASH */ diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 3c5038abaa..7576501f3d 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include "ModeUtil/ui.hpp" #include "UserModeIntention.hpp" @@ -53,6 +54,7 @@ public: static constexpr int MAX_NUM = 5; struct ModeExecutor { + config_overrides_s overrides{}; uint8_t owned_nav_state{}; bool valid{false}; }; @@ -103,6 +105,7 @@ public: bool unresponsive_reported{false}; int arming_check_registration_id{-1}; int mode_executor_registration_id{-1}; + config_overrides_s overrides{}; vehicle_control_mode_s config_control_setpoint{}; }; @@ -153,10 +156,13 @@ public: void printStatus() const; + void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out); + private: bool checkConfigControlSetpointUpdates(); void checkNewRegistrations(UpdateRequest &update_request); void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request); + void checkConfigOverrides(); void removeModeExecutor(int mode_executor_id); @@ -164,6 +170,8 @@ private: 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)}; + uORB::Publication _config_overrides_pub{ORB_ID(config_overrides)}; + uORB::Subscription _config_overrides_request_sub{ORB_ID(config_overrides_request)}; ExternalChecks &_external_checks; ModeExecutors _mode_executors; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e9f3c3004a..cdcb36080d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("camera_trigger"); add_topic("cellular_status", 200); add_topic("commander_state"); + add_topic("config_overrides"); add_topic("cpuload"); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index abdec2c246..0f819bb3be 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -69,6 +69,9 @@ subscriptions: - topic: /fmu/in/unregister_ext_component type: px4_msgs::msg::UnregisterExtComponent + - topic: /fmu/in/config_overrides_request + type: px4_msgs::msg::ConfigOverrides + - topic: /fmu/in/arming_check_reply type: px4_msgs::msg::ArmingCheckReply