commander: skip estimatorCheck for now

simulator_mavlink: add heading_good_for_control
simulation: HIL_STATE_QUATERNION add angular acceleration and protect from initial lat/lon 0
2026-06-11 14:50:05 +08:00 · 2024-02-15 15:46:31 -05:00 · 2024-02-15 15:46:14 -05:00 · 2024-02-15 15:20:33 -05:00 · 2024-02-15 14:57:37 -05:00 · 2024-02-15 13:24:40 -05:00
52 changed files with 1230 additions and 407 deletions
@@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then

 	echo "INFO  [init] Gazebo simulator"

-	# set local coordinate frame reference
-	if [ -n "${PX4_HOME_LAT}" ]; then
-		param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
-	fi
-
-	if [ -n "${PX4_HOME_LON}" ]; then
-		param set SIM_GZ_HOME_LON ${PX4_HOME_LON}
-	fi
-
-	if [ -n "${PX4_HOME_ALT}" ]; then
-		param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
-	fi
-
 	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
 	if [ -z "${PX4_GZ_STANDALONE}" ]; then

@@ -180,6 +167,12 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)"

 else
 	# otherwise start simulator (mavlink) module
+
+	# EKF2 specifics
+	param set-default EKF2_GPS_DELAY 10
+	param set-default EKF2_MULTI_IMU 3
+	param set-default SENS_IMU_MODE 0
+
 	simulator_tcp_port=$((4560+px4_instance))

 	# Check if PX4_SIM_HOSTNAME environment variable is empty
@@ -164,10 +164,6 @@ param set-default COM_RC_IN_MODE 1
 # Speedup SITL startup
 param set-default EKF2_REQ_GPS_H 0.5

-# Multi-EKF
-param set-default EKF2_MULTI_IMU 3
-param set-default SENS_IMU_MODE 0
-
 param set-default IMU_GYRO_FFT_EN 1
 param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets

@@ -257,7 +253,7 @@ fi
 tone_alarm start
 rc_update start
 manual_control start
-sensors start
+#sensors start
 commander start

 if ! pwm_out_sim start -m sim
@@ -5,7 +5,7 @@
 ekf2 start &

 # Start rover differential drive controller.
-differential_drive_control start
+differential_drive start

 # Start Land Detector.
 land_detector start rover
@@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -8,4 +8,4 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
@@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
@@ -0,0 +1,5 @@
+CONFIG_EKF2_AUX_GLOBAL_POSITION=n
+CONFIG_EKF2_VERBOSE_STATUS=n
+CONFIG_MODULES_EKF2=n
+CONFIG_MODULES_SENSORS=y
+CONFIG_BOARD_NOLOCKSTEP=y
@@ -1,4 +1,8 @@
 uint64 timestamp # time since system start (microseconds)

 float32 speed # [m/s] collective roll-off speed in body x-axis
+bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
 float32 yaw_rate # [rad/s] yaw rate
+bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
+
+# TOPICS differential_drive_setpoint differential_drive_control_output
@@ -392,6 +392,12 @@ int BATT_SMBUS::get_startup_info()
 	uint16_t state_of_health;
 	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);

+	/* ManufacturerAccess dummy command to init the ManufacturerBlockAccess routine
+	in the BQ40Zx0 and avoid timeout during LifetimeDataFlush.
+	test Sleep > 20 ms to give time to init the ManufacturerBlockAccess routine*/
+	ret |= _interface->write_word(BATT_SMBUS_MANUFACTURER_ACCESS, BATT_SMBUS_DEVICE_TYPE);
+	px4_usleep(30_ms);
+
 	if (!ret) {
 		_serial_number = serial_num;
 		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
@@ -107,6 +107,7 @@ using namespace time_literals;

 #define BATT_SMBUS_SECURITY_KEYS                        0x0035

+#define BATT_SMBUS_DEVICE_TYPE                          0x0001
 #define BATT_SMBUS_LIFETIME_FLUSH                       0x002E
 #define BATT_SMBUS_LIFETIME_BLOCK_ONE                   0x0060
 #define BATT_SMBUS_ENABLED_PROTECTIONS_A_ADDRESS        0x4938
@@ -156,18 +156,18 @@ private:
 #ifndef CONSTRAINED_FLASH
 		&_external_checks,
 #endif
-		&_accelerometer_checks,
-		&_airspeed_checks,
+		//&_accelerometer_checks,
+		//&_airspeed_checks,
 		&_arm_permission_checks,
-		&_baro_checks,
+		//&_baro_checks,
 		&_cpu_resource_checks,
 		&_distance_sensor_checks,
 		&_esc_checks,
 		&_estimator_checks,
 		&_failure_detector_checks,
-		&_gyro_checks,
-		&_imu_consistency_checks,
-		&_magnetometer_checks,
+		//&_gyro_checks,
+		//&_imu_consistency_checks,
+		//&_magnetometer_checks,
 		&_manual_control_checks,
 		&_home_position_checks,
 		&_mission_checks,
@@ -99,7 +99,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}

-	if (missing_data && _param_sys_mc_est_group.get() == 2) {
+	if (missing_data && _param_sys_mc_est_group.get() == 2 && false) {
 		/* EVENT
 		 */
 		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 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
@@ -31,17 +31,22 @@
 #
 ############################################################################

+add_subdirectory(DifferentialDriveControl)
+add_subdirectory(DifferentialDriveGuidance)
 add_subdirectory(DifferentialDriveKinematics)

 px4_add_module(
-	MODULE modules__differential_drive_control
-	MAIN differential_drive_control
+	MODULE modules__differential_drive
+	MAIN differential_drive
 	SRCS
-		DifferentialDriveControl.cpp
-		DifferentialDriveControl.hpp
+		DifferentialDrive.cpp
+		DifferentialDrive.hpp
 	DEPENDS
+		DifferentialDriveControl
+		DifferentialDriveGuidance
 		DifferentialDriveKinematics
 		px4_work_queue
+		modules__control_allocator # for parameter CA_R_REV
 	MODULE_CONFIG
 		module.yaml
 )
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 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
@@ -31,38 +31,37 @@
 *
 ****************************************************************************/

-#include "DifferentialDriveControl.hpp"
+#include "DifferentialDrive.hpp"

-using namespace time_literals;
-using namespace matrix;
-namespace differential_drive_control
-{
-
-DifferentialDriveControl::DifferentialDriveControl() :
+DifferentialDrive::DifferentialDrive() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
 }

-bool DifferentialDriveControl::init()
+bool DifferentialDrive::init()
 {
 	ScheduleOnInterval(10_ms); // 100 Hz
 	return true;
 }

-void DifferentialDriveControl::updateParams()
+void DifferentialDrive::updateParams()
 {
 	ModuleParams::updateParams();
-	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
-	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);

 	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+
+	_max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_differential_drive_guidance.setMaxSpeed(_max_speed);
 	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+	_differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
 	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
 }

-void DifferentialDriveControl::Run()
+void DifferentialDrive::Run()
 {
 	if (should_exit()) {
 		ScheduleClear();
@@ -70,20 +69,32 @@ void DifferentialDriveControl::Run()
 	}

 	hrt_abstime now = hrt_absolute_time();
+	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
+	_time_stamp_last = now;

 	if (_parameter_update_sub.updated()) {
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
 		updateParams();
 	}

 	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
+		vehicle_control_mode_s vehicle_control_mode{};

 		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
-			_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
+			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
+		}
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
+			_differential_drive_kinematics.setArmed(spooled_up);
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
 		}
 	}

