diff --git a/msg/px4_msgs_old/msg/VehicleStatusV1.msg b/msg/px4_msgs_old/msg/VehicleStatusV1.msg new file mode 100644 index 0000000000..185076a5d6 --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV1.msg @@ -0,0 +1,132 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 1 + +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) + +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 + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +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 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 12a8763ec6..78d30fc7fe 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -17,4 +17,5 @@ #include "translation_register_ext_component_request_v1.h" #include "translation_vehicle_attitude_setpoint_v1.h" #include "translation_vehicle_status_v1.h" +#include "translation_vehicle_status_v2.h" #include "translation_vehicle_local_position_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h index 2041b013d8..5650a8cf01 100644 --- a/msg/translation_node/translations/translation_vehicle_status_v1.h +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -6,14 +6,14 @@ // Translate VehicleStatus v0 <--> v1 #include -#include +#include class VehicleStatusV1Translation { public: using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; static_assert(MessageOlder::MESSAGE_VERSION == 0); - using MessageNewer = px4_msgs::msg::VehicleStatus; + using MessageNewer = px4_msgs_old::msg::VehicleStatusV1; static_assert(MessageNewer::MESSAGE_VERSION == 1); static constexpr const char* kTopic = "fmu/out/vehicle_status"; diff --git a/msg/translation_node/translations/translation_vehicle_status_v2.h b/msg/translation_node/translations/translation_vehicle_status_v2.h new file mode 100644 index 0000000000..926ade4da3 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v2.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v1 <--> v2 +#include +#include + +class VehicleStatusV2Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV1; + static_assert(MessageOlder::MESSAGE_VERSION == 1); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 2); + + 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; // use nav_state since active mode executor's navigation state was not known before + 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.failure_detector_status = msg_older.failure_detector_status; + 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.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) { + // copy everything except the new executor_nav_state + 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.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.failure_detector_status = msg_newer.failure_detector_status; + 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.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(VehicleStatusV2Translation); diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 185076a5d6..41daa384b5 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 = 1 +uint32 MESSAGE_VERSION = 2 uint64 timestamp # time since system start (microseconds) @@ -63,6 +63,7 @@ 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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ead99ad3d6..a6175e9c67 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2408,9 +2408,10 @@ bool Commander::handleModeIntentionAndFailsafe() _mode_management.setFailsafeState(_failsafe.selectedAction() > FailsafeBase::Action::Warn); _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.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 + _vehicle_status.nav_state_display = _mode_management.getNavStateDisplay(_vehicle_status.nav_state); switch (_failsafe.selectedAction()) { case FailsafeBase::Action::Disarm: diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index b7212f05b8..b7bd7cb21a 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -519,6 +519,18 @@ int ModeManagement::modeExecutorInCharge() const return _mode_executor_in_charge; } +uint8_t ModeManagement::getNavStateDisplay(uint8_t nav_state) const +{ + const int executor_in_charge = modeExecutorInCharge(); + + if (_mode_executors.valid(executor_in_charge)) { + return _mode_executors.executor(executor_in_charge).owned_nav_state; + + } else { + return nav_state; + } +} + bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const { bool ret = false; diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 52eff77dee..e65b340fa1 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -150,6 +150,12 @@ public: */ int modeExecutorInCharge() const; + /** + * Returns the executor's navigation state if active, otherwise nav_state. + * Exposes the high-level goal rather than the effective sub-mode. + */ + uint8_t getNavStateDisplay(uint8_t nav_state) const; + void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override; uint8_t getReplacedModeIfAny(uint8_t nav_state) override; @@ -208,6 +214,7 @@ public: void setFailsafeState(bool failsafe_action_active) {} int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; } + uint8_t getNavStateDisplay(uint8_t nav_state) const { return nav_state; } void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {} uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; } diff --git a/src/modules/mavlink/streams/CURRENT_MODE.hpp b/src/modules/mavlink/streams/CURRENT_MODE.hpp index 2be1c47c4b..50f641b48d 100644 --- a/src/modules/mavlink/streams/CURRENT_MODE.hpp +++ b/src/modules/mavlink/streams/CURRENT_MODE.hpp @@ -64,10 +64,10 @@ private: if (_vehicle_status_sub.update(&vehicle_status)) { mavlink_current_mode_t current_mode{}; - current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data; + current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state_display).data; current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data; - current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state, - vehicle_status.vehicle_type, vehicle_status.is_vtol); + current_mode.standard_mode = + (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state_display, vehicle_status.vehicle_type, vehicle_status.is_vtol); mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), ¤t_mode); return true; } diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index a401526781..47b4992650 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -100,8 +100,8 @@ private: } - // uint32_t custom_mode - A bitfield for use for autopilot-specific flags - union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)}; + // uint32_t custom_mode - Bitfield for autopilot-specific flags + union px4_custom_mode custom_mode = get_px4_custom_mode(vehicle_status.nav_state_display); // uint8_t system_status (MAV_STATE) - System status flag.