Improve intro, I hope

- Adds MSPv1 rx parsing to fetch VTX config
 - Allows to inspect and change VTX channel through CLI
 - Forward MSP_RC for stick commands osd
 - Forward MSP_STATUS for arming status for PIT and LP mode

boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld

@@ -132,6 +132,7 @@ ENTRY(_stext)
  */
 EXTERN(abort)
 EXTERN(_bootdelay_signature)
+EXTERN(board_get_manifest)
 
 SECTIONS
 {

boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt

@@ -48,6 +48,7 @@ else()
 		i2c.cpp
 		init.c
 		led.c
+		mtd.cpp
 		spi.cpp
 		timer_config.cpp
 		usb.c

boards/3dr/ctrl-zero-h7-oem-revg/src/mtd.cpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <nuttx/spi/spi.h>
+#include <px4_platform_common/px4_manifest.h>
+//                                                              KiB BS    nB
+static const px4_mft_device_t spi2 = {             // FM25V01A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
+	.bus_type = px4_mft_device_t::SPI,
+	.devid    = SPIDEV_FLASH(0)
+};
+
+static const px4_mtd_entry_t fmum_fram = {
+	.device = &spi2,
+	.npart = 1,
+	.partd = {
+		{
+			.type = MTD_PARAMETERS,
+			.path = "/fs/mtd_params",
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_PAGE_SHIFT))
+		},
+	},
+};
+
+static const px4_mtd_manifest_t board_mtd_config = {
+	.nconfigs = 1,
+	.entries  = {
+		&fmum_fram
+	}
+};
+
+static const px4_mft_entry_s mtd_mft = {
+	.type = MTD,
+	.pmft = (void *) &board_mtd_config,
+};
+
+static const px4_mft_s mft = {
+	.nmft = 1,
+	.mfts = {
+		&mtd_mft
+	}
+};
+
+const px4_mft_s *board_get_manifest(void)
+{
+	return &mft;
+}

docs/en/SUMMARY.md

@@ -730,6 +730,7 @@
       - [Protocols/Microservices](mavlink/protocols.md)
         - [Standard Modes Protocol](mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](middleware/uxrce_dds.md)
+      - [UORB Bridged to ROS 2](middleware/dds_topics.md)
   - [Modules & Commands](modules/modules_main.md)
     - [Autotune](modules/modules_autotune.md)
     - [Commands](modules/modules_command.md)

docs/en/_sidebar.md

@@ -490,12 +490,17 @@
       - [Versioned](/msg_docs/versioned_messages.md)
         - [ActuatorMotors](/msg_docs/ActuatorMotors.md)
         - [ActuatorServos](/msg_docs/ActuatorServos.md)
+        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [ArmingCheckReply](/msg_docs/ArmingCheckReply.md)
         - [ArmingCheckRequest](/msg_docs/ArmingCheckRequest.md)
         - [BatteryStatus](/msg_docs/BatteryStatus.md)
         - [ConfigOverrides](/msg_docs/ConfigOverrides.md)
+        - [FixedWingLateralSetpoint](/msg_docs/FixedWingLateralSetpoint.md)
+        - [FixedWingLongitudinalSetpoint](/msg_docs/FixedWingLongitudinalSetpoint.md)
         - [GotoSetpoint](/msg_docs/GotoSetpoint.md)
         - [HomePosition](/msg_docs/HomePosition.md)
+        - [LateralControlConfiguration](/msg_docs/LateralControlConfiguration.md)
+        - [LongitudinalControlConfiguration](/msg_docs/LongitudinalControlConfiguration.md)
         - [ManualControlSetpoint](/msg_docs/ManualControlSetpoint.md)
         - [ModeCompleted](/msg_docs/ModeCompleted.md)
         - [RegisterExtComponentReply](/msg_docs/RegisterExtComponentReply.md)
@@ -524,10 +529,8 @@
         - [ActuatorTest](/msg_docs/ActuatorTest.md)
         - [AdcReport](/msg_docs/AdcReport.md)
         - [Airspeed](/msg_docs/Airspeed.md)
-        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [AirspeedWind](/msg_docs/AirspeedWind.md)
         - [AutotuneAttitudeControlStatus](/msg_docs/AutotuneAttitudeControlStatus.md)
-        - [Buffer128](/msg_docs/Buffer128.md)
         - [ButtonEvent](/msg_docs/ButtonEvent.md)
         - [CameraCapture](/msg_docs/CameraCapture.md)
         - [CameraStatus](/msg_docs/CameraStatus.md)
@@ -566,6 +569,9 @@
         - [FailsafeFlags](/msg_docs/FailsafeFlags.md)
         - [FailureDetectorStatus](/msg_docs/FailureDetectorStatus.md)
         - [FigureEightStatus](/msg_docs/FigureEightStatus.md)
+        - [FixedWingLateralGuidanceStatus](/msg_docs/FixedWingLateralGuidanceStatus.md)
+        - [FixedWingLateralStatus](/msg_docs/FixedWingLateralStatus.md)
+        - [FixedWingRunwayControl](/msg_docs/FixedWingRunwayControl.md)
         - [FlightPhaseEstimation](/msg_docs/FlightPhaseEstimation.md)
         - [FollowTarget](/msg_docs/FollowTarget.md)
         - [FollowTargetEstimator](/msg_docs/FollowTargetEstimator.md)
@@ -618,7 +624,6 @@
         - [NavigatorMissionItem](/msg_docs/NavigatorMissionItem.md)
         - [NavigatorStatus](/msg_docs/NavigatorStatus.md)
         - [NormalizedUnsignedSetpoint](/msg_docs/NormalizedUnsignedSetpoint.md)
-        - [NpfgStatus](/msg_docs/NpfgStatus.md)
         - [ObstacleDistance](/msg_docs/ObstacleDistance.md)
         - [OffboardControlMode](/msg_docs/OffboardControlMode.md)
         - [OnboardComputerStatus](/msg_docs/OnboardComputerStatus.md)
@@ -654,13 +659,12 @@
         - [RcParameterMap](/msg_docs/RcParameterMap.md)
         - [RoverAttitudeSetpoint](/msg_docs/RoverAttitudeSetpoint.md)
         - [RoverAttitudeStatus](/msg_docs/RoverAttitudeStatus.md)
-        - [RoverMecanumGuidanceStatus](/msg_docs/RoverMecanumGuidanceStatus.md)
-        - [RoverMecanumSetpoint](/msg_docs/RoverMecanumSetpoint.md)
-        - [RoverMecanumStatus](/msg_docs/RoverMecanumStatus.md)
+        - [RoverPositionSetpoint](/msg_docs/RoverPositionSetpoint.md)
         - [RoverRateSetpoint](/msg_docs/RoverRateSetpoint.md)
         - [RoverRateStatus](/msg_docs/RoverRateStatus.md)
         - [RoverSteeringSetpoint](/msg_docs/RoverSteeringSetpoint.md)
         - [RoverThrottleSetpoint](/msg_docs/RoverThrottleSetpoint.md)
+        - [RoverVelocitySetpoint](/msg_docs/RoverVelocitySetpoint.md)
         - [RoverVelocityStatus](/msg_docs/RoverVelocityStatus.md)
         - [Rpm](/msg_docs/Rpm.md)
         - [RtlStatus](/msg_docs/RtlStatus.md)
@@ -692,6 +696,7 @@
         - [TelemetryStatus](/msg_docs/TelemetryStatus.md)
         - [TiltrotorExtraControls](/msg_docs/TiltrotorExtraControls.md)
         - [TimesyncStatus](/msg_docs/TimesyncStatus.md)
+        - [TrajectorySetpoint6dof](/msg_docs/TrajectorySetpoint6dof.md)
         - [TransponderReport](/msg_docs/TransponderReport.md)
         - [TuneControl](/msg_docs/TuneControl.md)
         - [UavcanParameterRequest](/msg_docs/UavcanParameterRequest.md)
@@ -715,14 +720,18 @@
         - [WheelEncoders](/msg_docs/WheelEncoders.md)
         - [Wind](/msg_docs/Wind.md)
         - [YawEstimatorStatus](/msg_docs/YawEstimatorStatus.md)
+        - [AirspeedValidatedV0](/msg_docs/AirspeedValidatedV0.md)
+        - [VehicleAttitudeSetpointV0](/msg_docs/VehicleAttitudeSetpointV0.md)
         - [VehicleStatusV0](/msg_docs/VehicleStatusV0.md)
-    - [MAVLink Messaging](/middleware/mavlink.md)
+    - [MAVLink Messaging](/mavlink/index.md)
       - [Adding Messages](/mavlink/adding_messages.md)
       - [Streaming Messages](/mavlink/streaming_messages.md)
       - [Receiving Messages](/mavlink/receiving_messages.md)
       - [Custom MAVLink Messages](/mavlink/custom_messages.md)
-      - [Standard Modes Protocol](/mavlink/standard_modes.md)
+      - [Protocols/Microservices](/mavlink/protocols.md)
+        - [Standard Modes Protocol](/mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](/middleware/uxrce_dds.md)
+      - [UORB Bridged to ROS 2](/middleware/dds_topics.md)
   - [Modules & Commands](/modules/modules_main.md)
     - [Autotune](/modules/modules_autotune.md)
     - [Commands](/modules/modules_command.md)
@@ -848,6 +857,7 @@
   - [Licenses](/contribute/licenses.md)
 - [Releases](/releases/index.md)
   - [main (alpha)](/releases/main.md)
+  - [1.16 (release candidate)](/releases/1.16.md)
   - [1.15 (stable)](/releases/1.15.md)
   - [1.14](/releases/1.14.md)
   - [1.13](/releases/1.13.md)

docs/en/computer_vision/collision_prevention.md

@@ -212,7 +212,7 @@ The steps are:
      type: px4_msgs::msg::ObstacleDistance
    ```
 
-   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in _uXRCE-DDS (PX4-ROS 2/DDS Bridge)_.
+   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
 
 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
    In the **Script Editor** tab, add following scripts in the appropriate sections:

docs/en/debug/plotting_realtime_uorb_data.md

@@ -94,13 +94,13 @@ Exporting the messages allows ROS 2 and the uXRCE-DDS agent to be independent of
 
 While `px4_msgs` has messages for all uORB topics in PX4, not all messages in `px4_msgs` are available to ROS 2/PlotJuggler by default.
 The set that are available must be built into the client running on PX4.
-These are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+These are defined in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The instructions below explain the changes needed to monitor topics that are not available by default.
 
 ### Missing Topics
 
-If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) to include the topic and rebuild PX4.
+If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](../middleware/dds_topics.md) ([source](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) to include the topic and rebuild PX4.
 
 If working with real hardware you will need to build and [install](../config/firmware.md#installing-px4-main-beta-or-custom-firmware) custom firmware after changing the YAML file.
 
@@ -120,7 +120,7 @@ cd ~/ros2_ws/ && colcon build
 
 ### Custom Topics
 
-After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and export the new message into your ROS 2 workspace.
+After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](../middleware/dds_topics.md), and export the new message into your ROS 2 workspace.
 
 ## See also
 

docs/en/debug/system_wide_replay.md

@@ -126,8 +126,7 @@ In EKF2 mode, the replay will automatically create the ORB publisher rules descr
 
 To perform an EKF2 replay:
 
-- Record the original log.
-  Optionally set `SDLOG_MODE` to `1` to log from boot.
+- Record the original log with `SDLOG_MODE` set to `1` to log from boot.
 
 - In addition to the `replay` environment variable, set `replay_mode` to `ekf2`:
 

docs/en/middleware/dds_topics.md

@@ -0,0 +1,276 @@
+# dds_topics.yaml — PX4 Topics Exposed to ROS 2
+
+::: info
+This document is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code.
+:::
+
+
+The [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) file specifies which uORB message definitions are compiled into the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) module when [PX4 is built](../middleware/uxrce_dds.md#code-generation), and hence which topics are available for ROS 2 applications to subscribe or publish (by default).
+
+This document shows a markdown-rendered version of [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), listing the publications, subscriptions, and so on.
+
+## Publications
+
+Topic | Type| Rate Limit
+--- | --- | ---
+`/fmu/out/register_ext_component_reply` | [px4_msgs::msg::RegisterExtComponentReply](../msg_docs/RegisterExtComponentReply.md) | 
+`/fmu/out/arming_check_request` | [px4_msgs::msg::ArmingCheckRequest](../msg_docs/ArmingCheckRequest.md) | 5.0
+`/fmu/out/mode_completed` | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md) | 50.0
+`/fmu/out/battery_status` | [px4_msgs::msg::BatteryStatus](../msg_docs/BatteryStatus.md) | 1.0
+`/fmu/out/collision_constraints` | [px4_msgs::msg::CollisionConstraints](../msg_docs/CollisionConstraints.md) | 50.0
+`/fmu/out/estimator_status_flags` | [px4_msgs::msg::EstimatorStatusFlags](../msg_docs/EstimatorStatusFlags.md) | 5.0
+`/fmu/out/failsafe_flags` | [px4_msgs::msg::FailsafeFlags](../msg_docs/FailsafeFlags.md) | 5.0
+`/fmu/out/manual_control_setpoint` | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md) | 25.0
+`/fmu/out/message_format_response` | [px4_msgs::msg::MessageFormatResponse](../msg_docs/MessageFormatResponse.md) | 
+`/fmu/out/position_setpoint_triplet` | [px4_msgs::msg::PositionSetpointTriplet](../msg_docs/PositionSetpointTriplet.md) | 5.0
+`/fmu/out/sensor_combined` | [px4_msgs::msg::SensorCombined](../msg_docs/SensorCombined.md) | 
+`/fmu/out/timesync_status` | [px4_msgs::msg::TimesyncStatus](../msg_docs/TimesyncStatus.md) | 10.0
+`/fmu/out/vehicle_land_detected` | [px4_msgs::msg::VehicleLandDetected](../msg_docs/VehicleLandDetected.md) | 5.0
+`/fmu/out/vehicle_attitude` | [px4_msgs::msg::VehicleAttitude](../msg_docs/VehicleAttitude.md) | 
+`/fmu/out/vehicle_control_mode` | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md) | 50.0
+`/fmu/out/vehicle_command_ack` | [px4_msgs::msg::VehicleCommandAck](../msg_docs/VehicleCommandAck.md) | 
+`/fmu/out/vehicle_global_position` | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md) | 50.0
+`/fmu/out/vehicle_gps_position` | [px4_msgs::msg::SensorGps](../msg_docs/SensorGps.md) | 50.0
+`/fmu/out/vehicle_local_position` | [px4_msgs::msg::VehicleLocalPosition](../msg_docs/VehicleLocalPosition.md) | 50.0
+`/fmu/out/vehicle_odometry` | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md) | 
+`/fmu/out/vehicle_status` | [px4_msgs::msg::VehicleStatus](../msg_docs/VehicleStatus.md) | 5.0
+`/fmu/out/airspeed_validated` | [px4_msgs::msg::AirspeedValidated](../msg_docs/AirspeedValidated.md) | 50.0
+`/fmu/out/vtol_vehicle_status` | [px4_msgs::msg::VtolVehicleStatus](../msg_docs/VtolVehicleStatus.md) | 
+`/fmu/out/home_position` | [px4_msgs::msg::HomePosition](../msg_docs/HomePosition.md) | 5.0
+
+## Subscriptions
+
+Topic | Type
+--- | ---
+/fmu/in/register_ext_component_request | [px4_msgs::msg::RegisterExtComponentRequest](../msg_docs/RegisterExtComponentRequest.md)
+/fmu/in/unregister_ext_component | [px4_msgs::msg::UnregisterExtComponent](../msg_docs/UnregisterExtComponent.md)
+/fmu/in/config_overrides_request | [px4_msgs::msg::ConfigOverrides](../msg_docs/ConfigOverrides.md)
+/fmu/in/arming_check_reply | [px4_msgs::msg::ArmingCheckReply](../msg_docs/ArmingCheckReply.md)
+/fmu/in/message_format_request | [px4_msgs::msg::MessageFormatRequest](../msg_docs/MessageFormatRequest.md)
+/fmu/in/mode_completed | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md)
+/fmu/in/config_control_setpoints | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md)
+/fmu/in/distance_sensor | [px4_msgs::msg::DistanceSensor](../msg_docs/DistanceSensor.md)
+/fmu/in/manual_control_input | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md)
+/fmu/in/offboard_control_mode | [px4_msgs::msg::OffboardControlMode](../msg_docs/OffboardControlMode.md)
+/fmu/in/onboard_computer_status | [px4_msgs::msg::OnboardComputerStatus](../msg_docs/OnboardComputerStatus.md)
+/fmu/in/obstacle_distance | [px4_msgs::msg::ObstacleDistance](../msg_docs/ObstacleDistance.md)
+/fmu/in/sensor_optical_flow | [px4_msgs::msg::SensorOpticalFlow](../msg_docs/SensorOpticalFlow.md)
+/fmu/in/goto_setpoint | [px4_msgs::msg::GotoSetpoint](../msg_docs/GotoSetpoint.md)
+/fmu/in/telemetry_status | [px4_msgs::msg::TelemetryStatus](../msg_docs/TelemetryStatus.md)
+/fmu/in/trajectory_setpoint | [px4_msgs::msg::TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md)
+/fmu/in/vehicle_attitude_setpoint | [px4_msgs::msg::VehicleAttitudeSetpoint](../msg_docs/VehicleAttitudeSetpoint.md)
+/fmu/in/vehicle_mocap_odometry | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)
+/fmu/in/vehicle_rates_setpoint | [px4_msgs::msg::VehicleRatesSetpoint](../msg_docs/VehicleRatesSetpoint.md)
+/fmu/in/vehicle_visual_odometry | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)
+/fmu/in/vehicle_command | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)
+/fmu/in/vehicle_command_mode_executor | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)
+/fmu/in/vehicle_thrust_setpoint | [px4_msgs::msg::VehicleThrustSetpoint](../msg_docs/VehicleThrustSetpoint.md)
+/fmu/in/vehicle_torque_setpoint | [px4_msgs::msg::VehicleTorqueSetpoint](../msg_docs/VehicleTorqueSetpoint.md)
+/fmu/in/actuator_motors | [px4_msgs::msg::ActuatorMotors](../msg_docs/ActuatorMotors.md)
+/fmu/in/actuator_servos | [px4_msgs::msg::ActuatorServos](../msg_docs/ActuatorServos.md)
+/fmu/in/aux_global_position | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md)
+/fmu/in/fixed_wing_longitudinal_setpoint | [px4_msgs::msg::FixedWingLongitudinalSetpoint](../msg_docs/FixedWingLongitudinalSetpoint.md)
+/fmu/in/fixed_wing_lateral_setpoint | [px4_msgs::msg::FixedWingLateralSetpoint](../msg_docs/FixedWingLateralSetpoint.md)
+/fmu/in/longitudinal_control_configuration | [px4_msgs::msg::LongitudinalControlConfiguration](../msg_docs/LongitudinalControlConfiguration.md)
+/fmu/in/lateral_control_configuration | [px4_msgs::msg::LateralControlConfiguration](../msg_docs/LateralControlConfiguration.md)
+
+## Subscriptions Multi
+
+None
+
+## Not Exported
+
+These messages are not listed in the yaml file.
+They are not build into the module, and hence are neither published or subscribed.
+
+::: details See messages
+
+- [SensorCorrection](../msg_docs/SensorCorrection.md)
+- [ActuatorOutputs](../msg_docs/ActuatorOutputs.md)
+- [FixedWingRunwayControl](../msg_docs/FixedWingRunwayControl.md)
+- [EstimatorInnovations](../msg_docs/EstimatorInnovations.md)
+- [FlightPhaseEstimation](../msg_docs/FlightPhaseEstimation.md)
+- [PurePursuitStatus](../msg_docs/PurePursuitStatus.md)
+- [Px4ioStatus](../msg_docs/Px4ioStatus.md)
+- [SatelliteInfo](../msg_docs/SatelliteInfo.md)
+- [GeofenceResult](../msg_docs/GeofenceResult.md)
+- [GimbalManagerStatus](../msg_docs/GimbalManagerStatus.md)
+- [ManualControlSwitches](../msg_docs/ManualControlSwitches.md)
+- [OpenDroneIdSelfId](../msg_docs/OpenDroneIdSelfId.md)
+- [OpenDroneIdSystem](../msg_docs/OpenDroneIdSystem.md)
+- [EventV0](../msg_docs/EventV0.md)
+- [QshellRetval](../msg_docs/QshellRetval.md)
+- [RoverThrottleSetpoint](../msg_docs/RoverThrottleSetpoint.md)
+- [AirspeedValidatedV0](../msg_docs/AirspeedValidatedV0.md)
+- [RcChannels](../msg_docs/RcChannels.md)
+- [SensorAccel](../msg_docs/SensorAccel.md)
+- [GimbalDeviceAttitudeStatus](../msg_docs/GimbalDeviceAttitudeStatus.md)
+- [EscStatus](../msg_docs/EscStatus.md)
+- [RoverAttitudeSetpoint](../msg_docs/RoverAttitudeSetpoint.md)
+- [RateCtrlStatus](../msg_docs/RateCtrlStatus.md)
+- [AirspeedWind](../msg_docs/AirspeedWind.md)
+- [InputRc](../msg_docs/InputRc.md)
+- [GpioIn](../msg_docs/GpioIn.md)
+- [LaunchDetectionStatus](../msg_docs/LaunchDetectionStatus.md)
+- [VehicleImu](../msg_docs/VehicleImu.md)
+- [Event](../msg_docs/Event.md)
+- [SensorUwb](../msg_docs/SensorUwb.md)
+- [ActuatorServosTrim](../msg_docs/ActuatorServosTrim.md)
+- [DatamanResponse](../msg_docs/DatamanResponse.md)
+- [OrbTest](../msg_docs/OrbTest.md)
+- [VehicleLocalPositionSetpoint](../msg_docs/VehicleLocalPositionSetpoint.md)
+- [VehicleAngularVelocity](../msg_docs/VehicleAngularVelocity.md)
+- [FollowTargetStatus](../msg_docs/FollowTargetStatus.md)
+- [NormalizedUnsignedSetpoint](../msg_docs/NormalizedUnsignedSetpoint.md)
+- [YawEstimatorStatus](../msg_docs/YawEstimatorStatus.md)
+- [TakeoffStatus](../msg_docs/TakeoffStatus.md)
+- [UlogStreamAck](../msg_docs/UlogStreamAck.md)
+- [OrbTestLarge](../msg_docs/OrbTestLarge.md)
+- [RoverSteeringSetpoint](../msg_docs/RoverSteeringSetpoint.md)
+- [CameraCapture](../msg_docs/CameraCapture.md)
+- [VehicleRoi](../msg_docs/VehicleRoi.md)
+- [ActuatorArmed](../msg_docs/ActuatorArmed.md)
+- [FixedWingLateralGuidanceStatus](../msg_docs/FixedWingLateralGuidanceStatus.md)
+- [ParameterSetValueResponse](../msg_docs/ParameterSetValueResponse.md)
+- [GeofenceStatus](../msg_docs/GeofenceStatus.md)
+- [VehicleAngularAccelerationSetpoint](../msg_docs/VehicleAngularAccelerationSetpoint.md)
+- [SensorGnssRelative](../msg_docs/SensorGnssRelative.md)
+- [PowerMonitor](../msg_docs/PowerMonitor.md)
+- [RoverVelocityStatus](../msg_docs/RoverVelocityStatus.md)
+- [ParameterResetRequest](../msg_docs/ParameterResetRequest.md)
+- [RoverAttitudeStatus](../msg_docs/RoverAttitudeStatus.md)
+- [TecsStatus](../msg_docs/TecsStatus.md)
+- [EstimatorSelectorStatus](../msg_docs/EstimatorSelectorStatus.md)
+- [CanInterfaceStatus](../msg_docs/CanInterfaceStatus.md)
+- [Ping](../msg_docs/Ping.md)
+- [LedControl](../msg_docs/LedControl.md)
+- [Wind](../msg_docs/Wind.md)
+- [VehicleStatusV0](../msg_docs/VehicleStatusV0.md)
+- [ActuatorTest](../msg_docs/ActuatorTest.md)
+- [IridiumsbdStatus](../msg_docs/IridiumsbdStatus.md)
+- [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md)
+- [GimbalManagerSetAttitude](../msg_docs/GimbalManagerSetAttitude.md)
+- [Gripper](../msg_docs/Gripper.md)
+- [SensorMag](../msg_docs/SensorMag.md)
+- [DebugValue](../msg_docs/DebugValue.md)
+- [SensorPreflightMag](../msg_docs/SensorPreflightMag.md)
+- [RcParameterMap](../msg_docs/RcParameterMap.md)
+- [LandingGear](../msg_docs/LandingGear.md)
+- [GimbalDeviceInformation](../msg_docs/GimbalDeviceInformation.md)
+- [VehicleOpticalFlow](../msg_docs/VehicleOpticalFlow.md)
+- [UlogStream](../msg_docs/UlogStream.md)
+- [GimbalControls](../msg_docs/GimbalControls.md)
+- [RoverRateSetpoint](../msg_docs/RoverRateSetpoint.md)
+- [LogMessage](../msg_docs/LogMessage.md)
+- [RoverVelocitySetpoint](../msg_docs/RoverVelocitySetpoint.md)
+- [GpioOut](../msg_docs/GpioOut.md)
+- [TaskStackInfo](../msg_docs/TaskStackInfo.md)
+- [VelocityLimits](../msg_docs/VelocityLimits.md)
+- [MagWorkerData](../msg_docs/MagWorkerData.md)
+- [ParameterUpdate](../msg_docs/ParameterUpdate.md)
+- [TrajectorySetpoint6dof](../msg_docs/TrajectorySetpoint6dof.md)
+- [SensorBaro](../msg_docs/SensorBaro.md)
+- [VehicleImuStatus](../msg_docs/VehicleImuStatus.md)
+- [InternalCombustionEngineStatus](../msg_docs/InternalCombustionEngineStatus.md)
+- [VehicleOpticalFlowVel](../msg_docs/VehicleOpticalFlowVel.md)
+- [GimbalManagerSetManualControl](../msg_docs/GimbalManagerSetManualControl.md)
+- [Rpm](../msg_docs/Rpm.md)
+- [MagnetometerBiasEstimate](../msg_docs/MagnetometerBiasEstimate.md)
+- [MountOrientation](../msg_docs/MountOrientation.md)
+- [ActionRequest](../msg_docs/ActionRequest.md)
+- [OpenDroneIdArmStatus](../msg_docs/OpenDroneIdArmStatus.md)
+- [SensorAccelFifo](../msg_docs/SensorAccelFifo.md)
+- [LoggerStatus](../msg_docs/LoggerStatus.md)
+- [GeneratorStatus](../msg_docs/GeneratorStatus.md)
+- [InternalCombustionEngineControl](../msg_docs/InternalCombustionEngineControl.md)
+- [Ekf2Timestamps](../msg_docs/Ekf2Timestamps.md)
+- [LandingTargetPose](../msg_docs/LandingTargetPose.md)
+- [PositionControllerLandingStatus](../msg_docs/PositionControllerLandingStatus.md)
+- [UavcanParameterValue](../msg_docs/UavcanParameterValue.md)
+- [OrbitStatus](../msg_docs/OrbitStatus.md)
+- [PositionControllerStatus](../msg_docs/PositionControllerStatus.md)
+- [EstimatorStatus](../msg_docs/EstimatorStatus.md)
+- [DatamanRequest](../msg_docs/DatamanRequest.md)
+- [HoverThrustEstimate](../msg_docs/HoverThrustEstimate.md)
+- [FixedWingLateralStatus](../msg_docs/FixedWingLateralStatus.md)
+- [NavigatorMissionItem](../msg_docs/NavigatorMissionItem.md)
+- [Cpuload](../msg_docs/Cpuload.md)
+- [EstimatorAidSource3d](../msg_docs/EstimatorAidSource3d.md)
+- [RoverRateStatus](../msg_docs/RoverRateStatus.md)
+- [EscReport](../msg_docs/EscReport.md)
+- [DebugArray](../msg_docs/DebugArray.md)
+- [ControlAllocatorStatus](../msg_docs/ControlAllocatorStatus.md)
+- [SensorHygrometer](../msg_docs/SensorHygrometer.md)
+- [EstimatorSensorBias](../msg_docs/EstimatorSensorBias.md)
+- [EstimatorBias3d](../msg_docs/EstimatorBias3d.md)
+- [GimbalManagerInformation](../msg_docs/GimbalManagerInformation.md)
+- [QshellReq](../msg_docs/QshellReq.md)
+- [CameraStatus](../msg_docs/CameraStatus.md)
+- [GpsInjectData](../msg_docs/GpsInjectData.md)
+- [FigureEightStatus](../msg_docs/FigureEightStatus.md)
+- [TransponderReport](../msg_docs/TransponderReport.md)
+- [UavcanParameterRequest](../msg_docs/UavcanParameterRequest.md)
+- [MavlinkLog](../msg_docs/MavlinkLog.md)
+- [EstimatorGpsStatus](../msg_docs/EstimatorGpsStatus.md)
+- [FuelTankStatus](../msg_docs/FuelTankStatus.md)
+- [Mission](../msg_docs/Mission.md)
+- [PositionSetpoint](../msg_docs/PositionSetpoint.md)
+- [MissionResult](../msg_docs/MissionResult.md)
+- [EstimatorEventFlags](../msg_docs/EstimatorEventFlags.md)
+- [VehicleMagnetometer](../msg_docs/VehicleMagnetometer.md)
+- [MavlinkTunnel](../msg_docs/MavlinkTunnel.md)
+- [DifferentialPressure](../msg_docs/DifferentialPressure.md)
+- [CellularStatus](../msg_docs/CellularStatus.md)
+- [GpsDump](../msg_docs/GpsDump.md)
+- [GimbalDeviceSetAttitude](../msg_docs/GimbalDeviceSetAttitude.md)
+- [ArmingCheckReplyV0](../msg_docs/ArmingCheckReplyV0.md)
+- [NavigatorStatus](../msg_docs/NavigatorStatus.md)
+- [RoverPositionSetpoint](../msg_docs/RoverPositionSetpoint.md)
+- [FollowTarget](../msg_docs/FollowTarget.md)
+- [SensorsStatusImu](../msg_docs/SensorsStatusImu.md)
+- [EstimatorStates](../msg_docs/EstimatorStates.md)
+- [SensorGyro](../msg_docs/SensorGyro.md)
+- [SensorAirflow](../msg_docs/SensorAirflow.md)
+- [ButtonEvent](../msg_docs/ButtonEvent.md)
+- [DebugKeyValue](../msg_docs/DebugKeyValue.md)
+- [GpioConfig](../msg_docs/GpioConfig.md)
+- [CameraTrigger](../msg_docs/CameraTrigger.md)
+- [LandingGearWheel](../msg_docs/LandingGearWheel.md)
+- [VehicleConstraints](../msg_docs/VehicleConstraints.md)
+- [HealthReport](../msg_docs/HealthReport.md)
+- [PowerButtonState](../msg_docs/PowerButtonState.md)
+- [RadioStatus](../msg_docs/RadioStatus.md)
+- [SensorGyroFifo](../msg_docs/SensorGyroFifo.md)
+- [EstimatorBias](../msg_docs/EstimatorBias.md)
+- [DebugVect](../msg_docs/DebugVect.md)
+- [DistanceSensorModeChangeRequest](../msg_docs/DistanceSensorModeChangeRequest.md)
+- [RtlTimeEstimate](../msg_docs/RtlTimeEstimate.md)
+- [PpsCapture](../msg_docs/PpsCapture.md)
+- [SensorSelection](../msg_docs/SensorSelection.md)
+- [SystemPower](../msg_docs/SystemPower.md)
+- [ActuatorControlsStatus](../msg_docs/ActuatorControlsStatus.md)
+- [SensorGyroFft](../msg_docs/SensorGyroFft.md)
+- [VehicleAirData](../msg_docs/VehicleAirData.md)
+- [FollowTargetEstimator](../msg_docs/FollowTargetEstimator.md)
+- [ParameterSetUsedRequest](../msg_docs/ParameterSetUsedRequest.md)
+- [GpioRequest](../msg_docs/GpioRequest.md)
+- [OpenDroneIdOperatorId](../msg_docs/OpenDroneIdOperatorId.md)
+- [RtlStatus](../msg_docs/RtlStatus.md)
+- [Airspeed](../msg_docs/Airspeed.md)
+- [VehicleAcceleration](../msg_docs/VehicleAcceleration.md)
+- [ParameterSetValueRequest](../msg_docs/ParameterSetValueRequest.md)
+- [IrlockReport](../msg_docs/IrlockReport.md)
+- [HeaterStatus](../msg_docs/HeaterStatus.md)
+- [AdcReport](../msg_docs/AdcReport.md)
+- [PwmInput](../msg_docs/PwmInput.md)
+- [TiltrotorExtraControls](../msg_docs/TiltrotorExtraControls.md)
+- [EstimatorAidSource1d](../msg_docs/EstimatorAidSource1d.md)
+- [OrbTestMedium](../msg_docs/OrbTestMedium.md)
+- [VehicleAttitudeSetpointV0](../msg_docs/VehicleAttitudeSetpointV0.md)
+- [EstimatorAidSource2d](../msg_docs/EstimatorAidSource2d.md)
+- [TuneControl](../msg_docs/TuneControl.md)
+- [WheelEncoders](../msg_docs/WheelEncoders.md)
+- [AutotuneAttitudeControlStatus](../msg_docs/AutotuneAttitudeControlStatus.md)
+- [LandingTargetInnovations](../msg_docs/LandingTargetInnovations.md)
+- [SensorsStatus](../msg_docs/SensorsStatus.md)
+:::

docs/en/middleware/uorb.md

@@ -1,11 +1,35 @@
 # uORB Messaging
 
+The Micro Object Request Broker (uORB) is PX4s low-latency asynchronous `publish()` / `subscribe()` messaging API for inter-thread/inter-process communication.
+It allows the different modules of the system to communicate effectively, while still being loosely coupled, and hence easily replaced.
+
 ## Introduction
 
-The uORB is an asynchronous `publish()` / `subscribe()` messaging API used for inter-thread/inter-process communication.
+PX4 modules communicate using uORB _topics_.
+A topic represents a communication channel for sending _messages_ between a publisher module and one or more subscriber modules.
+A module that is interested in receiving messages can subscribe to a topic and use it to check for, and read, new messages.
+A module that wants to send messages to a particular topic (and hence all subscribers) must _advertise_ that it is going to do so, and can then _publish_ messages when it has new data.
+
+A topic behaves like a queue to which publishers write, and from which subscribers read.
+By default he queue can only buffer a single message, which may be overwritten if the publisher writes new message data before subscribers can read it.
+The [queue size may be increased](#uorb-buffer-length-orb-queue-length) in the rare cases when subscribers really need to receive every message.
+
+A message is a discrete sample of data that can be published/subscribed via a topic.
+The message fields, the constants that can be used with those fields, and the topic(s) to which the message can be published/subscribed are defined in a uORB message definition file.
+
+By default a single topic is automatically created for each message definition file, which is created by `underscore_snake_casing` the (CamelCase) message definition file name.
+For example, topic `battery_status` is automatically created for the `BatteryStatus.msg`.
+This is generally what you want if the message is always about the same kind of data (batteries in this case) and so all the subscribers will be interested in the same messages.
+
+Sometimes the same message structure can be used to represent data from different kinds of sources, which will have different sets of interested publishers and subscribers.
+In this case the topics need to be explicitly declared.
+For example the [VehicleGlobalPosition.msg](../msg_docs/VehicleGlobalPosition.md) can be used for sending messages about global position from an estimator, GNSS, or an external source: the fields are the same, but the source and subscribers of the data may be different.
+
+uORB also provides a mechanism to publish multiple independent instances of the same topic.
+This is useful, for example, if the system has several sensors of the same type.
 
 uORB is implemented in the [`uorb` module](../modules/modules_communication.md#uorb).
-It is started automatically (with `uorb start`) early in the PX4 boot sequence, as many applications depend on it.
+The module is started automatically (with `uorb start`) early in the PX4 boot sequence, as many applications depend on it.
 Unit tests can be started with `uorb_tests`.
 
 This document explains how to add uORB message definitions and their corresponding topic(s), how to use reference a topic in code, and how to view topics as they change in PX4.
@@ -42,31 +66,31 @@ In code you refer to the topic using its id, which in this example would be: `OR
 
 ## Message Definitions
 