@@ -94,43 +105,42 @@ void DifferentialDriveControl::Run()
 			manual_control_setpoint_s manual_control_setpoint{};

 			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-				_differential_drive_setpoint.timestamp = now;
-				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
-				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
-									_max_angular_velocity;
-				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+				differential_drive_setpoint_s setpoint{};
+				setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
+				setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
+
+				// if acro mode, we activate the yaw rate control
+				if (_acro_driving) {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = true;
+
+				} else {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = false;
+				}
+
+				setpoint.timestamp = now;
+				_differential_drive_setpoint_pub.publish(setpoint);
 			}
 		}
+
+	} else if (_mission_driving) {
+		// Mission mode
+		// directly receive setpoints from the guidance library
+		_differential_drive_guidance.computeGuidance(
+			_differential_drive_control.getVehicleYaw(),
+			_differential_drive_control.getVehicleBodyYawRate(),
+			dt
+		);
 	}

-	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
-
-	// publish data to actuator_motors (output module)
-	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
-	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
-					_differential_drive_setpoint.speed,
-					_differential_drive_setpoint.yaw_rate);
-
-	// Check if max_angular_wheel_speed is zero
-	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
-	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
-
-	if (!_armed || setpoint_timeout || !valid_max_speed) {
-		wheel_speeds = {}; // stop
-	}
-
-	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
-
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
-	wheel_speeds.copyTo(actuator_motors.control);
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
+	_differential_drive_control.control(dt);
+	_differential_drive_kinematics.allocate();
 }

-int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+int DifferentialDrive::task_spawn(int argc, char *argv[])
 {
-	DifferentialDriveControl *instance = new DifferentialDriveControl();
+	DifferentialDrive *instance = new DifferentialDrive();

 	if (instance) {
 		_object.store(instance);
@@ -151,12 +161,12 @@ int DifferentialDriveControl::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }

-int DifferentialDriveControl::custom_command(int argc, char *argv[])
+int DifferentialDrive::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }

-int DifferentialDriveControl::print_usage(const char *reason)
+int DifferentialDrive::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_ERR("%s\n", reason);
@@ -168,15 +178,13 @@ int DifferentialDriveControl::print_usage(const char *reason)
 Rover Differential Drive controller.
 )DESCR_STR");

-	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
+	PRINT_MODULE_USAGE_NAME("differential_drive", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }

-extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
+extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
 {
-	return DifferentialDriveControl::main(argc, argv);
+	return DifferentialDrive::main(argc, argv);
 }
-
-} // namespace differential_drive_control
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 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
@@ -33,41 +33,31 @@

 #pragma once

-// PX4 includes
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-// uORB includes
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>

-// Standard library includes
-#include <math.h>
+#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
+#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
+#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"

-// Local includes
-#include <DifferentialDriveKinematics.hpp>
+using namespace time_literals;

-namespace differential_drive_control
-{
-
-class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
+class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
 public:
-	DifferentialDriveControl();
-	~DifferentialDriveControl() override = default;
+	DifferentialDrive();
+	~DifferentialDrive() override = default;

 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -85,32 +75,31 @@ protected:

 private:
 	void Run() override;
-
-	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};

-	differential_drive_setpoint_s _differential_drive_setpoint{};
-	DifferentialDriveKinematics _differential_drive_kinematics{};
-
-	bool _armed = false;
 	bool _manual_driving = false;
+	bool _mission_driving = false;
+	bool _acro_driving = false;
+	hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
+
+	DifferentialDriveGuidance _differential_drive_guidance{this};
+	DifferentialDriveControl _differential_drive_control{this};
+	DifferentialDriveKinematics _differential_drive_kinematics{this};
+
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};

 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
-		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
+		(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
 		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
 	)
 };
