diff --git a/msg/px4_msgs_old/msg/VehicleStatusV0.msg b/msg/px4_msgs_old/msg/VehicleStatusV0.msg new file mode 100644 index 0000000000..a347dfce3d --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV0.msg @@ -0,0 +1,140 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 0 + +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_TRANSITION_TO_STANDBY = 0 +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_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +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_FREE4 = 8 +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 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +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 +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 avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +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 2b3c2030b4..4369a3c58a 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -9,3 +9,5 @@ //#include "example_translation_direct_v1.h" //#include "example_translation_multi_v2.h" //#include "example_translation_service_v1.h" + +#include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h new file mode 100644 index 0000000000..2041b013d8 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v0 <--> v1 +#include +#include + +class VehicleStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + 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.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) { + // Set msg_older from msg_newer + 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.avoidance_system_required = false; + msg_older.avoidance_system_valid = false; + 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(VehicleStatusV1Translation);