-The message definition should start with a descriptive _comment_ that outlines its purpose (a comment starts with the `#` symbol and goes to the end of the line).
+Every message should start with a [message description](#message-description) that outlines its purpose (a comment starts with the `#` symbol and goes to the end of the line).
 The message will then define one or more fields, which are defined with a _type_, such as `bool`, `uint8`, and `float32`, followed by a _name_.
 By convention, each field is followed by a descriptive _comment_, which is any text from the `#` symbol to the end of the line.
 
-::: warning
-All message definitions **must** include the `uint64_t timestamp` field, and this should be filled in when publishing the associated topic(s).
-This field is needed in order for the logger to be able to record UORB topics.
-:::
-
 ::: info
 All _versioned_ messages definitions must include the `uint32 MESSAGE_VERSION` field.
 For more information, refer to the [Message Versioning](#message-versioning) section.
 :::
 
+::: warning
+All message definitions **must** include the `uint64_t timestamp` field, and this should be filled in when publishing the associated topic(s).
+This field is needed in order for the logger to be able to record UORB topics.
+:::
+
 For example the [VelocityLimits](../msg_docs/VelocityLimits.md) message definition shown below has a descriptive comment, followed by a number of fields, which each have a comment.
 
 ```text
 # Velocity and yaw rate limits for a multicopter position slow mode only
 
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 # absolute speeds, NAN means use default limit
-float32 horizontal_velocity # [m/s]
-float32 vertical_velocity # [m/s]
-float32 yaw_rate # [rad/s]
+float32 horizontal_velocity # [m/s] Horizontal velocity.
+float32 vertical_velocity # [m/s] Vertical velocity.
+float32 yaw_rate # [rad/s] Yaw rate.
 ```
 
 By default this message definition will be compiled to a single topic with an id `velocity_limits`, a direct conversion from the CamelCase name to a snake_case version.
@@ -74,16 +98,202 @@ By default this message definition will be compiled to a single topic with an id
 This is the simplest form of a message.
 See the existing [`msg`](../msg_docs/index.md) files for other examples of how messages are defined.
 
-### Multi-Topic Messages
+#### Message Description
 
-Sometimes it is useful to use the same message definition for multiple topics.
-This can be specified at the end of the message using a line prefixed with `# TOPICS `, followed by space-separated topic ids.
-For example, the [ActuatorOutputs](../msg_docs/ActuatorOutputs.md) message definition is used to define the topic ids as shown:
+Every message should start with a [comment](#comments) block that describes the message (one or more of lines that all start with `#`).
+The first comment line is mandatory and provides the short description.
+This may be followed by an empty comment line and then a optional long description.
 
 ```text
-# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug
+# Short description
+#
+# Long(er) description for the message
+# that can be multiline
 ```
 
+The short description should provide a succinct explanation for the purpose of the message.
+Minimally it may just mirror the message name.
+For example, [`BatteryStatus`](../msg_docs/BatteryStatus.md) has the short description `Battery status`.
+
+The long description should provide any additional context required to understand what how the message is used.
+It might explain who are the publishers and who are the expected consumers, such as MAVLink or the logging system.
+It might also cover whether the message is only used for a particular frame type or mode.
+
+Both short and long descriptions may be multi-line.
+The long description may also include empty comment lines (the short description cannot, because the first empty comment delineates the short and long description).
+A terminating full stop can be omitted from single line comments and from the final line.
+
+The block ends at the first non-comment line, such as an empty line, field, or constant.
+Any subsequent comment lines are considered "internal comments".
+
+### Comments
+
+Comments are text provided for explanation or documentation purposes.
+Any text after a `#` character is a comment (with the exception of lines that [start with `# TOPIC`](#multi-topic-message)).
+
+PX4 uses structured comments for message, field, and constant descriptions.
+Other comments are internal.
+
+### Fields
+
+Messages define one or more fields, which are the variables that are written and read by publishers and subscribers, respectively.
+
+A typical field might look like this:
+
+```sh
+float64 lat # [deg] Latitude (WGS84).
+```
+
+Each field has a _type_ followed by a _name_, and should also have a _comment_
+The format is as shown:
+
+```sh
+<type> <name> # [metadata] <description>
+```
+
+- `type`:
+  - A primitive data type: `bool`, `char`, `uint8`, `uint32`, `uint64`, `int8`, `int16`, `int32`, `float32` and `float64`.
+  - Another UORB message name, when creating complex types using [nested messages](#nested-messages).
+- `name`
+  - Any message-unique string.
+    By convention use lower case `underline_snake_case`.
+
+The comment must all be on the same line as the field, and should consist of optional metadata and a description:
+
+- `metadata` (Optional)
+  - Information about the field units and allowed values:
+    - `[<unit>]`
+      - The unit of measurement inside square brackets (note, no `@` delineator indicates a unit).
+        For example `[m]` or `[deg]`.
+        Typical units include: `m`, `deg`, `m/s`, `rad`, `rad/s`, and so on.
+    - `[@enum <enum_name>]`
+      - The `enum_name` gives the prefix of constant values in the message that can be assigned to the field.
+        Note that in UORB "enums" are a naming convention: they are not explicitly declared.
+        Multiple enum names indicate a possible error in the field design.
+    - `[@range <lower_value>, <upper_value>]`
+      - The allowed range of the field, specified as a `lower_value` and/or an `upper_value`.
+        Either value can be omitted to indicate an unbounded upper or lower value.
+        For example `[@range 0, 3]`, `[@range 5.3, ]`, `[@range , 3]`.
+    - `[@invalid <value> <description>]`
+      - The `value` to set the field to indicate that the field doesn't contain valid data.
+        The `description` is optional, and might be used to indicate the conditions under which data is invalid.
+- `description`
+  - A concise description of the purpose of the field and allowed values, and including any important information that can't be inferred from the name!
+    Use a capital first letter, and omit the full stop if the description is a single sentence.
+    Multiple sentences may also omit the final full stop.
+
+#### Array Fields
+
+A array field defines multiple variables in an array, where all values have the same type.
+The number of elements are given using square brackets after the type.
+Array fields are otherwise defined (and documented) in the same way as other fields.
+
+For example:
+
+```sh
+int32[12] raw_data # ADC channel raw value, accept negative value.
+```
+
+#### Mandatory Fields
+
+All message definitions **must** include following fields:
+
+- `uint64_t timestamp`
+  - This should be filled in when publishing the associated topic(s).
+    It is needed in order for the logger to be able to record UORB topics.
+  - The comment should be `# [us] Time since system start`.
+
+### Constants
+
+Constants specify a mapping between a name and a value.
+
+These are mainly used to predefine useful values that you might need to use for a particular field, such as a state or flag values.
+Often these are grouped together as [enums](#enums).
+There are also a small number of [metadata constants](#metadata-constants) that are used by the build infrastructure.
+
+For example, here are a number of constants for indicating battery warnings.
+
+```sh
+uint8 WARNING_NONE = 0 # No battery low voltage warning active
+uint8 WARNING_LOW = 1 # Low voltage warning
+```
+
+Constants are specified as a field assigned with a value.
+The field part has a _type_, which must match the type of the field they are to be used with, followed by a _name_.
+They should also have a _comment_ with a description.
+By convention they are defined immediately below the field with which they can used.
+
+The format is as shown:
+
+```sh
+<type> <name> = <value> # <description>
+```
+
+- `type`:
+  - Must match the `type` of the field with which it is to be used.
+- `name`
+  - The name of the constant.
+    This must be message-unique and is by convention `ALL_UPPER_CASE_UNDERLINE_SNAKE_CASE`
+  - Constant names that can be used with a field should share the same prefix and should indicate the value's purpose.
+
+The comment must all be on the same line as the field.
+Note that this is much like the field description (but there is no metadata):
+
+- `description`
+  - A terse description of the purpose of the constant.
+    Use a capital first letter, and omit the full stop if the description is a single sentence.
+    Multiple sentences may also omit the final full stop.
+
+#### Enums
+
+Enums are groups/sets of enumerated constants that can be used as values for a particular field.
+
+UORB does not define a formal syntax for enums.
+Instead we use a prefix naming convention to indicate all the constants that are part of the same enum.
+The constants in the enum should be declared immediately after the field in which they are used, and for parsing convenience, the prefix is listed in the field using `@enum` metadata.
+
+For example, here is the definition of the `warning` field and some of the `WARNING` enum values that can be used with it:
+
+```sh
+uint8 warning # [@enum WARNING] Current battery warning
+uint8 WARNING_NONE = 0 # No battery low voltage warning active
+uint8 WARNING_LOW = 1 # Low voltage warning
+uint8 WARNING_CRITICAL = 2 # Critical voltage, return / abort immediately
+...
+```
+
+#### Metadata Constants
+
+A number of constants provide information that is used by the PX4 build system to configure how the message may be used, such as the version and length of the message.
+If relevant, these should appear near the top of the file, immediately after the [Message Description](#message-description).
+
+The allowed constants are:
+
+- `ORB_QUEUE_LENGTH` - Sets the [uORB Buffer Length](#uorb-buffer-length-orb-queue-length), which is used in rare cases where a subscriber needs all values that are set for a field, rather than just the most recent sample.
+- `MESSAGE_VERSION` - Sets the version number of a versioned message.
+  This is used as part of the infrastructure to maintain compatibility between PX4 and ROS 2 versions compiled against different message definitions.
+  For more information see [Message Versioning](#message-versioning)
+
+#### Multi-Topic Messages (`# TOPICS`) {#multi-topic-messages}
+
+By default a single topic is automatically created for each message definition file, which is created by `underscore_snake_casing` the (CamelCase) message definition file name.
+For example, topic `battery_status` is automatically created for the `BatteryStatus.msg`.
+This is generally what you want if the message is always about the same kind of data (batteries in this case) and so all the subscribers will be interested in the same messages.
+
+Sometimes it is useful to use the same message definition for multiple topics.
+In this case the topics need to be explicitly declared.
+You can do this by adding one or more lines to the end of the message prefixed with `# TOPICS`, followed by space-separated topic ids.
+
+For example, the [VehicleGlobalPosition.msg](../msg_docs/VehicleGlobalPosition.md) message definition is used to define the topic ids as shown:
+
+```text
+# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
+# TOPICS estimator_global_position
+# TOPICS aux_global_position
+```
+
+Note that multiple topics are useful in this case because the likely subscribers for the different sources of global position are likely to be different.
+
 ### Nested Messages
 
 Message definitions can be nested within other messages to create complex data structures.
@@ -92,15 +302,34 @@ To nest a message, simply include the nested message type in the parent message
 
 ```text
 # Global position setpoint triplet in WGS84 coordinates.
+#
 # This are the three next waypoints (or just the next two or one).
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] Time since system start
 
 PositionSetpoint previous
 PositionSetpoint current
 PositionSetpoint next
 ```
 
+### uORB Buffer Length (ORB_QUEUE_LENGTH)
+
+uORB messages have a single-message buffer by default, which may be overwritten if the message publication rate is too high.
+In most cases this does not matter: either we are only interested in the latest sample of a topic, such as a sensor value or a setpoint, or losing a few samples is not a particular problem.
+For relatively few cases, such as vehicle commands, it is important that we don't drop topics.
+
+In order to reduce the chance that messages will be dropped we can use named constant `ORB_QUEUE_LENGTH` to create a buffer of the specified length.
+For example, to create a four-message queue, add the following line to your message definition:
+
+```sh
+uint8 ORB_QUEUE_LENGTH = 4
+```
+
+As long as subscribers are able to read messages out of the buffer quickly enough than it isn't ever fully filled to the queue length (by publishers), they will be able to get all messages that are sent.
+Messages will still be lost they are published when the queue is filled.
+
+Note that the queue length value must be a power of 2 (so 2, 4, 8, ...).
+
 ### Message/Field Deprecation {#deprecation}
 
 As there are external tools using uORB messages from log files, such as [Flight Review](https://github.com/PX4/flight_review), certain aspects need to be considered when updating existing messages:
@@ -114,7 +343,7 @@ As there are external tools using uORB messages from log files, such as [Flight
     This is to ensure that removed fields (or messages) are not re-added in future.
   - In case of a semantic change (e.g. the unit changes from degrees to radians), the field must be renamed as well and the previous one marked as deprecated as above.
 
-## Message Versioning
+## Message Versioning (MESSAGE_VERSION) {#message-versioning}
 
 <Badge type="tip" text="PX4 v1.16" />
 

docs/en/middleware/uxrce_dds.md

@@ -38,7 +38,7 @@ The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is gen
 The agent has no dependency on client code.
 It can be built standalone or in a ROS 2 workspace, or installed as a snap package on Ubuntu.
 
-When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
+When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [/src/modules/uxrce_dds_client/dds_topics.yaml](../middleware/dds_topics.md) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
 
 PX4 main or release builds automatically export the set of uORB messages definitions in the build to an associated branch in [PX4/px4_msgs](https://github.com/PX4/px4_msgs).
 
@@ -320,13 +320,11 @@ ROS_DOMAIN_ID=3 PX4_UXRCE_DDS_PORT=9999 PX4_UXRCE_DDS_NS=drone make px4_sitl gz_
 
 ## Supported uORB Messages
 
-The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The topics are release specific (support is compiled into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) at build time).
 While most releases should support a very similar set of messages, to be certain you would need to check the yaml file for your particular release.
 
-<!-- Jublish the set we use?: https://github.com/PX4/px4_msgs/issues/22 -->
-
 Note that ROS 2/DDS needs to have the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware in order to interpret the messages.
 The message definitions are stored in the ROS 2 interface package [PX4/px4_msgs](https://github.com/PX4/px4_msgs), and they are automatically synchronized by CI on the `main` and release branches.
 Note that all the messages from PX4 source code are present in the repository, but only those listed in `dds_topics.yaml` will be available as ROS 2 topics.
@@ -343,20 +341,20 @@ Therefore,
   ```
 
   ::: info
-  Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
+  Technically, [dds_topics.yaml](../middleware/dds_topics.md) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
   For more information see [DDS Topics YAML](#dds-topics-yaml) below.
   :::
 
 ## Customizing the Namespace
 
-Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) or at runtime (which is useful for multi vehicle operations):
+Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](../middleware/dds_topics.md)) or at runtime (which is useful for multi vehicle operations):
 
 - One possibility is to use the `-n` option when starting the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) from command line.
   This technique can be used both in simulation and real vehicles.
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 ::: info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#dds-ros-2-services).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#dds-ros-2-services).
 Therefore, commands like:
 
 ```sh
@@ -413,7 +411,7 @@ Deadline, lifespan, and lease durations are also all set to "default".
 
 ## DDS Topics YAML
 
-The PX4 yaml file [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) defines the set of PX4 uORB topics that are built into firmware and published.
+The PX4 [dds_topics.yaml](../middleware/dds_topics.md) file defines the set of PX4 uORB topics that are built into firmware and published.
 More precisely, it completely defines the relationship/pairing between PX4 uORB and ROS 2 messages.
 
 The file is structured as follows:
@@ -542,7 +540,7 @@ Take a look at the [client startup section](#starting-the-client) to learn how t
 
 #### New file for setting which topics are published
 
-The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
+The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topics.yaml](../middleware/dds_topics.md) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
 
 See [Supported uORB Messages](#supported-uorb-messages) and [DDS Topics YAML](#dds-topics-yaml) sections for more information.
 

docs/en/ros2/user_guide.md

@@ -28,7 +28,7 @@ The agent acts as a proxy for the client to publish and subscribe to topics in t
 
 The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is generated at build time and included in PX4 firmware by default.
 It includes both the "generic" micro XRCE-DDS client code, and PX4-specific translation code that it uses to publish to/from uORB topics.
-The subset of uORB messages that are generated into the client are listed in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The subset of uORB messages that are generated into the client are specified in [dds_topics.yaml](../middleware/dds_topics.md).
 The generator uses the uORB message definitions in the source tree: [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to create the code for sending ROS 2 messages.
 
 ROS 2 applications need to be built in a workspace that has the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware.
@@ -394,7 +394,7 @@ To control applications, ROS 2 applications:
 - subscribe to (listen to) telemetry topics published by PX4
 - publish to topics that cause PX4 to perform some action.
 
-The topics that you can use are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
+The topics that you can use are defined in [dds_topics.yaml](../middleware/dds_topics.md), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
 For example, [VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md) can be used to get the vehicle global position, while [VehicleCommand](../msg_docs/VehicleCommand.md) can be used to command actions such as takeoff and land.
 
 The [ROS 2 Example applications](#ros-2-example-applications) examples below provide concrete examples of how to use these topics.
@@ -728,7 +728,7 @@ Custom topic and service namespaces can be applied at build time (changing [dds_
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 ::: info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#px4-ros-2-service-servers).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#px4-ros-2-service-servers).
 Therefore, commands like:
 
 ```sh

docs/ko/_sidebar.md

@@ -460,6 +460,7 @@
       - [Vehicles](/sim_gazebo_gz/vehicles.md)
         - [Advanced Lift Drag Tool](/sim_gazebo_gz/tools_avl_automation.md)
       - [Worlds](/sim_gazebo_gz/worlds.md)
+      - [Plugins](/sim_gazebo_gz/plugins.md)
       - [Gazebo Models Repository](/sim_gazebo_gz/gazebo_models.md)
       - [Multi-Vehicle Sim](/sim_gazebo_gz/multi_vehicle_simulation.md)
     - [Gazebo Classic Simulation](/sim_gazebo_classic/index.md)
@@ -492,12 +493,17 @@
       - [Versioned](/msg_docs/versioned_messages.md)
         - [ActuatorMotors](/msg_docs/ActuatorMotors.md)
         - [ActuatorServos](/msg_docs/ActuatorServos.md)
+        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [ArmingCheckReply](/msg_docs/ArmingCheckReply.md)
         - [ArmingCheckRequest](/msg_docs/ArmingCheckRequest.md)
         - [BatteryStatus](/msg_docs/BatteryStatus.md)
         - [ConfigOverrides](/msg_docs/ConfigOverrides.md)
+        - [FixedWingLateralSetpoint](/msg_docs/FixedWingLateralSetpoint.md)
+        - [FixedWingLongitudinalSetpoint](/msg_docs/FixedWingLongitudinalSetpoint.md)
         - [GotoSetpoint](/msg_docs/GotoSetpoint.md)
         - [HomePosition](/msg_docs/HomePosition.md)
+        - [LateralControlConfiguration](/msg_docs/LateralControlConfiguration.md)
+        - [LongitudinalControlConfiguration](/msg_docs/LongitudinalControlConfiguration.md)
         - [ManualControlSetpoint](/msg_docs/ManualControlSetpoint.md)
         - [ModeCompleted](/msg_docs/ModeCompleted.md)
         - [RegisterExtComponentReply](/msg_docs/RegisterExtComponentReply.md)
@@ -526,10 +532,8 @@
         - [ActuatorTest](/msg_docs/ActuatorTest.md)
         - [AdcReport](/msg_docs/AdcReport.md)
         - [Airspeed](/msg_docs/Airspeed.md)
-        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [AirspeedWind](/msg_docs/AirspeedWind.md)
         - [AutotuneAttitudeControlStatus](/msg_docs/AutotuneAttitudeControlStatus.md)
-        - [Buffer128](/msg_docs/Buffer128.md)
         - [ButtonEvent](/msg_docs/ButtonEvent.md)
         - [CameraCapture](/msg_docs/CameraCapture.md)
         - [CameraStatus](/msg_docs/CameraStatus.md)
@@ -568,6 +572,9 @@
         - [FailsafeFlags](/msg_docs/FailsafeFlags.md)
         - [FailureDetectorStatus](/msg_docs/FailureDetectorStatus.md)
         - [FigureEightStatus](/msg_docs/FigureEightStatus.md)
+        - [FixedWingLateralGuidanceStatus](/msg_docs/FixedWingLateralGuidanceStatus.md)
+        - [FixedWingLateralStatus](/msg_docs/FixedWingLateralStatus.md)
+        - [FixedWingRunwayControl](/msg_docs/FixedWingRunwayControl.md)
         - [FlightPhaseEstimation](/msg_docs/FlightPhaseEstimation.md)
         - [FollowTarget](/msg_docs/FollowTarget.md)
         - [FollowTargetEstimator](/msg_docs/FollowTargetEstimator.md)
@@ -620,7 +627,6 @@
         - [NavigatorMissionItem](/msg_docs/NavigatorMissionItem.md)
         - [NavigatorStatus](/msg_docs/NavigatorStatus.md)
         - [NormalizedUnsignedSetpoint](/msg_docs/NormalizedUnsignedSetpoint.md)
-        - [NpfgStatus](/msg_docs/NpfgStatus.md)
         - [ObstacleDistance](/msg_docs/ObstacleDistance.md)
         - [OffboardControlMode](/msg_docs/OffboardControlMode.md)
         - [OnboardComputerStatus](/msg_docs/OnboardComputerStatus.md)
@@ -656,13 +662,12 @@
         - [RcParameterMap](/msg_docs/RcParameterMap.md)
         - [RoverAttitudeSetpoint](/msg_docs/RoverAttitudeSetpoint.md)
         - [RoverAttitudeStatus](/msg_docs/RoverAttitudeStatus.md)
-        - [RoverMecanumGuidanceStatus](/msg_docs/RoverMecanumGuidanceStatus.md)
-        - [RoverMecanumSetpoint](/msg_docs/RoverMecanumSetpoint.md)
-        - [RoverMecanumStatus](/msg_docs/RoverMecanumStatus.md)
+        - [RoverPositionSetpoint](/msg_docs/RoverPositionSetpoint.md)
         - [RoverRateSetpoint](/msg_docs/RoverRateSetpoint.md)
         - [RoverRateStatus](/msg_docs/RoverRateStatus.md)
         - [RoverSteeringSetpoint](/msg_docs/RoverSteeringSetpoint.md)
         - [RoverThrottleSetpoint](/msg_docs/RoverThrottleSetpoint.md)
+        - [RoverVelocitySetpoint](/msg_docs/RoverVelocitySetpoint.md)
         - [RoverVelocityStatus](/msg_docs/RoverVelocityStatus.md)
         - [Rpm](/msg_docs/Rpm.md)
         - [RtlStatus](/msg_docs/RtlStatus.md)
@@ -694,6 +699,7 @@
         - [TelemetryStatus](/msg_docs/TelemetryStatus.md)
         - [TiltrotorExtraControls](/msg_docs/TiltrotorExtraControls.md)
         - [TimesyncStatus](/msg_docs/TimesyncStatus.md)
+        - [TrajectorySetpoint6dof](/msg_docs/TrajectorySetpoint6dof.md)
         - [TransponderReport](/msg_docs/TransponderReport.md)
         - [TuneControl](/msg_docs/TuneControl.md)
         - [UavcanParameterRequest](/msg_docs/UavcanParameterRequest.md)
@@ -717,13 +723,16 @@
         - [WheelEncoders](/msg_docs/WheelEncoders.md)
         - [Wind](/msg_docs/Wind.md)
         - [YawEstimatorStatus](/msg_docs/YawEstimatorStatus.md)
+        - [AirspeedValidatedV0](/msg_docs/AirspeedValidatedV0.md)
+        - [VehicleAttitudeSetpointV0](/msg_docs/VehicleAttitudeSetpointV0.md)
         - [VehicleStatusV0](/msg_docs/VehicleStatusV0.md)
-    - [MAVLink Messaging](/middleware/mavlink.md)
+    - [MAVLink Messaging](/mavlink/index.md)
       - [Adding Messages](/mavlink/adding_messages.md)
       - [Streaming Messages](/mavlink/streaming_messages.md)
       - [Receiving Messages](/mavlink/receiving_messages.md)
       - [Custom MAVLink Messages](/mavlink/custom_messages.md)
-      - [Standard Modes Protocol](/mavlink/standard_modes.md)
+      - [Protocols/Microservices](/mavlink/protocols.md)
+        - [Standard Modes Protocol](/mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](/middleware/uxrce_dds.md)
   - [모듈과 명령어](/modules/modules_main.md)
     - [자동 튜닝](/modules/modules_autotune.md)
@@ -740,6 +749,7 @@
       - [자기 센서](/modules/modules_driver_magnetometer.md)
       - [광류 센서](/modules/modules_driver_optical_flow.md)
       - [Rpm Sensor](/modules/modules_driver_rpm_sensor.md)
+      - [Radio Control](/modules/modules_driver_radio_control.md)
       - [Transponder](/modules/modules_driver_transponder.md)
     - [추정기](/modules/modules_estimator.md)
     - [시뮬레이션](/modules/modules_simulation.md)
@@ -852,6 +862,7 @@
 
 - [출시](/releases/index.md)
   - [main (alpha)](/releases/main.md)
+  - [1.16 (release candidate)](/releases/1.16.md)
   - [1.15 (stable)](/releases/1.15.md)
   - [1.14](/releases/1.14.md)
   - [1.13](/releases/1.13.md)

docs/ko/computer_vision/collision_prevention.md

@@ -47,7 +47,7 @@ PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](../sensor/sf45_rotatin
 
 - Attach and configure the distance sensor on a particular port (see [sensor-specific docs](../sensor/rangefinders.md)) and enable collision prevention using [CP_DIST](#CP_DIST).
 - 방향을 설정하려면 드라이버를 수정하십시오.
- This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
+  This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
 - Modify the driver to set the _field of view_ in the distance sensor UORB topic (`distance_sensor_s.h_fov`).
 
 ## PX4 (Software) Setup
@@ -203,85 +203,85 @@ The Lua script works by extracting the `obstacle_distance_fused` data at each ti
 
 2. Configure PX4 to publish obstacle distance data (so that it is available to PlotJuggler):
 
- Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
+  Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
 
- ```sh
- - topic: /fmu/out/obstacle_distance_fused
-   type: px4_msgs::msg::ObstacleDistance
- ```
+  ```sh
+  - topic: /fmu/out/obstacle_distance_fused
+    type: px4_msgs::msg::ObstacleDistance
+  ```
 
- For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in _uXRCE-DDS (PX4-ROS 2/DDS Bridge)_.
+  For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
 
 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
- In the **Script Editor** tab, add following scripts in the appropriate sections:
+  In the **Script Editor** tab, add following scripts in the appropriate sections:
 
- - **Global code, executed once:**
+  - **Global code, executed once:**
 
-  ```lua
-  obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
-  obs_dist_min = Timeseries.new("obstacle_distance_minimum")
-  ```
+    ```lua
+    obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
+    obs_dist_min = Timeseries.new("obstacle_distance_minimum")
+    ```
 
- - **function(tracker_time)**
+  - **function(tracker_time)**
 
-  ```lua
-  obs_dist_fused_xy:clear()
+    ```lua
+    obs_dist_fused_xy:clear()
 
-  i = 0
-  angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
-  increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
-  min_dist = 65535
+    i = 0
+    angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
+    increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
+    min_dist = 65535
 
-  -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
-  local angle_offset_value = angle_offset:atTime(tracker_time)
-  local increment_value = increment:atTime(tracker_time)
+    -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
+    local angle_offset_value = angle_offset:atTime(tracker_time)
+    local increment_value = increment:atTime(tracker_time)
 
-  if increment_value == nil or increment_value <= 0 then
-      print("Invalid increment value: " .. tostring(increment_value))
-      return
-  end
+    if increment_value == nil or increment_value <= 0 then
+        print("Invalid increment value: " .. tostring(increment_value))
+        return
+    end
 
-  local max_steps = math.floor(360 / increment_value)
+    local max_steps = math.floor(360 / increment_value)
 
-  while i < max_steps do
-      local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
-      local distance = TimeseriesView.find(str)
-      if distance == nil then
-          print("No distance data for: " .. str)
-          break
-      end
+    while i < max_steps do
+        local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
+        local distance = TimeseriesView.find(str)
+        if distance == nil then
+            print("No distance data for: " .. str)
+            break
+        end
 
-      local dist = distance:atTime(tracker_time)
-      if dist ~= nil and dist < 65535 then
-          -- Calculate angle and Cartesian coordinates
-          local angle = angle_offset_value + i * increment_value
-          local y = dist * math.cos(math.rad(angle))
-          local x = dist * math.sin(math.rad(angle))
+        local dist = distance:atTime(tracker_time)
+        if dist ~= nil and dist < 65535 then
+            -- Calculate angle and Cartesian coordinates
+            local angle = angle_offset_value + i * increment_value
+            local y = dist * math.cos(math.rad(angle))
+            local x = dist * math.sin(math.rad(angle))
 
-          obs_dist_fused_xy:push_back(x, y)
+            obs_dist_fused_xy:push_back(x, y)
 
-          -- Update minimum distance
-          if dist < min_dist then
-              min_dist = dist
-          end
-      end
+            -- Update minimum distance
+            if dist < min_dist then
+                min_dist = dist
+            end
+        end
 
-      i = i + 1
-  end
+        i = i + 1
+    end
 
-  -- Push minimum distance once after the loop
-  if min_dist < 65535 then
-      obs_dist_min:push_back(tracker_time, min_dist)
-  else
-      print("No valid minimum distance found")
-  end
-  ```
+    -- Push minimum distance once after the loop
+    if min_dist < 65535 then
+        obs_dist_min:push_back(tracker_time, min_dist)
+    else
+        print("No valid minimum distance found")
+    end
+    ```
 
 4. Enter a name for the script on the top right, and press **Save**.
- Once saved, the script should appear in the _Active Scripts_ section.
+  Once saved, the script should appear in the _Active Scripts_ section.
 
 5. Start streaming the data using the approach described in [Plotting uORB Topic Data in Real Time using PlotJuggler](../debug/plotting_realtime_uorb_data.md).
- You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
+  You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
 
 Note that you have to press **Save** again to re-enable the scripts after loading a new log file or otherwise clearing data.
 

docs/ko/debug/plotting_realtime_uorb_data.md

@@ -96,13 +96,13 @@ Exporting the messages allows ROS 2 and the uXRCE-DDS agent to be independent of
 
 While `px4_msgs` has messages for all uORB topics in PX4, not all messages in `px4_msgs` are available to ROS 2/PlotJuggler by default.
 The set that are available must be built into the client running on PX4.
-These are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+These are defined in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The instructions below explain the changes needed to monitor topics that are not available by default.
 
 ### Missing Topics
 
-If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) to include the topic and rebuild PX4.
+If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](../middleware/dds_topics.md) ([source](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) to include the topic and rebuild PX4.
 
 If working with real hardware you will need to build and [install](../config/firmware.md#installing-px4-main-beta-or-custom-firmware) custom firmware after changing the YAML file.
 
@@ -122,7 +122,7 @@ cd ~/ros2_ws/ && colcon build
 
 ### Custom Topics
 
-After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and export the new message into your ROS 2 workspace.
+After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](../middleware/dds_topics.md), and export the new message into your ROS 2 workspace.
 
 ## See also
 

docs/ko/debug/system_wide_replay.md

@@ -128,8 +128,7 @@ In EKF2 mode, the replay will automatically create the ORB publisher rules descr
 
 To perform an EKF2 replay:
 
-- Record the original log.
-  Optionally set `SDLOG_MODE` to `1` to log from boot.
+- Record the original log with `SDLOG_MODE` set to `1` to log from boot.
 
 - In addition to the `replay` environment variable, set `replay_mode` to `ekf2`:
 

docs/ko/middleware/uorb.md

@@ -61,12 +61,12 @@ For example the [VelocityLimits](../msg_docs/VelocityLimits.md) message definiti
 ```text
 # Velocity and yaw rate limits for a multicopter position slow mode only
 
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 # absolute speeds, NAN means use default limit
-float32 horizontal_velocity # [m/s]
-float32 vertical_velocity # [m/s]
-float32 yaw_rate # [rad/s]
+float32 horizontal_velocity # [m/s] Horizontal velocity.
+float32 vertical_velocity # [m/s] Vertical velocity.
+float32 yaw_rate # [rad/s] Yaw rate.
 ```
 
 By default this message definition will be compiled to a single topic with an id `velocity_limits`, a direct conversion from the CamelCase name to a snake_case version.
@@ -92,15 +92,34 @@ To nest a message, simply include the nested message type in the parent message
 
 ```text
 # Global position setpoint triplet in WGS84 coordinates.
+#
 # This are the three next waypoints (or just the next two or one).
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 PositionSetpoint previous
 PositionSetpoint current
 PositionSetpoint next
 ```
 
+### uORB Buffer Length (ORB_QUEUE_LENGTH)
+
+uORB messages have a single-message buffer by default, which may be overwritten if the message publication rate is too high.
+In most cases this does not matter: either we are only interested in the latest sample of a topic, such as a sensor value or a setpoint, or losing a few samples is not a particular problem.
+For relatively few cases, such as vehicle commands, it is important that we don't drop topics.
+
+In order to reduce the chance that messages will be dropped we can use named constant `ORB_QUEUE_LENGTH` to create a buffer of the specified length.
+For example, to create a four-message queue, add the following line to your message definition:
+
+```sh
+uint8 ORB_QUEUE_LENGTH = 4
+```
+
+As long as subscribers are able to read messages out of the buffer quickly enough than it isn't ever fully filled to the queue length (by publishers), they will be able to get all messages that are sent.
+Messages will still be lost they are published when the queue is filled.
+
+Note that the queue length value must be a power of 2 (so 2, 4, 8, ...).
+
 ### Message/Field Deprecation {#deprecation}
 
 As there are external tools using uORB messages from log files, such as [Flight Review](https://github.com/PX4/flight_review), certain aspects need to be considered when updating existing messages:

docs/uk/_sidebar.md

@@ -460,6 +460,7 @@
       - [Vehicles](/sim_gazebo_gz/vehicles.md)
         - [Advanced Lift Drag Tool](/sim_gazebo_gz/tools_avl_automation.md)
       - [Worlds](/sim_gazebo_gz/worlds.md)
+      - [Plugins](/sim_gazebo_gz/plugins.md)
       - [Gazebo Models Repository](/sim_gazebo_gz/gazebo_models.md)
       - [Багатотранспортний Sim](/sim_gazebo_gz/multi_vehicle_simulation.md)
     - [Симуляція Gazebo Classic](/sim_gazebo_classic/index.md)
@@ -492,12 +493,17 @@
       - [Versioned](/msg_docs/versioned_messages.md)
         - [ActuatorMotors](/msg_docs/ActuatorMotors.md)
         - [ActuatorServos](/msg_docs/ActuatorServos.md)
+        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [ArmingCheckReply](/msg_docs/ArmingCheckReply.md)
         - [ArmingCheckRequest](/msg_docs/ArmingCheckRequest.md)
         - [BatteryStatus](/msg_docs/BatteryStatus.md)
         - [ConfigOverrides](/msg_docs/ConfigOverrides.md)
+        - [FixedWingLateralSetpoint](/msg_docs/FixedWingLateralSetpoint.md)
+        - [FixedWingLongitudinalSetpoint](/msg_docs/FixedWingLongitudinalSetpoint.md)
         - [GotoSetpoint](/msg_docs/GotoSetpoint.md)
         - [HomePosition](/msg_docs/HomePosition.md)
+        - [LateralControlConfiguration](/msg_docs/LateralControlConfiguration.md)
+        - [LongitudinalControlConfiguration](/msg_docs/LongitudinalControlConfiguration.md)
         - [ManualControlSetpoint](/msg_docs/ManualControlSetpoint.md)
         - [ModeCompleted](/msg_docs/ModeCompleted.md)
         - [RegisterExtComponentReply](/msg_docs/RegisterExtComponentReply.md)
@@ -526,10 +532,8 @@
         - [ActuatorTest](/msg_docs/ActuatorTest.md)
         - [AdcReport](/msg_docs/AdcReport.md)
         - [Airspeed](/msg_docs/Airspeed.md)
-        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [AirspeedWind](/msg_docs/AirspeedWind.md)
         - [AutotuneAttitudeControlStatus](/msg_docs/AutotuneAttitudeControlStatus.md)
-        - [Buffer128](/msg_docs/Buffer128.md)
         - [ButtonEvent](/msg_docs/ButtonEvent.md)
         - [CameraCapture](/msg_docs/CameraCapture.md)
         - [CameraStatus](/msg_docs/CameraStatus.md)
@@ -568,6 +572,9 @@
         - [FailsafeFlags](/msg_docs/FailsafeFlags.md)
         - [FailureDetectorStatus](/msg_docs/FailureDetectorStatus.md)
         - [FigureEightStatus](/msg_docs/FigureEightStatus.md)
+        - [FixedWingLateralGuidanceStatus](/msg_docs/FixedWingLateralGuidanceStatus.md)
+        - [FixedWingLateralStatus](/msg_docs/FixedWingLateralStatus.md)
+        - [FixedWingRunwayControl](/msg_docs/FixedWingRunwayControl.md)
         - [FlightPhaseEstimation](/msg_docs/FlightPhaseEstimation.md)
         - [FollowTarget](/msg_docs/FollowTarget.md)
         - [FollowTargetEstimator](/msg_docs/FollowTargetEstimator.md)
@@ -620,7 +627,6 @@
         - [NavigatorMissionItem](/msg_docs/NavigatorMissionItem.md)
         - [NavigatorStatus](/msg_docs/NavigatorStatus.md)
         - [NormalizedUnsignedSetpoint](/msg_docs/NormalizedUnsignedSetpoint.md)
-        - [NpfgStatus](/msg_docs/NpfgStatus.md)
         - [ObstacleDistance](/msg_docs/ObstacleDistance.md)
         - [OffboardControlMode](/msg_docs/OffboardControlMode.md)
         - [OnboardComputerStatus](/msg_docs/OnboardComputerStatus.md)
@@ -656,13 +662,12 @@
         - [RcParameterMap](/msg_docs/RcParameterMap.md)
         - [RoverAttitudeSetpoint](/msg_docs/RoverAttitudeSetpoint.md)
         - [RoverAttitudeStatus](/msg_docs/RoverAttitudeStatus.md)
-        - [RoverMecanumGuidanceStatus](/msg_docs/RoverMecanumGuidanceStatus.md)
-        - [RoverMecanumSetpoint](/msg_docs/RoverMecanumSetpoint.md)
-        - [RoverMecanumStatus](/msg_docs/RoverMecanumStatus.md)
+        - [RoverPositionSetpoint](/msg_docs/RoverPositionSetpoint.md)
         - [RoverRateSetpoint](/msg_docs/RoverRateSetpoint.md)
         - [RoverRateStatus](/msg_docs/RoverRateStatus.md)
         - [RoverSteeringSetpoint](/msg_docs/RoverSteeringSetpoint.md)
         - [RoverThrottleSetpoint](/msg_docs/RoverThrottleSetpoint.md)
+        - [RoverVelocitySetpoint](/msg_docs/RoverVelocitySetpoint.md)
         - [RoverVelocityStatus](/msg_docs/RoverVelocityStatus.md)
         - [Rpm](/msg_docs/Rpm.md)
         - [RtlStatus](/msg_docs/RtlStatus.md)
@@ -694,6 +699,7 @@
         - [TelemetryStatus](/msg_docs/TelemetryStatus.md)
         - [TiltrotorExtraControls](/msg_docs/TiltrotorExtraControls.md)
         - [TimesyncStatus](/msg_docs/TimesyncStatus.md)
+        - [TrajectorySetpoint6dof](/msg_docs/TrajectorySetpoint6dof.md)
         - [TransponderReport](/msg_docs/TransponderReport.md)
         - [TuneControl](/msg_docs/TuneControl.md)
         - [UavcanParameterRequest](/msg_docs/UavcanParameterRequest.md)
@@ -717,13 +723,16 @@
         - [WheelEncoders](/msg_docs/WheelEncoders.md)
         - [Wind](/msg_docs/Wind.md)
         - [YawEstimatorStatus](/msg_docs/YawEstimatorStatus.md)
+        - [AirspeedValidatedV0](/msg_docs/AirspeedValidatedV0.md)
+        - [VehicleAttitudeSetpointV0](/msg_docs/VehicleAttitudeSetpointV0.md)
         - [VehicleStatusV0](/msg_docs/VehicleStatusV0.md)
-    - [Повідомлення MAVLink](/middleware/mavlink.md)
+    - [MAVLink Messaging](/mavlink/index.md)
       - [Adding Messages](/mavlink/adding_messages.md)
       - [Streaming Messages](/mavlink/streaming_messages.md)
       - [Receiving Messages](/mavlink/receiving_messages.md)
       - [Custom MAVLink Messages](/mavlink/custom_messages.md)
-      - [Standard Modes Protocol](/mavlink/standard_modes.md)
+      - [Protocols/Microservices](/mavlink/protocols.md)
+        - [Standard Modes Protocol](/mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](/middleware/uxrce_dds.md)
   - [Модулі & Команди](/modules/modules_main.md)
     - [Автоматичне підлаштування](/modules/modules_autotune.md)
@@ -740,6 +749,7 @@
       - [Магнітометр](/modules/modules_driver_magnetometer.md)
       - [Оптичний потік](/modules/modules_driver_optical_flow.md)
       - [Датчик швидкості обертання](/modules/modules_driver_rpm_sensor.md)
+      - [Radio Control](/modules/modules_driver_radio_control.md)
       - [Транспондер](/modules/modules_driver_transponder.md)
     - [Естіматори](/modules/modules_estimator.md)
     - [Симуляції](/modules/modules_simulation.md)
@@ -852,6 +862,7 @@
 
 - [Релізи](/releases/index.md)
   - [main (alpha)](/releases/main.md)
+  - [1.16 (release candidate)](/releases/1.16.md)
   - [1.15 (stable)](/releases/1.15.md)
   - [1.14](/releases/1.14.md)
   - [1.13](/releases/1.13.md)

docs/uk/computer_vision/collision_prevention.md

@@ -47,7 +47,7 @@ PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](../sensor/sf45_rotatin
 
 - Attach and configure the distance sensor on a particular port (see [sensor-specific docs](../sensor/rangefinders.md)) and enable collision prevention using [CP_DIST](#CP_DIST).
 - Модифікуйте драйвер для встановлення орієнтації.
- This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
+  This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
 - Modify the driver to set the _field of view_ in the distance sensor UORB topic (`distance_sensor_s.h_fov`).
 
 ## PX4 (Software) Setup
@@ -203,85 +203,85 @@ The Lua script works by extracting the `obstacle_distance_fused` data at each ti
 
 2. Configure PX4 to publish obstacle distance data (so that it is available to PlotJuggler):
 
- Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
+  Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
 
- ```sh
- - topic: /fmu/out/obstacle_distance_fused
-   type: px4_msgs::msg::ObstacleDistance
- ```
+  ```sh
+  - topic: /fmu/out/obstacle_distance_fused
+    type: px4_msgs::msg::ObstacleDistance
+  ```
 
- For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in _uXRCE-DDS (PX4-ROS 2/DDS Bridge)_.
+  For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
 
 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
- In the **Script Editor** tab, add following scripts in the appropriate sections:
+  In the **Script Editor** tab, add following scripts in the appropriate sections:
 
- - **Global code, executed once:**
+  - **Global code, executed once:**
 
-  ```lua
-  obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
-  obs_dist_min = Timeseries.new("obstacle_distance_minimum")
-  ```
+    ```lua
+    obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
+    obs_dist_min = Timeseries.new("obstacle_distance_minimum")
+    ```
 
- - **function(tracker_time)**
+  - **function(tracker_time)**
 
-  ```lua
-  obs_dist_fused_xy:clear()
+    ```lua
+    obs_dist_fused_xy:clear()
 
-  i = 0
-  angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
-  increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
-  min_dist = 65535
+    i = 0
+    angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
+    increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
+    min_dist = 65535
 
-  -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
-  local angle_offset_value = angle_offset:atTime(tracker_time)
-  local increment_value = increment:atTime(tracker_time)
+    -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
+    local angle_offset_value = angle_offset:atTime(tracker_time)
+    local increment_value = increment:atTime(tracker_time)
 
-  if increment_value == nil or increment_value <= 0 then
-      print("Invalid increment value: " .. tostring(increment_value))
-      return
-  end
+    if increment_value == nil or increment_value <= 0 then
+        print("Invalid increment value: " .. tostring(increment_value))
+        return
+    end
 
-  local max_steps = math.floor(360 / increment_value)
+    local max_steps = math.floor(360 / increment_value)
 
-  while i < max_steps do
-      local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
-      local distance = TimeseriesView.find(str)
-      if distance == nil then
-          print("No distance data for: " .. str)
-          break
-      end
+    while i < max_steps do
+        local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
+        local distance = TimeseriesView.find(str)
+        if distance == nil then
+            print("No distance data for: " .. str)
+            break
+        end
 
-      local dist = distance:atTime(tracker_time)
-      if dist ~= nil and dist < 65535 then
-          -- Calculate angle and Cartesian coordinates
-          local angle = angle_offset_value + i * increment_value
-          local y = dist * math.cos(math.rad(angle))
-          local x = dist * math.sin(math.rad(angle))
+        local dist = distance:atTime(tracker_time)
+        if dist ~= nil and dist < 65535 then
+            -- Calculate angle and Cartesian coordinates
+            local angle = angle_offset_value + i * increment_value
+            local y = dist * math.cos(math.rad(angle))
+            local x = dist * math.sin(math.rad(angle))
 
-          obs_dist_fused_xy:push_back(x, y)
+            obs_dist_fused_xy:push_back(x, y)
 
-          -- Update minimum distance
-          if dist < min_dist then
-              min_dist = dist
-          end
-      end
+            -- Update minimum distance
+            if dist < min_dist then
+                min_dist = dist
+            end
+        end
 
-      i = i + 1
-  end
+        i = i + 1
+    end
 
-  -- Push minimum distance once after the loop
-  if min_dist < 65535 then
-      obs_dist_min:push_back(tracker_time, min_dist)
-  else
-      print("No valid minimum distance found")
-  end
-  ```
+    -- Push minimum distance once after the loop
+    if min_dist < 65535 then
+        obs_dist_min:push_back(tracker_time, min_dist)
+    else
+        print("No valid minimum distance found")
+    end
+    ```
 
 4. Enter a name for the script on the top right, and press **Save**.
- Once saved, the script should appear in the _Active Scripts_ section.
+  Once saved, the script should appear in the _Active Scripts_ section.
 
 5. Start streaming the data using the approach described in [Plotting uORB Topic Data in Real Time using PlotJuggler](../debug/plotting_realtime_uorb_data.md).
- You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
+  You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
 
 Note that you have to press **Save** again to re-enable the scripts after loading a new log file or otherwise clearing data.
 

docs/uk/debug/plotting_realtime_uorb_data.md

@@ -96,13 +96,13 @@ Exporting the messages allows ROS 2 and the uXRCE-DDS agent to be independent of
 
 While `px4_msgs` has messages for all uORB topics in PX4, not all messages in `px4_msgs` are available to ROS 2/PlotJuggler by default.
 The set that are available must be built into the client running on PX4.
-These are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+These are defined in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The instructions below explain the changes needed to monitor topics that are not available by default.
 
 ### Missing Topics
 
-If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) to include the topic and rebuild PX4.
+If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](../middleware/dds_topics.md) ([source](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) to include the topic and rebuild PX4.
 
 If working with real hardware you will need to build and [install](../config/firmware.md#installing-px4-main-beta-or-custom-firmware) custom firmware after changing the YAML file.
 
@@ -122,7 +122,7 @@ cd ~/ros2_ws/ && colcon build
 
 ### Custom Topics
 
-After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and export the new message into your ROS 2 workspace.
+After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](../middleware/dds_topics.md), and export the new message into your ROS 2 workspace.
 
 ## Дивись також
 

docs/uk/debug/system_wide_replay.md

@@ -128,8 +128,7 @@ To enable recording for EKF replay you must set the parameters to enable a [sing
 
 Для виконання повторення EKF2:
 
-- Запишіть оригінальний журнал.
-  Optionally set `SDLOG_MODE` to `1` to log from boot.
+- Record the original log with `SDLOG_MODE` set to `1` to log from boot.
 
 - In addition to the `replay` environment variable, set `replay_mode` to `ekf2`:
 

docs/uk/middleware/dds_topics.md

@@ -0,0 +1,277 @@
+# dds_topics.yaml — PX4 Topics Exposed to ROS 2
+
+:::info
+This document is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code.
+:::
+
+The [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) file specifies which uORB message definitions are compiled into the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) module when [PX4 is built](../middleware/uxrce_dds.md#code-generation), and hence which topics are available for ROS 2 applications to subscribe or publish (by default).
+
+This document shows a markdown-rendered version of [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), listing the publications, subscriptions, and so on.
+
+## Publications
+
+| Topic                                   | Тип                                                                                                                                                                       | Rate Limit           |
+| --------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------- |
+| `/fmu/out/register_ext_component_reply` | [px4_msgs::msg::RegisterExtComponentReply](../msg_docs/RegisterExtComponentReply.md) |                      |
+| `/fmu/out/arming_check_request`         | [px4_msgs::msg::ArmingCheckRequest](../msg_docs/ArmingCheckRequest.md)               | 5.0  |
+| `/fmu/out/mode_completed`               | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md)                         | 50.0 |
+| `/fmu/out/battery_status`               | [px4_msgs::msg::BatteryStatus](../msg_docs/BatteryStatus.md)                         | 1.0  |
+| `/fmu/out/collision_constraints`        | [px4_msgs::msg::CollisionConstraints](../msg_docs/CollisionConstraints.md)           | 50.0 |
+| `/fmu/out/estimator_status_flags`       | [px4_msgs::msg::EstimatorStatusFlags](../msg_docs/EstimatorStatusFlags.md)           | 5.0  |
+| `/fmu/out/failsafe_flags`               | [px4_msgs::msg::FailsafeFlags](../msg_docs/FailsafeFlags.md)                         | 5.0  |
+| `/fmu/out/manual_control_setpoint`      | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md)         | 25.0 |
+| `/fmu/out/message_format_response`      | [px4_msgs::msg::MessageFormatResponse](../msg_docs/MessageFormatResponse.md)         |                      |
+| `/fmu/out/position_setpoint_triplet`    | [px4_msgs::msg::PositionSetpointTriplet](../msg_docs/PositionSetpointTriplet.md)     | 5.0  |
+| `/fmu/out/sensor_combined`              | [px4_msgs::msg::SensorCombined](../msg_docs/SensorCombined.md)                       |                      |
+| `/fmu/out/timesync_status`              | [px4_msgs::msg::TimesyncStatus](../msg_docs/TimesyncStatus.md)                       | 10.0 |
+| `/fmu/out/vehicle_land_detected`        | [px4_msgs::msg::VehicleLandDetected](../msg_docs/VehicleLandDetected.md)             | 5.0  |
+| `/fmu/out/vehicle_attitude`             | [px4_msgs::msg::VehicleAttitude](../msg_docs/VehicleAttitude.md)                     |                      |
+| `/fmu/out/vehicle_control_mode`         | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md)               | 50.0 |
+| `/fmu/out/vehicle_command_ack`          | [px4_msgs::msg::VehicleCommandAck](../msg_docs/VehicleCommandAck.md)                 |                      |
+| `/fmu/out/vehicle_global_position`      | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md)         | 50.0 |
+| `/fmu/out/vehicle_gps_position`         | [px4_msgs::msg::SensorGps](../msg_docs/SensorGps.md)                                 | 50.0 |
+| `/fmu/out/vehicle_local_position`       | [px4_msgs::msg::VehicleLocalPosition](../msg_docs/VehicleLocalPosition.md)           | 50.0 |
+| `/fmu/out/vehicle_odometry`             | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                     |                      |
+| `/fmu/out/vehicle_status`               | [px4_msgs::msg::VehicleStatus](../msg_docs/VehicleStatus.md)                         | 5.0  |
+| `/fmu/out/airspeed_validated`           | [px4_msgs::msg::AirspeedValidated](../msg_docs/AirspeedValidated.md)                 | 50.0 |
+| `/fmu/out/vtol_vehicle_status`          | [px4_msgs::msg::VtolVehicleStatus](../msg_docs/VtolVehicleStatus.md)                 |                      |
+| `/fmu/out/home_position`                | [px4_msgs::msg::HomePosition](../msg_docs/HomePosition.md)                           | 5.0  |
+
+## Subscriptions
+
+| Topic                                                                                                   | Тип                                                                                                                                                                                     |
+| ------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| /fmu/in/register_ext_component_request   | [px4_msgs::msg::RegisterExtComponentRequest](../msg_docs/RegisterExtComponentRequest.md)           |
+| /fmu/in/unregister_ext_component                              | [px4_msgs::msg::UnregisterExtComponent](../msg_docs/UnregisterExtComponent.md)                     |
+| /fmu/in/config_overrides_request                              | [px4_msgs::msg::ConfigOverrides](../msg_docs/ConfigOverrides.md)                                   |
+| /fmu/in/arming_check_reply                                    | [px4_msgs::msg::ArmingCheckReply](../msg_docs/ArmingCheckReply.md)                                 |
+| /fmu/in/message_format_request                                | [px4_msgs::msg::MessageFormatRequest](../msg_docs/MessageFormatRequest.md)                         |
+| /fmu/in/mode_completed                                                             | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md)                                       |
+| /fmu/in/config_control_setpoints                              | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md)                             |
+| /fmu/in/distance_sensor                                                            | [px4_msgs::msg::DistanceSensor](../msg_docs/DistanceSensor.md)                                     |
+| /fmu/in/manual_control_input                                  | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md)                       |
+| /fmu/in/offboard_control_mode                                 | [px4_msgs::msg::OffboardControlMode](../msg_docs/OffboardControlMode.md)                           |
+| /fmu/in/onboard_computer_status                               | [px4_msgs::msg::OnboardComputerStatus](../msg_docs/OnboardComputerStatus.md)                       |
+| /fmu/in/obstacle_distance                                                          | [px4_msgs::msg::ObstacleDistance](../msg_docs/ObstacleDistance.md)                                 |
+| /fmu/in/sensor_optical_flow                                   | [px4_msgs::msg::SensorOpticalFlow](../msg_docs/SensorOpticalFlow.md)                               |
+| /fmu/in/goto_setpoint                                                              | [px4_msgs::msg::GotoSetpoint](../msg_docs/GotoSetpoint.md)                                         |
+| /fmu/in/telemetry_status                                                           | [px4_msgs::msg::TelemetryStatus](../msg_docs/TelemetryStatus.md)                                   |
+| /fmu/in/trajectory_setpoint                                                        | [px4_msgs::msg::TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md)                             |
+| /fmu/in/vehicle_attitude_setpoint                             | [px4_msgs::msg::VehicleAttitudeSetpoint](../msg_docs/VehicleAttitudeSetpoint.md)                   |
+| /fmu/in/vehicle_mocap_odometry                                | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                                   |
+| /fmu/in/vehicle_rates_setpoint                                | [px4_msgs::msg::VehicleRatesSetpoint](../msg_docs/VehicleRatesSetpoint.md)                         |
+| /fmu/in/vehicle_visual_odometry                               | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                                   |
+| /fmu/in/vehicle_command                                                            | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)                                     |
+| /fmu/in/vehicle_command_mode_executor    | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)                                     |
+| /fmu/in/vehicle_thrust_setpoint                               | [px4_msgs::msg::VehicleThrustSetpoint](../msg_docs/VehicleThrustSetpoint.md)                       |
+| /fmu/in/vehicle_torque_setpoint                               | [px4_msgs::msg::VehicleTorqueSetpoint](../msg_docs/VehicleTorqueSetpoint.md)                       |
+| /fmu/in/actuator_motors                                                            | [px4_msgs::msg::ActuatorMotors](../msg_docs/ActuatorMotors.md)                                     |
+| /fmu/in/actuator_servos                                                            | [px4_msgs::msg::ActuatorServos](../msg_docs/ActuatorServos.md)                                     |
+| /fmu/in/aux_global_position                                   | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md)                       |
+| /fmu/in/fixed_wing_longitudinal_setpoint | [px4_msgs::msg::FixedWingLongitudinalSetpoint](../msg_docs/FixedWingLongitudinalSetpoint.md)       |
+| /fmu/in/fixed_wing_lateral_setpoint      | [px4_msgs::msg::FixedWingLateralSetpoint](../msg_docs/FixedWingLateralSetpoint.md)                 |
+| /fmu/in/longitudinal_control_configuration                    | [px4_msgs::msg::LongitudinalControlConfiguration](../msg_docs/LongitudinalControlConfiguration.md) |
+| /fmu/in/lateral_control_configuration                         | [px4_msgs::msg::LateralControlConfiguration](../msg_docs/LateralControlConfiguration.md)           |
+
+## Subscriptions Multi
+
+None
+
+## Not Exported
+
+These messages are not listed in the yaml file.
+They are not build into the module, and hence are neither published or subscribed.
+
+:::details
+See messages
+
+- [SensorCorrection](../msg_docs/SensorCorrection.md)
+- [ActuatorOutputs](../msg_docs/ActuatorOutputs.md)
+- [FixedWingRunwayControl](../msg_docs/FixedWingRunwayControl.md)
+- [EstimatorInnovations](../msg_docs/EstimatorInnovations.md)
+- [FlightPhaseEstimation](../msg_docs/FlightPhaseEstimation.md)
+- [PurePursuitStatus](../msg_docs/PurePursuitStatus.md)
+- [Px4ioStatus](../msg_docs/Px4ioStatus.md)
+- [SatelliteInfo](../msg_docs/SatelliteInfo.md)
+- [GeofenceResult](../msg_docs/GeofenceResult.md)
+- [GimbalManagerStatus](../msg_docs/GimbalManagerStatus.md)
+- [ManualControlSwitches](../msg_docs/ManualControlSwitches.md)
+- [OpenDroneIdSelfId](../msg_docs/OpenDroneIdSelfId.md)
+- [OpenDroneIdSystem](../msg_docs/OpenDroneIdSystem.md)
+- [EventV0](../msg_docs/EventV0.md)
+- [QshellRetval](../msg_docs/QshellRetval.md)
+- [RoverThrottleSetpoint](../msg_docs/RoverThrottleSetpoint.md)
+- [AirspeedValidatedV0](../msg_docs/AirspeedValidatedV0.md)
+- [RcChannels](../msg_docs/RcChannels.md)
+- [SensorAccel](../msg_docs/SensorAccel.md)
+- [GimbalDeviceAttitudeStatus](../msg_docs/GimbalDeviceAttitudeStatus.md)
+- [EscStatus](../msg_docs/EscStatus.md)
+- [RoverAttitudeSetpoint](../msg_docs/RoverAttitudeSetpoint.md)
+- [RateCtrlStatus](../msg_docs/RateCtrlStatus.md)
+- [AirspeedWind](../msg_docs/AirspeedWind.md)
+- [InputRc](../msg_docs/InputRc.md)
+- [GpioIn](../msg_docs/GpioIn.md)
+- [LaunchDetectionStatus](../msg_docs/LaunchDetectionStatus.md)
+- [VehicleImu](../msg_docs/VehicleImu.md)
+- [Event](../msg_docs/Event.md)
+- [SensorUwb](../msg_docs/SensorUwb.md)
+- [ActuatorServosTrim](../msg_docs/ActuatorServosTrim.md)
+- [DatamanResponse](../msg_docs/DatamanResponse.md)
+- [OrbTest](../msg_docs/OrbTest.md)
+- [VehicleLocalPositionSetpoint](../msg_docs/VehicleLocalPositionSetpoint.md)
+- [VehicleAngularVelocity](../msg_docs/VehicleAngularVelocity.md)
+- [FollowTargetStatus](../msg_docs/FollowTargetStatus.md)
+- [NormalizedUnsignedSetpoint](../msg_docs/NormalizedUnsignedSetpoint.md)
+- [YawEstimatorStatus](../msg_docs/YawEstimatorStatus.md)
+- [TakeoffStatus](../msg_docs/TakeoffStatus.md)
+- [UlogStreamAck](../msg_docs/UlogStreamAck.md)
+- [OrbTestLarge](../msg_docs/OrbTestLarge.md)
+- [RoverSteeringSetpoint](../msg_docs/RoverSteeringSetpoint.md)
+- [CameraCapture](../msg_docs/CameraCapture.md)
+- [VehicleRoi](../msg_docs/VehicleRoi.md)
+- [ActuatorArmed](../msg_docs/ActuatorArmed.md)
+- [FixedWingLateralGuidanceStatus](../msg_docs/FixedWingLateralGuidanceStatus.md)
+- [ParameterSetValueResponse](../msg_docs/ParameterSetValueResponse.md)
+- [GeofenceStatus](../msg_docs/GeofenceStatus.md)
+- [VehicleAngularAccelerationSetpoint](../msg_docs/VehicleAngularAccelerationSetpoint.md)
+- [SensorGnssRelative](../msg_docs/SensorGnssRelative.md)
+- [PowerMonitor](../msg_docs/PowerMonitor.md)
+- [RoverVelocityStatus](../msg_docs/RoverVelocityStatus.md)
+- [ParameterResetRequest](../msg_docs/ParameterResetRequest.md)
+- [RoverAttitudeStatus](../msg_docs/RoverAttitudeStatus.md)
+- [TecsStatus](../msg_docs/TecsStatus.md)
+- [EstimatorSelectorStatus](../msg_docs/EstimatorSelectorStatus.md)
+- [CanInterfaceStatus](../msg_docs/CanInterfaceStatus.md)
+- [Ping](../msg_docs/Ping.md)
+- [LedControl](../msg_docs/LedControl.md)
+- [Wind](../msg_docs/Wind.md)
+- [VehicleStatusV0](../msg_docs/VehicleStatusV0.md)
+- [ActuatorTest](../msg_docs/ActuatorTest.md)
+- [IridiumsbdStatus](../msg_docs/IridiumsbdStatus.md)
+- [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md)
+- [GimbalManagerSetAttitude](../msg_docs/GimbalManagerSetAttitude.md)
+- [Gripper](../msg_docs/Gripper.md)
+- [SensorMag](../msg_docs/SensorMag.md)
+- [DebugValue](../msg_docs/DebugValue.md)
+- [SensorPreflightMag](../msg_docs/SensorPreflightMag.md)
+- [RcParameterMap](../msg_docs/RcParameterMap.md)
+- [LandingGear](../msg_docs/LandingGear.md)
+- [GimbalDeviceInformation](../msg_docs/GimbalDeviceInformation.md)
+- [VehicleOpticalFlow](../msg_docs/VehicleOpticalFlow.md)
+- [UlogStream](../msg_docs/UlogStream.md)
+- [GimbalControls](../msg_docs/GimbalControls.md)
+- [RoverRateSetpoint](../msg_docs/RoverRateSetpoint.md)
+- [LogMessage](../msg_docs/LogMessage.md)
+- [RoverVelocitySetpoint](../msg_docs/RoverVelocitySetpoint.md)
+- [GpioOut](../msg_docs/GpioOut.md)
+- [TaskStackInfo](../msg_docs/TaskStackInfo.md)
+- [VelocityLimits](../msg_docs/VelocityLimits.md)
+- [MagWorkerData](../msg_docs/MagWorkerData.md)
+- [ParameterUpdate](../msg_docs/ParameterUpdate.md)
+- [TrajectorySetpoint6dof](../msg_docs/TrajectorySetpoint6dof.md)
+- [SensorBaro](../msg_docs/SensorBaro.md)
+- [VehicleImuStatus](../msg_docs/VehicleImuStatus.md)
+- [InternalCombustionEngineStatus](../msg_docs/InternalCombustionEngineStatus.md)
+- [VehicleOpticalFlowVel](../msg_docs/VehicleOpticalFlowVel.md)
+- [GimbalManagerSetManualControl](../msg_docs/GimbalManagerSetManualControl.md)
+- [Rpm](../msg_docs/Rpm.md)
+- [MagnetometerBiasEstimate](../msg_docs/MagnetometerBiasEstimate.md)
+- [MountOrientation](../msg_docs/MountOrientation.md)
+- [ActionRequest](../msg_docs/ActionRequest.md)
+- [OpenDroneIdArmStatus](../msg_docs/OpenDroneIdArmStatus.md)
+- [SensorAccelFifo](../msg_docs/SensorAccelFifo.md)
+- [LoggerStatus](../msg_docs/LoggerStatus.md)
+- [GeneratorStatus](../msg_docs/GeneratorStatus.md)
+- [InternalCombustionEngineControl](../msg_docs/InternalCombustionEngineControl.md)
+- [Ekf2Timestamps](../msg_docs/Ekf2Timestamps.md)
+- [LandingTargetPose](../msg_docs/LandingTargetPose.md)
+- [PositionControllerLandingStatus](../msg_docs/PositionControllerLandingStatus.md)
+- [UavcanParameterValue](../msg_docs/UavcanParameterValue.md)
+- [OrbitStatus](../msg_docs/OrbitStatus.md)
+- [PositionControllerStatus](../msg_docs/PositionControllerStatus.md)
+- [EstimatorStatus](../msg_docs/EstimatorStatus.md)
+- [DatamanRequest](../msg_docs/DatamanRequest.md)
+- [HoverThrustEstimate](../msg_docs/HoverThrustEstimate.md)
+- [FixedWingLateralStatus](../msg_docs/FixedWingLateralStatus.md)
+- [NavigatorMissionItem](../msg_docs/NavigatorMissionItem.md)
+- [Cpuload](../msg_docs/Cpuload.md)
+- [EstimatorAidSource3d](../msg_docs/EstimatorAidSource3d.md)
+- [RoverRateStatus](../msg_docs/RoverRateStatus.md)
+- [EscReport](../msg_docs/EscReport.md)
+- [DebugArray](../msg_docs/DebugArray.md)
+- [ControlAllocatorStatus](../msg_docs/ControlAllocatorStatus.md)
+- [SensorHygrometer](../msg_docs/SensorHygrometer.md)
+- [EstimatorSensorBias](../msg_docs/EstimatorSensorBias.md)
+- [EstimatorBias3d](../msg_docs/EstimatorBias3d.md)
+- [GimbalManagerInformation](../msg_docs/GimbalManagerInformation.md)
+- [QshellReq](../msg_docs/QshellReq.md)
+- [CameraStatus](../msg_docs/CameraStatus.md)
+- [GpsInjectData](../msg_docs/GpsInjectData.md)
+- [FigureEightStatus](../msg_docs/FigureEightStatus.md)
+- [TransponderReport](../msg_docs/TransponderReport.md)
+- [UavcanParameterRequest](../msg_docs/UavcanParameterRequest.md)
+- [MavlinkLog](../msg_docs/MavlinkLog.md)
+- [EstimatorGpsStatus](../msg_docs/EstimatorGpsStatus.md)
+- [FuelTankStatus](../msg_docs/FuelTankStatus.md)
+- [Mission](../msg_docs/Mission.md)
+- [PositionSetpoint](../msg_docs/PositionSetpoint.md)
+- [MissionResult](../msg_docs/MissionResult.md)
+- [EstimatorEventFlags](../msg_docs/EstimatorEventFlags.md)
+- [VehicleMagnetometer](../msg_docs/VehicleMagnetometer.md)
+- [MavlinkTunnel](../msg_docs/MavlinkTunnel.md)
+- [DifferentialPressure](../msg_docs/DifferentialPressure.md)
+- [CellularStatus](../msg_docs/CellularStatus.md)
+- [GpsDump](../msg_docs/GpsDump.md)
+- [GimbalDeviceSetAttitude](../msg_docs/GimbalDeviceSetAttitude.md)
+- [ArmingCheckReplyV0](../msg_docs/ArmingCheckReplyV0.md)
+- [NavigatorStatus](../msg_docs/NavigatorStatus.md)
+- [RoverPositionSetpoint](../msg_docs/RoverPositionSetpoint.md)
+- [FollowTarget](../msg_docs/FollowTarget.md)
+- [SensorsStatusImu](../msg_docs/SensorsStatusImu.md)
+- [EstimatorStates](../msg_docs/EstimatorStates.md)
+- [SensorGyro](../msg_docs/SensorGyro.md)
+- [SensorAirflow](../msg_docs/SensorAirflow.md)
+- [ButtonEvent](../msg_docs/ButtonEvent.md)
+- [DebugKeyValue](../msg_docs/DebugKeyValue.md)
+- [GpioConfig](../msg_docs/GpioConfig.md)
+- [CameraTrigger](../msg_docs/CameraTrigger.md)
+- [LandingGearWheel](../msg_docs/LandingGearWheel.md)
+- [VehicleConstraints](../msg_docs/VehicleConstraints.md)
+- [HealthReport](../msg_docs/HealthReport.md)
+- [PowerButtonState](../msg_docs/PowerButtonState.md)
+- [RadioStatus](../msg_docs/RadioStatus.md)
+- [SensorGyroFifo](../msg_docs/SensorGyroFifo.md)
+- [EstimatorBias](../msg_docs/EstimatorBias.md)
+- [DebugVect](../msg_docs/DebugVect.md)
+- [DistanceSensorModeChangeRequest](../msg_docs/DistanceSensorModeChangeRequest.md)
+- [RtlTimeEstimate](../msg_docs/RtlTimeEstimate.md)
+- [PpsCapture](../msg_docs/PpsCapture.md)
+- [SensorSelection](../msg_docs/SensorSelection.md)
+- [SystemPower](../msg_docs/SystemPower.md)
+- [ActuatorControlsStatus](../msg_docs/ActuatorControlsStatus.md)
+- [SensorGyroFft](../msg_docs/SensorGyroFft.md)
+- [VehicleAirData](../msg_docs/VehicleAirData.md)
+- [FollowTargetEstimator](../msg_docs/FollowTargetEstimator.md)
+- [ParameterSetUsedRequest](../msg_docs/ParameterSetUsedRequest.md)
+- [GpioRequest](../msg_docs/GpioRequest.md)
+- [OpenDroneIdOperatorId](../msg_docs/OpenDroneIdOperatorId.md)
+- [RtlStatus](../msg_docs/RtlStatus.md)
+- [Airspeed](../msg_docs/Airspeed.md)
+- [VehicleAcceleration](../msg_docs/VehicleAcceleration.md)
+- [ParameterSetValueRequest](../msg_docs/ParameterSetValueRequest.md)
+- [IrlockReport](../msg_docs/IrlockReport.md)
+- [HeaterStatus](../msg_docs/HeaterStatus.md)
+- [AdcReport](../msg_docs/AdcReport.md)
+- [PwmInput](../msg_docs/PwmInput.md)
+- [TiltrotorExtraControls](../msg_docs/TiltrotorExtraControls.md)
+- [EstimatorAidSource1d](../msg_docs/EstimatorAidSource1d.md)
+- [OrbTestMedium](../msg_docs/OrbTestMedium.md)
+- [VehicleAttitudeSetpointV0](../msg_docs/VehicleAttitudeSetpointV0.md)
+- [EstimatorAidSource2d](../msg_docs/EstimatorAidSource2d.md)
+- [TuneControl](../msg_docs/TuneControl.md)
+- [WheelEncoders](../msg_docs/WheelEncoders.md)
+- [AutotuneAttitudeControlStatus](../msg_docs/AutotuneAttitudeControlStatus.md)
+- [LandingTargetInnovations](../msg_docs/LandingTargetInnovations.md)
+- [SensorsStatus](../msg_docs/SensorsStatus.md)
+
+:::

docs/uk/middleware/uorb.md

@@ -61,12 +61,12 @@ For example the [VelocityLimits](../msg_docs/VelocityLimits.md) message definiti
 ```text
 # Velocity and yaw rate limits for a multicopter position slow mode only
 
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 # absolute speeds, NAN means use default limit
-float32 horizontal_velocity # [m/s]
-float32 vertical_velocity # [m/s]
-float32 yaw_rate # [rad/s]
+float32 horizontal_velocity # [m/s] Horizontal velocity.
+float32 vertical_velocity # [m/s] Vertical velocity.
+float32 yaw_rate # [rad/s] Yaw rate.
 ```
 
 By default this message definition will be compiled to a single topic with an id `velocity_limits`, a direct conversion from the CamelCase name to a snake_case version.
@@ -92,15 +92,34 @@ To nest a message, simply include the nested message type in the parent message
 
 ```text
 # Global position setpoint triplet in WGS84 coordinates.
+#
 # This are the three next waypoints (or just the next two or one).
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 PositionSetpoint previous
 PositionSetpoint current
 PositionSetpoint next
 ```
 
+### uORB Buffer Length (ORB_QUEUE_LENGTH)
+
+uORB messages have a single-message buffer by default, which may be overwritten if the message publication rate is too high.
+In most cases this does not matter: either we are only interested in the latest sample of a topic, such as a sensor value or a setpoint, or losing a few samples is not a particular problem.
+For relatively few cases, such as vehicle commands, it is important that we don't drop topics.
+
+In order to reduce the chance that messages will be dropped we can use named constant `ORB_QUEUE_LENGTH` to create a buffer of the specified length.
+For example, to create a four-message queue, add the following line to your message definition:
+
+```sh
+uint8 ORB_QUEUE_LENGTH = 4
+```
+
+As long as subscribers are able to read messages out of the buffer quickly enough than it isn't ever fully filled to the queue length (by publishers), they will be able to get all messages that are sent.
+Messages will still be lost they are published when the queue is filled.
+
+Note that the queue length value must be a power of 2 (so 2, 4, 8, ...).
+
 ### Message/Field Deprecation {#deprecation}
 
 As there are external tools using uORB messages from log files, such as [Flight Review](https://github.com/PX4/flight_review), certain aspects need to be considered when updating existing messages:

docs/uk/middleware/uxrce_dds.md

@@ -38,7 +38,7 @@ The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is gen
 Агент не залежить від клієнтського коду.
 Він може бути побудований окремо або в робочому просторі ROS 2, або встановлений як snap пакет в Ubuntu.
 
-When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
+When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [/src/modules/uxrce_dds_client/dds_topics.yaml](../middleware/dds_topics.md) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
 
 PX4 main or release builds automatically export the set of uORB messages definitions in the build to an associated branch in [PX4/px4_msgs](https://github.com/PX4/px4_msgs).
 
@@ -326,13 +326,11 @@ ROS_DOMAIN_ID=3 PX4_UXRCE_DDS_PORT=9999 PX4_UXRCE_DDS_NS=drone make px4_sitl gz_
 
 ## Підтримувані повідомлення uORB
 
-The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The topics are release specific (support is compiled into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) at build time).
 Хоча більшість випусків мають підтримувати дуже схожий набір повідомлень, щоб бути впевненими, вам слід перевірити файл yaml для вашого конкретного релізу.
 
-<!-- Jublish the set we use?: https://github.com/PX4/px4_msgs/issues/22 -->
-
 Note that ROS 2/DDS needs to have the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware in order to interpret the messages.
 The message definitions are stored in the ROS 2 interface package [PX4/px4_msgs](https://github.com/PX4/px4_msgs), and they are automatically synchronized by CI on the `main` and release branches.
 Note that all the messages from PX4 source code are present in the repository, but only those listed in `dds_topics.yaml` will be available as ROS 2 topics.
@@ -349,21 +347,21 @@ Note that all the messages from PX4 source code are present in the repository, b
   ```
 
   ::: info
-  Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
+  Technically, [dds_topics.yaml](../middleware/dds_topics.md) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
   For more information see [DDS Topics YAML](#dds-topics-yaml) below.
 
 :::
 
 ## Customizing the Namespace
 
-Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) or at runtime (which is useful for multi vehicle operations):
+Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](../middleware/dds_topics.md)) or at runtime (which is useful for multi vehicle operations):
 
 - One possibility is to use the `-n` option when starting the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) from command line.
   Ця техніка може бути використана як у симуляторах, так і на реальних транспортних засобах.
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 :::info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#dds-ros-2-services).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#dds-ros-2-services).
 Отже, команди, подібні до:
 
 ```sh
@@ -385,13 +383,13 @@ PX4_UXRCE_DDS_NS=uav_1 make px4_sitl gz_x500
 
 :::
 
-## Налаштування PX4 ROS 2 QoS
+## PX4 ROS 2 QoS Settings
 
-Налаштування QoS PX4 для видавців несумісні з налаштуваннями QoS за замовчуванням для підписників ROS 2.
-Таким чином, якщо код ROS 2 потрібно підписатися на тему uORB, йому потрібно використовувати сумісні налаштування QoS.
+PX4 QoS settings for publishers are incompatible with the default QoS settings for ROS 2 subscribers.
+So if ROS 2 code needs to subscribe to a uORB topic, it will need to use compatible QoS settings.
 One example of which is shown in [ROS 2 User Guide > ROS 2 Subscriber QoS Settings](../ros2/user_guide.md#ros-2-subscriber-qos-settings).
 
-PX4 використовує наступні параметри QoS для видавців:
+PX4 uses the following QoS settings for publishers:
 
 ```cpp
 uxrQoS_t qos = {
@@ -402,7 +400,7 @@ uxrQoS_t qos = {
 };
 ```
 
-PX4 використовує наступні параметри QoS для підписників:
+PX4 uses the following QoS settings for subscribers:
 
 ```cpp
 uxrQoS_t qos = {
@@ -413,17 +411,17 @@ uxrQoS_t qos = {
 };
 ```
 
-ROS 2 використовує наступні налаштування QoS (за замовчуванням) для видавців та підписок: «зберігати останніми» для історії з розміром черги 10, «reliable» для надійності, «volatile» для тривалості і «system default» для життєздатності.
-Дедлайн, тривалість життя та оренда також налаштовані на "за замовчуванням".
+ROS 2 uses the following QoS settings (by default) for publishers and subscriptions: "keep last" for history with a queue size of 10, "reliable" for reliability, "volatile" for durability, and "system default" for liveliness.
+Deadline, lifespan, and lease durations are also all set to "default".
 
 <!-- From https://github.com/PX4/PX4-user_guide/pull/2259#discussion_r1099788316 -->
 
-## DDS теми YAML
+## DDS Topics YAML
 
-The PX4 yaml file [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) defines the set of PX4 uORB topics that are built into firmware and published.
-Точніше, він повністю визначає взаємозв'язок/сполучення між повідомленнями PX4 uORB і ROS 2.
+The PX4 [dds_topics.yaml](../middleware/dds_topics.md) file defines the set of PX4 uORB topics that are built into firmware and published.
+More precisely, it completely defines the relationship/pairing between PX4 uORB and ROS 2 messages.
 
-Файл структурований наступним чином:
+The file is structured as follows:
 
 ```yaml
 publications:
@@ -471,7 +469,7 @@ Each (`topic`,`type`) pairs defines:
   It is identified by the last token in `topic:` that starts with `/` and does not contains any `/` in it.
   `vehicle_odometry`, `vehicle_status` and `offboard_control_mode` are examples of base names.
 3. The topic [namespace](https://design.ros2.org/articles/topic_and_service_names.html#namespaces).
-  За замовчуванням встановлено ​​на:
+  By default it is set to:
   - `/fmu/out/` for topics that are _published_ by PX4.
   - `/fmu/in/` for topics that are _subscribed_ by PX4.
 4. The message type (`VehicleOdometry`, `VehicleStatus`, `OffboardControlMode`, etc.) and the ROS 2 package (`px4_msgs`) that is expected to provide the message definition.
@@ -485,22 +483,22 @@ Add a topic to the `subscriptions` section to:
 
 - Create a unidirectional route going from the ROS2 topic to the _default_ instance (instance 0) of the associated uORB topic.
   For example, it creates a ROS2 subscriber of `/fmu/in/vehicle_odometry` and a uORB publisher of `vehicle_odometry`.
-- Якщо інші (внутрішні) модулі PX4 вже публікують у тому ж екземплярі теми uORB, що й публікатор ROS2, підписники цього екземпляра будуть отримувати всі потоки повідомлень.
-  Підписник uORB не зможе визначити, чи вхідне повідомлення було опубліковане PX4 або ROS2.
-- Це бажана поведінка, коли очікується, що ROS2-видавець буде єдиним видавцем у екземплярі теми (наприклад, для заміни внутрішнього видавця теми під час автономного керування), або коли джерело декількох потоків публікацій не має значення.
+- If other (internal) PX4 modules are already publishing on the same uORB topic instance as the ROS2 publisher, the instance's subscribers will receive all streams of messages.
+  The uORB subscriber will not be able to determine if an incoming message was published by PX4 or by ROS2.
+- This is the desired behavior when the ROS2 publisher is expected to be the sole publisher on the topic instance (for example, replacing an internal publisher to the topic during offboard control), or when the source of multiple publishing streams does not matter.
 
 Add a topic to the `subscriptions_multi` section to:
 
 - Create a unidirectional route going from the ROS2 topic to a _new_ instance of the associated uORB topic.
   For example, if `vehicle_odometry` has already `2` instances, it creates a ROS2 subscriber of `/fmu/in/vehicle_odometry` and a uORB publisher on instance `3` of `vehicle_odometry`.
-- Це гарантує, що жоден інший внутрішній модуль PX4 не публікуватиметься на тому самому екземплярі, що використовується uXRCE-DDS.
-  Підписники зможуть підписатися на потрібний екземпляр і розрізняти видавців.
-- Зауважте, однак, що це гарантує розділення між видавцями PX4 і ROS2, а не між кількома видавцями ROS2.
-  У цьому випадку їхні повідомлення все одно будуть перенаправлені на той самий екземпляр.
-- Це бажана поведінка, наприклад, коли ви хочете, щоб PX4 реєстрував показання двох однакових датчиків; вони обидва публікуватимуться в одній темі, але один з них використовуватиме екземпляр 0, а інший - екземпляр 1.
+- This ensures that no other internal PX4 module will publish on the same instance used by uXRCE-DDS.
+  The subscribers will be able to subscribe to the desired instance and distinguish between publishers.
+- Note, however, that this guarantees separation between PX4 and ROS2 publishers, not among multiple ROS2 publishers.
+  In that scenario, their messages will still be routed to the same instance.
+- This is the desired behavior, for example, when you want PX4 to log the readings of two equal sensors; they will both publish on the same topic, but one will use instance 0 and the other will use instance 1.
 
-Ви можете довільно змінювати конфігурацію.
-Наприклад, ви можете використовувати різні простори імен за замовчуванням або використовувати власний пакет для зберігання визначень повідомлень.
+You can arbitrarily change the configuration.
+For example, you could use different default namespaces or use a custom package to store the message definitions.
 
 ## DDS (ROS 2) Services
 
@@ -513,62 +511,62 @@ For example, the `/fmu/vehicle_command` service server defined in [`px4_msgs::sr
 
 For a list of services, details and examples see the [service documentation](../ros2/user_guide.md#px4-ros-2-service-servers) in the ROS 2 User Guide.
 
-## Посібник міграції з Fast-RTPS на uXRCE-DDS
+## Fast-RTPS to uXRCE-DDS Migration Guidelines
 
 These guidelines explain how to migrate from using PX4 v1.13 [Fast-RTPS](../middleware/micrortps.md) middleware to PX4 v1.14 `uXRCE-DDS` middleware.
 These are useful if you have [ROS 2 applications written for PX4 v1.13](https://docs.px4.io/v1.13/en/ros/ros2_comm.html), or you have used Fast-RTPS to interface your applications to PX4 [directly](https://docs.px4.io/v1.13/en/middleware/micrortps.html#agent-in-an-offboard-fast-dds-interface-ros-independent).
 
 :::info
 This section contains migration-specific information.
-Вам також слід прочитати решту цієї сторінки, щоб правильно зрозуміти uXRCE-DDS.
+You should also read the rest of this page to properly understand uXRCE-DDS.
 :::
 
-#### Залежності не потрібно видаляти
+#### Dependencies do not need to be removed
 
 uXRCE-DDS does not need the dependencies that were required for Fast-RTPS, such as those installed by following the topic [Fast DDS Installation](https://docs.px4.io/v1.13/en/dev_setup/fast-dds-installation.html).
-Ви можете зберегти їх, якщо хочете, не впливаючи на ваші додатки uXRCE-DDS.
+You can keep them if you want, without affecting your uXRCE-DDS applications.
 
-Якщо ви вирішили видалити залежності, будьте обережні, щоб не видалити нічого, що використовується програмами (наприклад, Java).
+If you do choose to remove the dependencies, take care not to remove anything that is used by applications (for example, Java).
 
 #### `_rtps` targets have been removed
 
 Anywhere you previously used a build target with extension `_rtps`, such as `px4_fmu-v5_rtps` or `px4_sitl_rtps`, you can now use the equivalent default target (for these cases `px4_fmu-v5_default` and `px4_sitl_default`).
 
 The make targets with extension `_rtps` were used to build firmware that included client side RTPS code.
-Проміжне програмне забезпечення uXRCE-DDS за замовчуванням включено до збірок для більшості плат, тому вам більше не потрібна спеціальна прошивка для роботи з ROS 2.
+The uXRCE-DDS middleware is included by default in builds for most boards, so you no longer need a special firmware to work with ROS 2.
 
 To check if your board has the middleware, look for `CONFIG_MODULES_UXRCE_DDS_CLIENT=y` in the `.px4board` file of your board.
 Those files are nested in [PX4-Autopilot/boards](https://github.com/PX4/PX4-Autopilot/tree/main/boards).
 
 If it is not present, or if it is set to `n`, then you have to clone the PX4 repo, modify the board configuration and manually [compile](../dev_setup/building_px4.md) the firmware.
 
-#### Новий модуль клієнта та нові параметри запуску
+#### New client module and new start parameters
 
-Оскільки клієнт реалізовано новим модулем PX4, тепер у вас є нові параметри для його запуску.
+As the client is implemented by a new PX4 module, you now have new parameters to start it.
 Take a look at the [client startup section](#starting-the-client) to learn how this is done.
 
-#### Новий файл для налаштування того, які теми публікуються
+#### New file for setting which topics are published
 
-The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
+The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topics.yaml](../middleware/dds_topics.md) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
 
 See [Supported uORB Messages](#supported-uorb-messages) and [DDS Topics YAML](#dds-topics-yaml) sections for more information.
 
-#### Теми більше не потрібно синхронізувати між клієнтом і агентом.
+#### Topics no longer need to be synced between client and agent.
 
 The list of bridged topics between agent and client no longer needs to be synced for ROS 2, so the `update_px4_ros2_bridge.sh` script is no longer needed.
 
-#### Налаштування назви теми за замовчуванням змінено
+#### Default topic naming convention has changed
 
-Змінився формат назв тем:
+The topic naming format changed:
 
 - Published topics: `/fmu/topic-name/out` (Fast-RTPS) to `/fmu/out/topic-name` (XRCE-DDS).
 - Subscribed topics: `/fmu/topic-name/in`(Fast-RTPS) to `/fmu/in/topic-name` (XRCE-DDS).
 
-Вам слід оновити свій додаток відповідно до нової конвенції.
+You should update your application to the new convention.
 
 :::info
 You might also edit [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) to revert to the old convention.
-Це не рекомендується, оскільки це означає, що вам доведеться завжди використовувати кастомну прошивку.
+This is not recommended because it means that you would have to always use custom firmware.
 :::
 
 #### XRCE-DDS-Agent
@@ -576,18 +574,18 @@ You might also edit [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/m
 The XRCE-DDS agent is "generic" and independent of PX4: [micro-xrce-dds-agent](https://micro-xrce-dds.docs.eprosima.com/en/latest/agent.html).
 There are many ways to install it on your PC / companion computer - for more information see the [dedicated section](#micro-xrce-dds-agent-installation).
 
-#### Зміни, що стосуються конкретних додатків
+#### Application-Specific Changes
 
 If you where not using ROS 2 alongside the agent ([Fast DDS Interface ROS-Independent](https://docs.px4.io/v1.13/en/middleware/micrortps.html#agent-in-an-offboard-fast-dds-interface-ros-independent)), then you need to migrate to [eProsima Fast DDS](https://fast-dds.docs.eprosima.com/en/latest/index.html).
 
 ROS 2 applications still need to compile alongside the PX4 messages, which you do by adding the [px4_msgs](https://github.com/PX4/px4_msgs) package to your workspace.
 You can remove the [px4_ros_com](https://github.com/PX4/px4_ros_com) package as it is no longer needed, other than for example code.
 
-У ваших вузлах ROS 2 вам знадобиться:
+In your ROS 2 nodes, you will need to:
 
 - Update the [QoS](#px4-ros-2-qos-settings) of your publishers and subscribers as PX4 does not use the ROS 2 default settings.
 - Change the names of your topics, unless you edited [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
-- Видаліть все, що стосується синхронізації часу, оскільки XRCE-DDS автоматично піклується про синхронізацію часу агента/клієнта.
+- Remove everything related to time synchronization, as XRCE-DDS automatically takes care of agent/client time synchronization.
 
   In C++ applications you can set the `timestamp` field of your messages like this:
 
@@ -601,7 +599,7 @@ You can remove the [px4_ros_com](https://github.com/PX4/px4_ros_com) package as
   msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
   ```
 
-## Корисні ресурси
+## Helpful Resources
 
 - [ROS 2 in PX4: Technical Details of a Seamless Transition to XRCE-DDS](https://www.youtube.com/watch?v=F5oelooT67E) - Pablo Garrido & Nuno Marques (youtube)
 - [PX4 ROS 2 offboard tutorial](https://gist.github.com/julianoes/adbf76408663829cd9aed8d14c88fa29) (Github gist - JulianOes)

docs/uk/ros2/user_guide.md

@@ -28,7 +28,7 @@ The agent acts as a proxy for the client to publish and subscribe to topics in t
 
 The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is generated at build time and included in PX4 firmware by default.
 It includes both the "generic" micro XRCE-DDS client code, and PX4-specific translation code that it uses to publish to/from uORB topics.
-The subset of uORB messages that are generated into the client are listed in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The subset of uORB messages that are generated into the client are specified in [dds_topics.yaml](../middleware/dds_topics.md).
 The generator uses the uORB message definitions in the source tree: [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to create the code for sending ROS 2 messages.
 
 ROS 2 applications need to be built in a workspace that has the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware.
@@ -97,48 +97,48 @@ To install ROS 2 and its dependencies:
 
 1. Встановлення ROS 2.
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
- To install ROS 2 "Humble" on Ubuntu 22.04:
+  ::: tab humble
+  To install ROS 2 "Humble" on Ubuntu 22.04:
 
- ```sh
- sudo apt update && sudo apt install locales
- sudo locale-gen en_US en_US.UTF-8
- sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
- export LANG=en_US.UTF-8
- sudo apt install software-properties-common
- sudo add-apt-repository universe
- sudo apt update && sudo apt install curl -y
- sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
- echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
- sudo apt update && sudo apt upgrade -y
- sudo apt install ros-humble-desktop
- sudo apt install ros-dev-tools
- source /opt/ros/humble/setup.bash && echo "source /opt/ros/humble/setup.bash" >> .bashrc
- ```
+  ```sh
+  sudo apt update && sudo apt install locales
+  sudo locale-gen en_US en_US.UTF-8
+  sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+  export LANG=en_US.UTF-8
+  sudo apt install software-properties-common
+  sudo add-apt-repository universe
+  sudo apt update && sudo apt install curl -y
+  sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+  echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+  sudo apt update && sudo apt upgrade -y
+  sudo apt install ros-humble-desktop
+  sudo apt install ros-dev-tools
+  source /opt/ros/humble/setup.bash && echo "source /opt/ros/humble/setup.bash" >> .bashrc
+  ```
 
- The instructions above are reproduced from the official installation guide: [Install ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
- You can install _either_ the desktop (`ros-humble-desktop`) _or_ bare-bones versions (`ros-humble-ros-base`), _and_ the development tools (`ros-dev-tools`).
+  The instructions above are reproduced from the official installation guide: [Install ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
+  You can install _either_ the desktop (`ros-humble-desktop`) _or_ bare-bones versions (`ros-humble-ros-base`), _and_ the development tools (`ros-dev-tools`).
 
 :::
 
- ::: tab foxy
- To install ROS 2 "Foxy" on Ubuntu 20.04:
+  ::: tab foxy
+  To install ROS 2 "Foxy" on Ubuntu 20.04:
 
- - Follow the official installation guide: [Install ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html).
+  - Follow the official installation guide: [Install ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html).
 
- You can install _either_ the desktop (`ros-foxy-desktop`) _or_ bare-bones versions (`ros-foxy-ros-base`), _and_ the development tools (`ros-dev-tools`).
+  You can install _either_ the desktop (`ros-foxy-desktop`) _or_ bare-bones versions (`ros-foxy-ros-base`), _and_ the development tools (`ros-dev-tools`).
 
 :::
 
- ::::
+  ::::
 
 2. Some Python dependencies must also be installed (using **`pip`** or **`apt`**):
 
- ```sh
- pip install --user -U empy==3.3.4 pyros-genmsg setuptools
- ```
+  ```sh
+  pip install --user -U empy==3.3.4 pyros-genmsg setuptools
+  ```
 
 ### Setup Micro XRCE-DDS Agent & Client
 
@@ -155,22 +155,22 @@ To setup and start the agent:
 
 2. Введіть наступні команди для витягування та побудови агента з вихідного коду:
 
- ```sh
- git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
- cd Micro-XRCE-DDS-Agent
- mkdir build
- cd build
- cmake ..
- make
- sudo make install
- sudo ldconfig /usr/local/lib/
- ```
+  ```sh
+  git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
+  cd Micro-XRCE-DDS-Agent
+  mkdir build
+  cd build
+  cmake ..
+  make
+  sudo make install
+  sudo ldconfig /usr/local/lib/
+  ```
 
 3. Запустіть агента з налаштуваннями для підключення до клієнта uXRCE-DDS, який працює на симуляторі:
 
- ```sh
- MicroXRCEAgent udp4 -p 8888
- ```
+  ```sh
+  MicroXRCEAgent udp4 -p 8888
+  ```
 
 The agent is now running, but you won't see much until we start PX4 (in the next step).
 
@@ -187,31 +187,31 @@ To start the simulator (and client):
 
 1. Open a new terminal in the root of the **PX4 Autopilot** repo that was installed above.
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- - Start a PX4 [Gazebo](../sim_gazebo_gz/index.md) simulation using:
+  - Start a PX4 [Gazebo](../sim_gazebo_gz/index.md) simulation using:
 
-  ```sh
-  make px4_sitl gz_x500
-  ```
+    ```sh
+    make px4_sitl gz_x500
+    ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- - Start a PX4 [Gazebo Classic](../sim_gazebo_classic/index.md) simulation using:
+  - Start a PX4 [Gazebo Classic](../sim_gazebo_classic/index.md) simulation using:
 
-  ```sh
-  make px4_sitl gazebo-classic
-  ```
+    ```sh
+    make px4_sitl gazebo-classic
+    ```
 
 
 :::
 
- ::::
+  ::::
 
 The agent and client are now running they should connect.
 
@@ -261,52 +261,52 @@ To create and build the workspace:
 
 2. Створіть новий каталог робочого простору та перейдіть до нього за допомогою:
 
- ```sh
- mkdir -p ~/ws_sensor_combined/src/
- cd ~/ws_sensor_combined/src/
- ```
+  ```sh
+  mkdir -p ~/ws_sensor_combined/src/
+  cd ~/ws_sensor_combined/src/
+  ```
 
- ::: info
- A naming convention for workspace folders can make it easier to manage workspaces.
+  ::: info
+  A naming convention for workspace folders can make it easier to manage workspaces.
 
 :::
 
 3. Clone the example repository and [px4_msgs](https://github.com/PX4/px4_msgs) to the `/src` directory (the `main` branch is cloned by default, which corresponds to the version of PX4 we are running):
 
- ```sh
- git clone https://github.com/PX4/px4_msgs.git
- git clone https://github.com/PX4/px4_ros_com.git
- ```
+  ```sh
+  git clone https://github.com/PX4/px4_msgs.git
+  git clone https://github.com/PX4/px4_ros_com.git
+  ```
 
 4. Source the ROS 2 development environment into the current terminal and compile the workspace using `colcon`:
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- cd ..
- source /opt/ros/humble/setup.bash
- colcon build
- ```
+  ```sh
+  cd ..
+  source /opt/ros/humble/setup.bash
+  colcon build
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- cd ..
- source /opt/ros/foxy/setup.bash
- colcon build
- ```
+  ```sh
+  cd ..
+  source /opt/ros/foxy/setup.bash
+  colcon build
+  ```
 
 
 :::
 
- ::::
+  ::::
 
- This builds all the folders under `/src` using the sourced toolchain.
+  This builds all the folders under `/src` using the sourced toolchain.
 
 #### Запуск прикладу
 
@@ -322,42 +322,42 @@ In a new terminal:
 
 1. Перейдіть на верхній рівень каталогу вашого робочого простору та джерело середовища ROS 2 (у цьому випадку "Humble"):
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- cd ~/ws_sensor_combined/
- source /opt/ros/humble/setup.bash
- ```
+  ```sh
+  cd ~/ws_sensor_combined/
+  source /opt/ros/humble/setup.bash
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- cd ~/ws_sensor_combined/
- source /opt/ros/foxy/setup.bash
- ```
+  ```sh
+  cd ~/ws_sensor_combined/
+  source /opt/ros/foxy/setup.bash
+  ```
 
 
 :::
 
- ::::
+  ::::
 
 2. Source the `local_setup.bash`.
 
- ```sh
- source install/local_setup.bash
- ```
+  ```sh
+  source install/local_setup.bash
+  ```
 
 3. Тепер запустіть приклад.
- Note here that we use `ros2 launch`, which is described below.
+  Note here that we use `ros2 launch`, which is described below.
 
- ```sh
- ros2 launch px4_ros_com sensor_combined_listener.launch.py
- ```
+  ```sh
+  ros2 launch px4_ros_com sensor_combined_listener.launch.py
+  ```
 
 If this is working you should see data being printed on the terminal/console where you launched the ROS listener:
 
@@ -385,18 +385,18 @@ If you were to use incompatible [message versions](../middleware/uorb.md#message
 
 1. Include the [Message Translation Node](../ros2/px4_ros2_msg_translation_node.md) into the example workspace or a separate workspace by running the following script:
 
- ```sh
- cd /path/to/ros_ws
- /path/to/PX4-Autopilot/Tools/copy_to_ros_ws.sh .
- ```
+  ```sh
+  cd /path/to/ros_ws
+  /path/to/PX4-Autopilot/Tools/copy_to_ros_ws.sh .
+  ```
 
 2. Build and run the translation node:
 
- ```sh
- colcon build
- source install/local_setup.bash
- ros2 run translation_node translation_node_bin
- ```
+  ```sh
+  colcon build
+  source install/local_setup.bash
+  ros2 run translation_node translation_node_bin
+  ```
 
 ## Керування Транспортним Засобом
 
@@ -405,7 +405,7 @@ To control applications, ROS 2 applications:
 - підписатися на (слухати) тематичні теми, опубліковані PX4
 - опублікувати у темах, які спонукають PX4 виконати певну дію.
 
-The topics that you can use are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
+The topics that you can use are defined in [dds_topics.yaml](../middleware/dds_topics.md), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
 For example, [VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md) can be used to get the vehicle global position, while [VehicleCommand](../msg_docs/VehicleCommand.md) can be used to command actions such as takeoff and land.
 
 The [ROS 2 Example applications](#ros-2-example-applications) examples below provide concrete examples of how to use these topics.
@@ -456,13 +456,13 @@ Therefore, ROS 2 nodes that want to interface with PX4 must take care of the fra
 
 - Для повороту вектора з ENU на NED потрібно виконати дві основні обертання:
 
- - first a pi/2 rotation around the `Z`-axis (up),
- - then a pi rotation around the `X`-axis (old East/new North).
+  - first a pi/2 rotation around the `Z`-axis (up),
+  - then a pi rotation around the `X`-axis (old East/new North).
 
 - To rotate a vector from NED to ENU two basic rotations must be performed:
 
 - - first a pi/2 rotation around the `Z`-axis (down),
- - then a pi rotation around the `X`-axis (old North/new East). Note that the two resulting operations are mathematically equivalent.
+  - then a pi rotation around the `X`-axis (old North/new East). Note that the two resulting operations are mathematically equivalent.
 
 - To rotate a vector from FLU to FRD a pi rotation around the `X`-axis (front) is sufficient.
 
@@ -720,17 +720,17 @@ Note that all the messages from PX4 source code are present in the repository, b
 
 - If you're using a main or release version of PX4 you can get the message definitions by cloning the interface package [PX4/px4_msgs](https://github.com/PX4/px4_msgs) into your workspace.
 - Якщо ви створюєте або змінюєте повідомлення uORB, вам потрібно вручну оновити повідомлення у вашому робочому просторі з вихідного дерева PX4.
- Generally this means that you would update [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), clone the interface package, and then manually synchronize it by copying the new/modified message definitions from [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to its `msg` folders.
- Assuming that PX4-Autopilot is in your home directory `~`, while `px4_msgs` is in `~/ros2_ws/src/`, then the command might be:
+  Generally this means that you would update [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), clone the interface package, and then manually synchronize it by copying the new/modified message definitions from [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to its `msg` folders.
+  Assuming that PX4-Autopilot is in your home directory `~`, while `px4_msgs` is in `~/ros2_ws/src/`, then the command might be:
 
- ```sh
- rm ~/ros2_ws/src/px4_msgs/msg/*.msg
- cp ~/PX4-Autopilot/mgs/*.msg ~/ros2_ws/src/px4_msgs/msg/
- ```
+  ```sh
+  rm ~/ros2_ws/src/px4_msgs/msg/*.msg
+  cp ~/PX4-Autopilot/mgs/*.msg ~/ros2_ws/src/px4_msgs/msg/
+  ```
 
- ::: info
- Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
- For more information see [uXRCE-DDS > DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml).
+  ::: info
+  Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
+  For more information see [uXRCE-DDS > DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml).
 
 :::
 
@@ -739,11 +739,11 @@ Note that all the messages from PX4 source code are present in the repository, b
 Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) or at runtime (useful for multi vehicle operations):
 
 - One possibility is to use the `-n` option when starting the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) from command line.
- Ця техніка може бути використана як у симуляторах, так і на реальних транспортних засобах.
+  Ця техніка може бути використана як у симуляторах, так і на реальних транспортних засобах.
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 :::info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#px4-ros-2-service-servers).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#px4-ros-2-service-servers).
 Отже, команди, подібні до:
 
 ```sh
@@ -780,7 +780,7 @@ The service servers that are built into the PX4 [uxrce_dds_client](../modules/mo
 
 - `/fmu/vehicle_command` (definition: [`px4_msgs::srv::VehicleCommand`](https://github.com/PX4/px4_msgs/blob/main/srv/VehicleCommand.srv).)
 
- This service can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.
+  This service can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.
 
 All PX4 service names follow the convention `{extra_namespace}/fmu/{server_specific_name}` where `{extra_namespace}` is the same [custom namespace](#customizing-the-namespace) that can be given to the PX4 topics.
 
@@ -973,36 +973,36 @@ For information about launch files see [ROS 2 Tutorials > Creating launch files]
 Якщо щось відсутнє, його можна додати окремо:
 
 - **`colcon`** build tools should be in the development tools.
- Можна встановити за допомогою:
+  Можна встановити за допомогою:
 
- ```sh
- sudo apt install python3-colcon-common-extensions
- ```
+  ```sh
+  sudo apt install python3-colcon-common-extensions
+  ```
 
 - Бібліотеку Eigen3, яку використовує бібліотека трансформацій, повинно бути в обох пакунків: desktop та base.
- Воно повинно бути встановлено, як показано:
+  Воно повинно бути встановлено, як показано:
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- sudo apt install ros-humble-eigen3-cmake-module
- ```
+  ```sh
+  sudo apt install ros-humble-eigen3-cmake-module
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- sudo apt install ros-foxy-eigen3-cmake-module
- ```
+  ```sh
+  sudo apt install ros-foxy-eigen3-cmake-module
+  ```
 
 
 :::
 
- ::::
+  ::::
 
 ### ros_gz_bridge not publishing on the \clock topic
 

docs/zh/SUMMARY.md

@@ -733,6 +733,7 @@
       - [Protocols/Microservices](mavlink/protocols.md)
         - [Standard Modes Protocol](mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](middleware/uxrce_dds.md)
+      - [UORB Bridged to ROS 2](middleware/dds_topics.md)
   - [模块 & 命令](modules/modules_main.md)
     - [自动调参](modules/modules_autotune.md)
     - [命令](modules/modules_command.md)

docs/zh/_sidebar.md

@@ -460,6 +460,7 @@
       - [Vehicles](/sim_gazebo_gz/vehicles.md)
         - [Advanced Lift Drag Tool](/sim_gazebo_gz/tools_avl_automation.md)
       - [Worlds](/sim_gazebo_gz/worlds.md)
+      - [Plugins](/sim_gazebo_gz/plugins.md)
       - [Gazebo Models Repository](/sim_gazebo_gz/gazebo_models.md)
       - [Multi-Vehicle Sim](/sim_gazebo_gz/multi_vehicle_simulation.md)
     - [Gazebo Classic Simulation](/sim_gazebo_classic/index.md)
@@ -492,12 +493,17 @@
       - [Versioned](/msg_docs/versioned_messages.md)
         - [ActuatorMotors](/msg_docs/ActuatorMotors.md)
         - [ActuatorServos](/msg_docs/ActuatorServos.md)
+        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [ArmingCheckReply](/msg_docs/ArmingCheckReply.md)
         - [ArmingCheckRequest](/msg_docs/ArmingCheckRequest.md)
         - [BatteryStatus](/msg_docs/BatteryStatus.md)
         - [ConfigOverrides](/msg_docs/ConfigOverrides.md)
+        - [FixedWingLateralSetpoint](/msg_docs/FixedWingLateralSetpoint.md)
+        - [FixedWingLongitudinalSetpoint](/msg_docs/FixedWingLongitudinalSetpoint.md)
         - [GotoSetpoint](/msg_docs/GotoSetpoint.md)
         - [HomePosition](/msg_docs/HomePosition.md)
+        - [LateralControlConfiguration](/msg_docs/LateralControlConfiguration.md)
+        - [LongitudinalControlConfiguration](/msg_docs/LongitudinalControlConfiguration.md)
         - [ManualControlSetpoint](/msg_docs/ManualControlSetpoint.md)
         - [ModeCompleted](/msg_docs/ModeCompleted.md)
         - [RegisterExtComponentReply](/msg_docs/RegisterExtComponentReply.md)
@@ -526,10 +532,8 @@
         - [ActuatorTest](/msg_docs/ActuatorTest.md)
         - [AdcReport](/msg_docs/AdcReport.md)
         - [Airspeed](/msg_docs/Airspeed.md)
-        - [AirspeedValidated](/msg_docs/AirspeedValidated.md)
         - [AirspeedWind](/msg_docs/AirspeedWind.md)
         - [AutotuneAttitudeControlStatus](/msg_docs/AutotuneAttitudeControlStatus.md)
-        - [Buffer128](/msg_docs/Buffer128.md)
         - [ButtonEvent](/msg_docs/ButtonEvent.md)
         - [CameraCapture](/msg_docs/CameraCapture.md)
         - [CameraStatus](/msg_docs/CameraStatus.md)
@@ -568,6 +572,9 @@
         - [FailsafeFlags](/msg_docs/FailsafeFlags.md)
         - [FailureDetectorStatus](/msg_docs/FailureDetectorStatus.md)
         - [FigureEightStatus](/msg_docs/FigureEightStatus.md)
+        - [FixedWingLateralGuidanceStatus](/msg_docs/FixedWingLateralGuidanceStatus.md)
+        - [FixedWingLateralStatus](/msg_docs/FixedWingLateralStatus.md)
+        - [FixedWingRunwayControl](/msg_docs/FixedWingRunwayControl.md)
         - [FlightPhaseEstimation](/msg_docs/FlightPhaseEstimation.md)
         - [FollowTarget](/msg_docs/FollowTarget.md)
         - [FollowTargetEstimator](/msg_docs/FollowTargetEstimator.md)
@@ -620,7 +627,6 @@
         - [NavigatorMissionItem](/msg_docs/NavigatorMissionItem.md)
         - [NavigatorStatus](/msg_docs/NavigatorStatus.md)
         - [NormalizedUnsignedSetpoint](/msg_docs/NormalizedUnsignedSetpoint.md)
-        - [NpfgStatus](/msg_docs/NpfgStatus.md)
         - [ObstacleDistance](/msg_docs/ObstacleDistance.md)
         - [OffboardControlMode](/msg_docs/OffboardControlMode.md)
         - [OnboardComputerStatus](/msg_docs/OnboardComputerStatus.md)
@@ -656,13 +662,12 @@
         - [RcParameterMap](/msg_docs/RcParameterMap.md)
         - [RoverAttitudeSetpoint](/msg_docs/RoverAttitudeSetpoint.md)
         - [RoverAttitudeStatus](/msg_docs/RoverAttitudeStatus.md)
-        - [RoverMecanumGuidanceStatus](/msg_docs/RoverMecanumGuidanceStatus.md)
-        - [RoverMecanumSetpoint](/msg_docs/RoverMecanumSetpoint.md)
-        - [RoverMecanumStatus](/msg_docs/RoverMecanumStatus.md)
+        - [RoverPositionSetpoint](/msg_docs/RoverPositionSetpoint.md)
         - [RoverRateSetpoint](/msg_docs/RoverRateSetpoint.md)
         - [RoverRateStatus](/msg_docs/RoverRateStatus.md)
         - [RoverSteeringSetpoint](/msg_docs/RoverSteeringSetpoint.md)
         - [RoverThrottleSetpoint](/msg_docs/RoverThrottleSetpoint.md)
+        - [RoverVelocitySetpoint](/msg_docs/RoverVelocitySetpoint.md)
         - [RoverVelocityStatus](/msg_docs/RoverVelocityStatus.md)
         - [Rpm](/msg_docs/Rpm.md)
         - [RtlStatus](/msg_docs/RtlStatus.md)
@@ -694,6 +699,7 @@
         - [TelemetryStatus](/msg_docs/TelemetryStatus.md)
         - [TiltrotorExtraControls](/msg_docs/TiltrotorExtraControls.md)
         - [TimesyncStatus](/msg_docs/TimesyncStatus.md)
+        - [TrajectorySetpoint6dof](/msg_docs/TrajectorySetpoint6dof.md)
         - [TransponderReport](/msg_docs/TransponderReport.md)
         - [TuneControl](/msg_docs/TuneControl.md)
         - [UavcanParameterRequest](/msg_docs/UavcanParameterRequest.md)
@@ -717,13 +723,16 @@
         - [WheelEncoders](/msg_docs/WheelEncoders.md)
         - [Wind](/msg_docs/Wind.md)
         - [YawEstimatorStatus](/msg_docs/YawEstimatorStatus.md)
+        - [AirspeedValidatedV0](/msg_docs/AirspeedValidatedV0.md)
+        - [VehicleAttitudeSetpointV0](/msg_docs/VehicleAttitudeSetpointV0.md)
         - [VehicleStatusV0](/msg_docs/VehicleStatusV0.md)
-    - [MAVLink通讯](/middleware/mavlink.md)
+    - [MAVLink Messaging](/mavlink/index.md)
       - [Adding Messages](/mavlink/adding_messages.md)
       - [Streaming Messages](/mavlink/streaming_messages.md)
       - [Receiving Messages](/mavlink/receiving_messages.md)
       - [Custom MAVLink Messages](/mavlink/custom_messages.md)
-      - [Standard Modes Protocol](/mavlink/standard_modes.md)
+      - [Protocols/Microservices](/mavlink/protocols.md)
+        - [Standard Modes Protocol](/mavlink/standard_modes.md)
     - [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](/middleware/uxrce_dds.md)
   - [模块 & 命令](/modules/modules_main.md)
     - [自动调参](/modules/modules_autotune.md)
@@ -740,6 +749,7 @@
       - [磁罗盘](/modules/modules_driver_magnetometer.md)
       - [光流](/modules/modules_driver_optical_flow.md)
       - [Rpm Sensor](/modules/modules_driver_rpm_sensor.md)
+      - [Radio Control](/modules/modules_driver_radio_control.md)
       - [Transponder](/modules/modules_driver_transponder.md)
     - [估计器](/modules/modules_estimator.md)
     - [仿真](/modules/modules_simulation.md)
@@ -852,6 +862,7 @@
 
 - [版本发布](/releases/index.md)
   - [main (alpha)](/releases/main.md)
+  - [1.16 (release candidate)](/releases/1.16.md)
   - [1.15 (stable)](/releases/1.15.md)
   - [1.14](/releases/1.14.md)
   - [1.13](/releases/1.13.md)

docs/zh/computer_vision/collision_prevention.md

@@ -47,7 +47,7 @@ PX4 v1.14 (and later) supports the [LightWare LiDAR SF45](../sensor/sf45_rotatin
 
 - Attach and configure the distance sensor on a particular port (see [sensor-specific docs](../sensor/rangefinders.md)) and enable collision prevention using [CP_DIST](#CP_DIST).
 - 修改驱动程序以设置方向。
- This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
+  This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
 - Modify the driver to set the _field of view_ in the distance sensor UORB topic (`distance_sensor_s.h_fov`).
 
 ## PX4 (Software) Setup
@@ -203,85 +203,85 @@ The Lua script works by extracting the `obstacle_distance_fused` data at each ti
 
 2. Configure PX4 to publish obstacle distance data (so that it is available to PlotJuggler):
 
- Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
+  Add the [`obstacle_distance_fused`](../msg_docs/ObstacleDistance.md) UORB topic to your [`dds_topics.yaml`](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) so that it is published by PX4:
 
- ```sh
- - topic: /fmu/out/obstacle_distance_fused
-   type: px4_msgs::msg::ObstacleDistance
- ```
+  ```sh
+  - topic: /fmu/out/obstacle_distance_fused
+    type: px4_msgs::msg::ObstacleDistance
+  ```
 
- For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in _uXRCE-DDS (PX4-ROS 2/DDS Bridge)_.
+  For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
 
 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
- In the **Script Editor** tab, add following scripts in the appropriate sections:
+  In the **Script Editor** tab, add following scripts in the appropriate sections:
 
- - **Global code, executed once:**
+  - **Global code, executed once:**
 
-  ```lua
-  obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
-  obs_dist_min = Timeseries.new("obstacle_distance_minimum")
-  ```
+    ```lua
+    obs_dist_fused_xy = ScatterXY.new("obstacle_distance_fused_xy")
+    obs_dist_min = Timeseries.new("obstacle_distance_minimum")
+    ```
 
- - **function(tracker_time)**
+  - **function(tracker_time)**
 
-  ```lua
-  obs_dist_fused_xy:clear()
+    ```lua
+    obs_dist_fused_xy:clear()
 
-  i = 0
-  angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
-  increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
-  min_dist = 65535
+    i = 0
+    angle_offset = TimeseriesView.find("/fmu/out/obstacle_distance_fused/angle_offset")
+    increment = TimeseriesView.find("/fmu/out/obstacle_distance_fused/increment")
+    min_dist = 65535
 
-  -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
-  local angle_offset_value = angle_offset:atTime(tracker_time)
-  local increment_value = increment:atTime(tracker_time)
+    -- Cache increment and angle_offset values at tracker_time to avoid repeated calls
+    local angle_offset_value = angle_offset:atTime(tracker_time)
+    local increment_value = increment:atTime(tracker_time)
 
-  if increment_value == nil or increment_value <= 0 then
-      print("Invalid increment value: " .. tostring(increment_value))
-      return
-  end
+    if increment_value == nil or increment_value <= 0 then
+        print("Invalid increment value: " .. tostring(increment_value))
+        return
+    end
 
-  local max_steps = math.floor(360 / increment_value)
+    local max_steps = math.floor(360 / increment_value)
 
-  while i < max_steps do
-      local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
-      local distance = TimeseriesView.find(str)
-      if distance == nil then
-          print("No distance data for: " .. str)
-          break
-      end
+    while i < max_steps do
+        local str = string.format("/fmu/out/obstacle_distance_fused/distances[%d]", i)
+        local distance = TimeseriesView.find(str)
+        if distance == nil then
+            print("No distance data for: " .. str)
+            break
+        end
 
-      local dist = distance:atTime(tracker_time)
-      if dist ~= nil and dist < 65535 then
-          -- Calculate angle and Cartesian coordinates
-          local angle = angle_offset_value + i * increment_value
-          local y = dist * math.cos(math.rad(angle))
-          local x = dist * math.sin(math.rad(angle))
+        local dist = distance:atTime(tracker_time)
+        if dist ~= nil and dist < 65535 then
+            -- Calculate angle and Cartesian coordinates
+            local angle = angle_offset_value + i * increment_value
+            local y = dist * math.cos(math.rad(angle))
+            local x = dist * math.sin(math.rad(angle))
 
-          obs_dist_fused_xy:push_back(x, y)
+            obs_dist_fused_xy:push_back(x, y)
 
-          -- Update minimum distance
-          if dist < min_dist then
-              min_dist = dist
-          end
-      end
+            -- Update minimum distance
+            if dist < min_dist then
+                min_dist = dist
+            end
+        end
 
-      i = i + 1
-  end
+        i = i + 1
+    end
 
-  -- Push minimum distance once after the loop
-  if min_dist < 65535 then
-      obs_dist_min:push_back(tracker_time, min_dist)
-  else
-      print("No valid minimum distance found")
-  end
-  ```
+    -- Push minimum distance once after the loop
+    if min_dist < 65535 then
+        obs_dist_min:push_back(tracker_time, min_dist)
+    else
+        print("No valid minimum distance found")
+    end
+    ```
 
 4. Enter a name for the script on the top right, and press **Save**.
- Once saved, the script should appear in the _Active Scripts_ section.
+  Once saved, the script should appear in the _Active Scripts_ section.
 
 5. Start streaming the data using the approach described in [Plotting uORB Topic Data in Real Time using PlotJuggler](../debug/plotting_realtime_uorb_data.md).
- You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
+  You should see the `obstacle_distance_fused_xy` and `obstacle_distance_minimum` timeseries on the left.
 
 Note that you have to press **Save** again to re-enable the scripts after loading a new log file or otherwise clearing data.
 

docs/zh/debug/plotting_realtime_uorb_data.md

@@ -96,13 +96,13 @@ Exporting the messages allows ROS 2 and the uXRCE-DDS agent to be independent of
 
 While `px4_msgs` has messages for all uORB topics in PX4, not all messages in `px4_msgs` are available to ROS 2/PlotJuggler by default.
 The set that are available must be built into the client running on PX4.
-These are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+These are defined in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The instructions below explain the changes needed to monitor topics that are not available by default.
 
 ### Missing Topics
 
-If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) to include the topic and rebuild PX4.
+If a normal uORB topic is not available in PlotJuggler you will need to modify the [dds_topics.yaml](../middleware/dds_topics.md) ([source](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) to include the topic and rebuild PX4.
 
 If working with real hardware you will need to build and [install](../config/firmware.md#installing-px4-main-beta-or-custom-firmware) custom firmware after changing the YAML file.
 
@@ -122,7 +122,7 @@ cd ~/ros2_ws/ && colcon build
 
 ### Custom Topics
 
-After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and export the new message into your ROS 2 workspace.
+After defining the topic, follow the instructions above to add the topic to [dds_topics.yaml](../middleware/dds_topics.md), and export the new message into your ROS 2 workspace.
 
 ## See also
 

docs/zh/debug/system_wide_replay.md

@@ -128,8 +128,7 @@ In EKF2 mode, the replay will automatically create the ORB publisher rules descr
 
 To perform an EKF2 replay:
 
-- Record the original log.
-  Optionally set `SDLOG_MODE` to `1` to log from boot.
+- Record the original log with `SDLOG_MODE` set to `1` to log from boot.
 
 - In addition to the `replay` environment variable, set `replay_mode` to `ekf2`:
 

docs/zh/middleware/dds_topics.md

@@ -0,0 +1,277 @@
+# dds_topics.yaml — PX4 Topics Exposed to ROS 2
+
+:::info
+This document is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code.
+:::
+
+The [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) file specifies which uORB message definitions are compiled into the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) module when [PX4 is built](../middleware/uxrce_dds.md#code-generation), and hence which topics are available for ROS 2 applications to subscribe or publish (by default).
+
+This document shows a markdown-rendered version of [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), listing the publications, subscriptions, and so on.
+
+## Publications
+
+| Topic                                   | 类型                                                                                                                                                                        | Rate Limit           |
+| --------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------- |
+| `/fmu/out/register_ext_component_reply` | [px4_msgs::msg::RegisterExtComponentReply](../msg_docs/RegisterExtComponentReply.md) |                      |
+| `/fmu/out/arming_check_request`         | [px4_msgs::msg::ArmingCheckRequest](../msg_docs/ArmingCheckRequest.md)               | 5.0  |
+| `/fmu/out/mode_completed`               | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md)                         | 50.0 |
+| `/fmu/out/battery_status`               | [px4_msgs::msg::BatteryStatus](../msg_docs/BatteryStatus.md)                         | 1.0  |
+| `/fmu/out/collision_constraints`        | [px4_msgs::msg::CollisionConstraints](../msg_docs/CollisionConstraints.md)           | 50.0 |
+| `/fmu/out/estimator_status_flags`       | [px4_msgs::msg::EstimatorStatusFlags](../msg_docs/EstimatorStatusFlags.md)           | 5.0  |
+| `/fmu/out/failsafe_flags`               | [px4_msgs::msg::FailsafeFlags](../msg_docs/FailsafeFlags.md)                         | 5.0  |
+| `/fmu/out/manual_control_setpoint`      | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md)         | 25.0 |
+| `/fmu/out/message_format_response`      | [px4_msgs::msg::MessageFormatResponse](../msg_docs/MessageFormatResponse.md)         |                      |
+| `/fmu/out/position_setpoint_triplet`    | [px4_msgs::msg::PositionSetpointTriplet](../msg_docs/PositionSetpointTriplet.md)     | 5.0  |
+| `/fmu/out/sensor_combined`              | [px4_msgs::msg::SensorCombined](../msg_docs/SensorCombined.md)                       |                      |
+| `/fmu/out/timesync_status`              | [px4_msgs::msg::TimesyncStatus](../msg_docs/TimesyncStatus.md)                       | 10.0 |
+| `/fmu/out/vehicle_land_detected`        | [px4_msgs::msg::VehicleLandDetected](../msg_docs/VehicleLandDetected.md)             | 5.0  |
+| `/fmu/out/vehicle_attitude`             | [px4_msgs::msg::VehicleAttitude](../msg_docs/VehicleAttitude.md)                     |                      |
+| `/fmu/out/vehicle_control_mode`         | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md)               | 50.0 |
+| `/fmu/out/vehicle_command_ack`          | [px4_msgs::msg::VehicleCommandAck](../msg_docs/VehicleCommandAck.md)                 |                      |
+| `/fmu/out/vehicle_global_position`      | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md)         | 50.0 |
+| `/fmu/out/vehicle_gps_position`         | [px4_msgs::msg::SensorGps](../msg_docs/SensorGps.md)                                 | 50.0 |
+| `/fmu/out/vehicle_local_position`       | [px4_msgs::msg::VehicleLocalPosition](../msg_docs/VehicleLocalPosition.md)           | 50.0 |
+| `/fmu/out/vehicle_odometry`             | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                     |                      |
+| `/fmu/out/vehicle_status`               | [px4_msgs::msg::VehicleStatus](../msg_docs/VehicleStatus.md)                         | 5.0  |
+| `/fmu/out/airspeed_validated`           | [px4_msgs::msg::AirspeedValidated](../msg_docs/AirspeedValidated.md)                 | 50.0 |
+| `/fmu/out/vtol_vehicle_status`          | [px4_msgs::msg::VtolVehicleStatus](../msg_docs/VtolVehicleStatus.md)                 |                      |
+| `/fmu/out/home_position`                | [px4_msgs::msg::HomePosition](../msg_docs/HomePosition.md)                           | 5.0  |
+
+## Subscriptions
+
+| Topic                                                                                                   | 类型                                                                                                                                                                                      |
+| ------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| /fmu/in/register_ext_component_request   | [px4_msgs::msg::RegisterExtComponentRequest](../msg_docs/RegisterExtComponentRequest.md)           |
+| /fmu/in/unregister_ext_component                              | [px4_msgs::msg::UnregisterExtComponent](../msg_docs/UnregisterExtComponent.md)                     |
+| /fmu/in/config_overrides_request                              | [px4_msgs::msg::ConfigOverrides](../msg_docs/ConfigOverrides.md)                                   |
+| /fmu/in/arming_check_reply                                    | [px4_msgs::msg::ArmingCheckReply](../msg_docs/ArmingCheckReply.md)                                 |
+| /fmu/in/message_format_request                                | [px4_msgs::msg::MessageFormatRequest](../msg_docs/MessageFormatRequest.md)                         |
+| /fmu/in/mode_completed                                                             | [px4_msgs::msg::ModeCompleted](../msg_docs/ModeCompleted.md)                                       |
+| /fmu/in/config_control_setpoints                              | [px4_msgs::msg::VehicleControlMode](../msg_docs/VehicleControlMode.md)                             |
+| /fmu/in/distance_sensor                                                            | [px4_msgs::msg::DistanceSensor](../msg_docs/DistanceSensor.md)                                     |
+| /fmu/in/manual_control_input                                  | [px4_msgs::msg::ManualControlSetpoint](../msg_docs/ManualControlSetpoint.md)                       |
+| /fmu/in/offboard_control_mode                                 | [px4_msgs::msg::OffboardControlMode](../msg_docs/OffboardControlMode.md)                           |
+| /fmu/in/onboard_computer_status                               | [px4_msgs::msg::OnboardComputerStatus](../msg_docs/OnboardComputerStatus.md)                       |
+| /fmu/in/obstacle_distance                                                          | [px4_msgs::msg::ObstacleDistance](../msg_docs/ObstacleDistance.md)                                 |
+| /fmu/in/sensor_optical_flow                                   | [px4_msgs::msg::SensorOpticalFlow](../msg_docs/SensorOpticalFlow.md)                               |
+| /fmu/in/goto_setpoint                                                              | [px4_msgs::msg::GotoSetpoint](../msg_docs/GotoSetpoint.md)                                         |
+| /fmu/in/telemetry_status                                                           | [px4_msgs::msg::TelemetryStatus](../msg_docs/TelemetryStatus.md)                                   |
+| /fmu/in/trajectory_setpoint                                                        | [px4_msgs::msg::TrajectorySetpoint](../msg_docs/TrajectorySetpoint.md)                             |
+| /fmu/in/vehicle_attitude_setpoint                             | [px4_msgs::msg::VehicleAttitudeSetpoint](../msg_docs/VehicleAttitudeSetpoint.md)                   |
+| /fmu/in/vehicle_mocap_odometry                                | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                                   |
+| /fmu/in/vehicle_rates_setpoint                                | [px4_msgs::msg::VehicleRatesSetpoint](../msg_docs/VehicleRatesSetpoint.md)                         |
+| /fmu/in/vehicle_visual_odometry                               | [px4_msgs::msg::VehicleOdometry](../msg_docs/VehicleOdometry.md)                                   |
+| /fmu/in/vehicle_command                                                            | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)                                     |
+| /fmu/in/vehicle_command_mode_executor    | [px4_msgs::msg::VehicleCommand](../msg_docs/VehicleCommand.md)                                     |
+| /fmu/in/vehicle_thrust_setpoint                               | [px4_msgs::msg::VehicleThrustSetpoint](../msg_docs/VehicleThrustSetpoint.md)                       |
+| /fmu/in/vehicle_torque_setpoint                               | [px4_msgs::msg::VehicleTorqueSetpoint](../msg_docs/VehicleTorqueSetpoint.md)                       |
+| /fmu/in/actuator_motors                                                            | [px4_msgs::msg::ActuatorMotors](../msg_docs/ActuatorMotors.md)                                     |
+| /fmu/in/actuator_servos                                                            | [px4_msgs::msg::ActuatorServos](../msg_docs/ActuatorServos.md)                                     |
+| /fmu/in/aux_global_position                                   | [px4_msgs::msg::VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md)                       |
+| /fmu/in/fixed_wing_longitudinal_setpoint | [px4_msgs::msg::FixedWingLongitudinalSetpoint](../msg_docs/FixedWingLongitudinalSetpoint.md)       |
+| /fmu/in/fixed_wing_lateral_setpoint      | [px4_msgs::msg::FixedWingLateralSetpoint](../msg_docs/FixedWingLateralSetpoint.md)                 |
+| /fmu/in/longitudinal_control_configuration                    | [px4_msgs::msg::LongitudinalControlConfiguration](../msg_docs/LongitudinalControlConfiguration.md) |
+| /fmu/in/lateral_control_configuration                         | [px4_msgs::msg::LateralControlConfiguration](../msg_docs/LateralControlConfiguration.md)           |
+
+## Subscriptions Multi
+
+None
+
+## Not Exported
+
+These messages are not listed in the yaml file.
+They are not build into the module, and hence are neither published or subscribed.
+
+:::details
+See messages
+
+- [SensorCorrection](../msg_docs/SensorCorrection.md)
+- [ActuatorOutputs](../msg_docs/ActuatorOutputs.md)
+- [FixedWingRunwayControl](../msg_docs/FixedWingRunwayControl.md)
+- [EstimatorInnovations](../msg_docs/EstimatorInnovations.md)
+- [FlightPhaseEstimation](../msg_docs/FlightPhaseEstimation.md)
+- [PurePursuitStatus](../msg_docs/PurePursuitStatus.md)
+- [Px4ioStatus](../msg_docs/Px4ioStatus.md)
+- [SatelliteInfo](../msg_docs/SatelliteInfo.md)
+- [GeofenceResult](../msg_docs/GeofenceResult.md)
+- [GimbalManagerStatus](../msg_docs/GimbalManagerStatus.md)
+- [ManualControlSwitches](../msg_docs/ManualControlSwitches.md)
+- [OpenDroneIdSelfId](../msg_docs/OpenDroneIdSelfId.md)
+- [OpenDroneIdSystem](../msg_docs/OpenDroneIdSystem.md)
+- [EventV0](../msg_docs/EventV0.md)
+- [QshellRetval](../msg_docs/QshellRetval.md)
+- [RoverThrottleSetpoint](../msg_docs/RoverThrottleSetpoint.md)
+- [AirspeedValidatedV0](../msg_docs/AirspeedValidatedV0.md)
+- [RcChannels](../msg_docs/RcChannels.md)
+- [SensorAccel](../msg_docs/SensorAccel.md)
+- [GimbalDeviceAttitudeStatus](../msg_docs/GimbalDeviceAttitudeStatus.md)
+- [EscStatus](../msg_docs/EscStatus.md)
+- [RoverAttitudeSetpoint](../msg_docs/RoverAttitudeSetpoint.md)
+- [RateCtrlStatus](../msg_docs/RateCtrlStatus.md)
+- [AirspeedWind](../msg_docs/AirspeedWind.md)
+- [InputRc](../msg_docs/InputRc.md)
+- [GpioIn](../msg_docs/GpioIn.md)
+- [LaunchDetectionStatus](../msg_docs/LaunchDetectionStatus.md)
+- [VehicleImu](../msg_docs/VehicleImu.md)
+- [Event](../msg_docs/Event.md)
+- [SensorUwb](../msg_docs/SensorUwb.md)
+- [ActuatorServosTrim](../msg_docs/ActuatorServosTrim.md)
+- [DatamanResponse](../msg_docs/DatamanResponse.md)
+- [OrbTest](../msg_docs/OrbTest.md)
+- [VehicleLocalPositionSetpoint](../msg_docs/VehicleLocalPositionSetpoint.md)
+- [VehicleAngularVelocity](../msg_docs/VehicleAngularVelocity.md)
+- [FollowTargetStatus](../msg_docs/FollowTargetStatus.md)
+- [NormalizedUnsignedSetpoint](../msg_docs/NormalizedUnsignedSetpoint.md)
+- [YawEstimatorStatus](../msg_docs/YawEstimatorStatus.md)
+- [TakeoffStatus](../msg_docs/TakeoffStatus.md)
+- [UlogStreamAck](../msg_docs/UlogStreamAck.md)
+- [OrbTestLarge](../msg_docs/OrbTestLarge.md)
+- [RoverSteeringSetpoint](../msg_docs/RoverSteeringSetpoint.md)
+- [CameraCapture](../msg_docs/CameraCapture.md)
+- [VehicleRoi](../msg_docs/VehicleRoi.md)
+- [ActuatorArmed](../msg_docs/ActuatorArmed.md)
+- [FixedWingLateralGuidanceStatus](../msg_docs/FixedWingLateralGuidanceStatus.md)
+- [ParameterSetValueResponse](../msg_docs/ParameterSetValueResponse.md)
+- [GeofenceStatus](../msg_docs/GeofenceStatus.md)
+- [VehicleAngularAccelerationSetpoint](../msg_docs/VehicleAngularAccelerationSetpoint.md)
+- [SensorGnssRelative](../msg_docs/SensorGnssRelative.md)
+- [PowerMonitor](../msg_docs/PowerMonitor.md)
+- [RoverVelocityStatus](../msg_docs/RoverVelocityStatus.md)
+- [ParameterResetRequest](../msg_docs/ParameterResetRequest.md)
+- [RoverAttitudeStatus](../msg_docs/RoverAttitudeStatus.md)
+- [TecsStatus](../msg_docs/TecsStatus.md)
+- [EstimatorSelectorStatus](../msg_docs/EstimatorSelectorStatus.md)
+- [CanInterfaceStatus](../msg_docs/CanInterfaceStatus.md)
+- [Ping](../msg_docs/Ping.md)
+- [LedControl](../msg_docs/LedControl.md)
+- [Wind](../msg_docs/Wind.md)
+- [VehicleStatusV0](../msg_docs/VehicleStatusV0.md)
+- [ActuatorTest](../msg_docs/ActuatorTest.md)
+- [IridiumsbdStatus](../msg_docs/IridiumsbdStatus.md)
+- [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md)
+- [GimbalManagerSetAttitude](../msg_docs/GimbalManagerSetAttitude.md)
+- [Gripper](../msg_docs/Gripper.md)
+- [SensorMag](../msg_docs/SensorMag.md)
+- [DebugValue](../msg_docs/DebugValue.md)
+- [SensorPreflightMag](../msg_docs/SensorPreflightMag.md)
+- [RcParameterMap](../msg_docs/RcParameterMap.md)
+- [LandingGear](../msg_docs/LandingGear.md)
+- [GimbalDeviceInformation](../msg_docs/GimbalDeviceInformation.md)
+- [VehicleOpticalFlow](../msg_docs/VehicleOpticalFlow.md)
+- [UlogStream](../msg_docs/UlogStream.md)
+- [GimbalControls](../msg_docs/GimbalControls.md)
+- [RoverRateSetpoint](../msg_docs/RoverRateSetpoint.md)
+- [LogMessage](../msg_docs/LogMessage.md)
+- [RoverVelocitySetpoint](../msg_docs/RoverVelocitySetpoint.md)
+- [GpioOut](../msg_docs/GpioOut.md)
+- [TaskStackInfo](../msg_docs/TaskStackInfo.md)
+- [VelocityLimits](../msg_docs/VelocityLimits.md)
+- [MagWorkerData](../msg_docs/MagWorkerData.md)
+- [ParameterUpdate](../msg_docs/ParameterUpdate.md)
+- [TrajectorySetpoint6dof](../msg_docs/TrajectorySetpoint6dof.md)
+- [SensorBaro](../msg_docs/SensorBaro.md)
+- [VehicleImuStatus](../msg_docs/VehicleImuStatus.md)
+- [InternalCombustionEngineStatus](../msg_docs/InternalCombustionEngineStatus.md)
+- [VehicleOpticalFlowVel](../msg_docs/VehicleOpticalFlowVel.md)
+- [GimbalManagerSetManualControl](../msg_docs/GimbalManagerSetManualControl.md)
+- [Rpm](../msg_docs/Rpm.md)
+- [MagnetometerBiasEstimate](../msg_docs/MagnetometerBiasEstimate.md)
+- [MountOrientation](../msg_docs/MountOrientation.md)
+- [ActionRequest](../msg_docs/ActionRequest.md)
+- [OpenDroneIdArmStatus](../msg_docs/OpenDroneIdArmStatus.md)
+- [SensorAccelFifo](../msg_docs/SensorAccelFifo.md)
+- [LoggerStatus](../msg_docs/LoggerStatus.md)
+- [GeneratorStatus](../msg_docs/GeneratorStatus.md)
+- [InternalCombustionEngineControl](../msg_docs/InternalCombustionEngineControl.md)
+- [Ekf2Timestamps](../msg_docs/Ekf2Timestamps.md)
+- [LandingTargetPose](../msg_docs/LandingTargetPose.md)
+- [PositionControllerLandingStatus](../msg_docs/PositionControllerLandingStatus.md)
+- [UavcanParameterValue](../msg_docs/UavcanParameterValue.md)
+- [OrbitStatus](../msg_docs/OrbitStatus.md)
+- [PositionControllerStatus](../msg_docs/PositionControllerStatus.md)
+- [EstimatorStatus](../msg_docs/EstimatorStatus.md)
+- [DatamanRequest](../msg_docs/DatamanRequest.md)
+- [HoverThrustEstimate](../msg_docs/HoverThrustEstimate.md)
+- [FixedWingLateralStatus](../msg_docs/FixedWingLateralStatus.md)
+- [NavigatorMissionItem](../msg_docs/NavigatorMissionItem.md)
+- [Cpuload](../msg_docs/Cpuload.md)
+- [EstimatorAidSource3d](../msg_docs/EstimatorAidSource3d.md)
+- [RoverRateStatus](../msg_docs/RoverRateStatus.md)
+- [EscReport](../msg_docs/EscReport.md)
+- [DebugArray](../msg_docs/DebugArray.md)
+- [ControlAllocatorStatus](../msg_docs/ControlAllocatorStatus.md)
+- [SensorHygrometer](../msg_docs/SensorHygrometer.md)
+- [EstimatorSensorBias](../msg_docs/EstimatorSensorBias.md)
+- [EstimatorBias3d](../msg_docs/EstimatorBias3d.md)
+- [GimbalManagerInformation](../msg_docs/GimbalManagerInformation.md)
+- [QshellReq](../msg_docs/QshellReq.md)
+- [CameraStatus](../msg_docs/CameraStatus.md)
+- [GpsInjectData](../msg_docs/GpsInjectData.md)
+- [FigureEightStatus](../msg_docs/FigureEightStatus.md)
+- [TransponderReport](../msg_docs/TransponderReport.md)
+- [UavcanParameterRequest](../msg_docs/UavcanParameterRequest.md)
+- [MavlinkLog](../msg_docs/MavlinkLog.md)
+- [EstimatorGpsStatus](../msg_docs/EstimatorGpsStatus.md)
+- [FuelTankStatus](../msg_docs/FuelTankStatus.md)
+- [Mission](../msg_docs/Mission.md)
+- [PositionSetpoint](../msg_docs/PositionSetpoint.md)
+- [MissionResult](../msg_docs/MissionResult.md)
+- [EstimatorEventFlags](../msg_docs/EstimatorEventFlags.md)
+- [VehicleMagnetometer](../msg_docs/VehicleMagnetometer.md)
+- [MavlinkTunnel](../msg_docs/MavlinkTunnel.md)
+- [DifferentialPressure](../msg_docs/DifferentialPressure.md)
+- [CellularStatus](../msg_docs/CellularStatus.md)
+- [GpsDump](../msg_docs/GpsDump.md)
+- [GimbalDeviceSetAttitude](../msg_docs/GimbalDeviceSetAttitude.md)
+- [ArmingCheckReplyV0](../msg_docs/ArmingCheckReplyV0.md)
+- [NavigatorStatus](../msg_docs/NavigatorStatus.md)
+- [RoverPositionSetpoint](../msg_docs/RoverPositionSetpoint.md)
+- [FollowTarget](../msg_docs/FollowTarget.md)
+- [SensorsStatusImu](../msg_docs/SensorsStatusImu.md)
+- [EstimatorStates](../msg_docs/EstimatorStates.md)
+- [SensorGyro](../msg_docs/SensorGyro.md)
+- [SensorAirflow](../msg_docs/SensorAirflow.md)
+- [ButtonEvent](../msg_docs/ButtonEvent.md)
+- [DebugKeyValue](../msg_docs/DebugKeyValue.md)
+- [GpioConfig](../msg_docs/GpioConfig.md)
+- [CameraTrigger](../msg_docs/CameraTrigger.md)
+- [LandingGearWheel](../msg_docs/LandingGearWheel.md)
+- [VehicleConstraints](../msg_docs/VehicleConstraints.md)
+- [HealthReport](../msg_docs/HealthReport.md)
+- [PowerButtonState](../msg_docs/PowerButtonState.md)
+- [RadioStatus](../msg_docs/RadioStatus.md)
+- [SensorGyroFifo](../msg_docs/SensorGyroFifo.md)
+- [EstimatorBias](../msg_docs/EstimatorBias.md)
+- [DebugVect](../msg_docs/DebugVect.md)
+- [DistanceSensorModeChangeRequest](../msg_docs/DistanceSensorModeChangeRequest.md)
+- [RtlTimeEstimate](../msg_docs/RtlTimeEstimate.md)
+- [PpsCapture](../msg_docs/PpsCapture.md)
+- [SensorSelection](../msg_docs/SensorSelection.md)
+- [SystemPower](../msg_docs/SystemPower.md)
+- [ActuatorControlsStatus](../msg_docs/ActuatorControlsStatus.md)
+- [SensorGyroFft](../msg_docs/SensorGyroFft.md)
+- [VehicleAirData](../msg_docs/VehicleAirData.md)
+- [FollowTargetEstimator](../msg_docs/FollowTargetEstimator.md)
+- [ParameterSetUsedRequest](../msg_docs/ParameterSetUsedRequest.md)
+- [GpioRequest](../msg_docs/GpioRequest.md)
+- [OpenDroneIdOperatorId](../msg_docs/OpenDroneIdOperatorId.md)
+- [RtlStatus](../msg_docs/RtlStatus.md)
+- [Airspeed](../msg_docs/Airspeed.md)
+- [VehicleAcceleration](../msg_docs/VehicleAcceleration.md)
+- [ParameterSetValueRequest](../msg_docs/ParameterSetValueRequest.md)
+- [IrlockReport](../msg_docs/IrlockReport.md)
+- [HeaterStatus](../msg_docs/HeaterStatus.md)
+- [AdcReport](../msg_docs/AdcReport.md)
+- [PwmInput](../msg_docs/PwmInput.md)
+- [TiltrotorExtraControls](../msg_docs/TiltrotorExtraControls.md)
+- [EstimatorAidSource1d](../msg_docs/EstimatorAidSource1d.md)
+- [OrbTestMedium](../msg_docs/OrbTestMedium.md)
+- [VehicleAttitudeSetpointV0](../msg_docs/VehicleAttitudeSetpointV0.md)
+- [EstimatorAidSource2d](../msg_docs/EstimatorAidSource2d.md)
+- [TuneControl](../msg_docs/TuneControl.md)
+- [WheelEncoders](../msg_docs/WheelEncoders.md)
+- [AutotuneAttitudeControlStatus](../msg_docs/AutotuneAttitudeControlStatus.md)
+- [LandingTargetInnovations](../msg_docs/LandingTargetInnovations.md)
+- [SensorsStatus](../msg_docs/SensorsStatus.md)
+
+:::

docs/zh/middleware/uorb.md

@@ -61,12 +61,12 @@ For example the [VelocityLimits](../msg_docs/VelocityLimits.md) message definiti
 ```text
 # Velocity and yaw rate limits for a multicopter position slow mode only
 
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 # absolute speeds, NAN means use default limit
-float32 horizontal_velocity # [m/s]
-float32 vertical_velocity # [m/s]
-float32 yaw_rate # [rad/s]
+float32 horizontal_velocity # [m/s] Horizontal velocity.
+float32 vertical_velocity # [m/s] Vertical velocity.
+float32 yaw_rate # [rad/s] Yaw rate.
 ```
 
 By default this message definition will be compiled to a single topic with an id `velocity_limits`, a direct conversion from the CamelCase name to a snake_case version.
@@ -92,15 +92,34 @@ To nest a message, simply include the nested message type in the parent message
 
 ```text
 # Global position setpoint triplet in WGS84 coordinates.
+#
 # This are the three next waypoints (or just the next two or one).
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] Time since system start.
 
 PositionSetpoint previous
 PositionSetpoint current
 PositionSetpoint next
 ```
 
+### uORB Buffer Length (ORB_QUEUE_LENGTH)
+
+uORB messages have a single-message buffer by default, which may be overwritten if the message publication rate is too high.
+In most cases this does not matter: either we are only interested in the latest sample of a topic, such as a sensor value or a setpoint, or losing a few samples is not a particular problem.
+For relatively few cases, such as vehicle commands, it is important that we don't drop topics.
+
+In order to reduce the chance that messages will be dropped we can use named constant `ORB_QUEUE_LENGTH` to create a buffer of the specified length.
+For example, to create a four-message queue, add the following line to your message definition:
+
+```sh
+uint8 ORB_QUEUE_LENGTH = 4
+```
+
+As long as subscribers are able to read messages out of the buffer quickly enough than it isn't ever fully filled to the queue length (by publishers), they will be able to get all messages that are sent.
+Messages will still be lost they are published when the queue is filled.
+
+Note that the queue length value must be a power of 2 (so 2, 4, 8, ...).
+
 ### Message/Field Deprecation {#deprecation}
 
 As there are external tools using uORB messages from log files, such as [Flight Review](https://github.com/PX4/flight_review), certain aspects need to be considered when updating existing messages:

docs/zh/middleware/uxrce_dds.md

@@ -38,7 +38,7 @@ The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is gen
 The agent has no dependency on client code.
 It can be built standalone or in a ROS 2 workspace, or installed as a snap package on Ubuntu.
 
-When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
+When PX4 is built, a code generator uses the uORB message definitions in the source tree ([PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg)) to compile support for the subset of uORB topics in [/src/modules/uxrce_dds_client/dds_topics.yaml](../middleware/dds_topics.md) into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client).
 
 PX4 main or release builds automatically export the set of uORB messages definitions in the build to an associated branch in [PX4/px4_msgs](https://github.com/PX4/px4_msgs).
 
@@ -326,13 +326,11 @@ ROS_DOMAIN_ID=3 PX4_UXRCE_DDS_PORT=9999 PX4_UXRCE_DDS_NS=drone make px4_sitl gz_
 
 ## Supported uORB Messages
 
-The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The set of [PX4 uORB topics](../msg_docs/index.md) that are exposed through the client are set in [dds_topics.yaml](../middleware/dds_topics.md).
 
 The topics are release specific (support is compiled into [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) at build time).
 While most releases should support a very similar set of messages, to be certain you would need to check the yaml file for your particular release.
 
-<!-- Jublish the set we use?: https://github.com/PX4/px4_msgs/issues/22 -->
-
 Note that ROS 2/DDS needs to have the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware in order to interpret the messages.
 The message definitions are stored in the ROS 2 interface package [PX4/px4_msgs](https://github.com/PX4/px4_msgs), and they are automatically synchronized by CI on the `main` and release branches.
 Note that all the messages from PX4 source code are present in the repository, but only those listed in `dds_topics.yaml` will be available as ROS 2 topics.
@@ -349,21 +347,21 @@ Therefore,
   ```
 
   ::: info
-  Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
+  Technically, [dds_topics.yaml](../middleware/dds_topics.md) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
   For more information see [DDS Topics YAML](#dds-topics-yaml) below.
 
 :::
 
 ## Customizing the Namespace
 
-Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) or at runtime (which is useful for multi vehicle operations):
+Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](../middleware/dds_topics.md)) or at runtime (which is useful for multi vehicle operations):
 
 - One possibility is to use the `-n` option when starting the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) from command line.
   This technique can be used both in simulation and real vehicles.
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 :::info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#dds-ros-2-services).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#dds-ros-2-services).
 Therefore, commands like:
 
 ```sh
@@ -420,7 +418,7 @@ Deadline, lifespan, and lease durations are also all set to "default".
 
 ## DDS Topics YAML
 
-The PX4 yaml file [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) defines the set of PX4 uORB topics that are built into firmware and published.
+The PX4 [dds_topics.yaml](../middleware/dds_topics.md) file defines the set of PX4 uORB topics that are built into firmware and published.
 More precisely, it completely defines the relationship/pairing between PX4 uORB and ROS 2 messages.
 
 The file is structured as follows:
@@ -549,7 +547,7 @@ Take a look at the [client startup section](#starting-the-client) to learn how t
 
 #### New file for setting which topics are published
 
-The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topic.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
+The list of topics that are published and subscribed for a particular firmware is now managed by the [dds_topics.yaml](../middleware/dds_topics.md) configuration file, which replaces [urtps_bridge_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/release/1.13/msg/tools/urtps_bridge_topics.yaml)
 
 See [Supported uORB Messages](#supported-uorb-messages) and [DDS Topics YAML](#dds-topics-yaml) sections for more information.
 

docs/zh/ros2/user_guide.md

@@ -28,7 +28,7 @@ The agent acts as a proxy for the client to publish and subscribe to topics in t
 
 The PX4 [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) is generated at build time and included in PX4 firmware by default.
 It includes both the "generic" micro XRCE-DDS client code, and PX4-specific translation code that it uses to publish to/from uORB topics.
-The subset of uORB messages that are generated into the client are listed in [PX4-Autopilot/src/modules/uxrce_dds_client/dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml).
+The subset of uORB messages that are generated into the client are specified in [dds_topics.yaml](../middleware/dds_topics.md).
 The generator uses the uORB message definitions in the source tree: [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to create the code for sending ROS 2 messages.
 
 ROS 2 applications need to be built in a workspace that has the _same_ message definitions that were used to create the uXRCE-DDS client module in the PX4 Firmware.
@@ -97,48 +97,48 @@ To install ROS 2 and its dependencies:
 
 1. Install ROS 2.
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
- To install ROS 2 "Humble" on Ubuntu 22.04:
+  ::: tab humble
+  To install ROS 2 "Humble" on Ubuntu 22.04:
 
- ```sh
- sudo apt update && sudo apt install locales
- sudo locale-gen en_US en_US.UTF-8
- sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
- export LANG=en_US.UTF-8
- sudo apt install software-properties-common
- sudo add-apt-repository universe
- sudo apt update && sudo apt install curl -y
- sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
- echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
- sudo apt update && sudo apt upgrade -y
- sudo apt install ros-humble-desktop
- sudo apt install ros-dev-tools
- source /opt/ros/humble/setup.bash && echo "source /opt/ros/humble/setup.bash" >> .bashrc
- ```
+  ```sh
+  sudo apt update && sudo apt install locales
+  sudo locale-gen en_US en_US.UTF-8
+  sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
+  export LANG=en_US.UTF-8
+  sudo apt install software-properties-common
+  sudo add-apt-repository universe
+  sudo apt update && sudo apt install curl -y
+  sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
+  echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+  sudo apt update && sudo apt upgrade -y
+  sudo apt install ros-humble-desktop
+  sudo apt install ros-dev-tools
+  source /opt/ros/humble/setup.bash && echo "source /opt/ros/humble/setup.bash" >> .bashrc
+  ```
 
- The instructions above are reproduced from the official installation guide: [Install ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
- You can install _either_ the desktop (`ros-humble-desktop`) _or_ bare-bones versions (`ros-humble-ros-base`), _and_ the development tools (`ros-dev-tools`).
+  The instructions above are reproduced from the official installation guide: [Install ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
+  You can install _either_ the desktop (`ros-humble-desktop`) _or_ bare-bones versions (`ros-humble-ros-base`), _and_ the development tools (`ros-dev-tools`).
 
 :::
 
- ::: tab foxy
- To install ROS 2 "Foxy" on Ubuntu 20.04:
+  ::: tab foxy
+  To install ROS 2 "Foxy" on Ubuntu 20.04:
 
- - Follow the official installation guide: [Install ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html).
+  - Follow the official installation guide: [Install ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html).
 
- You can install _either_ the desktop (`ros-foxy-desktop`) _or_ bare-bones versions (`ros-foxy-ros-base`), _and_ the development tools (`ros-dev-tools`).
+  You can install _either_ the desktop (`ros-foxy-desktop`) _or_ bare-bones versions (`ros-foxy-ros-base`), _and_ the development tools (`ros-dev-tools`).
 
 :::
 
- ::::
+  ::::
 
 2. Some Python dependencies must also be installed (using **`pip`** or **`apt`**):
 
- ```sh
- pip install --user -U empy==3.3.4 pyros-genmsg setuptools
- ```
+  ```sh
+  pip install --user -U empy==3.3.4 pyros-genmsg setuptools
+  ```
 
 ### Setup Micro XRCE-DDS Agent & Client
 
@@ -155,22 +155,22 @@ To setup and start the agent:
 
 2. 输入以下命令从仓库获取源代码并构建代理(Agent)：
 
- ```sh
- git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
- cd Micro-XRCE-DDS-Agent
- mkdir build
- cd build
- cmake ..
- make
- sudo make install
- sudo ldconfig /usr/local/lib/
- ```
+  ```sh
+  git clone -b v2.4.2 https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
+  cd Micro-XRCE-DDS-Agent
+  mkdir build
+  cd build
+  cmake ..
+  make
+  sudo make install
+  sudo ldconfig /usr/local/lib/
+  ```
 
 3. 启动代理并设置以连接运行在模拟器上的 uXRCE-DDS客户端(Client)：
 
- ```sh
- MicroXRCEAgent udp4 -p 8888
- ```
+  ```sh
+  MicroXRCEAgent udp4 -p 8888
+  ```
 
 The agent is now running, but you won't see much until we start PX4 (in the next step).
 
@@ -187,31 +187,31 @@ To start the simulator (and client):
 
 1. Open a new terminal in the root of the **PX4 Autopilot** repo that was installed above.
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- - Start a PX4 [Gazebo](../sim_gazebo_gz/index.md) simulation using:
+  - Start a PX4 [Gazebo](../sim_gazebo_gz/index.md) simulation using:
 
-  ```sh
-  make px4_sitl gz_x500
-  ```
+    ```sh
+    make px4_sitl gz_x500
+    ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- - Start a PX4 [Gazebo Classic](../sim_gazebo_classic/index.md) simulation using:
+  - Start a PX4 [Gazebo Classic](../sim_gazebo_classic/index.md) simulation using:
 
-  ```sh
-  make px4_sitl gazebo-classic
-  ```
+    ```sh
+    make px4_sitl gazebo-classic
+    ```
 
 
 :::
 
- ::::
+  ::::
 
 The agent and client are now running they should connect.
 
@@ -261,52 +261,52 @@ To create and build the workspace:
 
 2. Create and navigate into a new workspace directory using:
 
- ```sh
- mkdir -p ~/ws_sensor_combined/src/
- cd ~/ws_sensor_combined/src/
- ```
+  ```sh
+  mkdir -p ~/ws_sensor_combined/src/
+  cd ~/ws_sensor_combined/src/
+  ```
 
- ::: info
- A naming convention for workspace folders can make it easier to manage workspaces.
+  ::: info
+  A naming convention for workspace folders can make it easier to manage workspaces.
 
 :::
 
 3. Clone the example repository and [px4_msgs](https://github.com/PX4/px4_msgs) to the `/src` directory (the `main` branch is cloned by default, which corresponds to the version of PX4 we are running):
 
- ```sh
- git clone https://github.com/PX4/px4_msgs.git
- git clone https://github.com/PX4/px4_ros_com.git
- ```
+  ```sh
+  git clone https://github.com/PX4/px4_msgs.git
+  git clone https://github.com/PX4/px4_ros_com.git
+  ```
 
 4. Source the ROS 2 development environment into the current terminal and compile the workspace using `colcon`:
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- cd ..
- source /opt/ros/humble/setup.bash
- colcon build
- ```
+  ```sh
+  cd ..
+  source /opt/ros/humble/setup.bash
+  colcon build
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- cd ..
- source /opt/ros/foxy/setup.bash
- colcon build
- ```
+  ```sh
+  cd ..
+  source /opt/ros/foxy/setup.bash
+  colcon build
+  ```
 
 
 :::
 
- ::::
+  ::::
 
- This builds all the folders under `/src` using the sourced toolchain.
+  This builds all the folders under `/src` using the sourced toolchain.
 
 #### Running the Example
 
@@ -322,42 +322,42 @@ In a new terminal:
 
 1. Navigate into the top level of your workspace directory and source the ROS 2 environment (in this case "Humble"):
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- cd ~/ws_sensor_combined/
- source /opt/ros/humble/setup.bash
- ```
+  ```sh
+  cd ~/ws_sensor_combined/
+  source /opt/ros/humble/setup.bash
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- cd ~/ws_sensor_combined/
- source /opt/ros/foxy/setup.bash
- ```
+  ```sh
+  cd ~/ws_sensor_combined/
+  source /opt/ros/foxy/setup.bash
+  ```
 
 
 :::
 
- ::::
+  ::::
 
 2. Source the `local_setup.bash`.
 
- ```sh
- source install/local_setup.bash
- ```
+  ```sh
+  source install/local_setup.bash
+  ```
 
 3. Now launch the example.
- Note here that we use `ros2 launch`, which is described below.
+  Note here that we use `ros2 launch`, which is described below.
 
- ```sh
- ros2 launch px4_ros_com sensor_combined_listener.launch.py
- ```
+  ```sh
+  ros2 launch px4_ros_com sensor_combined_listener.launch.py
+  ```
 
 If this is working you should see data being printed on the terminal/console where you launched the ROS listener:
 
@@ -385,18 +385,18 @@ If you were to use incompatible [message versions](../middleware/uorb.md#message
 
 1. Include the [Message Translation Node](../ros2/px4_ros2_msg_translation_node.md) into the example workspace or a separate workspace by running the following script:
 
- ```sh
- cd /path/to/ros_ws
- /path/to/PX4-Autopilot/Tools/copy_to_ros_ws.sh .
- ```
+  ```sh
+  cd /path/to/ros_ws
+  /path/to/PX4-Autopilot/Tools/copy_to_ros_ws.sh .
+  ```
 
 2. Build and run the translation node:
 
- ```sh
- colcon build
- source install/local_setup.bash
- ros2 run translation_node translation_node_bin
- ```
+  ```sh
+  colcon build
+  source install/local_setup.bash
+  ros2 run translation_node translation_node_bin
+  ```
 
 ## Controlling a Vehicle
 
@@ -405,7 +405,7 @@ To control applications, ROS 2 applications:
 - subscribe to (listen to) telemetry topics published by PX4
 - publish to topics that cause PX4 to perform some action.
 
-The topics that you can use are defined in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
+The topics that you can use are defined in [dds_topics.yaml](../middleware/dds_topics.md), and you can get more information about their data in the [uORB Message Reference](../msg_docs/index.md).
 For example, [VehicleGlobalPosition](../msg_docs/VehicleGlobalPosition.md) can be used to get the vehicle global position, while [VehicleCommand](../msg_docs/VehicleCommand.md) can be used to command actions such as takeoff and land.
 
 The [ROS 2 Example applications](#ros-2-example-applications) examples below provide concrete examples of how to use these topics.
@@ -456,13 +456,13 @@ Therefore, ROS 2 nodes that want to interface with PX4 must take care of the fra
 
 - To rotate a vector from ENU to NED two basic rotations must be performed:
 
- - first a pi/2 rotation around the `Z`-axis (up),
- - then a pi rotation around the `X`-axis (old East/new North).
+  - first a pi/2 rotation around the `Z`-axis (up),
+  - then a pi rotation around the `X`-axis (old East/new North).
 
 - To rotate a vector from NED to ENU two basic rotations must be performed:
 
 - - first a pi/2 rotation around the `Z`-axis (down),
- - then a pi rotation around the `X`-axis (old North/new East). Note that the two resulting operations are mathematically equivalent.
+  - then a pi rotation around the `X`-axis (old North/new East). Note that the two resulting operations are mathematically equivalent.
 
 - To rotate a vector from FLU to FRD a pi rotation around the `X`-axis (front) is sufficient.
 
@@ -720,17 +720,17 @@ Therefore,
 
 - If you're using a main or release version of PX4 you can get the message definitions by cloning the interface package [PX4/px4_msgs](https://github.com/PX4/px4_msgs) into your workspace.
 - If you're creating or modifying uORB messages you must manually update the messages in your workspace from your PX4 source tree.
- Generally this means that you would update [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), clone the interface package, and then manually synchronize it by copying the new/modified message definitions from [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to its `msg` folders.
- Assuming that PX4-Autopilot is in your home directory `~`, while `px4_msgs` is in `~/ros2_ws/src/`, then the command might be:
+  Generally this means that you would update [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), clone the interface package, and then manually synchronize it by copying the new/modified message definitions from [PX4-Autopilot/msg](https://github.com/PX4/PX4-Autopilot/tree/main/msg) to its `msg` folders.
+  Assuming that PX4-Autopilot is in your home directory `~`, while `px4_msgs` is in `~/ros2_ws/src/`, then the command might be:
 
- ```sh
- rm ~/ros2_ws/src/px4_msgs/msg/*.msg
- cp ~/PX4-Autopilot/mgs/*.msg ~/ros2_ws/src/px4_msgs/msg/
- ```
+  ```sh
+  rm ~/ros2_ws/src/px4_msgs/msg/*.msg
+  cp ~/PX4-Autopilot/mgs/*.msg ~/ros2_ws/src/px4_msgs/msg/
+  ```
 
- ::: info
- Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
- For more information see [uXRCE-DDS > DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml).
+  ::: info
+  Technically, [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) completely defines the relationship between PX4 uORB topics and ROS 2 messages.
+  For more information see [uXRCE-DDS > DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml).
 
 :::
 
@@ -739,11 +739,11 @@ Therefore,
 Custom topic and service namespaces can be applied at build time (changing [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml)) or at runtime (useful for multi vehicle operations):
 
 - One possibility is to use the `-n` option when starting the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) from command line.
- This technique can be used both in simulation and real vehicles.
+  This technique can be used both in simulation and real vehicles.
 - A custom namespace can be provided for simulations (only) by setting the environment variable `PX4_UXRCE_DDS_NS` before starting the simulation.
 
 :::info
-Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) and all [service servers](#px4-ros-2-service-servers).
+Changing the namespace at runtime will append the desired namespace as a prefix to all `topic` fields in [dds_topics.yaml](../middleware/dds_topics.md) and all [service servers](#px4-ros-2-service-servers).
 Therefore, commands like:
 
 ```sh
@@ -780,7 +780,7 @@ The service servers that are built into the PX4 [uxrce_dds_client](../modules/mo
 
 - `/fmu/vehicle_command` (definition: [`px4_msgs::srv::VehicleCommand`](https://github.com/PX4/px4_msgs/blob/main/srv/VehicleCommand.srv).)
 
- This service can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.
+  This service can be called by ROS 2 applications to send PX4 [VehicleCommand](../msg_docs/VehicleCommand.md) uORB messages and receive PX4 [VehicleCommandAck](../msg_docs/VehicleCommandAck.md) uORB messages in response.
 
 All PX4 service names follow the convention `{extra_namespace}/fmu/{server_specific_name}` where `{extra_namespace}` is the same [custom namespace](#customizing-the-namespace) that can be given to the PX4 topics.
 
@@ -973,36 +973,36 @@ The standard installation should include all the tools needed by ROS 2.
 If any are missing, they can be added separately:
 
 - **`colcon`** build tools should be in the development tools.
- It can be installed using:
+  It can be installed using:
 
- ```sh
- sudo apt install python3-colcon-common-extensions
- ```
+  ```sh
+  sudo apt install python3-colcon-common-extensions
+  ```
 
 - The Eigen3 library used by the transforms library should be in the both the desktop and base packages.
- It should be installed as shown:
+  It should be installed as shown:
 
- :::: tabs
+  :::: tabs
 
- ::: tab humble
+  ::: tab humble
 
- ```sh
- sudo apt install ros-humble-eigen3-cmake-module
- ```
+  ```sh
+  sudo apt install ros-humble-eigen3-cmake-module
+  ```
 
 
 :::
 
- ::: tab foxy
+  ::: tab foxy
 
- ```sh
- sudo apt install ros-foxy-eigen3-cmake-module
- ```
+  ```sh
+  sudo apt install ros-foxy-eigen3-cmake-module
+  ```
 
 
 :::
 
- ::::
+  ::::
 
 ### ros_gz_bridge not publishing on the \clock topic
 

msg/CMakeLists.txt

@@ -226,7 +226,6 @@ set(msg_files
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
-	TaskLocalPositionSetpoint.msg
 	VehicleMagnetometer.msg
 	VehicleOpticalFlow.msg
 	VehicleOpticalFlowVel.msg

msg/EstimatorStatus.msg

@@ -39,12 +39,14 @@ uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed win
 uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
+uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
 uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
+uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused
 
 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults

msg/EstimatorStatusFlags.msg

@@ -25,12 +25,14 @@ bool cs_fixed_wing            # 17 - true when the vehicle is operating as a fix
 bool cs_mag_fault             # 18 - true when the magnetometer has been declared faulty and is no longer being used
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
+bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
+bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing

msg/PositionSetpoint.msg

@@ -36,5 +36,3 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
-
-# TOPICS position_setpoint task_position_setpoint

msg/TaskLocalPositionSetpoint.msg

@@ -1,19 +0,0 @@
-# Local position setpoint in NED frame
-# Telemetry of PID position controller to monitor tracking.
-# NaN means the state was not controlled
-
-uint64 timestamp	# time since system start (microseconds)
-
-float32 x		# in meters NED
-float32 y		# in meters NED
-float32 z		# in meters NED
-
-float32 vx		# in meters/sec
-float32 vy		# in meters/sec
-float32 vz		# in meters/sec
-
-float32[3] acceleration # in meters/sec^2
-float32[3] thrust	# normalized thrust vector in NED
-
-float32 yaw		# in radians NED -PI..+PI
-float32 yawspeed	# in radians/sec

msg/VehicleConstraints.msg

@@ -7,5 +7,3 @@ float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
 
 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)
-
-bool lock_dist_bottom # altitude is locked to the current distance to ground

msg/versioned/VehicleStatus.msg

@@ -89,9 +89,9 @@ 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_ROTARY_WING = 0
-uint8 VEHICLE_TYPE_FIXED_WING = 1
-uint8 VEHICLE_TYPE_ROVER = 2
+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

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -206,7 +206,7 @@ static void init_timers_dma_up(void)
 		timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
 		if (timer_configs[timer_index].dma_handle == NULL) {
-			PX4_DEBUG("Failed to allocate Timer %u DMA UP", timer_index);
+			PX4_WARN("Failed to allocate Timer %u DMA UP", timer_index);
 			continue;
 		}
 
@@ -214,12 +214,16 @@ static void init_timers_dma_up(void)
 		timer_configs[timer_index].initialized = true;
 	}
 
-	// Free the allocated DMA channels
-	for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
-		if (timer_configs[timer_index].dma_handle != NULL) {
-			stm32_dmafree(timer_configs[timer_index].dma_handle);
-			timer_configs[timer_index].dma_handle = NULL;
-			PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
+	// Bidirectional DShot will free/allocate DMA stream on every update event. This is required
+	// in order to reconfigure the DMA stream between Timer Burst and CaptureCompare.
+	if (_bidirectional) {
+		// Free the allocated DMA channels
+		for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
+			if (timer_configs[timer_index].dma_handle != NULL) {
+				stm32_dmafree(timer_configs[timer_index].dma_handle);
+				timer_configs[timer_index].dma_handle = NULL;
+				PX4_INFO("Freed DMA UP Timer Index %u", timer_index);
+			}
 		}
 	}
 }
@@ -341,8 +345,15 @@ void up_dshot_trigger()
 
 			io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
 
-			// Allocate DMA
-			if (timer_configs[timer_index].dma_handle == NULL) {
+			if (_bidirectional) {
+				// Deallocate DMA from previous transaction
+				if (timer_configs[timer_index].dma_handle != NULL) {
+					stm32_dmastop(timer_configs[timer_index].dma_handle);
+					stm32_dmafree(timer_configs[timer_index].dma_handle);
+					timer_configs[timer_index].dma_handle = NULL;
+				}
+
+				// Allocate DMA
 				timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
 				if (timer_configs[timer_index].dma_handle == NULL) {
@@ -502,11 +513,8 @@ static void capture_complete_callback(void *arg)
 	// Disable capture DMA
 	io_timer_capture_dma_req(timer_index, capture_channel, false);
 
-	if (timer_configs[timer_index].dma_handle != NULL) {
-		stm32_dmastop(timer_configs[timer_index].dma_handle);
-		stm32_dmafree(timer_configs[timer_index].dma_handle);
-		timer_configs[timer_index].dma_handle = NULL;
-	}
+	// Stop DMA (should already be finished)
+	stm32_dmastop(timer_configs[timer_index].dma_handle);
 
 	// Re-initialize all output channels on this timer
 	for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
@@ -553,7 +561,7 @@ void process_capture_results(uint8_t timer_index, uint8_t channel_index)
 
 	} else {
 		// Convert the period to eRPM
-		_erpms[output_channel] = (1000000 * 60) / period;
+		_erpms[output_channel] = (1000000 * 60 / 100 + period / 2) / period;
 	}
 
 	// We set it ready anyway, not to hold up other channels when used in round robin.

range_alt_notes.txt

@@ -1,20 +0,0 @@
-## FlightTaskManualAltitude
-- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
-- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
-- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
-- _respectMinAltitude ... weird, remove? No need for a function
-
-
-## FlightTask
-- _dist_to_bottom and _dist_to_ground ... confusing, unify
--
-
-## FlightTaskAuto
-- reuse logic for range alt lock
-- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
-
-## EKF
-- Vz does not reflect true Vz due to noisy baro
-- Vz errors cause rangefinder kinematic consistency check to fail
-- Position setpoint error remains over long periods (might be related to Vz issues above)
-

src/drivers/osd/msp_osd/MspV1.cpp

@@ -39,6 +39,7 @@
 #include <float.h>
 #include <string.h>
 #include <math.h>
+#include <stdio.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -62,7 +63,7 @@ struct msp_message_descriptor_t {
 	uint8_t message_size;
 };
 
-#define MSP_DESCRIPTOR_COUNT 11
+#define MSP_DESCRIPTOR_COUNT 12
 const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_OSD_CONFIG, true, sizeof(msp_osd_config_t)},
 	{MSP_NAME, true, sizeof(msp_name_t)},
@@ -75,10 +76,9 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_COMP_GPS, true, sizeof(msp_comp_gps_t)},
 	{MSP_ESC_SENSOR_DATA, true, sizeof(msp_esc_sensor_data_dji_t)},
 	{MSP_MOTOR_TELEMETRY, true, sizeof(msp_motor_telemetry_t)},
+	{MSP_RC, true, sizeof(msp_rc_t)},
 };
 
-#define MSP_FRAME_START_SIZE 5
-#define MSP_CRC_SIZE 1
 bool MspV1::Send(const uint8_t message_id, const void *payload)
 {
 	uint32_t payload_size = 0;
@@ -149,3 +149,76 @@ bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload
 	int packet_size =  MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
 	return  write(_fd, packet, packet_size) == packet_size;
 }
+
+
+int MspV1::Receive(uint8_t *payload, uint8_t *message_id)
+{
+	uint8_t payload_size;
+	uint8_t crc;
+	uint8_t calc_crc;
+	int ret;
+
+	while (!has_header) {
+		int bytes_available = 0;
+
+		if (ioctl(_fd, FIONREAD, &bytes_available) < 0) {
+			return -EIO;
+		}
+
+		if (bytes_available < 5) {
+			return -EWOULDBLOCK;
+		}
+
+		while (bytes_available > 4) {
+			if ((ret = read(_fd, header, 1)) != 1) {
+				return ret;
+			}
+
+			bytes_available--;
+
+			if (header[0] == '$') {
+				break;
+			}
+
+		}
+
+		if (header[0] != '$') {
+			return -EWOULDBLOCK;
+		}
+
+		if ((ret = read(_fd, &header[1], 4)) != 4) {
+			return ret;
+		}
+
+		if (header[0] == '$' && header[1] == 'M' && header[2] == '<') {
+			has_header = true;
+		}
+	}
+
+	payload_size = header[3];
+	*message_id = header[4];
+
+	if ((ret = read(_fd, payload, payload_size + MSP_CRC_SIZE)) != payload_size + MSP_CRC_SIZE) {
+		if (ret != -EWOULDBLOCK) {
+			has_header = false;
+		}
+
+		return ret;
+	}
+
+	has_header = false;
+
+	crc = payload[payload_size];
+
+	calc_crc = payload_size ^ header[4];
+
+	for (int i = 0; i < payload_size; i++) {
+		calc_crc ^= payload[i];
+	}
+
+	if (calc_crc != crc) {
+		return -EINVAL;
+	}
+
+	return payload_size;
+}

src/drivers/osd/msp_osd/MspV1.hpp

@@ -33,6 +33,9 @@
 
 #pragma once
 
+#define MSP_FRAME_START_SIZE 5
+#define MSP_CRC_SIZE 1
+
 class MspV1
 {
 public:
@@ -40,7 +43,10 @@ public:
 	int GetMessageSize(int message_type);
 	bool Send(const uint8_t message_id, const void *payload);
 	bool Send(const uint8_t message_id, const void *payload, uint32_t payload_size);
+	int Receive(uint8_t *payload, uint8_t *message_id);
 
 private:
 	int _fd{-1};
+	uint8_t header[MSP_FRAME_START_SIZE + MSP_CRC_SIZE];
+	bool has_header{false};
 };

src/drivers/osd/msp_osd/module.yaml

@@ -97,3 +97,14 @@ parameters:
             min: 100
             max: 10000
             default: 500
+
+        # RC Stick input
+        OSD_RC_STICK:
+            description:
+                short: OSD RC Stick commands
+                long: |
+                    Forward RC stick input to VTX when disarmed
+            type: int32
+            min: 0
+            max: 1
+            default: 1

src/drivers/osd/msp_osd/msp_defines.h

@@ -48,6 +48,8 @@
 #define MSP_ARMING_CONFIG         61
 #define MSP_RX_MAP                64 // get channel map (also returns number of channels total)
 #define MSP_LOOP_TIME             73 // FC cycle time i.e looptime parameter
+#define MSP_GET_VTX_CONFIG        88
+#define MSP_SET_VTX_CONFIG        89
 #define MSP_STATUS               101
 #define MSP_RAW_IMU              102
 #define MSP_SERVO                103
@@ -75,10 +77,12 @@
 #define MSP_SET_PID              202 // set P I D coeff
 
 // commands
-#define MSP_SET_HEAD             211 // define a new heading hold direction
-#define MSP_SET_RAW_RC           200 // 8 rc chan
-#define MSP_SET_RAW_GPS          201 // fix, numsat, lat, lon, alt, speed
-#define MSP_SET_WP               209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_HEAD                211 // define a new heading hold direction
+#define MSP_SET_RAW_RC              200 // 8 rc chan
+#define MSP_SET_RAW_GPS             201 // fix, numsat, lat, lon, alt, speed
+#define MSP_SET_WP                  209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_VTXTABLE_BAND       227
+#define MSP_SET_VTXTABLE_POWERLEVEL 228
 
 // bits of getActiveModes() return value
 #define MSP_MODE_ARM          0
@@ -895,6 +899,58 @@ struct msp_status_BF_t {
 	uint8_t  extra_flags;
 } __attribute__((packed));
 
+struct msp_set_vtx_config_t {
+	uint16_t new_freq; //  if setting frequency then full uint16 is the frequency in MHz (ie. 5800)
+	//if setting band channel than band is high 8 bits and channel is low 8 bits
+	uint8_t power_level;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint8_t low_power_disarm;
+	uint16_t pit_freq;
+	uint8_t user_band;
+	uint8_t user_channel;
+	uint16_t user_freq; // in MHz, 0 if using band & channel
+	uint8_t band_count;
+	uint8_t channel_count;
+	uint8_t power_count;
+	uint8_t clear_vtxtable; // Bool
+} __attribute__((packed));
+
+struct msp_get_vtx_config_t {
+	uint8_t vtx_type;
+	uint8_t band;
+	uint8_t channel;
+	uint8_t power_index;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint16_t freq; // in MHz, 0 if using band & channel
+	uint8_t device_ready;
+	uint8_t low_power_disarm;
+} __attribute__((packed));
+
+struct msp_set_vtxtable_powerlevel_t {
+	uint8_t index;
+	uint16_t power_value;
+	uint8_t power_label_length;
+	uint8_t power_label_name[3];
+} __attribute__((packed));
+
+#define VTX_TABLE_BAND_NAME_LENGTH 8
+#define VTXDEV_MSP 5
+
+//29 bytes
+
+struct msp_set_vtxtable_band_t {
+	uint8_t band;
+	uint8_t band_name_length;
+	uint8_t band_label_name[VTX_TABLE_BAND_NAME_LENGTH];
+	uint8_t band_letter;
+	uint8_t is_factory_band;
+	uint8_t channel_count;
+	uint16_t frequency[8];
+} __attribute__((packed));
+
+
+
+
 ////ArduPlane
 enum arduPlaneModes_e {
 	MANUAL        = 0,

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -47,6 +47,7 @@
 #include <unistd.h>
 #include <termios.h>
 #include <string.h>
+#include <ctype.h>
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
@@ -239,6 +240,7 @@ void MspOsd::SendConfig()
 
 	_msp.Send(MSP_OSD_CONFIG, &msp_osd_config);
 }
+
 // extract it to MSPOSD_BF_Run() and MSPOSD_DJIFPV_Run() for compatibility?
 void MspOsd::Run()
 {
@@ -280,6 +282,23 @@ void MspOsd::Run()
 		_is_initialized = true;
 	}
 
+	if (change_channel) {
+		msp_get_vtx_config_t vtx_set_config{0};
+		vtx_set_config.low_power_disarm = vtx_config.low_power_disarm;
+		vtx_set_config.pit_mode = vtx_config.pit_mode;
+		vtx_set_config.vtx_type = VTXDEV_MSP;
+		vtx_set_config.band = vtx_config.user_band;
+		vtx_set_config.channel = vtx_config.user_channel;
+		this->Send(MSP_GET_VTX_CONFIG, &vtx_set_config, sizeof(msp_get_vtx_config_t));
+		change_channel = false;
+	}
+
+	this->Receive();
+
+	if (!has_vtx_config) {
+		this->Send(MSP_GET_VTX_CONFIG, nullptr, 0);
+	}
+
 	// avoid premature pessimization; if skip processing if we're effectively disabled
 	if (_param_osd_symbols.get() == 0) {
 		return;
@@ -420,6 +439,32 @@ void MspOsd::Run()
 	{
 
 	}
+
+	// MSP_RC
+	{
+		if (_param_osd_rc_stick.get() == 1) {
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+
+			if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+				input_rc_s input_rc{};
+				_input_rc_sub.copy(&input_rc);
+				const auto msg = msp_osd::construct_MSP_RC(input_rc);
+				this->Send(MSP_RC, &msg, sizeof(msp_rc_t));
+			}
+		}
+
+	}
+
+	// MSP_STATUS
+	{
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		const auto msg = msp_osd::construct_MSP_STATUS(vehicle_status);
+		this->Send(MSP_STATUS, &msg, sizeof(msp_status_t));
+	}
+
 	subcmd = MSP_DP_DRAW_SCREEN;
 	this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1);
 }
@@ -443,6 +488,58 @@ void MspOsd::Send(const unsigned int message_type, const void *payload, int32_t
 	}
 }
 
+void MspOsd::Receive()
+{
+	uint8_t packet[255];
+	uint8_t message_id;
+	int ret;
+
+	while ((ret = _msp.Receive(packet, &message_id)) != -EWOULDBLOCK) {
+		if (ret >= 0) {
+			switch (message_id) {
+
+			case MSP_SET_VTX_CONFIG: {
+					if (ret == 0xF) {
+						memcpy((void *)&vtx_config, packet, sizeof(vtx_config));
+						has_vtx_config = true;
+					}
+
+					break;
+				}
+
+			case MSP_SET_VTXTABLE_BAND: {
+					msp_set_vtxtable_band_t *band_info = (msp_set_vtxtable_band_t *)&packet[0];
+
+					// Only supported fixed name lenght and < 8 channels for now
+					if (band_info->band <= BAND_COUNT && band_info->band_name_length == 8 && band_info->channel_count <= 8) {
+						memcpy((void *)&vtx_bands[band_info->band - 1], packet, sizeof(msp_set_vtxtable_band_t));
+
+						if (has_vtx_config && band_info->band == vtx_config.band_count) {
+							has_vtx_bands = true;
+						}
+					}
+
+					break;
+				}
+
+			case MSP_SET_VTXTABLE_POWERLEVEL: {
+					if ((packet[0] - 1) < POWER_LEVEL_COUNT) {
+						memcpy((void *)&power_levels[packet[0] - 1], packet, sizeof(msp_set_vtxtable_powerlevel_t));
+						has_power_config = true;
+					}
+
+					break;
+				}
+
+			default:
+				break;
+
+			}
+		}
+	}
+
+}
+
 void MspOsd::parameters_update()
 {
 	// update our display rate and dwell time
@@ -523,11 +620,102 @@ int MspOsd::print_status()
 	_display.get(msg, hrt_absolute_time());
 	PX4_INFO("Current message: \n\t%s", msg);
 
+	if (has_vtx_config) {
+		PX4_INFO("=== VTX Configuration ===");
+
+		if (has_vtx_bands) {
+			PX4_INFO("Channel: %c%u", vtx_bands[vtx_config.user_band - 1].band_letter, vtx_config.user_channel);
+
+		} else {
+			PX4_INFO("Band: %u", vtx_config.user_band);
+			PX4_INFO("Channel: %u", vtx_config.user_channel);
+		}
+
+		PX4_INFO("Frequency: %u MHz", vtx_config.user_freq);
+
+		if (has_power_config && (vtx_config.power_level - 1) < POWER_LEVEL_COUNT) {
+			PX4_INFO("Transmit power: %.*s mW", power_levels[vtx_config.power_level - 1].power_label_length,
+				 power_levels[vtx_config.power_level - 1].power_label_name);
+
+		} else {
+			PX4_INFO("Power Level: %u/%u", vtx_config.power_level,  vtx_config.power_count);
+
+		}
+
+		PX4_INFO("PIT Mode: %s", vtx_config.pit_mode ? "On" : "Off");
+
+		const char *disarm_modes[] = {
+			"Off",
+			"Always",
+			"Until First Arm"
+		};
+
+		if (vtx_config.low_power_disarm < 3) {
+			PX4_INFO("Low Power Disarm: %s", disarm_modes[vtx_config.low_power_disarm]);
+
+		} else {
+			PX4_INFO("Low Power Disarm: Unknown (%u)", vtx_config.low_power_disarm);
+		}
+
+		PX4_INFO("PIT Frequency: %u MHz", vtx_config.pit_freq);
+
+	} else {
+		PX4_INFO("No VTX Configuration available, can't do channel switching");
+	}
+
 	return 0;
 }
 
+int MspOsd::set_channel(char *new_channel)
+{
+	char band_letter = toupper(new_channel[0]);
+
+	if (!has_vtx_bands) {
+		return -2;
+	}
+
+	for (int i = 0; i < BAND_COUNT; i++) {
+		if (vtx_bands[i].band != 0) {
+			if (band_letter == toupper(vtx_bands[i].band_letter)) {
+				int channel = atoi(&new_channel[1]);
+
+				if (channel > 0 && channel <= vtx_config.channel_count && vtx_bands[i].frequency[channel - 1] != 0) {
+					vtx_config.user_band = vtx_bands[i].band;
+					vtx_config.user_channel = channel;
+					vtx_config.user_freq = vtx_bands[i].frequency[channel - 1];
+					change_channel = true;
+					return 0;
+				}
+
+			}
+		}
+	}
+
+	return -1;
+}
+
 int MspOsd::custom_command(int argc, char *argv[])
 {
+	if (argc > 0 && strcmp("channel", argv[0]) == 0) {
+		if (argc == 1) {
+			PX4_INFO("Please provide a channel");
+
+		} else if (is_running() && _object.load()) {
+			MspOsd *object = _object.load();
+			int ret = object->set_channel(argv[1]);
+
+			if (ret == -1) {
+				PX4_INFO("Channel not found");
+
+			} else if (ret == -2) {
+				PX4_INFO("No VTX Channel table available");
+			}
+
+		} else {
+			PX4_INFO("not running");
+		}
+	}
+
 	return 0;
 }
 
@@ -553,6 +741,7 @@ $ msp_osd
 
 	PRINT_MODULE_USAGE_NAME("msp_osd", "driver");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	PRINT_MODULE_USAGE_COMMAND_DESCR("channel", "Change VTX channel");
 
 	return 0;
 }

src/drivers/osd/msp_osd/msp_osd.hpp

@@ -64,6 +64,9 @@ using namespace time_literals;
 // location to "hide" unused display elements
 #define LOCATION_HIDDEN 234;
 
+#define POWER_LEVEL_COUNT 5
+#define BAND_COUNT 7
+
 struct PerformanceData {
 	bool initialization_problems{false};
 	long unsigned int successful_sends{0};
@@ -118,6 +121,8 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
+	int set_channel(char *new_channel);
+
 private:
 	void Run() override;
 
@@ -125,6 +130,9 @@ private:
 	void Send(const unsigned int message_type, const void *payload);
 	void Send(const unsigned int message_type, const void *payload, int32_t payload_size);
 
+	// receive vtx data
+	void Receive();
+
 	// send full configuration to MSP (triggers the actual update)
 	void SendConfig();
 	void SendTelemetry();
@@ -166,10 +174,19 @@ private:
 		(ParamInt<px4::params::OSD_CH_HEIGHT>) _param_osd_ch_height,
 		(ParamInt<px4::params::OSD_SCROLL_RATE>) _param_osd_scroll_rate,
 		(ParamInt<px4::params::OSD_DWELL_TIME>) _param_osd_dwell_time,
-		(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level
+		(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level,
+		(ParamInt<px4::params::OSD_RC_STICK>) _param_osd_rc_stick
 	)
 
 	// metadata
 	char _device[64] {};
 	PerformanceData _performance_data{};
+
+	msp_set_vtx_config_t vtx_config;
+	msp_set_vtxtable_powerlevel_t power_levels[POWER_LEVEL_COUNT];
+	msp_set_vtxtable_band_t vtx_bands[BAND_COUNT] {};
+	bool has_vtx_config {false};
+	bool has_power_config {false};
+	bool has_vtx_bands {false};
+	bool change_channel {false};
 };

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -214,14 +214,8 @@ msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc)
 	rssi.screenYPosition = 0x02;
 	rssi.screenXPosition = 0x02;
 
-	int len = snprintf(&rssi.str[0], sizeof(rssi.str) - 1, "%d", input_rc.link_quality);
-
-	if (len >= 3) {
-		rssi.str[3] = '%';
-
-	} else {
-		rssi.str[len] = '%';
-	}
+	snprintf(&rssi.str[0], sizeof(rssi.str), "%3d", input_rc.link_quality);
+	rssi.str[3] = '%';
 
 	return rssi;
 }
@@ -569,4 +563,30 @@ msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA()
 	return esc_sensor_data;
 }
 
+msp_rc_t construct_MSP_RC(const input_rc_s &input_rc)
+{
+	// initialize result
+	msp_rc_t rc;
+
+	rc.channelValue[0] = input_rc.values[0]; // roll
+	rc.channelValue[1] = input_rc.values[1]; // pitch
+	rc.channelValue[2] = input_rc.values[3]; // yaw
+	rc.channelValue[3] = input_rc.values[2]; // Throttle
+	return rc;
+}
+
+msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status)
+{
+	// initialize result
+	msp_status_t status{0};
+
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		status.flightModeFlags |= (1 << MSP_MODE_ARM);
+	}
+
+	return status;
+}
+
+
+
 } // namespace msp_osd

src/drivers/osd/msp_osd/uorb_to_msp.hpp

@@ -126,4 +126,10 @@ msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_
 // construct an MSP_ESC_SENSOR_DATA struct
 msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();
 
+// construct an MSP_RC struct
+msp_rc_t construct_MSP_RC(const input_rc_s &input_rc);
+
+// construct an MSP_STATUS struct
+msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status);
+
 } // namespace msp_osd

src/modules/commander/Commander.cpp

@@ -2255,9 +2255,7 @@ void Commander::handleAutoDisarm()
 		// Check for auto-disarm on landing or pre-flight
 		if (_param_com_disarm_land.get() > 0 || _param_com_disarm_prflt.get() > 0) {
 
-			const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
-							 && !_mission_result_sub.get().finished;
-			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
+			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !_mission_in_progress
 							      && !_config_overrides.disable_auto_disarm;
 
 			if (auto_disarm_land_enabled && _have_taken_off_since_arming) {

src/modules/ekf2/CMakeLists.txt

@@ -211,7 +211,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/jake_range_height_control.cpp
+		EKF/aid_sources/range_finder/range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -124,7 +124,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/jake_range_height_control.cpp
+		aid_sources/range_finder/range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Sensor.hpp
+ * Abstract class for sensors
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ *
+ */
+
+#ifndef EKF_SENSOR_HPP
+#define EKF_SENSOR_HPP
+
+#include <cstdint>
+
+namespace estimator
+{
+namespace sensor
+{
+
+class Sensor
+{
+public:
+	virtual ~Sensor() {};
+
+	/*
+	 * run sanity checks on the current data
+	 * this has to be called immediately after
+	 * setting new data
+	 */
+	virtual void runChecks() {};
+
+	/*
+	 * return true if the sensor is healthy
+	 */
+	virtual bool isHealthy() const = 0;
+
+	/*
+	 * return true if the delayed sample is healthy
+	 * and can be fused in the estimator
+	 */
+	virtual bool isDataHealthy() const = 0;
+
+	/*
+	 * return true if the sensor data rate is
+	 * stable and high enough
+	 */
+	virtual bool isRegularlySendingData() const = 0;
+};
+
+} // namespace sensor
+} // namespace estimator
+#endif // !EKF_SENSOR_HPP

src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp

@@ -1,425 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file range_height_control.cpp
- * Control functions for ekf range finder height fusion
- */
-
-#include "ekf.h"
-#include "ekf_derivation/generated/compute_hagl_h.h"
-#include "ekf_derivation/generated/compute_hagl_innov_var.h"
-
-void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
-{
-	// Check if rangefinder is available/enabled
-	if (!_range_buffer) {
-		// PX4_INFO("no buff");
-		return;
-	}
-
-	// Pop rangefinder measurement from buffer of samples into active sample
-	sensor::rangeSample sample = {};
-	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
-		if (_range_sensor.timedOut(imu_sample.time_us)) {
-			// Disable fusion if it's currently enabled
-			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
-				PX4_INFO("stopping RNG fusion, sensor timed out");
-				stopRangeAltitudeFusion();
-				stopRangeTerrainFusion();
-			}
-			// PX4_INFO("timed out1");
-		}
-		return;
-	}
-
-	// PX4_INFO("got one!");
-	// PX4_INFO("rng: %f", (double)sample.rng);
-	// PX4_INFO("quality: %d", sample.quality);
-
-	// Set the raw sample
-	_range_sensor.setSample(sample);
-
-	// TODO: move setting params to init function
-	// Set all of the parameters
-	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
-	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-	_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-
-	// Update sensor to earth rotation
-	_range_sensor.updateSensorToEarthRotation(_R_to_earth);
-
-
-	// TODO: refactor into validity_checks()
-	// Gate sample consumption on these checks
-	bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
-	bool tilt_ok = _range_sensor.isTiltOk();
-	bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
-	// - Not stuck value
-	// - Not fog detected
-
-	// If quality, tilt, or value are outside of bounds -- throw away measurement
-	if (!quality_ok || !tilt_ok || !range_ok) {
-		if (_range_sensor.timedOut(imu_sample.time_us)) {
-			// Disable fusion if it's currently enabled
-			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
-				PX4_INFO("stopping RNG fusion, sensor data invalid");
-				stopRangeAltitudeFusion();
-				stopRangeTerrainFusion();
-			}
-			PX4_INFO("timed out2");
-		}
-
-		if (!quality_ok) {
-			PX4_INFO("!quality_ok");
-		}
-		if (!tilt_ok) {
-			PX4_INFO("!tilt_ok");
-		}
-		if (!range_ok) {
-			PX4_INFO("!range_ok");
-		} // commander takeoff
-		return;
-	}
-
-
-	// TODO: refactor into apply_body_offset()
-	// Correct the range data for position offset relative to the IMU
-	const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
-	const Vector3f imu_pos_body = _params.imu_pos_body;
-	const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
-	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-	sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
-
-	// Provide sample from buffer to object
-	_range_sensor.setSample(sample);
-
-
-	// TODO: refactor into consintency_filter_update() that runs consistency status
-	// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
-	_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
-
-	const float z = _gpos.altitude();
-	const float vz = _state.vel(2);
-	const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame
-	const float dist_var = 0.05;
-	const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
-	const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
-
-	// Run the kinematic consistency check
-	_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
-
-	// Track kinematic consistency
-	// _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-
-	// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
-	updateRangeHagl(_aid_src_rng_hgt);
-
-	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
-		PX4_INFO("INFINIFY OBSERVATION INVALID");
-	}
-
-	// Fuse Range data as Primary height source
-	// NOTE: terrain is not estimated in this mode
-	if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
-		fuseRangeAsHeightSource();
-	} else {
-		// Fuse Range data as aiding height source (Primary GPS or Baro)
-		fuseRangeAsHeightAiding();
-	}
-}
-
-void Ekf::fuseRangeAsHeightSource()
-{
-	// When primary height source is RANGE, we always fuse it
-	// I don't think conditional range aid mode makes sense in the context of RANGE = primary
-
-	// Fusion init logic
-	if (_height_sensor_ref != HeightSensor::RANGE) {
-
-		_height_sensor_ref = HeightSensor::RANGE;
-		_information_events.flags.reset_hgt_to_rng = true;
-
-		// Reset aid source innovation
-		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-
-		// Reset altitude to RANGE
-		resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
-		_control_status.flags.rng_hgt = true;
-
-		// Cannot have terrain estimate fusion while RANGE is primary
-		stopRangeTerrainFusion();
-		_state.terrain = 0.f;
-
-		// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
-		// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
-	}
-
-
-}
-
-void Ekf::fuseRangeAsHeightAiding()
-{
-	bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL;
-	bool range_aid_enabled = _params.rng_ctrl == RngCtrl::ENABLED;
-
-	bool range_aid_conditions_passed = rangeAidConditionsPassed();
-	bool kinematically_consistent = _control_status.flags.rng_kin_consistent;
-
-	bool do_range_aid = kinematically_consistent &&
-						(range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed));
-
-	bool fuse_measurement = false;
-
-
-	// Variables to use below
-	bool innovation_rejected = _aid_src_rng_hgt.innovation_rejected;
-	bool optical_flow_for_terrain = _control_status.flags.opt_flow_terrain;
-
-	// Fuse Range into Altitude if:
-	// - passes range_aid_conditionalchecks
-	// - kinematically consistent
-	if (do_range_aid) {
-		// Start fusion
-		if (!_control_status.flags.rng_hgt) {
-			// Fusion init logic
-			PX4_INFO("starting RNG Altitude fusion");
-			_control_status.flags.rng_hgt = true;
-			_height_sensor_ref = HeightSensor::RANGE;
-
-			// TODO: do we really need to stop terrain fusion here?
-			stopRangeTerrainFusion();
-
-			// TODO: review for correctness
-			if (innovation_rejected && kinematically_consistent) {
-				// Reset aid source
-				PX4_INFO("range alt fusion, resetting aid source");
-				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-			}
-		}
-
-		// Fuse
-		fuse_measurement = true;
-
-	} else {
-		// Stop fusion
-		stopRangeAltitudeFusion();
-
-		// if (!range_aid_conditions_passed) {
-		// 	PX4_INFO("range aid conditions failed");
-		// }
-		// if (!kinematically_consistent) {
-		// 	PX4_INFO("kinematically inconsistent");
-		// }
-	}
-
-	// Fuse Range into Terrain if:
-	// - kinematically consistent
-	if (kinematically_consistent) {
-
-		// Start fusion
-		if (!_control_status.flags.rng_terrain) {
-			// Fusion init logic
-			PX4_INFO("starting RNG Terrain fusion");
-			_control_status.flags.rng_terrain = true;
-
-			static bool first_init = true;
-			// Reset terrain when we first init
-			if (!optical_flow_for_terrain && first_init) {
-				first_init = false;
-				// Reset aid source and then reset terrain estimate
-				PX4_INFO("FIRST range terrain fusion, resetting terrain to range");
-				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
-				resetTerrainToRng(_aid_src_rng_hgt);
-			}
-		}
-
-		// Reset terrain to range if innovation is rejected
-		if (!optical_flow_for_terrain && innovation_rejected
-				&& kinematically_consistent) {
-			PX4_INFO("range terrain fusion, resetting terrain to range");
-			resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-			resetTerrainToRng(_aid_src_rng_hgt);
-		}
-
-		// Fuse
-		fuse_measurement = true;
-
-	} else {
-		// Stop fusion
-		stopRangeTerrainFusion();
-	}
-
-	if (fuse_measurement) {
-		// PX4_INFO("fusing: %f", (double)_aid_src_rng_hgt.observation);
-		// PX4_INFO("_control_status.flags.rng_hgt: %d", _control_status.flags.rng_hgt);
-		// PX4_INFO("_control_status.flags.rng_terrain: %d", _control_status.flags.rng_terrain);
-		fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
-
-		// commander takeoff
-
-		// TODO: _height_sensor_ref == HeightSensor::RANGE  is redundant
-		if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
-			PX4_INFO("RNG height fusion reset required, all height sources failing");
-
-			uint64_t timestamp = _aid_src_rng_hgt.timestamp;
-			_information_events.flags.reset_hgt_to_rng = true;
-			resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
-			resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
-
-			// reset vertical velocity if no valid sources available
-			if (!isVerticalVelocityAidingActive()) {
-				PX4_INFO("resetting vertical velocity");
-				resetVerticalVelocityToZero();
-			}
-
-			_aid_src_rng_hgt.time_last_fuse = timestamp;
-		}
-	}
-}
-
-bool Ekf::rangeAidConditionsPassed()
-{
-	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-	float range_hagl_max = _params.max_hagl_for_range_aid;
-	float max_vel_xy = _params.max_vel_for_range_aid;
-
-	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
-
-	bool is_hagl_stable = (hagl_test_ratio < 1.f);
-
-	if (!_control_status.flags.rng_hgt) {
-		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
-		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
-		is_hagl_stable = (hagl_test_ratio < 0.01f);
-	}
-
-	const bool is_in_range = (getHagl() < range_hagl_max);
-
-	bool is_below_max_speed = true;
-
-	if (isHorizontalAidingActive()) {
-		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
-	}
-
-	return is_in_range && is_hagl_stable && is_below_max_speed;
-}
-
-void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
-{
-	// Since the distance is not a direct observation of the terrain state but is based
-	// on the height state, a reset should consider the height uncertainty. This can be
-	// done by manipulating the Kalman gain to inject all the innovation in the terrain state
-	// and create the correct correlation with the terrain state with a covariance update.
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
-
-	const float old_terrain = _state.terrain;
-
-	VectorState H;
-	sym::ComputeHaglH(&H);
-
-	VectorState K;
-	K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
-
-	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
-
-	// record the state change
-	const float delta_terrain = _state.terrain - old_terrain;
-
-	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
-		_state_reset_status.hagl_change = delta_terrain;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.hagl_change += delta_terrain;
-	}
-
-	_state_reset_status.reset_count.hagl++;
-
-	aid_src.time_last_fuse = _time_delayed_us;
-}
-
-void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
-{
-	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
-	const float measurement_variance = getRngVar();
-
-	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
-
-	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
-	updateAidSourceStatus(aid_src,
-			      _range_sensor.sample()->time_us, // sample timestamp
-			      measurement,                               // observation
-			      measurement_variance,                      // observation variance
-			      getHagl() - measurement,                   // innovation
-			      innovation_variance,                       // innovation variance
-			      innov_gate);                               // innovation gate
-
-	// z special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
-		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
-	}
-}
-
-float Ekf::getRngVar() const
-{
-	const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
-	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
-	return dist_var;
-}
-
-void Ekf::stopRangeAltitudeFusion()
-{
-	if (_control_status.flags.rng_hgt) {
-		PX4_INFO("stopping RNG Altitude fusion");
-		_control_status.flags.rng_hgt = false;
-		if (_height_sensor_ref == HeightSensor::RANGE) {
-			_height_sensor_ref = HeightSensor::UNKNOWN;
-		}
-	}
-}
-
-void Ekf::stopRangeTerrainFusion()
-{
-	if (_control_status.flags.rng_terrain) {
-		PX4_INFO("stopping RNG Terrain fusion");
-		_control_status.flags.rng_terrain = false;
-	}
-}

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

@@ -36,123 +36,56 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
-#include "ekf_derivation/generated/range_validation_filter_h.h"
-#include "ekf_derivation/generated/range_validation_filter_P_init.h"
 
-using namespace matrix;
-
-void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
-				       const float dist_bottom_var)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
+		bool horizontal_motion, uint64_t time_us)
 {
-	printf("RangeFinderConsistencyCheck::init\n");
+	if (horizontal_motion) {
+		_time_last_horizontal_motion = time_us;
+	}
 
-	_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
-	_Ht = sym::RangeValidationFilterH<float>();
-
-	_x(RangeFilter::z.idx) = z;
-	_x(RangeFilter::terrain.idx) = z - dist_bottom;
-	_initialized = true;
-	_test_ratio_lpf.reset(0.f);
-	_t_since_first_sample = 0.f;
-}
-
-void RangeFinderConsistencyCheck::update(const float z, const float z_var, const float vz, const float vz_var,
-		const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
-{
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
-	if (dt > 1.f) {
+	if ((_time_last_update_us == 0)
+	    || (dt < 0.001f) || (dt > 0.5f)) {
 		_time_last_update_us = time_us;
-		_initialized = false;
-		return;
-
-	} else if (!_initialized) {
-		init(z, z_var, dist_bottom, dist_bottom_var);
+		_dist_bottom_prev = dist_bottom;
 		return;
 	}
 
-	// prediction step
+	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
+	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
+
+	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	const float var = 2.f * dist_bottom_var / (dt * dt);
+	_innov_var = var + vz_var;
+
+	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
+	_test_ratio = normalized_innov_sq / (_gate * _gate);
+	_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
+	const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
+	_signed_test_ratio_lpf.update(signed_test_ratio);
+
+	updateConsistency(vz, time_us);
+
 	_time_last_update_us = time_us;
-	_x(RangeFilter::z.idx) -= dt * vz;
-	_P(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
-	_P(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
+	_dist_bottom_prev = dist_bottom;
+}
 
-	// iterate through both measurements (z and dist_bottom)
-	const Vector2f measurements{z, dist_bottom};
-
-	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
-
-		// dist_bottom
-		Vector2f H = _Ht;
-		float R = dist_bottom_var;
-
-		// z, direct state measurement
-		if (measurement_idx == 0) {
-			H(RangeFilter::z.idx) = 1.f;
-			H(RangeFilter::terrain.idx) = 0.f;
-			R = z_var;
+void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
+{
+	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
+		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
+			_is_kinematically_consistent = false;
+			_time_last_inconsistent_us = time_us;
 		}
 
-		// residual
-		const float measurement_pred = H * _x;
-		const float y = measurements(measurement_idx) - measurement_pred;
-
-		// for H as col-vector:
-		// innovation variance S = H^T * P * H + R
-		// kalman gain K = P * H / S
-		const float S = (H.transpose() * _P * H + R)(0, 0);
-		Vector2f K = (_P * H / S);
-
-		if (measurement_idx == 0) {
-			K(RangeFilter::z.idx) = 1.f;
-
-		} else if (measurement_idx == 1) {
-			_innov = y;
-			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
-			_test_ratio_lpf.setParameters(dt, _t_to_init);
-			_test_ratio_lpf.update(sign(_innov) * test_ratio);
+	} else {
+		if ((fabsf(vz) > _min_vz_for_valid_consistency)
+		    && (_test_ratio < 1.f)
+		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
+		   ) {
+			_is_kinematically_consistent = true;
 		}
-
-		// update step
-		_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
-		_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y;
-
-		// covariance update with Joseph form:
-		// P = (I - K H) P (I - K H)^T + K R K^T
-		Matrix2f I;
-		I.setIdentity();
-		Matrix2f IKH = I - K.multiplyByTranspose(H);
-		_P = IKH * _P * IKH.transpose() + (K * R).multiplyByTranspose(K);
 	}
-
-	evaluateState(dt, vz, vz_var);
-}
-
-void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
-{
-	// start the consistency check after 1s
-	if (_t_since_first_sample < _t_to_init) {
-		_t_since_first_sample += dt;
-		return;
-	}
-
-	if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
-		printf("_test_ratio_lpf failed (>1)\n");
-		_t_since_first_sample = 0.f;
-		_state = KinematicState::INCONSISTENT;
-		return;
-	}
-
-	_state = KinematicState::CONSISTENT;
-}
-
-void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var,
-				      const float dist_bottom, const float dist_bottom_var, const uint64_t time_us)
-{
-	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
-		_last_posD_reset_count = current_posD_reset_count;
-		_initialized = false;
-	}
-
-	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
 }

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp

@@ -48,61 +48,36 @@ public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	enum class KinematicState : int {
-		INCONSISTENT = 0,
-		CONSISTENT = 1,
-		UNKNOWN = 2
-	};
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
 
-	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
-	float getInnov() const { return _initialized ? _innov : 0.f; }
-	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
-	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
-	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
-	void setGate(const float gate) { _gate = gate; }
-	void run(float z, float z_var, float vz, float vz_var,
-		 float dist_bottom, float dist_bottom_var, uint64_t time_us);
-	void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
-	void reset()
-	{
-		if (_initialized && _state == KinematicState::CONSISTENT) {
-			_state = KinematicState::UNKNOWN;
-		}
+	void setGate(float gate) { _gate = gate; }
 
-		_initialized = false;
-	}
-
-	uint8_t current_posD_reset_count{0};
+	float getTestRatio() const { return _test_ratio; }
+	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
+	float getInnov() const { return _innov; }
+	float getInnovVar() const { return _innov_var; }
+	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
 
 private:
+	void updateConsistency(float vz, uint64_t time_us);
 
-	void update(float z, float z_var, float vz, float vz_var, float dist_bottom,
-		    float dist_bottom_var, uint64_t time_us);
-	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
-	void evaluateState(float dt, float vz, float vz_var);
-	float _terrain_process_noise{0.0f};
-	matrix::SquareMatrix<float, 2> _P{};
-	matrix::Vector2f _Ht{};
-	matrix::Vector2f _x{};
-	bool _initialized{false};
-	float _innov{0.f};
-	float _innov_var{0.f};
-	uint64_t _time_last_update_us{0};
-	static constexpr float time_constant{1.f};
-	AlphaFilter<float> _test_ratio_lpf{time_constant};
-	float _gate{1.0f};
-	KinematicState _state{KinematicState::UNKNOWN};
-	float _t_since_first_sample{0.f};
-	uint8_t _last_posD_reset_count{0};
-	static constexpr float _t_to_init{1.f};
+	uint64_t _time_last_update_us{};
+	float _dist_bottom_prev{};
+
+	float _test_ratio{};
+	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
+	float _gate{.2f};
+	float _innov{};
+	float _innov_var{};
+
+	bool _is_kinematically_consistent{true};
+	uint64_t _time_last_inconsistent_us{};
+	uint64_t _time_last_horizontal_motion{};
+
+	static constexpr float _signed_test_ratio_tau = 2.f;
+
+	static constexpr float _min_vz_for_valid_consistency = .5f;
+	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
 };
 
-namespace RangeFilter
-{
-struct IdxDof { unsigned idx; unsigned dof; };
-static constexpr IdxDof z{0, 1};
-static constexpr IdxDof terrain{1, 1};
-static constexpr uint8_t size{2};
-}
-
 #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -44,23 +44,18 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
-	if (!_range_buffer) {
-		return;
-	}
+	bool rng_data_ready = false;
 
-	// Get range data from buffer and check validity
-	bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample());
-
-	_range_sensor.setSample();
-
-	_range_sensor.setDataReadiness(rng_data_ready);
-
-	if (_range_sensor.isDataReady()) {
+	if (_range_buffer) {
+		// Get range data from buffer and check validity
+		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
+		_range_sensor.setDataReadiness(rng_data_ready);
 
+		// update range sensor angle parameters in case they have changed
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 		_range_sensor.setMaxFogDistance(_params.rng_fog);
-		_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
@@ -70,35 +65,39 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-			// TODO: this is a constant
-			const float dist_var = getRngVar();
-			_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
+			if (_control_status.flags.in_air) {
+				const bool horizontal_motion = _control_status.flags.fixed_wing
+							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
 
-			const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
-			const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
+				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+				const float var = sq(_params.range_noise) + dist_dependant_var;
 
-			// TODO: review -- variance
-			_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
-						   dist_var, imu_sample.time_us);
-
-		} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
-			_range_sensor.setRange(_params.rng_gnd_clearance);
-			_range_sensor.setValidity(true);
+				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
+							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
+			}
 
 		} else {
-			_rng_consistency_check.reset();
+			// If we are supposed to be using range finder data but have bad range measurements
+			// and are on the ground, then synthesise a measurement at the expected on ground value
+			if (!_control_status.flags.in_air
+			    && _range_sensor.isRegularlySendingData()
+			    && _range_sensor.isDataReady()) {
+
+				_range_sensor.setRange(_params.rng_gnd_clearance);
+				_range_sensor.setValidity(true); // bypass the checks
+			}
 		}
+
+		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	} else {
+		return;
 	}
 
-	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-
-	_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
-						&& _rng_consistency_check.isNotKinematicallyInconsistent();
-
-
 	auto &aid_src = _aid_src_rng_hgt;
 
-	if (_range_sensor.isDataReady() && _range_sensor.sample()) {
+	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -108,26 +107,23 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isNotKinematicallyInconsistent();
-
-		// SUS: _rng_consistency_check.isNotKinematicallyInconsistent()
+				&& _rng_consistency_check.isKinematicallyConsistent();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
+
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
 
-		// SUS: isConditionalRangeAidSuitable()
-
 		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
 
 		if (_control_status.flags.rng_hgt) {
 			if (!(do_conditional_range_aid || do_range_aid)) {
-				PX4_INFO("stopping %s fusion", HGT_SRC_NAME);
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
 				stopRngHgtFusion();
 			}
 
@@ -135,14 +131,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
 				if (do_conditional_range_aid) {
 					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
-					PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
-					    && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -150,7 +145,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} else if (do_range_aid) {
 					// Range finder is the primary height source, the ground is now the datum used
 					// to compute the local vertical position
-					PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 
 					_information_events.flags.reset_hgt_to_rng = true;
@@ -165,11 +160,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 			} else {
 				if (do_conditional_range_aid || do_range_aid) {
-					PX4_INFO("starting %s height fusion", HGT_SRC_NAME);
+					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
-					    && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -186,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 				if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
 					// All height sources are failing
-					PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
+					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_rng = true;
 					resetAltitudeTo(aid_src.observation - _state.terrain);
@@ -202,18 +196,18 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
 					if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) {
-						PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
+					} else {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 				}
 
 			} else {
-				PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}
@@ -227,7 +221,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
+					if (aid_src.innovation_rejected) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -240,7 +234,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 	} else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain)
 		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME);
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
 		stopRngTerrFusion();
 	}
@@ -256,7 +250,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
 	updateAidSourceStatus(aid_src,
-			      _range_sensor.sample()->time_us, // sample timestamp
+			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
 			      measurement,                               // observation
 			      measurement_variance,                      // observation variance
 			      getHagl() - measurement,                   // innovation
@@ -274,9 +268,11 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
-	return dist_var;
+	return fmaxf(
+		       P(State::pos.idx + 2, State::pos.idx + 2)
+		       + sq(_params.range_noise)
+		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
+		       0.f);
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -47,34 +47,10 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::setSample(const rangeSample &sample)
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
 {
-	_sample = sample;
-}
-
-bool SensorRangeFinder::timedOut(uint64_t time_now) const
-{
-	if (_sample.time_us > time_now) {
-		return false;
-	}
-
-	// TODO: 200ms?
-	return time_now > _sample.time_us + 200'000;
-}
-
-void SensorRangeFinder::setPitchOffset(float new_pitch_offset)
-{
-	if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
-		_sin_pitch_offset = sinf(new_pitch_offset);
-		_cos_pitch_offset = cosf(new_pitch_offset);
-		_pitch_offset_rad = new_pitch_offset;
-	}
-}
-
-void SensorRangeFinder::setLimits(float min_distance, float max_distance)
-{
-	_rng_valid_min_val = min_distance;
-	_rng_valid_max_val = max_distance;
+	updateSensorToEarthRotation(R_to_earth);
+	updateValidity(current_time_us);
 }
 
 void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -84,5 +60,112 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear
 	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
 }
 
+void SensorRangeFinder::updateValidity(uint64_t current_time_us)
+{
+	updateDtDataLpf(current_time_us);
+
+	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
+		_is_sample_valid = false;
+		_is_regularly_sending_data = false;
+		return;
+	}
+
+	_is_regularly_sending_data = true;
+
+	// Don't run the checks unless we have retrieved new data from the buffer
+	if (_is_sample_ready) {
+		_is_sample_valid = false;
+
+		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
+
+		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
+			return;
+		}
+
+		updateStuckCheck();
+		updateFogCheck(getDistBottom(), _sample.time_us);
+
+		if (!_is_stuck && !_is_blocked) {
+			_is_sample_valid = true;
+			_time_last_valid_us = _sample.time_us;
+		}
+	}
+}
+
+bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
+{
+	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
+}
+
+void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
+{
+	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
+	float alpha = 0.5f * _dt_update;
+	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
+
+	// Apply spike protection to the filter state.
+	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
+}
+
+inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
+{
+	return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
+}
+
+inline bool SensorRangeFinder::isDataInRange() const
+{
+	return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
+}
+
+void SensorRangeFinder::updateStuckCheck()
+{
+	if (!isStuckDetectorEnabled()) {
+		// Stuck detector disabled
+		_is_stuck = false;
+		return;
+	}
+
+	// Check for "stuck" range finder measurements when range was not valid for certain period
+	// This handles a failure mode observed with some lidar sensors
+	if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {
+
+		// require a variance of rangefinder values to check for "stuck" measurements
+		if (_stuck_max_val - _stuck_min_val > _stuck_threshold) {
+			_stuck_min_val = 0.0f;
+			_stuck_max_val = 0.0f;
+			_is_stuck = false;
+
+		} else {
+			if (_sample.rng > _stuck_max_val) {
+				_stuck_max_val = _sample.rng;
+			}
+
+			if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) {
+				_stuck_min_val = _sample.rng;
+			}
+
+			_is_stuck = true;
+		}
+	}
+}
+
+void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
+{
+	if (_max_fog_dist > 0.f) {
+
+		const float median_dist = _median_dist.apply(dist_bottom);
+		const float factor = 2.f; // magic hardcoded factor
+
+		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
+			_is_blocked = true;
+
+		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
+			_is_blocked = false;
+		}
+
+		_prev_median_dist = median_dist;
+	}
+}
+
 } // namespace sensor
 } // namespace estimator

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -41,6 +41,8 @@
 #ifndef EKF_SENSOR_RANGE_FINDER_HPP
 #define EKF_SENSOR_RANGE_FINDER_HPP
 
+#include "Sensor.hpp"
+
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/MedianFilter.hpp>
 
@@ -55,62 +57,105 @@ struct rangeSample {
 	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
 };
 
+static constexpr uint64_t RNG_MAX_INTERVAL =
+	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 
-class SensorRangeFinder
+class SensorRangeFinder : public Sensor
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() = default;
+	~SensorRangeFinder() override = default;
 
-	struct Parameters {
-		float ekf2_imu_pos_x{};
-		float ekf2_imu_pos_y{};
-		float ekf2_imu_pos_z{};
-
-		float ekf2_rng_pos_x{};
-		float ekf2_rng_pos_y{};
-		float ekf2_rng_pos_z{};
-
-		float ekf2_rng_pitch{};
-
-		float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt
-	};
-
-	void updateParameters(Parameters& parameters) { _parameters = parameters; };
-
-	bool timedOut(uint64_t time_now) const;
-	void setSample(const rangeSample &sample);
+	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
+	bool isHealthy() const override { return _is_sample_valid; }
+	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
+	bool isDataReady() const { return _is_sample_ready; }
+	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
+	bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
 
+	void setSample(const rangeSample &sample)
+	{
+		_sample = sample;
+		_is_sample_ready = true;
+	}
 
 	// This is required because of the ring buffer
 	// TODO: move the ring buffer here
-	rangeSample *sample() { return &_sample; }
+	rangeSample *getSampleAddress() { return &_sample; }
 
-	void setPitchOffset(float new_pitch_offset);
+	void setPitchOffset(float new_pitch_offset)
+	{
+		if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
+			_sin_pitch_offset = sinf(new_pitch_offset);
+			_cos_pitch_offset = cosf(new_pitch_offset);
+			_pitch_offset_rad = new_pitch_offset;
+		}
+	}
 
 	void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; }
 