-
-} // namespace differential_drive_control
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 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(DifferentialDriveControl
+	DifferentialDriveControl.cpp
+)
+
+target_link_libraries(DifferentialDriveControl PUBLIC pid)
+target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 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 "DifferentialDriveControl.hpp"
+
+using namespace matrix;
+
+DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_pid_angular_velocity,
+			   _param_rdd_p_gain_angular_velocity.get(), // Proportional gain
+			   _param_rdd_i_gain_angular_velocity.get(), // Integral gain
+			   0, // Derivative gain
+			   20, // Integral limit
+			   200); // Output limit
+
+	pid_set_parameters(&_pid_speed,
+			   _param_rdd_p_gain_speed.get(), // Proportional gain
+			   _param_rdd_i_gain_speed.get(), // Integral gain
+			   0, // Derivative gain
+			   2, // Integral limit
+			   200); // Output limit
+}
+
+void DifferentialDriveControl::control(float dt)
+{
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+
+		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
+			_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
+		}
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			_vehicle_attitude_quaternion = 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{};
+
+		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+			Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+			_vehicle_forward_speed = velocity_in_body_frame(0);
+		}
+	}
+
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+
+	// PID to reach setpoint using control_output
+	differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
+
+	if (_differential_drive_setpoint.closed_loop_speed_control) {
+		differential_drive_control_output.speed +=
+			pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt);
+	}
+
+	if (_differential_drive_setpoint.closed_loop_yaw_rate_control) {
+		differential_drive_control_output.yaw_rate +=
+			pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
+	}
+
+	differential_drive_control_output.timestamp = hrt_absolute_time();
+	_differential_drive_control_output_pub.publish(differential_drive_control_output);
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 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 DifferentialDriveControl.hpp
+ *
+ * Controller for heading rate and forward speed.
+ */
+
+#pragma once
+
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+class DifferentialDriveControl : public ModuleParams
+{
+public:
+	DifferentialDriveControl(ModuleParams *parent);
+	~DifferentialDriveControl() = default;
+
+	void control(float dt);
+	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
+	float getVehicleYaw() const { return _vehicle_yaw; }
+
+protected:
+	void updateParams() override;
+
+private:
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw{0.f};
+
+	// States
+	float _vehicle_body_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+
+	PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
+	PID_t _pid_speed; ///< The PID controller for velocity.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
+		(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
+		(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
+		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
+	)
+};
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2023-2024 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(DifferentialDriveGuidance
+	DifferentialDriveGuidance.cpp
+)
+
+target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 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 "DifferentialDriveGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
+{
+	if (_position_setpoint_triplet_sub.updated()) {
+		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
+	}
+
+	if (_vehicle_global_position_sub.updated()) {
+		_vehicle_global_position_sub.copy(&_vehicle_global_position);
+	}
+
+	matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
+	matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
+	matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
+
+	const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
+					  current_waypoint(0),
+					  current_waypoint(1));
+
+	float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
+				current_waypoint(1));
+	float heading_error = matrix::wrap_pi(desired_heading - yaw);
+
+	if (_current_waypoint != current_waypoint) {
+		_currentState = GuidanceState::TURNING;
+	}
+
+	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
+	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
+		_currentState = GuidanceState::GOAL_REACHED;
+	}
+
+	float desired_speed = 0.f;
+
+	switch (_currentState) {
+	case GuidanceState::TURNING:
+		desired_speed = 0.f;
+
+		if (fabsf(heading_error) < 0.05f) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+		break;
+
+	case GuidanceState::DRIVING: {
+			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
+						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
+			_forwards_velocity_smoothing.updateDurations(max_velocity);
+			_forwards_velocity_smoothing.updateTraj(dt);
+			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+					_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
+			break;
+		}
+
+	case GuidanceState::GOAL_REACHED:
+		// temporary till I find a better way to stop the vehicle
+		desired_speed = 0.f;
+		heading_error = 0.f;
+		angular_velocity = 0.f;
+		_desired_angular_velocity = 0.f;
+		break;
+	}
+
+	// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
+	float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
+				     dt) + heading_error;
+
+	differential_drive_setpoint_s output{};
+	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
+	output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
+	output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
+	output.timestamp = hrt_absolute_time();
+
+	_differential_drive_setpoint_pub.publish(output);
+
+	_current_waypoint = current_waypoint;
+}
+
+void DifferentialDriveGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_heading_p_controller,
+			   _param_rdd_p_gain_heading.get(),  // Proportional gain
+			   0,  // Integral gain
+			   0,  // Derivative gain
+			   0,  // Integral limit
+			   200);  // Output limit
+
+	_forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get());
+	_forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get());
+	_forwards_velocity_smoothing.setMaxVel(_max_speed);
+}
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 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
+
+#include <px4_platform_common/module_params.h>
+
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include <lib/pid/pid.h>
+
+
+/**
+ * @brief Enum class for the different states of guidance.
+ */
+enum class GuidanceState {
+	TURNING, ///< The vehicle is currently turning.
+	DRIVING, ///< The vehicle is currently driving straight.
+	GOAL_REACHED ///< The vehicle has reached its goal.
+};
+
+/**
+ * @brief Class for differential drive guidance.
+ */
+class DifferentialDriveGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for DifferentialDriveGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	DifferentialDriveGuidance(ModuleParams *parent);
+	~DifferentialDriveGuidance() = default;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param global_pos The global position of the vehicle in degrees.
+	 * @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
+	 * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
+	 * @param vehicle_yaw The yaw orientation of the vehicle in radians.
+	 * @param body_velocity The velocity of the vehicle in m/s.
+	 * @param angular_velocity The angular velocity of the vehicle in rad/s.
+	 * @param dt The time step in seconds.
+	 */
+	void computeGuidance(float yaw, float angular_velocity, float dt);
+
+	/**
+	 * @brief Set the maximum speed for the vehicle.
+	 * @param max_speed The maximum speed in m/s.
+	 * @return The set maximum speed in m/s.
+	 */
+	float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
+
+
+	/**
+	 * @brief Set the maximum angular velocity for the vehicle.
+	 * @param max_angular_velocity The maximum angular velocity in rad/s.
+	 * @return The set maximum angular velocity in rad/s.
+	 */
+	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
+	position_setpoint_triplet_s _position_setpoint_triplet{};
+	vehicle_global_position_s _vehicle_global_position{};
+
+	GuidanceState _currentState; ///< The current state of guidance.
+
+	float _desired_angular_velocity; ///< The desired angular velocity.
+
+	float _max_speed; ///< The maximum speed.
+	float _max_angular_velocity; ///< The maximum angular velocity.
+
+	matrix::Vector2d _current_waypoint; ///< The current waypoint.
+
+	VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
+	PositionSmoothing _position_smoothing; ///< The position smoothing.
+
+	PID_t _heading_p_controller; ///< The PID controller for yaw rate.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
+		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
+	)
+};
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 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
@@ -37,4 +37,4 @@ px4_add_library(DifferentialDriveKinematics

 target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

-px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
+px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 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
@@ -36,8 +36,38 @@
 #include <mathlib/mathlib.h>

 using namespace matrix;
+using namespace time_literals;

-matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
+{}
+
+void DifferentialDriveKinematics::allocate()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_differential_drive_control_output_sub.updated()) {
+		_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
+	}
+
+	const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now;
+
+	Vector2f wheel_speeds =
+		computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate);
+
+	if (!_armed || setpoint_timeout) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
 {
 	if (_max_speed < FLT_EPSILON) {
 		return Vector2f();
@@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin

 	if (combined_velocity > _max_speed) {
 		float excess_velocity = fabsf(combined_velocity - _max_speed);
-		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
 		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
 	}

@@ -65,4 +95,3 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
 	return Vector2f(linear_velocity_x - rotational_velocity,
 			linear_velocity_x + rotational_velocity) / _max_speed;
 }
-
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 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
@@ -34,6 +34,11 @@
 #pragma once

 #include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/differential_drive_setpoint.h>

 /**
 * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
@@ -41,10 +46,10 @@
 * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
 * given linear velocity and yaw rate.
 */
-class DifferentialDriveKinematics
+class DifferentialDriveKinematics : public ModuleParams
 {
 public:
-	DifferentialDriveKinematics() = default;
+	DifferentialDriveKinematics(ModuleParams *parent);
 	~DifferentialDriveKinematics() = default;

 	/**
@@ -68,6 +73,10 @@ public:
 	 */
 	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };

+	void setArmed(const bool armed) { _armed = armed; };
+
+	void allocate();
+
 	/**
 	 * @brief Computes the inverse kinematics for differential drive.
 	 *
@@ -75,10 +84,20 @@ public:
 	 * @param yaw_rate Yaw rate of the robot.
 	 * @return matrix::Vector2f Motor velocities for the right and left motors.
 	 */
-	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;

 private:
+	uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	differential_drive_setpoint_s _differential_drive_control_output{};
+	bool _armed = false;
+
 	float _wheel_base{0.f};
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 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
@@ -37,9 +37,14 @@

 using namespace matrix;

-TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+class DifferentialDriveKinematicsTest : public ::testing::Test
+{
+public:
+	DifferentialDriveKinematics kinematics{nullptr};
+};
+
+TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -49,9 +54,8 @@ TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
 }


-TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(0.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -61,9 +65,8 @@ TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
 }


-TEST(DifferentialDriveKinematicsTest, UnitCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -73,9 +76,8 @@ TEST(DifferentialDriveKinematicsTest, UnitCase)
 }


-TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -85,9 +87,8 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
 }


-TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
 }

-TEST(DifferentialDriveKinematicsTest, RandomCase)
+TEST_F(DifferentialDriveKinematicsTest, RandomCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -107,9 +107,8 @@ TEST(DifferentialDriveKinematicsTest, RandomCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
 }

-TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -118,9 +117,8 @@ TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
 }

-TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -129,9 +127,8 @@ TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(FLT_MIN);
 	kinematics.setMaxSpeed(FLT_MIN);
 	kinematics.setMaxAngularVelocity(FLT_MIN);
@@ -140,9 +137,8 @@ TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -151,9 +147,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -162,9 +157,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
 }

-TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -1,5 +1,5 @@
-menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
-	bool "differential_drive_control"
+menuconfig MODULES_DIFFERENTIAL_DRIVE
+	bool "differential_drive"
 	default n
 	depends on MODULES_CONTROL_ALLOCATOR
 	---help---
