From 7d97be15d9391186dd7c4edc7c2c9fad554180f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 7 Oct 2024 10:33:59 +0200 Subject: [PATCH] add registration for generic translations (N input and M output messages) --- px4_msgs/msg/VehicleGlobalPosition.msg | 7 +- px4_msgs/msg/VehicleLocalPosition.msg | 8 +- px4_msgs_old/msg/GotoSetpoint.msg | 24 --- px4_msgs_old/msg/ModeCompleted.msg | 14 -- px4_msgs_old/msg/TrajectorySetpoint.msg | 15 -- px4_msgs_old/msg/VehicleAttitudeSetpoint.msg | 16 -- px4_msgs_old/msg/VehicleCommand.msg | 196 ------------------ px4_msgs_old/msg/VehicleCommandAck.msg | 33 --- ...sition.msg => VehicleGlobalPositionV1.msg} | 1 + ...osition.msg => VehicleLocalPositionV1.msg} | 1 + px4_msgs_old/msg/VehicleStatus.msg | 138 ------------ px4_msgs_old/srv/VehicleCommand.srv | 3 - subscriber_test/src/listener.cpp | 44 +++- translation_node/src/main.cpp | 1 + translation_node/src/pub_sub_graph.cpp | 4 +- translation_node/src/template_util.h | 62 ++++++ translation_node/src/translation_util.h | 156 +++++++++++++- translation_node/src/translations.h | 3 +- translation_node/src/vehicle_attitude_v2.h | 4 +- translation_node/src/vehicle_attitude_v3.h | 4 +- .../src/vehicle_local_global_position_v2.h | 190 +++++++++++++++++ 21 files changed, 454 insertions(+), 470 deletions(-) delete mode 100644 px4_msgs_old/msg/GotoSetpoint.msg delete mode 100644 px4_msgs_old/msg/ModeCompleted.msg delete mode 100644 px4_msgs_old/msg/TrajectorySetpoint.msg delete mode 100644 px4_msgs_old/msg/VehicleAttitudeSetpoint.msg delete mode 100644 px4_msgs_old/msg/VehicleCommand.msg delete mode 100644 px4_msgs_old/msg/VehicleCommandAck.msg rename px4_msgs_old/msg/{VehicleGlobalPosition.msg => VehicleGlobalPositionV1.msg} (98%) rename px4_msgs_old/msg/{VehicleLocalPosition.msg => VehicleLocalPositionV1.msg} (99%) delete mode 100644 px4_msgs_old/msg/VehicleStatus.msg delete mode 100644 px4_msgs_old/srv/VehicleCommand.srv create mode 100644 translation_node/src/template_util.h create mode 100644 translation_node/src/vehicle_local_global_position_v2.h diff --git a/px4_msgs/msg/VehicleGlobalPosition.msg b/px4_msgs/msg/VehicleGlobalPosition.msg index e37155fe..9d87ef55 100644 --- a/px4_msgs/msg/VehicleGlobalPosition.msg +++ b/px4_msgs/msg/VehicleGlobalPosition.msg @@ -4,6 +4,7 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # +uint32 MESSAGE_VERSION = 2 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -13,11 +14,7 @@ float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) float32 alt_ellipsoid # Altitude above ellipsoid, (meters) -float32 delta_alt # Reset delta for altitude -float32 delta_terrain # Reset delta for terrain -uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates -uint8 alt_reset_counter # Counter for reset events on altitude -uint8 terrain_reset_counter # Counter for reset events on terrain +float32 dist_bottom # Distance from from bottom surface to ground, (metres) float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) diff --git a/px4_msgs/msg/VehicleLocalPosition.msg b/px4_msgs/msg/VehicleLocalPosition.msg index 0e74ac0f..9f66a90e 100644 --- a/px4_msgs/msg/VehicleLocalPosition.msg +++ b/px4_msgs/msg/VehicleLocalPosition.msg @@ -1,5 +1,6 @@ # Fused local position in NED. # The coordinate system origin is the vehicle position at the time when the EKF2-module was started. +uint32 MESSAGE_VERSION = 2 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -56,9 +57,14 @@ float32 ref_alt # Reference altitude AMSL, (metres) # Distance to surface bool dist_bottom_valid # true if distance to bottom surface is valid -float32 dist_bottom # Distance from from bottom surface to ground, (metres) float32 dist_bottom_var # terrain estimate variance (m^2) +float32 delta_alt # Reset delta for altitude +float32 delta_terrain # Reset delta for terrain +uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 alt_reset_counter # Counter for reset events on altitude +uint8 terrain_reset_counter # Counter for reset events on terrain + float32 delta_dist_bottom # Amount of vertical shift of dist bottom estimate in latest reset [m] uint8 dist_bottom_reset_counter # Index of latest dist bottom estimate reset diff --git a/px4_msgs_old/msg/GotoSetpoint.msg b/px4_msgs_old/msg/GotoSetpoint.msg deleted file mode 100644 index 5fe3ab8a..00000000 --- a/px4_msgs_old/msg/GotoSetpoint.msg +++ /dev/null @@ -1,24 +0,0 @@ -# Position and (optional) heading setpoints with corresponding speed constraints -# Setpoints are intended as inputs to position and heading smoothers, respectively -# Setpoints do not need to be kinematically consistent -# Optional heading setpoints may be specified as controlled by the respective flag -# Unset optional setpoints are not controlled -# Unset optional constraints default to vehicle specifications - -uint64 timestamp # time since system start (microseconds) - -# setpoints -float32[3] position # [m] NED local world frame - -bool flag_control_heading # true if heading is to be controlled -float32 heading # (optional) [rad] [-pi,pi] from North - -# constraints -bool flag_set_max_horizontal_speed # true if setting a non-default horizontal speed limit -float32 max_horizontal_speed # (optional) [m/s] maximum speed (absolute) in the NE-plane - -bool flag_set_max_vertical_speed # true if setting a non-default vertical speed limit -float32 max_vertical_speed # (optional) [m/s] maximum speed (absolute) in the D-axis - -bool flag_set_max_heading_rate # true if setting a non-default heading rate limit -float32 max_heading_rate # (optional) [rad/s] maximum heading rate (absolute) diff --git a/px4_msgs_old/msg/ModeCompleted.msg b/px4_msgs_old/msg/ModeCompleted.msg deleted file mode 100644 index 0a20b029..00000000 --- a/px4_msgs_old/msg/ModeCompleted.msg +++ /dev/null @@ -1,14 +0,0 @@ -# Mode completion result, published by an active mode. -# The possible values of nav_state are defined in the VehicleStatus msg. -# Note that this is not always published (e.g. when a user switches modes or on -# failsafe activation) -uint64 timestamp # time since system start (microseconds) - - -uint8 RESULT_SUCCESS = 0 -# [1-99]: reserved -uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error) - -uint8 result # One of RESULT_* - -uint8 nav_state # Source mode (values in VehicleStatus) diff --git a/px4_msgs_old/msg/TrajectorySetpoint.msg b/px4_msgs_old/msg/TrajectorySetpoint.msg deleted file mode 100644 index 4a88c867..00000000 --- a/px4_msgs_old/msg/TrajectorySetpoint.msg +++ /dev/null @@ -1,15 +0,0 @@ -# Trajectory setpoint in NED frame -# Input to PID position controller. -# Needs to be kinematically consistent and feasible for smooth flight. -# setting a value to NaN means the state should not be controlled - -uint64 timestamp # time since system start (microseconds) - -# NED local world frame -float32[3] position # in meters -float32[3] velocity # in meters/second -float32[3] acceleration # in meters/second^2 -float32[3] jerk # in meters/second^3 (for logging only) - -float32 yaw # euler angle of desired attitude in radians -PI..+PI -float32 yawspeed # angular velocity around NED frame z-axis in radians/second diff --git a/px4_msgs_old/msg/VehicleAttitudeSetpoint.msg b/px4_msgs_old/msg/VehicleAttitudeSetpoint.msg deleted file mode 100644 index 74a75302..00000000 --- a/px4_msgs_old/msg/VehicleAttitudeSetpoint.msg +++ /dev/null @@ -1,16 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 yaw_sp_move_rate # rad/s (commanded by user) - -# For quaternion-based attitude control -float32[4] q_d # Desired quaternion for quaternion control - -# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. -# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. -float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] - -bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) - -bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) - -# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/px4_msgs_old/msg/VehicleCommand.msg b/px4_msgs_old/msg/VehicleCommand.msg deleted file mode 100644 index 8df0ca56..00000000 --- a/px4_msgs_old/msg/VehicleCommand.msg +++ /dev/null @@ -1,196 +0,0 @@ -# Vehicle Command uORB message. Used for commanding a mission / action / etc. -# Follows the MAVLink COMMAND_INT / COMMAND_LONG definition - -uint64 timestamp # time since system start (microseconds) - -uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command -uint16 VEHICLE_CMD_CUSTOM_1 = 1 # test command -uint16 VEHICLE_CMD_CUSTOM_2 = 2 # test command -uint16 VEHICLE_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing -uint16 VEHICLE_CMD_DO_ORBIT = 34 # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z | -uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35 # Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z | -uint16 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| -uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| -uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| -uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| -uint16 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| -uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_CONDITION_YAW = 115 # Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| -uint16 VEHICLE_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_CONDITION_GATE = 4501 # Wait until passing a threshold |2D coord mode: 0: Orthogonal to planned route | Altitude mode: 0: Ignore altitude| Empty| Empty| Lat| Lon| Alt| -uint16 VEHICLE_CMD_DO_SET_MODE = 176 # Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| -uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| -uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_REPOSITION = 192 # Reposition to specific WGS84 GPS position. |Ground speed [m/s] |Bitmask |Loiter radius [m] for planes |Yaw [deg] |Latitude |Longitude |Altitude | -uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 -uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| -uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| -uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| -uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203 -uint16 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| -uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty| -uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_GRIPPER = 211 # Command to operate a gripper -uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION -uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration -uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| -uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started -uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| -uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| -uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| -uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode |MAV_STANDARD_MODE| -uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal - -uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| -uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| -uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| -uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm -uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks. -uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes -uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| -uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message -uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) -uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom -uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 -uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw -uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control -uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence. -uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system -uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500 # Start a video capture. -uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture. -uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data -uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data -uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link -uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition -uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization -uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan -uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment -uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. -uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. - -uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning -uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004 - -# PX4 vehicle commands (beyond 16 bit mavlink commands) -uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) -uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| -uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty| - -uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | -uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | -uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | -uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | -uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt | -uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5 # - -uint8 VEHICLE_ROI_NONE = 0 # No region of interest | -uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION | -uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION | -uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location | -uint8 VEHICLE_ROI_TARGET = 4 # Point toward target -uint8 VEHICLE_ROI_ENUM_END = 5 - -uint8 PARACHUTE_ACTION_DISABLE = 0 -uint8 PARACHUTE_ACTION_ENABLE = 1 -uint8 PARACHUTE_ACTION_RELEASE = 2 - -uint8 FAILURE_UNIT_SENSOR_GYRO = 0 -uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 -uint8 FAILURE_UNIT_SENSOR_MAG = 2 -uint8 FAILURE_UNIT_SENSOR_BARO = 3 -uint8 FAILURE_UNIT_SENSOR_GPS = 4 -uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5 -uint8 FAILURE_UNIT_SENSOR_VIO = 6 -uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 -uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 -uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 -uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 -uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 -uint8 FAILURE_UNIT_SYSTEM_AVOIDANCE = 103 -uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 -uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 - -uint8 FAILURE_TYPE_OK = 0 -uint8 FAILURE_TYPE_OFF = 1 -uint8 FAILURE_TYPE_STUCK = 2 -uint8 FAILURE_TYPE_GARBAGE = 3 -uint8 FAILURE_TYPE_WRONG = 4 -uint8 FAILURE_TYPE_SLOW = 5 -uint8 FAILURE_TYPE_DELAYED = 6 -uint8 FAILURE_TYPE_INTERMITTENT = 7 - -# used as param1 in DO_CHANGE_SPEED command -uint8 SPEED_TYPE_AIRSPEED = 0 -uint8 SPEED_TYPE_GROUNDSPEED = 1 -uint8 SPEED_TYPE_CLIMB_SPEED = 2 -uint8 SPEED_TYPE_DESCEND_SPEED = 3 - -# used as param3 in CMD_DO_ORBIT -uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 -uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 -uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 -uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 -uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 -uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 - -# used as param1 in ARM_DISARM command -int8 ARMING_ACTION_DISARM = 0 -int8 ARMING_ACTION_ARM = 1 - -# param2 in VEHICLE_CMD_DO_GRIPPER -uint8 GRIPPER_ACTION_RELEASE = 0 -uint8 GRIPPER_ACTION_GRAB = 1 - -uint8 ORB_QUEUE_LENGTH = 8 - -float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. -float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. -float32 param3 # Parameter 3, as defined by MAVLink uint16 VEHICLE_CMD enum. -float32 param4 # Parameter 4, as defined by MAVLink uint16 VEHICLE_CMD enum. -float64 param5 # Parameter 5, as defined by MAVLink uint16 VEHICLE_CMD enum. -float64 param6 # Parameter 6, as defined by MAVLink uint16 VEHICLE_CMD enum. -float32 param7 # Parameter 7, as defined by MAVLink uint16 VEHICLE_CMD enum. -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 -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 - -uint16 COMPONENT_MODE_EXECUTOR_START = 1000 - -# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor diff --git a/px4_msgs_old/msg/VehicleCommandAck.msg b/px4_msgs_old/msg/VehicleCommandAck.msg deleted file mode 100644 index 6f54fa46..00000000 --- a/px4_msgs_old/msg/VehicleCommandAck.msg +++ /dev/null @@ -1,33 +0,0 @@ -# Vehicle Command Ackonwledgement uORB message. -# Used for acknowledging the vehicle command being received. -# Follows the MAVLink COMMAND_ACK message definition - -uint64 timestamp # time since system start (microseconds) - -# Result cases. This follows the MAVLink MAV_RESULT enum definition -uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | -uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | -uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | -uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | -uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | -uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | -uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled - -# Arming denied specific cases -uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 -uint16 ARM_AUTH_DENIED_REASON_NONE = 1 -uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 -uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 -uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 -uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 - -uint8 ORB_QUEUE_LENGTH = 4 - -uint32 command # Command that is being acknowledged -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 -uint16 target_component # Target component / mode executor - -bool from_external # Indicates if the command came from an external source diff --git a/px4_msgs_old/msg/VehicleGlobalPosition.msg b/px4_msgs_old/msg/VehicleGlobalPositionV1.msg similarity index 98% rename from px4_msgs_old/msg/VehicleGlobalPosition.msg rename to px4_msgs_old/msg/VehicleGlobalPositionV1.msg index e37155fe..4cad8591 100644 --- a/px4_msgs_old/msg/VehicleGlobalPosition.msg +++ b/px4_msgs_old/msg/VehicleGlobalPositionV1.msg @@ -4,6 +4,7 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/px4_msgs_old/msg/VehicleLocalPosition.msg b/px4_msgs_old/msg/VehicleLocalPositionV1.msg similarity index 99% rename from px4_msgs_old/msg/VehicleLocalPosition.msg rename to px4_msgs_old/msg/VehicleLocalPositionV1.msg index 0e74ac0f..61a06e94 100644 --- a/px4_msgs_old/msg/VehicleLocalPosition.msg +++ b/px4_msgs_old/msg/VehicleLocalPositionV1.msg @@ -1,5 +1,6 @@ # Fused local position in NED. # The coordinate system origin is the vehicle position at the time when the EKF2-module was started. +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/px4_msgs_old/msg/VehicleStatus.msg b/px4_msgs_old/msg/VehicleStatus.msg deleted file mode 100644 index 4c711b97..00000000 --- a/px4_msgs_old/msg/VehicleStatus.msg +++ /dev/null @@ -1,138 +0,0 @@ -# Encodes the system state of the vehicle published by commander - -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_RC_STICK = 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/px4_msgs_old/srv/VehicleCommand.srv b/px4_msgs_old/srv/VehicleCommand.srv deleted file mode 100644 index 134e2a81..00000000 --- a/px4_msgs_old/srv/VehicleCommand.srv +++ /dev/null @@ -1,3 +0,0 @@ -VehicleCommand request ---- -VehicleCommandAck reply diff --git a/subscriber_test/src/listener.cpp b/subscriber_test/src/listener.cpp index 1ec01bef..3a44afaf 100644 --- a/subscriber_test/src/listener.cpp +++ b/subscriber_test/src/listener.cpp @@ -2,6 +2,8 @@ #include "rclcpp/rclcpp.hpp" #include "px4_msgs/msg/vehicle_attitude.hpp" +#include "px4_msgs/msg/vehicle_global_position.hpp" +#include "px4_msgs/msg/vehicle_local_position.hpp" template static std::string topic_version_suffix() { @@ -17,17 +19,43 @@ class MinimalSubscriber : public rclcpp::Node MinimalSubscriber() : Node("minimal_subscriber") { - auto topic_callback = - [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) -> void { - RCLCPP_INFO(this->get_logger(), "Got: timestamp: '%lli', new_field: '%i'", msg->timestamp, msg->new_field); - }; - subscription_ = this->create_subscription( - "/fmu/out/vehicle_attitude" + topic_version_suffix(), - rclcpp::QoS(1).best_effort(), topic_callback); + { + auto topic_callback = + [this](px4_msgs::msg::VehicleAttitude::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "Got attitude: timestamp: '%lli', new_field: '%i'", msg->timestamp, + (int) msg->new_field); + }; + subscription_att_ = this->create_subscription( + "/fmu/out/vehicle_attitude" + topic_version_suffix(), + rclcpp::QoS(1).best_effort(), topic_callback); + } + { + auto topic_callback = + [this](px4_msgs::msg::VehicleGlobalPosition::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "Got global pos: timestamp: '%lli', alt: %.3f dist bottom: %.3f, eph: %.3f, terrain_alt: %.3f", msg->timestamp, + (double)msg->alt, (double)msg->dist_bottom, (double)msg->eph, (double)msg->terrain_alt); + }; + subscription_gpos_ = this->create_subscription( + "/fmu/out/vehicle_global_position" + topic_version_suffix(), + rclcpp::QoS(1).best_effort(), topic_callback); + } + { + auto topic_callback = + [this](px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "Got local pos: timestamp: '%lli', dist_bottom_var: %.3f, delta_alt: %.3f, alt_reset_counter: %i, delta_bottom: %.3f, eph: %.3f", + msg->timestamp, + (double)msg->dist_bottom_var, (double)msg->delta_alt, msg->alt_reset_counter, (double)msg->delta_dist_bottom, (double)msg->eph); + }; + subscription_lpos_ = this->create_subscription( + "/fmu/out/vehicle_local_position" + topic_version_suffix(), + rclcpp::QoS(1).best_effort(), topic_callback); + } } private: - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_att_; + rclcpp::Subscription::SharedPtr subscription_gpos_; + rclcpp::Subscription::SharedPtr subscription_lpos_; }; int main(int argc, char * argv[]) diff --git a/translation_node/src/main.cpp b/translation_node/src/main.cpp index 7a4d65d8..4dc03b51 100644 --- a/translation_node/src/main.cpp +++ b/translation_node/src/main.cpp @@ -8,6 +8,7 @@ #include "vehicle_attitude_v2.h" #include "vehicle_attitude_v3.h" +#include "vehicle_local_global_position_v2.h" #include "pub_sub_graph.h" using namespace std::chrono_literals; diff --git a/translation_node/src/pub_sub_graph.cpp b/translation_node/src/pub_sub_graph.cpp index 7657b592..863ba7ef 100644 --- a/translation_node/src/pub_sub_graph.cpp +++ b/translation_node/src/pub_sub_graph.cpp @@ -10,10 +10,10 @@ PubSubGraph::PubSubGraph(rclcpp::Node &node, const Translations &translations) : std::unordered_map> known_versions; for (const auto& topic : translations.topics()) { - const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), topic.topic_name); + const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), topic.id.topic_name); _known_topics_warned.insert({full_topic_name, false}); - const MessageIdentifier id{full_topic_name, topic.version}; + const MessageIdentifier id{full_topic_name, topic.id.version}; NodeDataPubSub node_data{topic.subscription_factory, topic.publication_factory, id, topic.max_serialized_message_size}; _pub_sub_graph.addNodeIfNotExists(id, std::move(node_data), topic.message_buffer); known_versions[full_topic_name].insert(id.version); diff --git a/translation_node/src/template_util.h b/translation_node/src/template_util.h new file mode 100644 index 00000000..947367a6 --- /dev/null +++ b/translation_node/src/template_util.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +/** + * Helper struct to store template parameter packs + */ +template +struct Pack { +}; + +/** + * Struct for a template parameter pack with access to the individual types + */ +template +struct TypesArray { + template + struct TypeHelper { + using Type = T; + using Next = TypeHelper; + }; + + using Type1 = typename TypeHelper::Type; + using Type2 = typename TypeHelper::Next::Type; + using Type3 = typename TypeHelper::Next::Next::Type; + using Type4 = typename TypeHelper::Next::Next::Next::Type; + using Type5 = typename TypeHelper::Next::Next::Next::Next::Type; + using Type6 = typename TypeHelper::Next::Next::Next::Next::Next::Type; + + using args = Pack; +}; + + +/** + * Helper for call_translation_function() + */ +template +inline void call_translation_function_impl(F f, Pack, Pack, + const std::vector>& messages_in, + std::vector>& messages_out, + std::integer_sequence, std::integer_sequence) +{ + f(*static_cast(messages_in[Is].get())..., *static_cast(messages_out[Os].get())...); +} + +/** + * Call a translation function F which takes the arguments (const ArgsIn&..., ArgsOut&...), + * by passing messages_in and messages_out as arguments. + * Note that sizeof(ArgsIn) == messages_in.length() && sizeof(ArgsOut) == messages_out.length() must hold. + */ +template +inline void call_translation_function(F f, Pack pack_in, Pack pack_out, + const std::vector>& messages_in, + std::vector>& messages_out) { + call_translation_function_impl(f, pack_in, pack_out, messages_in, messages_out, + std::index_sequence_for{}, std::index_sequence_for{}); +} diff --git a/translation_node/src/translation_util.h b/translation_node/src/translation_util.h index 3a896c58..bee48e6b 100644 --- a/translation_node/src/translation_util.h +++ b/translation_node/src/translation_util.h @@ -6,6 +6,7 @@ #include "translations.h" #include "util.h" +#include "template_util.h" #include #include @@ -22,8 +23,33 @@ class RegisteredTranslations { return instance; } + /** + * @brief Register a translation class with 1 input and 1 output message. + * + * The translation class has the form: + * + * ``` + * class MyTranslation { + * public: + * using MessageOlder = px4_msgs_old::msg::VehicleAttitudeV2; + * + * using MessageNewer = px4_msgs::msg::VehicleAttitude; + * + * static constexpr const char* kTopic = "fmu/out/vehicle_attitude"; + * + * static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + * // set msg_older from msg_newer + * } + * }; + * ``` + */ template - void registerDirectTranslation(const std::string& topic_name) { + void registerDirectTranslation() { + const std::string topic_name = T::kTopic; _translations.addTopic(getTopicForMessageType(topic_name)); _translations.addTopic(getTopicForMessageType(topic_name)); @@ -42,6 +68,79 @@ class RegisteredTranslations { {MessageIdentifier{topic_name, T::MessageOlder::MESSAGE_VERSION}}}); } + /** + * @brief Register a translation class with N input and M output messages. + * + * The translation class has the form: + * ``` + * class MyTranslation { + * public: + * using MessagesOlder = TypesArray; + * static constexpr const char* kTopicsOlder[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * using MessagesNewer = TypesArray; + * static constexpr const char* kTopicsNewer[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, ... + * MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, ...) { + * // Set msg_newerX from msg_olderX + * } + * + * static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, ... + * MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, ...) { + * // Set msg_olderX from msg_newerX + * } + * }; + * ``` + */ + template + void registerTranslation() { + const auto topics_older = getTopicsForMessageType(typename T::MessagesOlder::args(), T::kTopicsOlder); + std::vector topics_older_identifiers; + for (const auto& topic : topics_older) { + _translations.addTopic(topic); + topics_older_identifiers.emplace_back(topic.id); + } + const auto topics_newer = getTopicsForMessageType(typename T::MessagesNewer::args(),T::kTopicsNewer); + std::vector topics_newer_identifiers; + for (const auto& topic : topics_newer) { + _translations.addTopic(topic); + topics_newer_identifiers.emplace_back(topic.id); + } + + // Translation callbacks + const auto translation_cb_from_older = [](const std::vector& older_msgs, std::vector& newer_msgs) { + call_translation_function(&T::fromOlder, typename T::MessagesOlder::args(), typename T::MessagesNewer::args(), older_msgs, newer_msgs); + }; + const auto translation_cb_to_older = [](const std::vector& newer_msgs, std::vector& older_msgs) { + call_translation_function(&T::toOlder, typename T::MessagesNewer::args(), typename T::MessagesOlder::args(), newer_msgs, older_msgs); + }; + { + // Older -> Newer + Translation translation; + translation.cb = translation_cb_from_older; + translation.inputs = topics_older_identifiers; + translation.outputs = topics_newer_identifiers; + _translations.addTranslation(std::move(translation)); + } + { + // Newer -> Older + Translation translation; + translation.cb = translation_cb_to_older; + translation.inputs = topics_newer_identifiers; + translation.outputs = topics_older_identifiers; + _translations.addTranslation(std::move(translation)); + } + } + const Translations& translations() const { return _translations; } private: @@ -50,16 +149,18 @@ class RegisteredTranslations { template Topic getTopicForMessageType(const std::string& topic_name) { Topic ret{}; - ret.topic_name = topic_name; - ret.version = RosMessageType::MESSAGE_VERSION; + ret.id.topic_name = topic_name; + ret.id.version = RosMessageType::MESSAGE_VERSION; auto message_buffer = std::make_shared(); ret.message_buffer = std::static_pointer_cast(message_buffer); // Subscription/Publication factory methods - const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.version); + const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.id.version); ret.subscription_factory = [topic_name_versioned, message_buffer](rclcpp::Node& node, const std::function& on_topic_cb) -> rclcpp::SubscriptionBase::SharedPtr { return std::dynamic_pointer_cast( + // Note: template instantiation of subscriptions slows down compilation considerably, see + // https://github.com/ros2/rclcpp/issues/1949 node.create_subscription(topic_name_versioned, rclcpp::QoS(1).best_effort(), [on_topic_cb=on_topic_cb, message_buffer](typename RosMessageType::UniquePtr msg) -> void { *message_buffer = *msg; @@ -82,19 +183,52 @@ class RegisteredTranslations { return ret; } + template + std::vector getTopicsForMessageTypeImpl(const char* const topics[], std::integer_sequence) { + std::vector ret { + getTopicForMessageType(topics[Is])... + }; + return ret; + } + + template + std::vector getTopicsForMessageType(Pack, const char* const (&topics)[N]) { + static_assert(N == sizeof...(RosMessageTypes), "Number of topics does not match number of message types"); + return getTopicsForMessageTypeImpl(topics, std::index_sequence_for{}); + } + Translations _translations; }; template -class RegistrationHelperDirect { +class TopicRegistrationHelperDirect { +public: + explicit TopicRegistrationHelperDirect(const char* dummy) { + // There's something strange: when there is no argument passed, the + // compiler removes the static object completely. I don't know + // why but this dummy variable prevents that. + (void)dummy; + RegisteredTranslations::instance().registerDirectTranslation(); + } + TopicRegistrationHelperDirect(TopicRegistrationHelperDirect const&) = delete; + void operator=(TopicRegistrationHelperDirect const&) = delete; +private: +}; + +#define REGISTER_TOPIC_TRANSLATION_DIRECT(class_name) \ + TopicRegistrationHelperDirect class_name##_registration_direct("dummy"); + +template +class TopicRegistrationHelper { public: - explicit RegistrationHelperDirect(const std::string& topic_name) { - RegisteredTranslations::instance().registerDirectTranslation(topic_name); + explicit TopicRegistrationHelper(const char* dummy) { + (void)dummy; + RegisteredTranslations::instance().registerTranslation(); } - RegistrationHelperDirect(RegistrationHelperDirect const&) = delete; - void operator=(RegistrationHelperDirect const&) = delete; + TopicRegistrationHelper(TopicRegistrationHelper const&) = delete; + void operator=(TopicRegistrationHelper const&) = delete; private: }; -#define REGISTER_MESSAGE_TRANSLATION_DIRECT(topic_name, class_name) \ - static RegistrationHelperDirect class_name##_registration(topic_name); +#define REGISTER_TOPIC_TRANSLATION(class_name) \ + TopicRegistrationHelper class_name##_registration("dummy"); diff --git a/translation_node/src/translations.h b/translation_node/src/translations.h index 5b5d0497..f2eadf27 100644 --- a/translation_node/src/translations.h +++ b/translation_node/src/translations.h @@ -23,8 +23,7 @@ using SubscriptionFactoryCB = std::function; struct Topic { - std::string topic_name; - MessageVersionType version{}; + MessageIdentifier id; SubscriptionFactoryCB subscription_factory; PublicationFactoryCB publication_factory; diff --git a/translation_node/src/vehicle_attitude_v2.h b/translation_node/src/vehicle_attitude_v2.h index eefc2d72..6dc859c5 100644 --- a/translation_node/src/vehicle_attitude_v2.h +++ b/translation_node/src/vehicle_attitude_v2.h @@ -19,6 +19,8 @@ class VehicleAttitudeV2Translation { using MessageNewer = px4_msgs_old::msg::VehicleAttitudeV2; static_assert(MessageNewer::MESSAGE_VERSION == 2); + static constexpr const char* kTopic = "fmu/out/vehicle_attitude"; + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { // No change in message definition static_assert(sizeof(msg_newer) == sizeof(msg_older)); @@ -32,4 +34,4 @@ class VehicleAttitudeV2Translation { private: }; -REGISTER_MESSAGE_TRANSLATION_DIRECT("fmu/out/vehicle_attitude", VehicleAttitudeV2Translation); \ No newline at end of file +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleAttitudeV2Translation); \ No newline at end of file diff --git a/translation_node/src/vehicle_attitude_v3.h b/translation_node/src/vehicle_attitude_v3.h index 53fd06d9..d3ff0651 100644 --- a/translation_node/src/vehicle_attitude_v3.h +++ b/translation_node/src/vehicle_attitude_v3.h @@ -18,6 +18,8 @@ class VehicleAttitudeV3Translation { using MessageNewer = px4_msgs::msg::VehicleAttitude; static_assert(MessageNewer::MESSAGE_VERSION == 3); + static constexpr const char* kTopic = "fmu/out/vehicle_attitude"; + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { msg_newer.timestamp = msg_older.timestamp; msg_newer.timestamp_sample = msg_older.timestamp_sample; @@ -45,4 +47,4 @@ class VehicleAttitudeV3Translation { private: }; -REGISTER_MESSAGE_TRANSLATION_DIRECT("fmu/out/vehicle_attitude", VehicleAttitudeV3Translation); \ No newline at end of file +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleAttitudeV3Translation); \ No newline at end of file diff --git a/translation_node/src/vehicle_local_global_position_v2.h b/translation_node/src/vehicle_local_global_position_v2.h new file mode 100644 index 00000000..b617f24f --- /dev/null +++ b/translation_node/src/vehicle_local_global_position_v2.h @@ -0,0 +1,190 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleGlobalPosition/VehicleLocalPosition v1 <--> v2 +#include +#include +#include +#include + +#include "translation_util.h" + +class VehicleLocalGlobalPositionV2Translation { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "fmu/out/vehicle_global_position", + "fmu/out/vehicle_local_position", + }; + static_assert(px4_msgs_old::msg::VehicleGlobalPositionV1::MESSAGE_VERSION == 1); + static_assert(px4_msgs_old::msg::VehicleLocalPositionV1::MESSAGE_VERSION == 1); + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "fmu/out/vehicle_global_position", + "fmu/out/vehicle_local_position", + }; + static_assert(px4_msgs::msg::VehicleGlobalPosition::MESSAGE_VERSION == 2); + static_assert(px4_msgs::msg::VehicleLocalPosition::MESSAGE_VERSION == 2); + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) { + // Moved from Global to Local: + msg_newer2.delta_alt = msg_older1.delta_alt; + msg_newer2.delta_terrain = msg_older1.delta_terrain; + msg_newer2.lat_lon_reset_counter = msg_older1.lat_lon_reset_counter; + msg_newer2.alt_reset_counter = msg_older1.alt_reset_counter; + msg_newer2.terrain_reset_counter = msg_older1.terrain_reset_counter; + + // Moved from Local to Global: + msg_newer1.dist_bottom = msg_older2.dist_bottom; + + // Global position + msg_newer1.timestamp = msg_older1.timestamp; + msg_newer1.timestamp_sample = msg_older1.timestamp_sample; + msg_newer1.lat = msg_older1.lat; + msg_newer1.lon = msg_older1.lon; + msg_newer1.alt = msg_older1.alt; + msg_newer1.alt_ellipsoid = msg_older1.alt_ellipsoid; + msg_newer1.eph = msg_older1.eph; + msg_newer1.epv = msg_older1.epv; + msg_newer1.terrain_alt = msg_older1.terrain_alt; + msg_newer1.terrain_alt_valid = msg_older1.terrain_alt_valid; + msg_newer1.dead_reckoning = msg_older1.dead_reckoning; + + // Local position + msg_newer2.timestamp = msg_older2.timestamp; + msg_newer2.timestamp_sample = msg_older2.timestamp_sample; + msg_newer2.xy_valid = msg_older2.xy_valid; + msg_newer2.z_valid = msg_older2.z_valid; + msg_newer2.v_xy_valid = msg_older2.v_xy_valid; + msg_newer2.v_z_valid = msg_older2.v_z_valid; + msg_newer2.x = msg_older2.x; + msg_newer2.y = msg_older2.y; + msg_newer2.z = msg_older2.z; + msg_newer2.delta_xy = msg_older2.delta_vxy; + msg_newer2.xy_reset_counter = msg_older2.xy_reset_counter; + msg_newer2.delta_z = msg_older2.delta_z; + msg_newer2.z_reset_counter = msg_older2.z_reset_counter; + msg_newer2.vx = msg_older2.vx; + msg_newer2.vy = msg_older2.vy; + msg_newer2.vz = msg_older2.vz; + msg_newer2.z_deriv = msg_older2.z_deriv; + msg_newer2.delta_vxy = msg_older2.delta_vxy; + msg_newer2.vxy_reset_counter = msg_older2.vxy_reset_counter; + msg_newer2.delta_vz = msg_older2.delta_vz; + msg_newer2.vz_reset_counter = msg_older2.vz_reset_counter; + msg_newer2.ax = msg_older2.ax; + msg_newer2.ay = msg_older2.ay; + msg_newer2.az = msg_older2.az; + msg_newer2.heading = msg_older2.heading; + msg_newer2.heading_var = msg_older2.heading_var; + msg_newer2.unaided_heading = msg_older2.unaided_heading; + msg_newer2.delta_heading = msg_older2.delta_heading; + msg_newer2.heading_reset_counter = msg_older2.heading_reset_counter; + msg_newer2.heading_good_for_control = msg_older2.heading_good_for_control; + msg_newer2.tilt_var = msg_older2.tilt_var; + msg_newer2.xy_global = msg_older2.xy_global; + msg_newer2.z_global = msg_older2.z_global; + msg_newer2.ref_timestamp = msg_older2.ref_timestamp; + msg_newer2.ref_lat = msg_older2.ref_lat; + msg_newer2.ref_lon = msg_older2.ref_lon; + msg_newer2.ref_alt = msg_older2.ref_alt; + msg_newer2.dist_bottom_valid = msg_older2.dist_bottom_valid; + msg_newer2.dist_bottom_var = msg_older2.dist_bottom_var; + msg_newer2.delta_dist_bottom = msg_older2.delta_dist_bottom; + msg_newer2.dist_bottom_reset_counter = msg_older2.dist_bottom_reset_counter; + msg_newer2.eph = msg_older2.eph; + msg_newer2.epv = msg_older2.epv; + msg_newer2.evh = msg_older2.evh; + msg_newer2.evv = msg_older2.evv; + msg_newer2.dead_reckoning = msg_older2.dead_reckoning; + msg_newer2.vxy_max = msg_older2.vxy_max; + msg_newer2.vz_max = msg_older2.vz_max; + msg_newer2.hagl_min = msg_older2.hagl_min; + msg_newer2.hagl_max = msg_older2.hagl_max; + } + + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) { + // Moved from Global to Local: + msg_older1.delta_alt = msg_newer2.delta_alt; + msg_older1.delta_terrain = msg_newer2.delta_terrain; + msg_older1.lat_lon_reset_counter = msg_newer2.lat_lon_reset_counter; + msg_older1.alt_reset_counter = msg_newer2.alt_reset_counter; + msg_older1.terrain_reset_counter = msg_newer2.terrain_reset_counter; + + // Moved from Local to Global: + msg_older2.dist_bottom = msg_newer1.dist_bottom; + + // Global position + msg_older1.timestamp = msg_newer1.timestamp; + msg_older1.timestamp_sample = msg_newer1.timestamp_sample; + msg_older1.lat = msg_newer1.lat; + msg_older1.lon = msg_newer1.lon; + msg_older1.alt = msg_newer1.alt; + msg_older1.alt_ellipsoid = msg_newer1.alt_ellipsoid; + msg_older1.eph = msg_newer1.eph; + msg_older1.epv = msg_newer1.epv; + msg_older1.terrain_alt = msg_newer1.terrain_alt; + msg_older1.terrain_alt_valid = msg_newer1.terrain_alt_valid; + msg_older1.dead_reckoning = msg_newer1.dead_reckoning; + + // Local position + msg_older2.timestamp = msg_newer2.timestamp; + msg_older2.timestamp_sample = msg_newer2.timestamp_sample; + msg_older2.xy_valid = msg_newer2.xy_valid; + msg_older2.z_valid = msg_newer2.z_valid; + msg_older2.v_xy_valid = msg_newer2.v_xy_valid; + msg_older2.v_z_valid = msg_newer2.v_z_valid; + msg_older2.x = msg_newer2.x; + msg_older2.y = msg_newer2.y; + msg_older2.z = msg_newer2.z; + msg_older2.delta_xy = msg_newer2.delta_vxy; + msg_older2.xy_reset_counter = msg_newer2.xy_reset_counter; + msg_older2.delta_z = msg_newer2.delta_z; + msg_older2.z_reset_counter = msg_newer2.z_reset_counter; + msg_older2.vx = msg_newer2.vx; + msg_older2.vy = msg_newer2.vy; + msg_older2.vz = msg_newer2.vz; + msg_older2.z_deriv = msg_newer2.z_deriv; + msg_older2.delta_vxy = msg_newer2.delta_vxy; + msg_older2.vxy_reset_counter = msg_newer2.vxy_reset_counter; + msg_older2.delta_vz = msg_newer2.delta_vz; + msg_older2.vz_reset_counter = msg_newer2.vz_reset_counter; + msg_older2.ax = msg_newer2.ax; + msg_older2.ay = msg_newer2.ay; + msg_older2.az = msg_newer2.az; + msg_older2.heading = msg_newer2.heading; + msg_older2.heading_var = msg_newer2.heading_var; + msg_older2.unaided_heading = msg_newer2.unaided_heading; + msg_older2.delta_heading = msg_newer2.delta_heading; + msg_older2.heading_reset_counter = msg_newer2.heading_reset_counter; + msg_older2.heading_good_for_control = msg_newer2.heading_good_for_control; + msg_older2.tilt_var = msg_newer2.tilt_var; + msg_older2.xy_global = msg_newer2.xy_global; + msg_older2.z_global = msg_newer2.z_global; + msg_older2.ref_timestamp = msg_newer2.ref_timestamp; + msg_older2.ref_lat = msg_newer2.ref_lat; + msg_older2.ref_lon = msg_newer2.ref_lon; + msg_older2.ref_alt = msg_newer2.ref_alt; + msg_older2.dist_bottom_valid = msg_newer2.dist_bottom_valid; + msg_older2.dist_bottom_var = msg_newer2.dist_bottom_var; + msg_older2.delta_dist_bottom = msg_newer2.delta_dist_bottom; + msg_older2.dist_bottom_reset_counter = msg_newer2.dist_bottom_reset_counter; + msg_older2.eph = msg_newer2.eph; + msg_older2.epv = msg_newer2.epv; + msg_older2.evh = msg_newer2.evh; + msg_older2.evv = msg_newer2.evv; + msg_older2.dead_reckoning = msg_newer2.dead_reckoning; + msg_older2.vxy_max = msg_newer2.vxy_max; + msg_older2.vz_max = msg_newer2.vz_max; + msg_older2.hagl_min = msg_newer2.hagl_min; + msg_older2.hagl_max = msg_newer2.hagl_max; + } +}; + +REGISTER_TOPIC_TRANSLATION(VehicleLocalGlobalPositionV2Translation); \ No newline at end of file