+	void setLimits(float min_distance, float max_distance)
+	{
+		_rng_valid_min_val = min_distance;
+		_rng_valid_max_val = max_distance;
+	}
+
+	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
+
+	void setQualityHysteresis(float valid_quality_threshold_s)
+	{
+		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
+	}
+
 	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
 
-	void setLimits(float min_distance, float max_distance);
-
-	// void setRange(float rng) { _sample.rng = rng; }
-	// float getRange() const { return _sample.rng; }
+	void setRange(float rng) { _sample.rng = rng; }
+	float getRange() const { return _sample.rng; }
 
 	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
 
+	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
+	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
+
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
-	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
-
-	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
+	void setFaulty(bool faulty = true) { _is_faulty = faulty; }
 
 private:
+	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
+
+	void updateValidity(uint64_t current_time_us);
+	void updateDtDataLpf(uint64_t current_time_us);
+	bool isSampleOutOfDate(uint64_t current_time_us) const;
+	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
+	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
+	bool isDataInRange() const;
+	bool isQualityOk(uint64_t current_time_us) const;
+	void updateStuckCheck();
+	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
 	rangeSample _sample{};
 
-	Parameters _parameters{};
+	bool _is_sample_ready{};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
+	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
+	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
+	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
+	bool _is_faulty{false};         ///< the sensor should not be used anymore
+
+	/*
+	 * Stuck check
+	 */
+	bool _is_stuck{};
+	float _stuck_threshold{0.1f};	///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
+	float _stuck_min_val{};		///< minimum value for new rng measurement when being stuck
+	float _stuck_max_val{};		///< maximum value for new rng measurement when being stuck
+
+	/*
+	 * Data regularity check
+	 */
+	static constexpr float _dt_update{0.01f}; 	///< delta time since last ekf update TODO: this should be a parameter
+	float _dt_data_lpf{};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
 
 	/*
 	 * Tilt check
@@ -127,6 +172,20 @@ private:
 	float _rng_valid_min_val{};	///< minimum distance that the rangefinder can measure (m)
 	float _rng_valid_max_val{};	///< maximum distance that the rangefinder can measure (m)
 
+	/*
+	 * Quality check
+	 */
+	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
+	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+
+	/*
+	 * Fog check
+	 */
+	bool _is_blocked{false};
+	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
+	math::MedianFilter<float, 5> _median_dist{};
+	float _prev_median_dist{0.f};
+
 };
 
 } // namespace sensor