@@ -0,0 +1,122 @@
+module_name: Differential Drive
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_WHEEL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.3
+      RDD_P_HEADING:
+        description:
+          short: Proportional gain for heading controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_P_SPEED:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_I_SPEED:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      RDD_P_ANG_VEL:
+        description:
+          short: Proportional gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_I_ANG_VEL:
+        description:
+          short: Integral gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      RDD_MAX_JERK:
+        description:
+          short: Maximum jerk
+          long: Limit for forwards acc/deceleration change.
+        type: float
+        unit: m/s^3
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+      RDD_MAX_ACCEL:
+        description:
+          short: Maximum acceleration
+          long: Maximum acceleration is used to limit the acceleration of the rover
+        type: float
+        unit: m/s^2
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
@@ -1,55 +0,0 @@
-module_name: Differential Drive Control
-
-parameters:
-  - group: Rover Differential Drive
-    definitions:
-      RDD_WHEEL_BASE:
-        description:
-          short: Wheel base
-          long: Distance from the center of the right wheel to the center of the left wheel
-        type: float
-        unit: m
-        min: 0.001
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 0.5
-      RDD_WHEEL_RADIUS:
-        description:
-          short: Wheel radius
-          long: Size of the wheel, half the diameter of the wheel
-        type: float
-        unit: m
-        min: 0.001
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 0.1
-      RDD_SPEED_SCALE:
-        description:
-          short: Manual speed scale
-        type: float
-        min: 0
-        max: 1.0
-        increment: 0.01
-        decimal: 2
-        default: 1.0
-      RDD_ANG_SCALE:
-        description:
-          short: Manual angular velocity scale
-        type: float
-        min: 0
-        max: 1.0
-        increment: 0.01
-        decimal: 2
-        default: 1.0
-      RDD_WHL_SPEED:
-        description:
-          short: Maximum wheel speed
-        type: float
-        unit: rad/s
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 10
@@ -80,8 +80,11 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			}
 		}

+		if (_pos_ref.isInitialized()) {
+			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
+		}
+
 		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
-		updateGnssPos(gnss_sample, _aid_src_gnss_pos);

 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
@@ -510,18 +510,38 @@ float FixedwingPositionControl::getCorrectedNpfgRollSetpoint()
 {
 	// Scale the npfg output to zero if npfg is not certain for correct output
 	float new_roll_setpoint(_npfg.getRollSetpoint());
-	const float can_run_factor(_npfg.canRun(_local_pos, _wind_valid));
+	const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f));

-	if ((1.f - can_run_factor) < FLT_EPSILON) {
+	// If the npfg was not running before, reset the user warning variables.
+	hrt_abstime now{hrt_absolute_time()};
+
+	if ((now - _time_since_last_npfg_call) > 2_s) {
 		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
 	}

-	if (((1.f - can_run_factor) > FLT_EPSILON) && _need_report_npfg_uncertain_condition) {
-		_need_report_npfg_uncertain_condition = false;
-		events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
-			     "Roll command reduced due to uncertain velocity/wind estimates!");
+	// Warn the user when the scale is less than 90% for at least 2 seconds.
+	if ((1.f - can_run_factor) < 0.1f) {
+		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
+
+	} else if (_need_report_npfg_uncertain_condition) {
+		if (_time_since_first_reduced_roll == 0U) {
+			_time_since_first_reduced_roll = hrt_absolute_time();
+		}
+
+		if ((now - _time_since_first_reduced_roll) > 2_s) {
+			_need_report_npfg_uncertain_condition = false;
+			events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
+				     "Roll command reduced due to uncertain velocity/wind estimates!");
+		}
+
+	} else {
+		// Nothing to do, already reported.
 	}

+	_time_since_last_npfg_call = now;
+
 	return can_run_factor * (new_roll_setpoint);
 }

@@ -1368,7 +1388,7 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				0.0f;
 	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);

-	_att_sp.roll_body = _npfg.getRollSetpoint();
+	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@@ -414,6 +414,8 @@ private:
 	// nonlinear path following guidance - lateral-directional position control
 	NPFG _npfg;
 	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
+	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
+	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed

 	PerformanceModel _performance_model;

@@ -57,6 +57,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_optional_topic("differential_drive_control_output", 100);
 	add_optional_topic("differential_drive_setpoint", 100);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
@@ -928,16 +928,15 @@ MissionBlock::initialize()
 	_mission_item.origin = ORIGIN_ONBOARD;
 }

-void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
-		HeadingMode heading_mode) const
+void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp,
+		float loiter_radius) const
 {
 	item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
-	item.lat = dest.lat;
-	item.lon = dest.lon;
-	item.altitude = dest.alt;
+	item.lat = pos_yaw_sp.lat;
+	item.lon = pos_yaw_sp.lon;
+	item.altitude = pos_yaw_sp.alt;
 	item.altitude_is_relative = false;
-
-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = pos_yaw_sp.yaw;

 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	item.time_inside = 0.0f;
@@ -946,8 +945,8 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina
 	item.loiter_radius = loiter_radius;
 }

-void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
-		float loiter_radius, HeadingMode heading_mode) const
+void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp,
+		float loiter_time, float loiter_radius) const
 {
 	const bool autocontinue = (loiter_time > -FLT_EPSILON);

@@ -958,12 +957,12 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat
 		item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 	}

-	item.lat = dest.lat;
-	item.lon = dest.lon;
-	item.altitude = dest.alt;
+	item.lat = pos_yaw_sp.lat;
+	item.lon = pos_yaw_sp.lon;
+	item.altitude = pos_yaw_sp.alt;
 	item.altitude_is_relative = false;

-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = NAN;

 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	item.time_inside = math::max(loiter_time, 0.0f);
@@ -972,13 +971,12 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat
 	item.loiter_radius = loiter_radius;
 }

-void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
-		HeadingMode heading_mode) const
+void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const
 {
 	item.nav_cmd = NAV_CMD_WAYPOINT;
-	item.lat = dest.lat;
-	item.lon = dest.lon;
-	item.altitude = dest.alt;
+	item.lat = pos_yaw_sp.lat;
+	item.lon = pos_yaw_sp.lon;
+	item.altitude = pos_yaw_sp.alt;
 	item.altitude_is_relative = false;

 	item.autocontinue = true;
@@ -986,46 +984,22 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest
 	item.time_inside = 0.f;
 	item.origin = ORIGIN_ONBOARD;

-	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+	item.yaw = pos_yaw_sp.yaw;
 }

-void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest,
-				      HeadingMode heading_mode) const
+void MissionBlock::setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const
 {
 	item.nav_cmd = NAV_CMD_LAND;
-	item.lat = dest.lat;
-	item.lon = dest.lon;
-	item.altitude = dest.alt;
-
-	if (heading_mode == HeadingMode::CURRENT_HEADING) {
-		item.yaw = _navigator->get_local_position()->heading;
-
-	} else {
-		item.yaw = dest.yaw;
-	}
-
+	item.lat = pos_yaw_sp.lat;
+	item.lon = pos_yaw_sp.lon;
+	item.altitude = pos_yaw_sp.alt;
+	item.yaw = pos_yaw_sp.yaw;
 	item.acceptance_radius = _navigator->get_acceptance_radius();
 	item.time_inside = 0.0f;
 	item.autocontinue = true;
 	item.origin = ORIGIN_ONBOARD;
 }

-float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const
-{
-	float desired_yaw(_navigator->get_local_position()->heading);
-
-	if (heading_mode == HeadingMode::NAVIGATION_HEADING) {
-		desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon, dest.lat, dest.lon);
-
-	} else if (heading_mode == HeadingMode::DESTINATION_HEADING) {
-		desired_yaw = dest.yaw;
-
-	}
-
-	return desired_yaw;
-}
-
 void MissionBlock::startPrecLand(uint16_t land_precision)
 {
 	if (_mission_item.land_precision == 1) {
@@ -205,18 +205,14 @@ protected:
 	 */
 	void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode);

-	void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
-				       HeadingMode heading_mode) const;
+	void setLoiterToAltMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_radius) const;

