diff --git a/msg/px4_msgs_old/msg/RegisterExtComponentRequestV1.msg b/msg/px4_msgs_old/msg/RegisterExtComponentRequestV1.msg new file mode 100644 index 0000000000..3b76aef15e --- /dev/null +++ b/msg/px4_msgs_old/msg/RegisterExtComponentRequestV1.msg @@ -0,0 +1,24 @@ +# Request to register an external component + +uint32 MESSAGE_VERSION = 1 + +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) +bool not_user_selectable # mode cannot be selected by the user + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/px4_msgs_old/msg/VehicleStatusV3.msg b/msg/px4_msgs_old/msg/VehicleStatusV3.msg new file mode 100644 index 0000000000..888ba6cde8 --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV3.msg @@ -0,0 +1,123 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 3 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_LANDING = 6 +uint8 ARM_DISARM_REASON_PREFLIGHT_INACTION = 7 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 8 +uint8 ARM_DISARM_REASON_RC_BUTTON = 13 +uint8 ARM_DISARM_REASON_FAILSAFE = 14 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +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_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) +uint8 nav_state_display # User-visible nav state sent via MAVLink (executor state if active, otherwise nav_state) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNSPECIFIED = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 + +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 +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool traffic_avoidance_system_present + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index b8d658cbbf..d59b232462 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -16,9 +16,11 @@ #include "translation_home_position_v1.h" #include "translation_register_ext_component_reply_v1.h" #include "translation_register_ext_component_request_v1.h" +#include "translation_register_ext_component_request_v2.h" #include "translation_vehicle_attitude_setpoint_v1.h" #include "translation_vehicle_command_ack_v1.h" #include "translation_vehicle_local_position_v1.h" #include "translation_vehicle_status_v1.h" #include "translation_vehicle_status_v2.h" #include "translation_vehicle_status_v3.h" +#include "translation_vehicle_status_v4.h" diff --git a/msg/translation_node/translations/translation_register_ext_component_request_v1.h b/msg/translation_node/translations/translation_register_ext_component_request_v1.h index 9f90b90784..39fac52bf6 100644 --- a/msg/translation_node/translations/translation_register_ext_component_request_v1.h +++ b/msg/translation_node/translations/translation_register_ext_component_request_v1.h @@ -6,14 +6,14 @@ // Translate RegisterExtComponentRequest v0 <--> v1 #include -#include +#include class RegisterExtComponentRequestV1Translation { public: using MessageOlder = px4_msgs_old::msg::RegisterExtComponentRequestV0; static_assert(MessageOlder::MESSAGE_VERSION == 0); - using MessageNewer = px4_msgs::msg::RegisterExtComponentRequest; + using MessageNewer = px4_msgs_old::msg::RegisterExtComponentRequestV1; static_assert(MessageNewer::MESSAGE_VERSION == 1); static constexpr const char* kTopic = "fmu/in/register_ext_component_request"; diff --git a/msg/translation_node/translations/translation_register_ext_component_request_v2.h b/msg/translation_node/translations/translation_register_ext_component_request_v2.h new file mode 100644 index 0000000000..d7c199e336 --- /dev/null +++ b/msg/translation_node/translations/translation_register_ext_component_request_v2.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate RegisterExtComponentRequest v1 <--> v2 +#include +#include + +class RegisterExtComponentRequestV2Translation { +public: + using MessageOlder = px4_msgs_old::msg::RegisterExtComponentRequestV1; + static_assert(MessageOlder::MESSAGE_VERSION == 1); + + using MessageNewer = px4_msgs::msg::RegisterExtComponentRequest; + static_assert(MessageNewer::MESSAGE_VERSION == 2); + + static constexpr const char* kTopic = "fmu/in/register_ext_component_request"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.request_id = msg_older.request_id; + msg_newer.name = msg_older.name; + msg_newer.px4_ros2_api_version = msg_older.px4_ros2_api_version; + msg_newer.register_arming_check = msg_older.register_arming_check; + msg_newer.register_mode = msg_older.register_mode; + msg_newer.register_mode_executor = msg_older.register_mode_executor; + msg_newer.enable_replace_internal_mode = msg_older.enable_replace_internal_mode; + msg_newer.replace_internal_mode = msg_older.replace_internal_mode; + msg_newer.activate_mode_immediately = msg_older.activate_mode_immediately; + msg_newer.not_user_selectable = msg_older.not_user_selectable; + msg_newer.request_offboard_setpoints = false; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.request_id = msg_newer.request_id; + msg_older.name = msg_newer.name; + msg_older.px4_ros2_api_version = msg_newer.px4_ros2_api_version; + msg_older.register_arming_check = msg_newer.register_arming_check; + msg_older.register_mode = msg_newer.register_mode; + msg_older.register_mode_executor = msg_newer.register_mode_executor; + msg_older.enable_replace_internal_mode = msg_newer.enable_replace_internal_mode; + msg_older.replace_internal_mode = msg_newer.replace_internal_mode; + msg_older.activate_mode_immediately = msg_newer.activate_mode_immediately; + msg_older.not_user_selectable = msg_newer.not_user_selectable; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(RegisterExtComponentRequestV2Translation); diff --git a/msg/translation_node/translations/translation_vehicle_status_v3.h b/msg/translation_node/translations/translation_vehicle_status_v3.h index 98935947c0..92e51121ef 100644 --- a/msg/translation_node/translations/translation_vehicle_status_v3.h +++ b/msg/translation_node/translations/translation_vehicle_status_v3.h @@ -6,14 +6,14 @@ // Translate VehicleStatus v2 <--> v3 #include -#include +#include class VehicleStatusV3Translation { public: using MessageOlder = px4_msgs_old::msg::VehicleStatusV2; static_assert(MessageOlder::MESSAGE_VERSION == 2); - using MessageNewer = px4_msgs::msg::VehicleStatus; + using MessageNewer = px4_msgs_old::msg::VehicleStatusV3; static_assert(MessageNewer::MESSAGE_VERSION == 3); static constexpr const char* kTopic = "fmu/out/vehicle_status"; diff --git a/msg/translation_node/translations/translation_vehicle_status_v4.h b/msg/translation_node/translations/translation_vehicle_status_v4.h new file mode 100644 index 0000000000..36413cce8b --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v4.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v3 <--> v4 +#include +#include + +class VehicleStatusV4Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV3; + static_assert(MessageOlder::MESSAGE_VERSION == 3); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 4); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.nav_state_display = msg_older.nav_state_display; + msg_newer.accepts_offboard_setpoints = false; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.traffic_avoidance_system_present = msg_older.traffic_avoidance_system_present; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.nav_state_display = msg_newer.nav_state_display; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.traffic_avoidance_system_present = msg_newer.traffic_avoidance_system_present; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV4Translation); diff --git a/msg/versioned/RegisterExtComponentRequest.msg b/msg/versioned/RegisterExtComponentRequest.msg index 3b76aef15e..1ed4295f71 100644 --- a/msg/versioned/RegisterExtComponentRequest.msg +++ b/msg/versioned/RegisterExtComponentRequest.msg @@ -1,6 +1,6 @@ # Request to register an external component -uint32 MESSAGE_VERSION = 1 +uint32 MESSAGE_VERSION = 2 uint64 timestamp # time since system start (microseconds) @@ -20,5 +20,6 @@ bool enable_replace_internal_mode # set to true if an internal mode should be r 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) bool not_user_selectable # mode cannot be selected by the user +bool request_offboard_setpoints # set to true if the registered mode wants to receive offboard trajectory setpoints via MAVLink uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 888ba6cde8..28cbe379e9 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,6 +1,6 @@ # Encodes the system state of the vehicle published by commander -uint32 MESSAGE_VERSION = 3 +uint32 MESSAGE_VERSION = 4 uint64 timestamp # time since system start (microseconds) @@ -65,6 +65,8 @@ uint8 NAVIGATION_STATE_MAX = 31 uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) uint8 nav_state_display # User-visible nav state sent via MAVLink (executor state if active, otherwise nav_state) +bool accepts_offboard_setpoints # True if the current mode accepts offboard trajectory setpoints via MAVLink + uint32 valid_nav_states_mask # Bitmask for all valid nav_state values uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 36343b97e0..f39e2d140c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2026,6 +2026,7 @@ void Commander::run() // vehicle_status publish (after prearm/preflight updates above) _mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask); + _vehicle_status.accepts_offboard_setpoints = _mode_management.currentModeAcceptsOffboardSetpoints(_vehicle_status.nav_state); _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index b7bd7cb21a..a3ad01d86e 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -295,6 +295,8 @@ void ModeManagement::checkNewRegistrations(UpdateRequest &update_request) mode.replaces_nav_state = request.replace_internal_mode; } + mode.request_offboard_setpoints = request.request_offboard_setpoints; + nav_mode_id = _modes.addExternalMode(mode); reply.mode_id = nav_mode_id; } @@ -671,4 +673,19 @@ void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can } } +bool ModeManagement::currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const +{ + // OFFBOARD mode always accepts offboard setpoints + if (nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + return true; + } + + // Check if it's an external mode that requests offboard setpoints + if (_modes.valid(nav_state)) { + return _modes.mode(nav_state).request_offboard_setpoints; + } + + return false; +} + #endif /* CONSTRAINED_FLASH */ diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index e55e5c2f3d..e20a1c6c47 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -105,6 +105,7 @@ public: bool unresponsive_reported{false}; int arming_check_registration_id{-1}; int mode_executor_registration_id{-1}; + bool request_offboard_setpoints{false}; config_overrides_s overrides{}; vehicle_control_mode_s config_control_setpoint{}; }; @@ -169,6 +170,8 @@ public: void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const; + bool currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const; + void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out); private: @@ -232,6 +235,11 @@ public: can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION); } + bool currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const + { + return nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + } + void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { } private: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 652c48bfbc..60507e533f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1169,8 +1169,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { - // only publish setpoint once in OFFBOARD + if (vehicle_status.accepts_offboard_setpoints) { + // only publish setpoint once in mode that accepts offboard setpoints setpoint.timestamp = hrt_absolute_time(); _trajectory_setpoint_pub.publish(setpoint); } diff --git a/src/modules/mc_raptor/mc_raptor.cpp b/src/modules/mc_raptor/mc_raptor.cpp index b84599e35b..cdf2b2af23 100644 --- a/src/modules/mc_raptor/mc_raptor.cpp +++ b/src/modules/mc_raptor/mc_raptor.cpp @@ -260,6 +260,7 @@ bool Raptor::init() register_ext_component_request.register_mode = true; register_ext_component_request.enable_replace_internal_mode = _param_mc_raptor_offboard.get(); register_ext_component_request.replace_internal_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + register_ext_component_request.request_offboard_setpoints = true; _register_ext_component_request_pub.publish(register_ext_component_request); int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();