src/modules/ekf2/EKF/common.h

@@ -148,7 +148,7 @@ enum class GnssCtrl : uint8_t {
 	YAW  = (1 << 3)
 };
 
-enum RngCtrl : uint8_t {
+enum class RngCtrl : uint8_t {
 	DISABLED    = 0,
 	CONDITIONAL = 1,
 	ENABLED     = 2
@@ -415,19 +415,17 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float ekf2_rng_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
-	float ekf2_rng_sfe{0.0f};         ///< scaling from range measurement to noise (m/m)
+	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
+	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
+	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
-	float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
+	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
-	float ekf2_rng_pos_x{0.f};
-	float ekf2_rng_pos_y{0.f};
-	float ekf2_rng_pos_z{0.f};
-
+	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -572,6 +570,8 @@ uint64_t mag_fault               :
 		uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
 uint64_t gnd_effect              :
 		1; ///< 20 - true when protection from ground effect induced static pressure rise is active
+uint64_t rng_stuck               :
+		1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 uint64_t gnss_yaw                 :
 		1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
@@ -582,6 +582,8 @@ uint64_t synthetic_mag_z         :
 		uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
 uint64_t gnss_yaw_fault           :
 		1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
+uint64_t rng_fault               :
+		1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 uint64_t inertial_dead_reckoning :
 		1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements

src/modules/ekf2/EKF/covariance.cpp

@@ -227,10 +227,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
-#endif // CONFIG_EKF2_RANGE_FINDER
 	}
 
 #endif // CONFIG_EKF2_TERRAIN

src/modules/ekf2/EKF/ekf.cpp

@@ -77,6 +77,13 @@ void Ekf::reset()
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
 #endif // CONFIG_EKF2_TERRAIN
 
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+	_range_sensor.setMaxFogDistance(_params.rng_fog);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
@@ -383,18 +390,6 @@ void Ekf::updateParameters()
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.updateParameters();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
-#if defined (CONFIG_EKF2_RANGE_FINDER)
-
-	sensor::SensorRangeFinder::Parameters params = {};
-	// params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get();
-	// params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get();
-	// params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get();
-	// params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get();
-	// params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get();
-	// params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get();
-	params.ekf2_rng_pitch = _params.ekf2_rng_pitch;
-	_range_sensor.updateParameters(params);
-#endif
 }
 
 template<typename T>