-	void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
-				      float loiter_radius, HeadingMode heading_mode) const;
+	void setLoiterHoldMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp, float loiter_time,
+				      float loiter_radius) const;

-	void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
-					  HeadingMode heading_mode) const;
+	void setMoveToPositionMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const;

-	void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const;
-
-	float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
+	void setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const;

 	void startPrecLand(uint16_t land_precision);

@@ -230,15 +230,15 @@ struct mission_fence_point_s {
 };

 /**
- * @brief Return to launch position.
- * Defines the position and landing yaw for the return to launch destination.
+ * @brief Position and yaw setpoint struct.
+ * Used in RTL state machine.
 *
 */
-struct DestinationPosition {
-	double lat;	/**< latitude in WGS84 [rad].*/
-	double lon;	/**< longitude in WGS84 [rad].*/
-	float alt;	/**< altitude in MSL [m].*/
-	float yaw;	/**< final yaw when landed [rad].*/
+struct PositionYawSetpoint {
+	double lat;	/**< latitude setpoint in WGS84 [rad].*/
+	double lon;	/**< longitude setpoint in WGS84 [rad].*/
+	float alt;	/**< altitude setpoint in MSL [m].*/
+	float yaw;	/**< yaw setpoint [rad].*/
 };


@@ -297,7 +297,7 @@ void RTL::setRtlTypeAndDestination()
 	if (_param_rtl_type.get() != 2) {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
-		DestinationPosition rtl_position;
+		PositionYawSetpoint rtl_position;
 		float rtl_alt;
 		findRtlDestination(destination_type, rtl_position, rtl_alt);

@@ -333,7 +333,7 @@ void RTL::setRtlTypeAndDestination()
 	}
 }

-void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
 	rtl_position.alt = _home_pos_sub.get().alt;
@@ -413,7 +413,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit
 			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
 				float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};

-				DestinationPosition safepoint_position;
+				PositionYawSetpoint safepoint_position;
 				setSafepointAsDestination(safepoint_position, mission_safe_point);

 				if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
@@ -435,7 +435,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit
 	}
 }

-void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const
+void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const
 {
 	rtl_position.alt = land_mission_item.altitude_is_relative ?	land_mission_item.altitude +
 			   _home_pos_sub.get().alt : land_mission_item.altitude;
@@ -444,7 +444,7 @@ void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_ite
 	rtl_position.yaw = _home_pos_sub.get().yaw;
 }

-void RTL::setSafepointAsDestination(DestinationPosition &rtl_position,
+void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position,
 				    const mission_item_s &mission_safe_point) const
 {
 	// There is a safe point closer than home/mission landing
@@ -472,7 +472,7 @@ void RTL::setSafepointAsDestination(DestinationPosition &rtl_position,
 	}
 }

-float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position,
+float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
 		float cone_half_angle_deg) const
 {
 	// horizontal distance to destination
@@ -576,7 +576,7 @@ bool RTL::hasMissionLandStart() const
 	return _mission_sub.get().land_start_index > 0;
 }

-bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const
+bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const
 {
 	return readVtolLandApproaches(rtl_position).isAnyApproachValid();
 }
@@ -611,7 +611,7 @@ loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land
 	}
 }

-land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const
+land_approaches_s RTL::readVtolLandApproaches(PositionYawSetpoint rtl_position) const
 {

 	// go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT
@@ -109,20 +109,20 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt);

 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
 	 *
 	 */
-	void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const;
+	void setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const;

 	/**
 	 * @brief Set the safepoint as destination.
 	 *
 	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
 	 */
-	void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point) const;
+	void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const;

 	/**
 	 * @brief calculate return altitude from cone half angle
@@ -131,7 +131,7 @@ private:
 	 * @param[in] cone_half_angle_deg half angle of the cone [deg]
 	 * @return return altitude
 	 */
-	float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position,
+	float calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
 			float cone_half_angle_deg) const;

 	/**
@@ -152,7 +152,7 @@ private:
 	 * @param[in] rtl_position landing position of the rtl
 	 *
 	 */
-	land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const;
+	land_approaches_s readVtolLandApproaches(PositionYawSetpoint rtl_position) const;

 	/**
 	 * @brief Has VTOL land approach
@@ -162,7 +162,7 @@ private:
 	 * @return true if home land approaches are defined for home position
 	 * @return false otherwise
 	 */