src/modules/ekf2/EKF/ekf.h

@@ -117,7 +117,7 @@ public:
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
 	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -762,18 +762,9 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
-
-
-	void fuseRangeAsHeightSource();
-	void fuseRangeAsHeightAiding();
-
 	bool isConditionalRangeAidSuitable();
-	bool rangeAidConditionsPassed();
-
-
-	void stopRangeAltitudeFusion();
-	void stopRangeTerrainFusion();
-
+	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/estimator_interface.h

@@ -120,6 +120,8 @@ public:
 	{
 		_range_sensor.setLimits(min_distance, max_distance);
 	}
+
+	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,37 +721,6 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
-def range_validation_filter_h() -> sf.Matrix:
-
-    state = Values(
-        z=sf.Symbol("z"),
-        terrain=sf.Symbol("terrain")
-    )
-    dist_bottom = state["z"] - state["terrain"]
-
-    H = sf.M21()
-    H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose()
-
-    return H
-
-def range_validation_filter_P_init(
-        z_var: sf.Scalar,
-        dist_bottom_var: sf.Scalar
-    ) -> sf.Matrix:
-
-    H = range_validation_filter_h().T
-    # enforce terrain to match the measurement
-    K = sf.V2(0, 1/H[1])
-    R = sf.V1(dist_bottom_var)
-
-    # dist_bottom = z - terrain
-    P = sf.M22.diag([z_var, z_var + dist_bottom_var])
-    I = sf.M22.eye()
-    # Joseph form
-    P = (I - K * H) * P * (I - K * H).T + K * R * K.T
-
-    return P
-
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -783,7 +752,5 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
-generate_px4_function(range_validation_filter_h, output_names=None)
-generate_px4_function(range_validation_filter_P_init, output_names=None)
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h

@@ -1,46 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: range_validation_filter_p_init
- *
- * Args:
- *     z_var: Scalar
- *     dist_bottom_var: Scalar
- *
- * Outputs:
- *     res: Matrix22
- */
-template <typename Scalar>
-matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
-                                                       const Scalar dist_bottom_var) {
-  // Total ops: 1
-
-  // Input arrays
-
-  // Intermediate terms (0)
-
-  // Output terms (1)
-  matrix::Matrix<Scalar, 2, 2> _res;
-
-  _res(0, 0) = z_var;
-  _res(1, 0) = z_var;
-  _res(0, 1) = z_var;
-  _res(1, 1) = dist_bottom_var + z_var;
-
-  return _res;
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h

@@ -1,41 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: range_validation_filter_h
- *
- * Args:
- *
- * Outputs:
- *     res: Matrix21
- */
-template <typename Scalar>
-matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
-  // Total ops: 0
-
-  // Input arrays
-
-  // Intermediate terms (0)
-
-  // Output terms (1)
-  matrix::Matrix<Scalar, 2, 1> _res;
-
-  _res(0, 0) = 1;
-  _res(1, 0) = -1;
-
-  return _res;
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/terrain_control.cpp

@@ -54,6 +54,7 @@ void Ekf::controlTerrainFakeFusion()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = -_gpos.altitude();
+		_control_status.flags.rng_fault = false;
 
 	} else if (!_control_status_prev.flags.in_air) {
 		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff

src/modules/ekf2/EKF2.cpp

@@ -155,17 +155,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_ctrl(_params->rng_ctrl),
 	_param_ekf2_rng_delay(_params->range_delay_ms),
 	_param_ekf2_rng_noise(_params->range_noise),
-	_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
+	_param_ekf2_rng_sfe(_params->range_noise_scaler),
 	_param_ekf2_rng_gate(_params->range_innov_gate),
-	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
+	_param_ekf2_rng_pitch(_params->rng_sens_pitch),
 	_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
 	_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
+	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
 	_param_ekf2_rng_fog(_params->rng_fog),
-	_param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x),
-	_param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y),
-	_param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z),
+	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
+	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
+	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ev_delay_ms),
@@ -1600,16 +1601,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
-
-	// TODO: this makes dist_bottom a function of the Terrain State estimate, which is not
-	// exactly correct. Terrain State can diverge from the distance_sensor.current_distance
-	// when fusion is disabled. This causes Terrain Hold to drift due to dist_bottom diverging.
-	// Ultimately the logic for rangefinder fusion needs to be improved, and when distance_sensor
-	// is not fused into the Terrain State estimate we should not use dist_bottom for Altitude Lock.
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
-	// TODO: review, we should use sensor variance not terrain
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
-
 	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;