-	bool hasVtolLandApproach(const DestinationPosition &rtl_position) const;
+	bool hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const;

 	/**
 	 * @brief Choose best landing approach
@@ -122,7 +122,7 @@ void RtlDirect::on_active()
 	}
 }

-void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s loiter_pos)
+void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos)
 {
 	_home_pos_sub.update();

@@ -178,41 +178,39 @@ void RtlDirect::set_rtl_item()
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
 	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);

-	HeadingMode rtl_heading_mode = static_cast<HeadingMode>(_param_rtl_hdg_md.get());
-
-	if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) {
-		rtl_heading_mode = HeadingMode::DESTINATION_HEADING;
-	}
+	const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get();

 	switch (_rtl_state) {
 	case RTLState::CLIMBING: {
-			DestinationPosition dest {
+			PositionYawSetpoint pos_yaw_sp {
 				.lat = _global_pos_sub.get().lat,
 				.lon = _global_pos_sub.get().lon,
 				.alt = _rtl_alt,
+				.yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading,
 			};
-
-			setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING);
+			setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius());

 			_rtl_state = RTLState::MOVE_TO_LOITER;
 			break;
 		}

 	case RTLState::MOVE_TO_LOITER: {
-			DestinationPosition dest {
+			PositionYawSetpoint pos_yaw_sp {
 				.lat = _land_approach.lat,
 				.lon = _land_approach.lon,
 				.alt = _rtl_alt,
-				.yaw = _destination.yaw,
 			};

 			// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status
 			// can be displayed on groundstation and the WP is accepted once within loiter radius
 			if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-				setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, rtl_heading_mode);
+				pos_yaw_sp.yaw = NAN;
+				setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, 0.f, _land_approach.loiter_radius_m);

 			} else {
-				setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
+				// already set final yaw if close to destination and weather vane is disabled
+				pos_yaw_sp.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN;
+				setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
 			}

 			_rtl_state = RTLState::LOITER_DOWN;
@@ -221,14 +219,14 @@ void RtlDirect::set_rtl_item()
 		}

 	case RTLState::LOITER_DOWN: {
-			DestinationPosition dest{
+			PositionYawSetpoint pos_yaw_sp{
 				.lat = _land_approach.lat,
 				.lon = _land_approach.lon,
 				.alt = loiter_altitude,
-				.yaw = _destination.yaw,
+				.yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if weather vane is disabled
 			};

-			setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode);
+			setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _land_approach.loiter_radius_m);

 			pos_sp_triplet->next.valid = true;
 			pos_sp_triplet->next.lat = _destination.lat;
@@ -248,15 +246,14 @@ void RtlDirect::set_rtl_item()
 		}

 	case RTLState::LOITER_HOLD: {
-			DestinationPosition dest {
+			PositionYawSetpoint pos_yaw_sp {
 				.lat = _land_approach.lat,
 				.lon = _land_approach.lon,
 				.alt = loiter_altitude,
-				.yaw = _destination.yaw,
+				.yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if weather vane is disabled
 			};

-			setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m,
-						 rtl_heading_mode);
+			setLoiterHoldMissionItem(_mission_item, pos_yaw_sp, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m);

 			if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
@@ -276,10 +273,11 @@ void RtlDirect::set_rtl_item()

 	case RTLState::MOVE_TO_LAND: {

-			DestinationPosition dest{_destination};
-			dest.alt = loiter_altitude;
+			PositionYawSetpoint pos_yaw_sp{_destination};
+			pos_yaw_sp.alt = loiter_altitude;
+			pos_yaw_sp.yaw = NAN;

-			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
+			setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);

 			// Prepare for transition
 			_mission_item.vtol_back_transition = true;
@@ -306,10 +304,11 @@ void RtlDirect::set_rtl_item()
 		}

 	case RTLState::MOVE_TO_LAND_HOVER: {
-			DestinationPosition dest{_destination};
-			dest.alt = loiter_altitude;
+			PositionYawSetpoint pos_yaw_sp{_destination};
+			pos_yaw_sp.alt = loiter_altitude;
+			pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled

-			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
+			setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);

 			_rtl_state = RTLState::LAND;
@@ -318,8 +317,9 @@ void RtlDirect::set_rtl_item()
 		}

 	case RTLState::LAND: {
-
-			setLandMissionItem(_mission_item, _destination, rtl_heading_mode);
+			PositionYawSetpoint pos_yaw_sp{_destination};
+			pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled
+			setLandMissionItem(_mission_item, pos_yaw_sp);

 			_mission_item.land_precision = _param_rtl_pld_md.get();

@@ -99,7 +99,7 @@ public:
 	void setReturnAltMin(bool min) { _enforce_rtl_alt = min; }
 	void setRtlAlt(float alt) {_rtl_alt = alt;};

-	void setRtlPosition(DestinationPosition position, loiter_point_s loiter_pos);
+	void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos);

 private:
 	/**
@@ -179,7 +179,7 @@ private:
 	bool _enforce_rtl_alt{false};
 	bool _force_heading{false};

-	DestinationPosition _destination; ///< the RTL position to fly to
+	PositionYawSetpoint _destination; ///< the RTL position to fly to
 	loiter_point_s _land_approach;

 	float _rtl_alt{0.0f};	///< AMSL altitude at which the vehicle should return to the home position
@@ -190,9 +190,11 @@ private:
 		(ParamFloat<px4::params::RTL_MIN_DIST>)    _param_rtl_min_dist,
 		(ParamInt<px4::params::RTL_PLD_MD>)        _param_rtl_pld_md,
 		(ParamFloat<px4::params::RTL_LOITER_RAD>)  _param_rtl_loiter_rad,
-		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md,
 		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
-		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin
+		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin,
+
+		// external params
+		(ParamBool<px4::params::WV_EN>) _param_wv_en
 	)

 	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID};
@@ -160,18 +160,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
 */
 PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 80.0f);

-/**
- * RTL heading mode
- *
- * Defines the heading behavior during RTL
- *
- * @value 0 Towards next waypoint.
- * @value 1 Heading matches destination.
- * @value 2 Use current heading.
- * @group Return Mode
- */
-PARAM_DEFINE_INT32(RTL_HDG_MD, 0);
-
 /**
 * RTL time estimate safety margin factor
 *
@@ -216,6 +216,15 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}

+	// GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat
+	std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name +
+				    "/link/base_link/sensor/navsat_sensor/navsat";
+
+	if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str());
+		return PX4_ERROR;
+	}
+
 	if (!_mixing_interface_esc.init(_model_name)) {
 		PX4_ERR("failed to init ESC output");
 		return PX4_ERROR;
@@ -565,10 +574,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 			vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
 			_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);

-			if (!_pos_ref.isInitialized()) {
-				_pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time());
-			}
-
 			vehicle_local_position_s local_position_groundtruth{};
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 			local_position_groundtruth.timestamp_sample = time_us;
@@ -595,31 +600,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)

 			local_position_groundtruth.heading = euler.psi();

-			local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
-			local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
-			local_position_groundtruth.ref_alt = _param_sim_home_alt.get();
-			local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+			if (_pos_ref.isInitialized()) {
+
+				local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
+				local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
+				local_position_groundtruth.ref_alt = _alt_ref;
+				local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+				local_position_groundtruth.xy_global = true;
+				local_position_groundtruth.z_global = true;
+
+			} else {
+				local_position_groundtruth.ref_lat = static_cast<double>(NAN);
+				local_position_groundtruth.ref_lon = static_cast<double>(NAN);
+				local_position_groundtruth.ref_alt = NAN;
+				local_position_groundtruth.ref_timestamp = 0;
+				local_position_groundtruth.xy_global = false;
+				local_position_groundtruth.z_global = false;
+			}

 			local_position_groundtruth.timestamp = hrt_absolute_time();
 			_lpos_ground_truth_pub.publish(local_position_groundtruth);

-			if (_pos_ref.isInitialized()) {
-				// publish position groundtruth
-				vehicle_global_position_s global_position_groundtruth{};
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-				global_position_groundtruth.timestamp_sample = time_us;
-#else
-				global_position_groundtruth.timestamp_sample = hrt_absolute_time();
-#endif
-
-				_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
-						   global_position_groundtruth.lat, global_position_groundtruth.lon);
-
-				global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
-				global_position_groundtruth.timestamp = hrt_absolute_time();
-				_gpos_ground_truth_pub.publish(global_position_groundtruth);
-			}
-
 			pthread_mutex_unlock(&_node_mutex);
 			return;
 		}
@@ -704,6 +705,44 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry
 	pthread_mutex_unlock(&_node_mutex);
 }

+void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000);
+
+	if (time_us > _world_time_us.load()) {
+		updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec());
+	}
+
+	_timestamp_prev = time_us;
+
+	// initialize gps position
+	if (!_pos_ref.isInitialized()) {
+		_pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time());
+		_alt_ref = nav_sat.altitude();
+
+	} else {
+		// publish GPS groundtruth
+		vehicle_global_position_s global_position_groundtruth{};
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		global_position_groundtruth.timestamp_sample = time_us;
+#else
+		global_position_groundtruth.timestamp_sample = hrt_absolute_time();
+#endif
+		global_position_groundtruth.lat = nav_sat.latitude_deg();
+		global_position_groundtruth.lon = nav_sat.longitude_deg();
+		global_position_groundtruth.alt = nav_sat.altitude();
+		_gpos_ground_truth_pub.publish(global_position_groundtruth);
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU)
 {
 	// FLU (ROS) to FRD (PX4) static rotation
@@ -105,6 +105,7 @@ private:
 	void imuCallback(const gz::msgs::IMU &imu);
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
 	void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
+	void navSatCallback(const gz::msgs::NavSat &nav_sat);

 	/**
 	*
@@ -139,6 +140,7 @@ private:
 	pthread_mutex_t _node_mutex;

 	MapProjection _pos_ref{};
+	double _alt_ref{}; // starting altitude reference

 	matrix::Vector3d _position_prev{};
 	matrix::Vector3d _velocity_prev{};
@@ -153,10 +155,4 @@ private:
 	float _temperature{288.15};  // 15 degrees

 	gz::transport::Node _node;
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
-		(ParamFloat<px4::params::SIM_GZ_HOME_LON>) _param_sim_home_lon,
-		(ParamFloat<px4::params::SIM_GZ_HOME_ALT>) _param_sim_home_alt
-	)
 };
@@ -40,26 +40,3 @@
 */
 PARAM_DEFINE_INT32(SIM_GZ_EN, 0);