@@ -1917,12 +1910,14 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
 		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
 		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
+		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
 		status_flags.cs_gnss_yaw               = _ekf.control_status_flags().gnss_yaw;
 		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
 		status_flags.cs_gnss_yaw_fault         = _ekf.control_status_flags().gnss_yaw_fault;
+		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;

src/modules/ekf2/EKF2.hpp

@@ -619,6 +619,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
+		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
 		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,

src/modules/ekf2/params_range_finder.yaml

@@ -48,7 +48,7 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.05
+      default: 0.1
       min: 0.01
       unit: m
       decimal: 2
@@ -93,6 +93,16 @@ parameters:
       unit: SD
       min: 0.1
       max: 5.0
+    EKF2_RNG_QLTY_T:
+      description:
+        short: Minumum range validity period
+        long: Minimum duration during which the reported range finder signal quality
+          needs to be non-zero in order to be declared valid (s)
+      type: float
+      default: 1.0
+      unit: s
+      min: 0.1
+      max: 5
     EKF2_RNG_K_GATE:
       description:
         short: Gate size used for range finder kinematic consistency check
@@ -103,7 +113,7 @@ parameters:
           ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE)
           before tuning this gate.'
       type: float
-      default: 0.5
+      default: 1.0
       unit: SD
       min: 0.1
       max: 5.0