-/**
- * simulator origin latitude
- *
- * @unit deg
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f);
-
-/**
- * simulator origin longitude
- *
- * @unit deg
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594);
-
-/**
- * simulator origin altitude
- *
- * @unit m
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0);
@@ -531,6 +531,140 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);

+	if ((hil_state.lat == 0) && (hil_state.lon == 0)) {
+		_time_us_prev = hil_state.time_usec;
+		return;
+	}
+
+	float dt_s = (hil_state.time_usec - _time_us_prev) * 1e-6f;
+
+	uint64_t timestamp_sample = hrt_absolute_time(); // TODO: HIL_STATE_QUATERNION.time_us
+
+	matrix::Quatf q(hil_state.attitude_quaternion);
+	q.normalize();
+
+	const matrix::Eulerf euler(q);
+
+	// angular velocity
+	{
+		vehicle_angular_velocity_s angular_velocity{};
+		angular_velocity.timestamp_sample = timestamp_sample;
+
+		angular_velocity.xyz[0] = hil_state.rollspeed;
+		angular_velocity.xyz[1] = hil_state.pitchspeed;
+		angular_velocity.xyz[2] = hil_state.yawspeed;
+
+		angular_velocity.xyz_derivative[0] = (hil_state.rollspeed - _rollspeed_prev) / dt_s;
+		angular_velocity.xyz_derivative[1] = (hil_state.pitchspeed - _pitchspeed_prev) / dt_s;
+		angular_velocity.xyz_derivative[2] = (hil_state.yawspeed - _yawspeed_prev) / dt_s;
+
+		angular_velocity.timestamp = hrt_absolute_time();
+		_vehicle_angular_velocity_pub.publish(angular_velocity);
+	}
+
+	// vehicle_acceleration // TODO:
+
+	// vehicle_attitude
+	{
+		vehicle_attitude_s attitude{};
+		attitude.timestamp_sample = timestamp_sample;
+
+		q.copyTo(attitude.q);
+
+		attitude.timestamp = hrt_absolute_time();
+		_vehicle_attitude_pub.publish(attitude);
+	}
+
+	// vehicle_local_position
+	{
+		vehicle_local_position_s local_position{};
+		local_position.timestamp_sample = timestamp_sample;
+
+
+		double lat = hil_state.lat * 1e-7; // degE7 -> deg
+		double lon = hil_state.lon * 1e-7; // degE7 -> deg
+
+		if (!_global_local_proj_ref.isInitialized() && (hil_state.lon != 0)) {
+			_global_local_proj_ref.initReference(lat, lon);
+			_global_local_alt0 = hil_state.alt / 1000.f; // mm -> m
+		}
+
+		if (_global_local_proj_ref.isInitialized()) {
+
+
+			local_position.xy_valid = true;
+			local_position.z_valid = true;
+			local_position.v_xy_valid = true;
+			local_position.v_z_valid = true;
+
+			_global_local_proj_ref.project(lat, lon, local_position.x, local_position.y);
+			local_position.z = _global_local_alt0 - (hil_state.alt / 1000.f); // mm -> m
+
+			local_position.vx = hil_state.vx / 100.f; // cm/s -> m/s
+			local_position.vy = hil_state.vy / 100.f; // cm/s -> m/s
+			local_position.vz = hil_state.vz / 100.f; // cm/s -> m/s
+			local_position.z_deriv = local_position.vz;
+
+			local_position.ax = hil_state.xacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+			local_position.ay = hil_state.yacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+			local_position.az = hil_state.zacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+
+			local_position.heading = euler.psi();
+			local_position.unaided_heading = local_position.heading;
+			local_position.heading_good_for_control = true;
+
+			local_position.xy_global = true;
+			local_position.z_global = true;
+			local_position.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
+			local_position.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
+			local_position.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
+			local_position.ref_alt = _global_local_alt0;
+
+			local_position.dist_bottom = NAN;
+
+			local_position.eph = 1.f;
+			local_position.epv = 2.f;
+			local_position.evh = 1.f;
+			local_position.evv = 1.f;
+
+			local_position.vxy_max = std::numeric_limits<float>::infinity();
+			local_position.vz_max = std::numeric_limits<float>::infinity();
+			local_position.hagl_min = std::numeric_limits<float>::infinity();
+			local_position.hagl_max = std::numeric_limits<float>::infinity();
+
+			local_position.timestamp = hrt_absolute_time();
+			_vehicle_local_position_pub.publish(local_position);
+		}
+	}
+
+
+	// global position
+	{
+		vehicle_global_position_s global_position{};
+		global_position.timestamp_sample = timestamp_sample;
+
+		global_position.lat = hil_state.lat / 1e7;
+		global_position.lon = hil_state.lon / 1e7;
+		global_position.alt = hil_state.alt / 1e3;
+		global_position.alt_ellipsoid = global_position.alt;
+
+		global_position.eph = 1.f;
+		global_position.epv = 2.f;
+
+		global_position.terrain_alt = NAN;
+
+		global_position.timestamp = hrt_absolute_time();
+		_vehicle_global_position_pub.publish(global_position);
+	}
+
+
+	_time_us_prev = hil_state.time_usec;
+	_rollspeed_prev = hil_state.rollspeed;
+	_pitchspeed_prev = hil_state.pitchspeed;
+	_yawspeed_prev = hil_state.yawspeed;
+
+#if 0
+
 	uint64_t timestamp = hrt_absolute_time();

 	/* angular velocity */
@@ -614,6 +748,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 		// always publish ground truth attitude message
 		_lpos_ground_truth_pub.publish(hil_lpos);
 	}
+#endif
+
 }

 void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *msg)
@@ -253,6 +253,19 @@ private:
 	uORB::Publication<vehicle_local_position_s>	_lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
 	uORB::Publication<input_rc_s>			_input_rc_pub{ORB_ID(input_rc)};

+
+	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Publication<vehicle_attitude_s>         _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
+	uORB::Publication<vehicle_global_position_s>  _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
+	uORB::Publication<vehicle_local_position_s>   _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::Publication<vehicle_odometry_s>         _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
+
+	uint64_t _time_us_prev{0};
+	float _rollspeed_prev{0.f};
+	float _pitchspeed_prev{0.f};
+	float _yawspeed_prev{0.f};
+
+
 	//rpm
 	uORB::Publication<rpm_s>			_rpm_pub{ORB_ID(rpm)};

@@ -225,6 +225,10 @@ menu "Zenoh publishers/subscribers"
 		bool "figure_eight_status"
 		default n

+	config ZENOH_PUBSUB_FLIGHT_PHASE_ESTIMATION
+		bool "flight_phase_estimation"
+		default n
+
 	config ZENOH_PUBSUB_FOLLOW_TARGET
 		bool "follow_target"
 		default n
@@ -882,6 +886,7 @@ config ZENOH_PUBSUB_ALL_SELECTION
 	select ZENOH_PUBSUB_FAILSAFE_FLAGS
 	select ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS
 	select ZENOH_PUBSUB_FIGURE_EIGHT_STATUS