@@ -112,7 +122,7 @@ parameters:
         short: Range finder range dependent noise scaler
         long: Specifies the increase in range finder noise with range.
       type: float
-      default: 0.01
+      default: 0.05
       min: 0.0
       max: 0.2
       unit: m/m

src/modules/ekf2/test/test_EKF_accelerometer.cpp

@@ -192,13 +192,12 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange)
 	_sensor_simulator.runSeconds(5);
 
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND: an accelerometer with a really large Z bias
 	const float bias = CONSTANTS_ONE_G;
 	_sensor_simulator._imu.setAccelData(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G + bias));
 	_sensor_simulator.runSeconds(2);
-	_sensor_simulator._imu.setAccelClipping(false, false, true);
-	_sensor_simulator.runSeconds(2);
 
 	// THEN: the bad vertical is detected because both sources agree
 	EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -123,6 +123,7 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
@@ -178,6 +179,7 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 	_sensor_simulator.startRangeFinder();
-	_sensor_simulator.runSeconds(2.0);
+	_sensor_simulator.runSeconds(1.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 	/* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -125,6 +125,9 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
 	EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);
 
+	const float terrain = _ekf->getTerrainVertPos();
+	EXPECT_NEAR(terrain, -baro_increment, 1.2f);
+
 	const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus();
 	EXPECT_EQ(ev_status.bias, 0.f);
 
@@ -136,6 +139,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND WHEN: the gps height increases
 	const float gps_increment = 1.f;
@@ -146,6 +150,9 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_EQ(gps_status.bias, _ekf->getGpsHgtBiasEstimatorStatus().bias);
 	// the estimated height follows the GPS height
 	EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f);
+	// and the range finder bias is adjusted to follow the new reference
+	const float terrain2 = _ekf->getTerrainVertPos();
+	EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f);
 }
 
 TEST_F(EkfHeightFusionTest, gpsRef)
@@ -155,7 +162,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -167,7 +174,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 
 	const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus();
 	const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt;
-	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.8f);
+	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f);
 
 	// AND WHEN: the baro data increases
 	const float baro_increment = 5.f;
@@ -208,7 +215,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.disableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -235,7 +242,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(0.1);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 
@@ -244,7 +251,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 
 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(14);
+	_sensor_simulator.runSeconds(10);
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);
 
 	_sensor_simulator.stopRangeFinder();
@@ -401,7 +408,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
 	reset_logging_checker.capturePostResetState();
 
 	// An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS)
-	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.4f);
+	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f);
 
 	EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f);
 

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(100);
+	_sensor_simulator.runSeconds(10);
 
 	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
@@ -219,36 +219,11 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 {
 	// GIVEN: rng for terrain but not flow
 	_ekf_wrapper.disableFlowFusion();
-	const float rng_height = 16.f;
+	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.startGps();
-	_ekf->set_min_required_gps_health_time(1e6);
-	_ekf->set_in_air_status(false);
-	_ekf->set_vehicle_at_rest(true);
-	_ekf_wrapper.enableGpsFusion();
-	_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
-
-	const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f);
-
-	// Configure GPS simulator data
-	_sensor_simulator._gps.setVelocity(simulated_velocity);
-	_sensor_simulator._gps.setPositionRateNED(simulated_velocity);
-
-	// Simulate flight above max range finder distance
-	// run for a while to let terrain uncertainty increase
-	_sensor_simulator._rng.setData(30.f, 100);
-	_sensor_simulator._rng.setLimits(0.1f, 20.f);
-	_sensor_simulator.startRangeFinder();
-	_ekf->set_in_air_status(true);
-	_ekf->set_vehicle_at_rest(false);
-	_sensor_simulator.runSeconds(40);
-
-	// quick range finder change to bypass stuck-check
-	_sensor_simulator._rng.setData(rng_height - 1.f, 100);
-	_sensor_simulator.runSeconds(1);
-	_sensor_simulator._rng.setData(rng_height, 100);
-	_sensor_simulator.runSeconds(10);
+	const float rng_height = 18;
+	const float flow_height = 1.f;
+	runFlowAndRngScenario(rng_height, flow_height);
 
 	// THEN: the terrain should reset using rng
 	EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
@@ -259,13 +234,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 
 	const float corr_terrain_vz = P(State::vel.idx + 2,
 					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
-	EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f);
+	EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
 
 	const float corr_terrain_z = P(State::pos.idx + 2,
 				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
-	EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f);
+	EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
 
 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);
+	EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
 }

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -51,6 +51,7 @@ public:
 		_range_finder.setPitchOffset(0.f);
 		_range_finder.setCosMaxTilt(0.707f);
 		_range_finder.setLimits(_min_range, _max_range);
+		_range_finder.setMaxFogDistance(2.f);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -117,8 +118,9 @@ TEST_F(SensorRangeFinderTest, setRange)
 	sample.time_us = 1e6;
 	sample.quality = 9;
 
-	_range_finder.setSample(sample)
-
+	_range_finder.setRange(sample.rng);
+	_range_finder.setDataReadiness(true);
+	_range_finder.setValidity(true);
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 }
@@ -361,7 +363,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: sensor is then blocked by fog
 	// range jumps to value below 2m
-	uint64_t t_now_us = _range_finder.sample()->time_us;
+	uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
 	rangeSample sample{t_now_us, 1.f, 100};
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
@@ -371,7 +373,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: the sensor is not blocked by fog anymore
 	sample.rng = 5.f;
-	sample.time_us = _range_finder.sample()->time_us;
+	sample.time_us = _range_finder.getSampleAddress()->time_us;
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
 	// THEN: the data should be marked as healthy again

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -97,132 +97,94 @@ void FlightTaskManualAltitude::_scaleSticks()
 
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
-	// - check if sticks are released (braking)
-	// - check if no vertical motion (altitude lock) (noisy baro, increase default threshold)
-	// - check if horizontal motion is within limit (altitude lock)
+	// Depending on stick inputs and velocity, position is locked.
+	// If not locked, altitude setpoint is set to NAN.
 
-	// First check if user is controlling Z velocity with sticks
-	if (fabsf(_sticks.getPositionExpo()(2)) > 0.001f) {
-		if (PX4_ISFINITE(_position_setpoint(2)) || PX4_ISFINITE(_dist_to_bottom_lock)) {
-			// PX4_INFO("Setting position sp to NAN");
-			_current_mode = AltitudeMode::None;
+	// Check if user wants to break
+	const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
+
+	// Check if vehicle has stopped
+	const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
+
+	// Manage transition between use of distance to ground and distance to local origin
+	// when terrain hold behaviour has been selected.
+	if (_param_mpc_alt_mode.get() == 2) {
+		// Use horizontal speed as a transition criteria
+		float spd_xy = Vector2f(_velocity).length();
+
+		// Use presence of horizontal stick inputs as a transition criteria
+		float stick_xy = Vector2f(_sticks.getPitchRollExpo()).length();
+		bool stick_input = stick_xy > 0.001f;
+
+		if (_terrain_hold) {
+			bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();
+
+			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
+				// Stop using distance to ground
+				_terrain_hold = false;
+
+				// Adjust the setpoint to maintain the same height error to reduce control transients
+				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
+					_position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom);
+
+				} else {
+					_position_setpoint(2) = _position(2);
+					_dist_to_ground_lock = NAN;
+				}
+			}
+
+		} else {
+			bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get() && stopped;
+
+			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
+				// Start using distance to ground
+				_terrain_hold = true;
+
+				// Adjust the setpoint to maintain the same height error to reduce control transients
+				if (PX4_ISFINITE(_position_setpoint(2))) {
+					_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+				}
+			}
+		}
+
+	}
+
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
+		// terrain following
+		_terrainFollowing(apply_brake, stopped);
+
+	} else {
+		// normal mode where height is dependent on local frame
+
+		if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) {
+			// lock position
+			_position_setpoint(2) = _position(2);
+
+			// Ensure that minimum altitude is respected if
+			// there is a distance sensor and distance to bottom is below minimum.
+			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) {
+				_terrainFollowing(apply_brake, stopped);
+
+			} else {
+				_dist_to_ground_lock = NAN;
+			}
+
+		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
+			// Position is locked but check if a reset event has happened.
+			// We will shift the setpoints.
+			if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
+				_position_setpoint(2) = _position(2);
+				_reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
+			}
+
+		} else  {
+			// user demands velocity change
 			_position_setpoint(2) = NAN;
-			_dist_to_bottom_lock = NAN;
-			_constraints.lock_dist_bottom = false;
-		}
-
-		return;
-	}
-
-	switch ((AltitudeMode)_param_mpc_alt_mode.get()) {
-	case AltitudeMode::AltitudeFollow: {
-			// Altitude following - relative earth frame origin which may drift due to sensors
-			// - No user throttle
-			_altitude_follow_mode();
-			break;
-		}
-
-	case AltitudeMode::TerrainFollow: {
-			// Terrain following - relative to ground which changes with terrain variation
-			// - distance sensor valid
-			// - No user throttle
-
-			// Cannot perform terrain follow without distance sensor
-			if (PX4_ISFINITE(_dist_to_bottom)) {
-				_terrain_follow_mode();
-
-			} else {
-				_altitude_follow_mode();
-				break;
-			}
-			break;
-		}
-
-	case AltitudeMode::TerrainHold: {
-			// Terrain hold - relative to ground when within thresholds
-			// - distance sensor valid
-			// - No user throttle
-			// - XY vel low
-			// - Z vel stopped
-
-			// Cannot perform terrain hold without distance sensor
-			if (PX4_ISFINITE(_dist_to_bottom)) {
-				_terrain_hold_mode();
-
-			} else {
-				_altitude_follow_mode();
-				break;
-			}
-		}
-
-	case AltitudeMode::None: {
-			// Nothing to do
-			break;
+			// ensure that maximum altitude is respected
 		}
 	}
-}
-
-void FlightTaskManualAltitude::_terrain_hold_mode()
-{
-	// Check if XY velocity is within limit to activate Terrain Hold
-	bool xy_vel_okay = Vector2f(_velocity).length() < _param_mpc_hold_max_xy.get();
-
-	if (!xy_vel_okay) {
-		// XY_vel is above threshold, just follow altitude setpoint
-		// Lock the position setpoint to the current Z position estimate
-		if (_current_mode != AltitudeMode::AltitudeFollow) {
-			_current_mode = AltitudeMode::AltitudeFollow;
-
-			PX4_INFO("Locking to Z estimate");
-			_position_setpoint(2) = _position(2);
-			_dist_to_bottom_lock = NAN;
-			_constraints.lock_dist_bottom = false;
-		}
-
-		return;
-	}
-
-	if (_current_mode != AltitudeMode::TerrainHold) {
-		// Set velocity setpoint to 0, switch into TerrainHold
-		_current_mode = AltitudeMode::TerrainHold;
-		_velocity_setpoint(2) = 0.f;
-		PX4_INFO("setting terrain hold");
-		return;
-	}
-
-	if (!PX4_ISFINITE(_dist_to_bottom_lock)) {
-		// Wait for Z to come to a stop before locking in the distance
-		if (fabsf(_velocity(2)) < 0.1f) {
-			_dist_to_bottom_lock = _dist_to_bottom;
-			_position_setpoint(2) = _position(2);
-			_constraints.lock_dist_bottom = true;
-			PX4_INFO("Locking distance to %f", (double)_dist_to_bottom);
-		}
-
-		return;
-	}
-
-	// TODO:
-	// - use dist_bottom_var as heuristic for distance lock
-
-	// All criteria met
-	// - distance sensor valid
-	// - No user throttle
-	// - XY vel low
-	// - Z vel stopped
-	// Update position setpoint to keep fixed distance from terrain
-	float delta_distance = _dist_to_bottom - _dist_to_bottom_lock;
-	_position_setpoint(2) = _position(2) + delta_distance;
-}
-
-void FlightTaskManualAltitude::_terrain_follow_mode()
-{
-
-}
-
-void FlightTaskManualAltitude::_altitude_follow_mode()
-{
 
+	_respectMaxAltitude();
 }
 
 void FlightTaskManualAltitude::_respectMinAltitude()
@@ -236,7 +198,7 @@ void FlightTaskManualAltitude::_respectMinAltitude()
 
 void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 {
-	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_bottom_lock)) {
+	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
 		// User wants to break and vehicle reached zero velocity. Lock height to ground.
 
 		// lock position
@@ -244,19 +206,19 @@ void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 		// ensure that minimum altitude is respected
 		_respectMinAltitude();
 		// lock distance to ground but adjust first for minimum altitude
-		_dist_to_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+		_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
 
-	} else if (apply_brake && PX4_ISFINITE(_dist_to_bottom_lock)) {
+	} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) {
 		// vehicle needs to follow terrain
 
 		// difference between the current distance to ground and the desired distance to ground
-		const float delta_distance_to_ground = _dist_to_bottom_lock - _dist_to_bottom;
+		const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom;
 		// adjust position setpoint for the delta (note: NED frame)
 		_position_setpoint(2) = _position(2) - delta_distance_to_ground;
 
 	} else {
 		// user demands velocity change in D-direction
-		_dist_to_bottom_lock = _position_setpoint(2) = NAN;
+		_dist_to_ground_lock = _position_setpoint(2) = NAN;
 	}
 }
 
@@ -307,8 +269,7 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 {
-	PX4_INFO("_ekfResetHandlerHagl");
-	_dist_to_bottom_lock = NAN;
+	_dist_to_ground_lock = NAN;
 }
 
 void FlightTaskManualAltitude::_updateSetpoints()
@@ -317,7 +278,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
 				      _yaw_setpoint);
 	_updateAltitudeLock();
-	// _respectGroundSlowdown();
+	_respectGroundSlowdown();
 }
 
 bool FlightTaskManualAltitude::_checkTakeoff()

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -45,13 +45,6 @@
 #include "StickTiltXY.hpp"
 #include <uORB/Subscription.hpp>
 
-enum class AltitudeMode : int {
-	AltitudeFollow,
-	TerrainFollow,
-	TerrainHold,
-	None,
-};
-
 class FlightTaskManualAltitude : public FlightTask
 {
 public:
@@ -83,8 +76,7 @@ protected:
 	StickYaw _stick_yaw{this};
 
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
-	// bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
-	AltitudeMode _current_mode = AltitudeMode::AltitudeFollow;
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
 
 	float _velocity_constraint_up{INFINITY};
 	float _velocity_constraint_down{INFINITY};
@@ -93,20 +85,15 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
-					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
-					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
-					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
-					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
-					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
-					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
+					(ParamFloat<px4::params::MPC_LAND_SPEED>)
+					_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
+					(ParamFloat<px4::params::MPC_TKO_SPEED>)
+					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
 				       )
 private:
-
-	void _terrain_hold_mode();
-	void _terrain_follow_mode();
-	void _altitude_follow_mode();
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -132,6 +119,8 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
+	void setGearAccordingToSwitch();
+
 	bool _updateYawCorrection();
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
@@ -143,7 +132,7 @@ private:
 	 * Distance to ground during terrain following.
 	 * If user does not demand velocity change in D-direction and the vehcile
 	 * is in terrain-following mode, then height to ground will be locked to
-	 * _dist_to_bottom_lock.
+	 * _dist_to_ground_lock.
 	 */
-	float _dist_to_bottom_lock = NAN;
+	float _dist_to_ground_lock = NAN;
 };

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -107,7 +107,7 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
 
-	if (_current_mode != AltitudeMode::TerrainHold) {
+	if (!_terrain_hold) {
 		if (_terrain_hold_previous) {
 			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
 			_smoothing.setCurrentPosition(_position(2));
@@ -116,18 +116,5 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 		_position_setpoint(2) = _smoothing.getCurrentPosition();
 	}
 
-	_terrain_hold_previous = _current_mode == AltitudeMode::TerrainHold;
-
-	task_local_position_setpoint_s msg = {};
-
-	msg.timestamp = hrt_absolute_time();
-
-	msg.x = _position_setpoint(0);
-	msg.y = _position_setpoint(1);
-	msg.z = _position_setpoint(2);
-	msg.vx = _velocity_setpoint(0);
-	msg.vy = _velocity_setpoint(1);
-	msg.vz = _velocity_setpoint(2);
-
-	_setpoint_pub.publish(msg);
+	_terrain_hold_previous = _terrain_hold;
 }

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -42,9 +42,6 @@
 #include "FlightTaskManualAltitude.hpp"
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
-#include <uORB/Publication.hpp>
-#include <uORB/topics/task_local_position_setpoint.h>
-
 class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 {
 public:
@@ -65,7 +62,12 @@ protected:
 
 	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
 
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
+					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
+				       )
+
 private:
-	uORB::Publication<task_local_position_setpoint_s> _setpoint_pub{ORB_ID(task_local_position_setpoint)};
 	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };

src/modules/logger/logged_topics.cpp

@@ -142,8 +142,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_local_position", 100);
 	add_topic("vehicle_local_position_setpoint", 100);
-	// DEBUG
-	add_topic("task_local_position_setpoint");
 	add_topic("vehicle_magnetometer", 200);
 	add_topic("vehicle_rates_setpoint", 20);
 	add_topic("vehicle_roi", 1000);
@@ -317,10 +315,6 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("vehicle_visual_odometry");
 	add_topic("aux_global_position");
 	add_topic_multi("distance_sensor");
-	add_topic("vehicle_local_position");
-	add_topic("estimator_states");
-	add_topic("estimator_status");
-	add_topic("estimator_aid_src_rng_hgt");
 }
 
 void LoggedTopics::add_thermal_calibration_topics()

src/modules/rover_mecanum/CMakeLists.txt

@@ -31,10 +31,12 @@
 #
 ############################################################################
 
+add_subdirectory(MecanumActControl)
 add_subdirectory(MecanumRateControl)
 add_subdirectory(MecanumAttControl)
 add_subdirectory(MecanumVelControl)
 add_subdirectory(MecanumPosControl)
+add_subdirectory(MecanumDriveModes)
 
 px4_add_module(
 	MODULE modules__rover_mecanum
@@ -43,10 +45,14 @@ px4_add_module(
 		RoverMecanum.cpp
 		RoverMecanum.hpp
 	DEPENDS
+		MecanumActControl
 		MecanumRateControl
 		MecanumAttControl
 		MecanumVelControl
 		MecanumPosControl
+		MecanumAutoMode
+		MecanumManualMode
+		MecanumOffboardMode
 		px4_work_queue
 		rover_control
 		pure_pursuit

src/modules/rover_mecanum/MecanumActControl/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(MecanumActControl
+	MecanumActControl.cpp
+)
+
+target_include_directories(MecanumActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MecanumActControl.hpp"
+
+using namespace time_literals;
+
+MecanumActControl::MecanumActControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+}
+
+void MecanumActControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
+		_adjusted_throttle_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
+		_adjusted_throttle_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
+	}
+}
+
+void MecanumActControl::updateActControl()
+{
+	const hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	// Motor control
+	if (_rover_throttle_setpoint_sub.updated()) {
+		rover_throttle_setpoint_s rover_throttle_setpoint{};
+		_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
+		_throttle_x_setpoint = rover_throttle_setpoint.throttle_body_x;
+		_throttle_y_setpoint = rover_throttle_setpoint.throttle_body_y;
+	}
+
+	if (_rover_steering_setpoint_sub.updated()) {
+		rover_steering_setpoint_s rover_steering_setpoint{};
+		_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
+		_speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff;
+	}
+
+	if (PX4_ISFINITE(_throttle_x_setpoint) && PX4_ISFINITE(_throttle_y_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) {
+		actuator_motors_s actuator_motors_sub{};
+		_actuator_motors_sub.copy(&actuator_motors_sub);
+		const float current_throttle_x = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f;
+		const float current_throttle_y = (actuator_motors_sub.control[2] - actuator_motors_sub.control[0]) / 2.f;
+		const float adjusted_throttle_x_setpoint = RoverControl::throttleControl(_adjusted_throttle_x_setpoint,
+				_throttle_x_setpoint, current_throttle_x, _param_ro_accel_limit.get(),
+				_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt);
+		const float adjusted_throttle_y_setpoint = RoverControl::throttleControl(_adjusted_throttle_y_setpoint,
+				_throttle_y_setpoint, current_throttle_y, _param_ro_accel_limit.get(),
+				_param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt);
+		actuator_motors_s actuator_motors{};
+		actuator_motors.reversible_flags = _param_r_rev.get();
+		computeInverseKinematics(adjusted_throttle_x_setpoint, adjusted_throttle_y_setpoint,
+					 _speed_diff_setpoint).copyTo(actuator_motors.control);
+		actuator_motors.timestamp = _timestamp;
+		_actuator_motors_pub.publish(actuator_motors);
+
+	}
+
+}
+
+Vector4f MecanumActControl::computeInverseKinematics(float throttle_body_x, float throttle_body_y,
+		const float speed_diff_normalized)
+{
+	const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized);
+
+	if (total_speed > 1.f) { // Adjust speed setpoints if infeasible
+		const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x));
+		const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta));
+		const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f)));
+		throttle_body_x *= magnitude * normalization;
+		throttle_body_y *= magnitude * normalization;
+
+	}
+
+	// Calculate motor commands
+	const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized};
+	const Matrix<float, 3, 1> input(input_data);
+	const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
+	const Matrix<float, 4, 3> m(m_data);
+	const Vector4f motor_commands = m * input;
+
+	return motor_commands;
+}
+
+void MecanumActControl::stopVehicle()
+{
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get();
+	actuator_motors.control[0] = 0.f;
+	actuator_motors.control[1] = 0.f;
+	actuator_motors.control[2] = 0.f;
+	actuator_motors.control[3] = 0.f;
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+}

src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// Libraries
+#include <lib/rover_control/RoverControl.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <math.h>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
+
+/**
+ * @brief Class for mecanum actuator control.
+ */
+class MecanumActControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumActControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumActControl(ModuleParams *parent);
+	~MecanumActControl() = default;
+
+	/**
+	 * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
+	 */
+	void updateActControl();
+
+	/**
+	 * @brief Stop the vehicle by sending 0 commands to motors and servos.
+	 */
+	void stopVehicle();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Compute normalized motor commands based on normalized setpoints.
+	 * @param throttle_body_x Normalized speed in body x direction [-1, 1].
+	 * @param throttle_body_y Normalized speed in body y direction [-1, 1].
+	 * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
+	 * @return Motor speeds [-1, 1].
+	 */
+	Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const float speed_diff_normalized);
+
+	// uORB subscriptions
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
+	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
+
+	// uORB publications
+	uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+	float _throttle_x_setpoint{NAN};
+	float _throttle_y_setpoint{NAN};
+	float _speed_diff_setpoint{NAN};
+
+	// Controllers
+	SlewRate<float> _adjusted_throttle_x_setpoint{0.f};
+	SlewRate<float> _adjusted_throttle_y_setpoint{0.f};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev,
+		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
+		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
+	)
+};

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp

@@ -38,8 +38,6 @@ using namespace time_literals;
 MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent)
 {
 	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
 	_rover_attitude_status_pub.advertise();
 	updateParams();
 }
@@ -52,21 +50,20 @@ void MecanumAttControl::updateParams()
 		_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
 	}
 
+	// Set up PID controller
 	_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
 	_pid_yaw.setIntegralLimit(_max_yaw_rate);
 	_pid_yaw.setOutputLimit(_max_yaw_rate);
+
+	// Set up slew rate
 	_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
 }
 
 void MecanumAttControl::updateAttControl()
 {
-	const hrt_abstime timestamp_prev = _timestamp;
+	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -75,17 +72,20 @@ void MecanumAttControl::updateAttControl()
 		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
 	}
 
-	if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+	if (_rover_attitude_setpoint_sub.updated()) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
+		_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
+	}
 
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateAttitudeAndThrottleSetpoint();
-		}
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
+						_vehicle_yaw, _yaw_setpoint, dt);
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 
-		generateRateSetpoint();
-
-	} else { // Reset pid and slew rate when attitude control is not active
-		_pid_yaw.resetIntegral();
-		_adjusted_yaw_setpoint.setForcedValue(0.f);
 	}
 
 	// Publish attitude controller status (logging only)
@@ -97,93 +97,6 @@ void MecanumAttControl::updateAttControl()
 
 }
 
-void MecanumAttControl::generateAttitudeAndThrottleSetpoint()
-{
-	const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-			rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-
-			const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
-						_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-						_max_yaw_rate / _param_ro_yaw_p.get());
-
-			if (fabsf(yaw_delta) > FLT_EPSILON) { // Closed loop yaw rate control
-				_stab_yaw_setpoint = NAN;
-				const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
-				if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
-					_stab_yaw_setpoint = _vehicle_yaw;
-				}
-
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			}
-
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_att_control = _offboard_control_mode.attitude;
-
-		if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) {
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		}
-	}
-}
-
-void MecanumAttControl::generateRateSetpoint()
-{
-	if (_rover_attitude_setpoint_sub.updated()) {
-		_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
-	}
-
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-	}
-
-	// Check if a new rate setpoint was already published from somewhere else
-	if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
-	    && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
-		return;
-	}
-
-	const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
-					_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
-
-	_last_rate_setpoint_update = _timestamp;
-	rover_rate_setpoint_s rover_rate_setpoint{};
-	rover_rate_setpoint.timestamp = _timestamp;
-	rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
-	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-}
-
 bool MecanumAttControl::runSanityChecks()
 {
 	bool ret = true;
@@ -194,13 +107,9 @@ bool MecanumAttControl::runSanityChecks()
 
 	if (_param_ro_yaw_p.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
-		}
+		events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
+				    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp

@@ -48,15 +48,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/rover_attitude_status.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
 
 /**
  * @brief Class for mecanum attitude control.
@@ -72,10 +66,21 @@ public:
 	~MecanumAttControl() = default;
 
 	/**
-	 * @brief Update attitude controller.
+	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
 	 */
 	void updateAttControl();
 
+	/**
+	 * @brief Reset attitude controller.
+	 */
+	void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;};
+
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -83,51 +88,20 @@ protected:
 	void updateParams() override;
 
 private:
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode)
-	 * 	  or trajectorySetpoint (Offboard attitude control).
-	 */
-	void generateAttitudeAndThrottleSetpoint();
-
-	/**
-	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
-	 */
-	void generateRateSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
 
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	vehicle_control_mode_s _vehicle_control_mode{};
-	rover_attitude_setpoint_s _rover_attitude_setpoint{};
-	rover_rate_setpoint_s _rover_rate_setpoint{};
-	offboard_control_mode_s _offboard_control_mode{};
 
 	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_rate_setpoint_s>   _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
 	uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
 
 	// Variables
-	hrt_abstime _timestamp{0};
-	hrt_abstime _last_rate_setpoint_update{0};
 	float _vehicle_yaw{0.f};
-	float _dt{0.f};
+	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};
-	float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
-	bool _prev_param_check_passed{true};
+	float _yaw_setpoint{NAN};
 
 	// Controllers
 	PID _pid_yaw;
@@ -135,8 +109,8 @@ private:
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
-		(ParamFloat<px4::params::RO_YAW_P>)         _param_ro_yaw_p,
-		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_P>)        _param_ro_yaw_p,
+		(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
 	)
 };

src/modules/rover_mecanum/MecanumDriveModes/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_subdirectory(MecanumAutoMode)
+add_subdirectory(MecanumManualMode)
+add_subdirectory(MecanumOffboardMode)

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(MecanumAutoMode
+MecanumAutoMode.cpp
+)
+
+target_include_directories(MecanumAutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MecanumAutoMode.hpp"
+
+using namespace time_literals;
+
+MecanumAutoMode::MecanumAutoMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumAutoMode::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void MecanumAutoMode::autoControl()
+{
+	if (_position_setpoint_triplet_sub.updated()) {
+		position_setpoint_triplet_s position_setpoint_triplet{};
+		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+		int curr_wp_type = position_setpoint_triplet.current.type;
+
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+
+		MapProjection global_ned_proj_ref{};
+
+		if (!global_ned_proj_ref.isInitialized()
+		    || (global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
+			global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
+							  vehicle_local_position.ref_timestamp);
+		}
+
+		Vector2f curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector2f curr_wp_ned{NAN, NAN};
+		Vector2f prev_wp_ned{NAN, NAN};
+		Vector2f next_wp_ned{NAN, NAN};
+
+		RoverControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet,
+				curr_pos_ned, global_ned_proj_ref);
+
+		float waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+
+		// Waypoint cruising speed
+		float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
+					       position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
+
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = curr_wp_ned(0);
+		rover_position_setpoint.position_ned[1] = curr_wp_ned(1);
+		rover_position_setpoint.start_ned[0] = prev_wp_ned(0);
+		rover_position_setpoint.start_ned[1] = prev_wp_ned(1);
+		rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle,
+							_param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), curr_wp_type);
+		rover_position_setpoint.cruising_speed = cruising_speed;
+		rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ?
+					      position_setpoint_triplet.current.yaw : NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+	}
+}
+
+float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
+				    const float max_speed, const float miss_spd_gain, int curr_wp_type)
+{
+	// Upcoming stop
+	if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		return 0.f;
+	}
+
+	// Straight line speed
+	if (miss_spd_gain > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
+					      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		return max_speed * (1.f - speed_reduction);
+	}
+
+	return cruising_speed; // Fallthrough
+
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// Libraries
+#include <lib/rover_control/RoverControl.hpp>
+#include <math.h>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/rover_position_setpoint.h>
+
+/**
+ * @brief Class for Mecanum auto mode.
+ */
+class MecanumAutoMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for auto mode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumAutoMode(ModuleParams *parent);
+	~MecanumAutoMode() = default;
+
+	/**
+	 * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet.
+	 */
+	void autoControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Calculate the speed at which the rover should arrive at the current waypoint. During waypoint transition the speed is restricted to
+	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
+	 * @param cruising_speed Cruising speed [m/s].
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param max_speed Maximum speed setpoint [m/s]
+	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
+	 * @param curr_wp_type Type of the current waypoint.
+	 * @return Speed setpoint [m/s].
+	 */
+	float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed,
+			   const float miss_spd_gain, int curr_wp_type);
+
+	// uORB subscriptions
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+
+	// uORB publications
+	uORB::Publication<rover_position_setpoint_s>    _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
+		(ParamFloat<px4::params::RM_MISS_SPD_GAIN>) _param_rm_miss_spd_gain
+	)
+};

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(MecanumManualMode
+	MecanumManualMode.cpp
+)
+
+target_include_directories(MecanumManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp

@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MecanumManualMode.hpp"
+
+using namespace time_literals;
+
+MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_steering_setpoint_pub.advertise();
+	_rover_rate_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_velocity_setpoint_pub.advertise();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumManualMode::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+}
+
+void MecanumManualMode::manual()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_steering_setpoint_s rover_steering_setpoint{};
+	rover_steering_setpoint.timestamp = hrt_absolute_time();
+	rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw;
+	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+}
+
+void MecanumManualMode::acro()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+	rover_rate_setpoint_s rover_rate_setpoint{};
+	rover_rate_setpoint.timestamp = hrt_absolute_time();
+	rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float> (manual_control_setpoint.yaw, -1.f, 1.f,
+						-_max_yaw_rate, _max_yaw_rate);
+	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+}
+
+void MecanumManualMode::stab()
+{
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	}
+
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+
+	if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON) {
+		_stab_yaw_setpoint = NAN;
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoint invalid
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = NAN;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+	} else { // Heading control
+		if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
+			_stab_yaw_setpoint = _vehicle_yaw;
+		}
+
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+	}
+}
+
+void MecanumManualMode::position()
+{
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+	}
+
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	Vector3f velocity_setpoint_body{};
+	velocity_setpoint_body(0) = math::interpolate<float>(manual_control_setpoint.throttle,
+				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+	velocity_setpoint_body(1) = math::interpolate<float>(manual_control_setpoint.roll,
+				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+	velocity_setpoint_body(2) = 0.f;
+	const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
+
+
+	if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_ned.norm() < FLT_EPSILON) {
+		_pos_ctl_yaw_setpoint = NAN;
+
+		// Speed control
+		rover_velocity_setpoint_s rover_velocity_setpoint{};
+		rover_velocity_setpoint.timestamp = hrt_absolute_time();
+		rover_velocity_setpoint.speed = velocity_setpoint_ned.norm();
+		rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_ned(1), velocity_setpoint_ned(0));
+		rover_velocity_setpoint.yaw = NAN;
+		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoints invalid
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = NAN;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = NAN;
+		rover_position_setpoint.position_ned[1] = NAN;
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = NAN;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+
+	} else { // Course control
+		const Vector3f pos_ctl_course_direction_local = velocity_setpoint_ned.normalized();
+		const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
+				pos_ctl_course_direction_local(1));
+
+		// Reset course control if course direction change is above threshold
+		if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp(
+					1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) {
+			_pos_ctl_yaw_setpoint = NAN;
+		}
+
+		if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
+			_pos_ctl_start_position_ned = _curr_pos_ned;
+			_pos_ctl_yaw_setpoint = _vehicle_yaw;
+			_pos_ctl_course_direction = pos_ctl_course_direction_temp;
+		}
+
+		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+		const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
+		const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
+		const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
+		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = velocity_setpoint_ned.norm();
+		rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+	}
+}
+
+void MecanumManualMode::reset()
+{
+	_stab_yaw_setpoint = NAN;
+	_pos_ctl_course_direction = Vector2f(NAN, NAN);
+	_pos_ctl_start_position_ned = Vector2f(NAN, NAN);
+	_curr_pos_ned = Vector2f(NAN, NAN);
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// Libraries
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_rate_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_position_setpoint.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for Mecanum manual mode.
+ */
+class MecanumManualMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumManualMode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumManualMode(ModuleParams *parent);
+	~MecanumManualMode() = default;
+
+	/**
+	 * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
+	 */
+	void manual();
+
+	/**
+	 * @brief Generate and publish roverThrottleSetpoint/RoverRateSetpoint from manualControlSetpoint.
+	 */
+	void acro();
+
+	/**
+	 * @brief Generate and publish roverSetpoints from manualControlSetpoint.
+	 */
+	void stab();
+
+	/**
+	 * @brief Generate and publish roverSetpoints from manualControlSetpoint.
+	 */
+	void position();
+
+	/**
+	 * @brief Reset manual mode variables.
+	 */
+	void reset();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// uORB subscriptions
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	// uORB publications
+	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
+	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+
+	// Variables
+	Vector2f _pos_ctl_course_direction{NAN, NAN};
+	Vector2f _pos_ctl_start_position_ned{NAN, NAN};
+	Vector2f _curr_pos_ned{NAN, NAN};
+	Quatf _vehicle_attitude_quaternion;
+	float _pos_ctl_yaw_setpoint{NAN};
+	float _stab_yaw_setpoint{NAN};
+	float _vehicle_yaw{NAN};
+	float _max_yaw_rate{NAN};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
+		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
+		(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th
+	)
+};

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(MecanumOffboardMode
+	MecanumOffboardMode.cpp
+)
+
+target_include_directories(MecanumOffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MecanumOffboardMode.hpp"
+
+using namespace time_literals;
+
+MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_rate_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_velocity_setpoint_pub.advertise();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumOffboardMode::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void MecanumOffboardMode::offboardControl()
+{
+	offboard_control_mode_s offboard_control_mode{};
+	_offboard_control_mode_sub.copy(&offboard_control_mode);
+
+	trajectory_setpoint_s trajectory_setpoint{};
+	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
+
+	if (offboard_control_mode.position) {
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
+		rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.cruising_speed = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+
+	} else if (offboard_control_mode.velocity) {
+		const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
+		rover_velocity_setpoint_s rover_velocity_setpoint{};
+		rover_velocity_setpoint.timestamp = hrt_absolute_time();
+		rover_velocity_setpoint.speed = velocity_ned.norm();
+		rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
+		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+	} else if (offboard_control_mode.attitude) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+	} else if (offboard_control_mode.body_rate) {
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+	}
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// Libraries
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/rover_rate_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_position_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/trajectory_setpoint.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for Mecanum manual mode.
+ */
+class MecanumOffboardMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumOffboardMode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumOffboardMode(ModuleParams *parent);
+	~MecanumOffboardMode() = default;
+
+	/**
+	 * @brief Generate and publish roverSetpoints from trajectorySetpoint.
+	 */
+	void offboardControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// uORB subscriptions
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+
+	// uORB publications
+	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+};