+	select ZENOH_PUBSUB_FLIGHT_PHASE_ESTIMATION
 	select ZENOH_PUBSUB_FOLLOW_TARGET
 	select ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR
 	select ZENOH_PUBSUB_FOLLOW_TARGET_STATUS
Author	SHA1	Message	Date
Daniel Agar	c11f61d57d	commander: skip estimatorCheck for now	2024-02-15 15:46:31 -05:00
Daniel Agar	d9a924225c	simulator_mavlink: add heading_good_for_control	2024-02-15 15:46:14 -05:00
Daniel Agar	5f91c7fc2b	simulation: HIL_STATE_QUATERNION add angular acceleration and protect from initial lat/lon 0	2024-02-15 15:20:33 -05:00
Daniel Agar	79538ac013	[WIP] simple simulation without sensor/estimator	2024-02-15 14:57:37 -05:00
Cyril C	74303a79e1	drivers/batt_smbus: fix BQ40Z80 timeout problem (#22751 ) Co-authored-by: cyril.calvez <c.calvez@elistair.com>	2024-02-15 13:24:40 -05:00
Daniel Agar	8dc3975456	ekf2: only populate gnss pos aid src status if ref initialized - this is a minor logging improvement when plotting the position from the beginning of the log (often a replay session)	2024-02-15 13:13:10 -05:00
Matthias Grob	84a7d42566	rover build: correct differential drive kconfig name	2024-02-15 10:08:51 -05:00
Matthias Grob	f26df8492f	Update GPS drivers to contain the astyle fix	2024-02-15 15:23:06 +01:00
Konrad	cb09dde606	FixedwingPositionControl: Used corrected npfg roll output in path mode	2024-02-13 17:17:44 +01:00
Konrad	1a1891073e	FixedwingPositionControl: Only warn user when roll is reduced for a longer period of time	2024-02-13 17:17:44 +01:00
Daniel Agar	b8714f8980	ROMFS: rc.simulator EKF2 setup specific to gazebo classic	2024-02-13 11:14:44 -05:00
PX4 BuildBot	0c099f2b56	Update submodule gz to latest Tue Feb 13 12:39:17 UTC 2024 - gz in PX4/Firmware (`c9ad60e3cc`): https://github.com/PX4/PX4-gazebo-models/commit/c78f7f01417168e8faab7a83ade2129c0d26b39d - gz current upstream: https://github.com/PX4/PX4-gazebo-models/commit/f1c461fffb8567d6f0af770fb533f60f6ec62c22 - Changes: https://github.com/PX4/PX4-gazebo-models/compare/c78f7f01417168e8faab7a83ade2129c0d26b39d...f1c461fffb8567d6f0af770fb533f60f6ec62c22 f1c461f 2024-02-08 frederik - increase monocam clipping distance 6d5db73 2024-02-07 Sergei Grichine - Added Zero Turn Lawnmower model (#27) 5332071 2024-02-06 Frederik Markus - add navsat plugin to worlds and navsat sensor to models (#26)	2024-02-13 11:13:17 -05:00
Frederik Markus	bb53781b8f	simulation/gz_bridge: enable navsat plugin for accurate positioning of real life maps in Gazebo (#22638 ) * publish the global groundtruth from the navsat callback and rearrange the local groundtruth as the altitude reference now has a dependency on the global groundtruth being initialized --------- Signed-off-by: frederik <frederik.anilmarkus@gmail.com>	2024-02-13 11:09:35 -05:00
Silvan Fuhrer	c9ad60e3cc	Update src/modules/navigator/mission_block.cpp Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	a6ef7b6da9	RTL: write out weather vane in comments (instead of WV) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	6957818603	RTL: clean up naming of function arguments Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
Matthias Grob	cb03835124	RTL: use dest.yaw instead of a separate heading_sp	2024-02-13 10:34:57 +01:00
Silvan Fuhrer	b19e35ec7c	RTL: change when to set a heading setpoint, generally leave it up to the executer -remove RTL_HDG_MD -only set heading setpoint in Navigator::RTL once above landing point, or when RTL is triggered close to it -never set a heading during RTL if weather vane is enabled Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-02-13 10:34:57 +01:00
PX4 BuildBot	dce53a626e	boards: update all NuttX defconfigs	2024-02-12 08:58:49 -05:00
Daniel Agar	5f589bdda3	Update submodule GPSDrivers to latest Mon Feb 12 12:39:19 UTC 2024 - GPSDrivers in PX4/Firmware (17ff40898c683e1fe96ff9e2d2790594d188f872): https://github.com/PX4/PX4-GPSDrivers/commit/3393191fbb842f8e13a3f296218efec832640112 - GPSDrivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/f48cc01d31607baa4963bde090f530b44df3de12 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/3393191fbb842f8e13a3f296218efec832640112...f48cc01d31607baa4963bde090f530b44df3de12 f48cc01 2024-02-08 Julian Oes - ubx: separate config for jamming monitor bc72f55 2024-02-08 Julian Oes - sbf: simplify odd define Co-authored-by: PX4 BuildBot <bot@px4.io>	2024-02-12 08:58:21 -05:00
Matthias Grob	1998f54ea6	DifferentialDrive: move spoolup consideration to the main module	2024-02-12 14:29:10 +01:00
PerFrivik	bef694f9ba	Added spoolup and removed temporary timeout for EKF	2024-02-12 14:29:10 +01:00
PerFrivik	560d6a9d4b	cleanup + updated acro	2024-02-12 14:29:10 +01:00
PerFrivik	f996caa5bd	Fixed bug in the guidance logic After smoothing the linera velocity setpoint, the EKF has trouble initializing, becuase the acceleration is too smooth, to combat this issue, there is a 1 second delay when initializing the mission mode	2024-02-12 14:29:10 +01:00
PerFrivik	bb0dfba4e6	added acro mode Acro mode is manual mode, but with rate control	2024-02-12 14:29:10 +01:00
PerFrivik	d197d94889	Fixed guidance logic and added feedforward term to compute the angular velocity	2024-02-12 14:29:10 +01:00
Matthias Grob	396ef222ee	DifferentialDrive: Rework structure 3 Components Guidance - Control - Allocation with their corresponding uORB interface.	2024-02-12 14:29:10 +01:00
Matthias Grob	f85144ca76	DifferentialDrive: remove trailing zeros from prameter metadata	2024-02-12 14:29:10 +01:00
Matthias Grob	b54b4f7dce	Rename module differential_drive_control -> differential_drive	2024-02-12 14:29:10 +01:00
Matthias Grob	fc90e235f1	Rename differential drive setpoint topics	2024-02-12 14:29:10 +01:00
Matthias Grob	f7baeae1a0	DifferentialDriveControl: only save required parts of uORB message	2024-02-12 14:29:10 +01:00
PerFrivik	e457a5baed	Differential Drive Guidance: Add guidance also add dependency on control allocation parameter CA_R_REV Differential Drive Guidance: Added mission logic Differential Drive Guidance Differential Drive Guidance Differential Guidance: Inlcude library Differential Guidance: Compiles, does not work though Differential Guidance: Works somewhat Differential Guidance: Temp Differential Guidance: Tuning Differeital Drive Guidance: Remove waypoint mover Differential Guidance: Fixed accuracy issue by converting from float to double Differential Guidance: rebased on differentialdrive and improved waypoint accuracy Temp Differential Guidance: cleanup temp	2024-02-12 14:29:10 +01:00