make boat setpoint msg less pedantic

updated pure pursuit

temp

forgot to wrap heading error

working guidance, with every case covered

removed unwanted files

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -72,8 +72,6 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		rc.rover_apps
 		rc.rover_defaults
-
-		rc.boat_defaults # hack
 	)
 endif()
 
@@ -84,6 +82,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
+if(CONFIG_MODULES_BOAT)
+	px4_add_romfs_files(
+		rc.boat_apps
+		rc.boat_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps

ROMFS/px4fmu_common/init.d/airframes/70001_boat

@@ -0,0 +1,59 @@
+#!/bin/sh
+#
+# @name RC Boat
+#
+# @type Boat
+# @class Boat
+#
+# @output Servo1 Steering
+# @output Motor1 Throttle
+
+. ${R}etc/init.d/rc.boat_defaults
+
+# Failsafe
+param set-default NAV_RCL_ACT 6 # Disarm on stick input loss
+param set-default NAV_DLL_ACT 6 # Disarm on ground station loss
+param set-default COM_DL_LOSS_T 5 # timeout for ground station connection
+param set-default COM_RC_LOSS_T 10 # timeout for stick input
+param set-default COM_RCL_EXCEPT 1 # Continue mission without stick input
+
+param set-default COM_ARM_WO_GPS 1 # Disable GPS prearm check
+param set-default COM_DISARM_LAND -1 # Don't disarm automatically
+param set-default COM_DISARM_PRFLT -1
+
+# Steering PWM output
+param set-default PWM_AUX_FUNC1 201
+param set-default PWM_AUX_DIS1 1500
+param set-default PWM_AUX_MIN1 1000
+param set-default PWM_AUX_MAX1 2000
+# Engine throttle PWM output
+param set-default PWM_AUX_FUNC2 101
+param set-default PWM_AUX_DIS2 900
+param set-default PWM_AUX_MIN2 940 # Engine doesn't rev below that
+param set-default PWM_AUX_MAX2 2000 # Maximum trhottle at which it's not so easy to flip the boat
+
+param set-default CA_AIRFRAME 5
+
+# Estimation
+param set-default EKF2_MULTI_IMU 0 # Disable multi-IMU
+param set-default SENS_IMU_MODE 1
+param set-default CAL_MAG0_PRIO 0 # Disable internal Magnetometer
+param set-default MBE_ENABLE 0 # Disable online mag calibration when disarmed
+param set-default EKF2_GPS_CTRL 15 # Enable GPS heading fusion
+param set-default SENS_GPS_MASK 0 # Disable multi-GPS blending
+
+# Estimation workarounds, should be double-checked if necessary
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_SYNT_MAG_Z 1 # Why? Who said magnetometer Z is not usable?
+
+param set-default EKF2_GPS_V_NOISE 0.751 # Lower certainty for GPS
+param set-default EKF2_GPS_POS_X 2.5 # GPS position, accurate?
+param set-default EKF2_REQ_EPV 10 # Accept GPS easier
+param set-default EKF2_REQ_HDRIFT 1
+param set-default EKF2_REQ_SACC 2
+param set-default EKF2_REQ_VDRIFT 20
+
+param set-default EKF2_MAG_CHK_STR 0.5 # Accept mag easier
+param set-default COM_ARM_MAG_STR 0 # Disable magnetic field strength prearm check
+param set-default COM_ARM_MAG_ANG 360 # Disable internal/external magnetometer consistency check
+param set-default EKF2_ABL_LIM 1 # Higher accelerometer bias limit, was sometimes above the threshold, GPS reporting 0 altitude issue?

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -157,3 +157,9 @@ if(CONFIG_MODULES_UUV_ATT_CONTROL)
 		60002_uuv_bluerov2_heavy
 	)
 endif()
+
+if(CONFIG_MODULES_BOAT)
+	px4_add_romfs_files(
+		70001_boat
+	)
+endif()

ROMFS/px4fmu_common/init.d/rc.boat_apps

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Standard apps for a differential drive rover.
+
+# Start the attitude and position estimator.
+ekf2 start &
+
+# Start rover differential drive controller.
+boat start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.boat_defaults

@@ -5,7 +5,7 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-set VEHICLE_TYPE rover
+set VEHICLE_TYPE boat
 
 # MAV_TYPE_SURFACE_BOAT 11
 param set-default MAV_TYPE 11

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -32,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi
 
+#
+# Boat setup.
+#
+if [ $VEHICLE_TYPE = boat ]
+then
+	# Start standard Boat apps.
+	. ${R}etc/init.d/rc.boat_apps
+fi
+
 #
 # Differential Rover setup.
 #

boards/px4/fmu-v5x/boat.px4board

@@ -0,0 +1,17 @@
+CONFIG_DRIVERS_IRLOCK=n
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
+CONFIG_MODULES_BOAT=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
+CONFIG_MODULES_MC_ATT_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
+CONFIG_MODULES_MC_POS_CONTROL=n
+CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
+CONFIG_EKF2_AUX_GLOBAL_POSITION=y
+# CONFIG_EKF2_WIND is not set

boards/px4/fmu-v6x/boat.px4board

@@ -0,0 +1,17 @@
+CONFIG_DRIVERS_IRLOCK=n
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
+CONFIG_MODULES_BOAT=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
+CONFIG_MODULES_MC_ATT_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
+CONFIG_MODULES_MC_POS_CONTROL=n
+CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
+CONFIG_EKF2_AUX_GLOBAL_POSITION=y
+# CONFIG_EKF2_WIND is not set

msg/BoatSetpoint.msg

@@ -0,0 +1,10 @@
+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] yaw_rate
+bool closed_loop_yaw_rate_control # true if rudder angle is controlled using feedback, false if direct feed-forward
+float32 heading_setpoint # [rad] desired heading
+bool closed_loop_heading_control # true if heading is controlled using feedback, false if direct feed-forward
+
+# TOPICS boat_setpoint boat_control_output

msg/CMakeLists.txt

@@ -53,6 +53,7 @@ set(msg_files
 	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
+	BoatSetpoint.msg
 	Buffer128.msg
 	ButtonEvent.msg
 	CameraCapture.msg

src/modules/boat/Boat.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   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 "Boat.hpp"
+
+Boat::Boat() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool Boat::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void Boat::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_max_speed = _param_bt_spd_max.get();
+	_boat_guidance.setMaxSpeed(_max_speed);
+	_boat_kinematics.setMaxSpeed(_max_speed);
+
+	_max_angular_velocity = _param_bt_ang_max.get();
+	_boat_guidance.setMaxAngularVelocity(_max_angular_velocity);
+	_boat_kinematics.setMaxAngularVelocity(_max_angular_velocity);
+}
+
+void Boat::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	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 parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+		updateParams();
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode{};
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_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);
+			_boat_kinematics.setArmed(spooled_up);
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
+		}
+	}
+
+	if (_manual_driving) {
+		// Manual mode
+		// Directly produce setpoints from the manual control setpoint (joystick)
+		if (_manual_control_setpoint_sub.updated()) {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				boat_setpoint_s setpoint{};
+				setpoint.speed = ((manual_control_setpoint.throttle + 1.f) * 0.5f) * math::max(0.f, _param_bt_spd_scale.get());
+				setpoint.yaw_rate = manual_control_setpoint.roll * _param_bt_ang_vel_scale.get();
+
+				// 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;
+				_boat_setpoint_pub.publish(setpoint);
+			}
+		}
+
+	} else if (_mission_driving) {
+		// Mission mode
+		// Directly receive setpoints from the guidance library
+		_boat_guidance.computeGuidance(
+			_boat_control.getVehicleYaw(),
+			_boat_control.getLocalPosition(),
+			dt
+		);
+	}
+
+	_boat_control.control(dt);
+	_boat_kinematics.allocate();
+}
+
+int Boat::task_spawn(int argc, char *argv[])
+{
+	Boat *instance = new Boat();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int Boat::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int Boat::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Boat Drive controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("boat", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int boat_main(int argc, char *argv[])
+{
+	return Boat::main(argc, argv);
+}

src/modules/boat/Boat.hpp

@@ -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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#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>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_servos.h>
+#include <uORB/topics/boat_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>
+
+#include "BoatControl/BoatControl.hpp"
+#include "BoatGuidance/BoatGuidance.hpp"
+#include "BoatKinematics/BoatKinematics.hpp"
+
+using namespace time_literals;
+
+class Boat : public ModuleBase<Boat>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	Boat();
+	~Boat() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+private:
+	void Run() override;
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
+
+	bool _manual_driving = false;
+	bool _mission_driving = false;
+	bool _acro_driving = false;
+
+
+	BoatGuidance _boat_guidance{this};
+	BoatControl _boat_control{this};
+	BoatKinematics _boat_kinematics{this};
+
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+
+	hrt_abstime _time_stamp_last{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
+		(ParamFloat<px4::params::BT_ANG_VEL_SCALE>) _param_bt_ang_vel_scale,
+		(ParamFloat<px4::params::BT_SPD_SCALE>) _param_bt_spd_scale,
+		(ParamFloat<px4::params::BT_ANG_MAX>) _param_bt_ang_max,
+		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max
+	)
+};

src/modules/boat/BoatControl/BoatControl.cpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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 "BoatControl.hpp"
+
+using namespace matrix;
+
+BoatControl::BoatControl(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 BoatControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_pid_angular_velocity,
+			   _param_bt_ang_p.get(),
+			   _param_bt_ang_i.get(),
+			   0,
+			   _param_bt_ang_imax.get(),
+			   _param_bt_ang_outlim.get());
+
+	pid_set_parameters(&_pid_speed,
+			   _param_bt_spd_p.get(),
+			   _param_bt_spd_i.get(),
+			   0,
+			   _param_bt_spd_imax.get(),
+			   _param_bt_spd_outlim.get());
+}
+
+void BoatControl::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)) {
+			_vehicle_local_position = 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);
+		}
+	}
+
+	_boat_setpoint_sub.update(&_boat_setpoint);
+
+	// PID to reach setpoint using control_output
+	boat_setpoint_s boat_control_output = _boat_setpoint;
+
+	if (_boat_setpoint.closed_loop_speed_control) {
+		boat_control_output.speed +=
+			pid_calculate(&_pid_speed, _boat_setpoint.speed, _vehicle_forward_speed, 0, dt);
+	}
+
+	if (_boat_setpoint.closed_loop_yaw_rate_control) {
+		boat_control_output.yaw_rate +=
+			pid_calculate(&_pid_angular_velocity, _boat_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
+	}
+
+	boat_control_output.timestamp = hrt_absolute_time();
+	_boat_control_output_pub.publish(boat_control_output);
+}

src/modules/boat/BoatControl/BoatControl.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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 BoatControl.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/boat_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+using namespace matrix;
+
+class BoatControl : public ModuleParams
+{
+public:
+	BoatControl(ModuleParams *parent);
+	~BoatControl() = default;
+
+	void control(float dt);
+	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
+	float getVehicleYaw() const { return _vehicle_yaw; }
+	vehicle_local_position_s getLocalPosition() const { return _vehicle_local_position; }
+
+protected:
+	void updateParams() override;
+
+private:
+	uORB::Subscription _boat_setpoint_sub{ORB_ID(boat_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<boat_setpoint_s> _boat_control_output_pub{ORB_ID(boat_control_output)};
+
+	boat_setpoint_s _boat_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};
+	Vector2f _vehicle_speed{0.f, 0.f};
+	vehicle_local_position_s _vehicle_local_position{};
+
+	PID_t _pid_angular_velocity;
+	PID_t _pid_speed;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::BT_SPD_P>) _param_bt_spd_p,
+		(ParamFloat<px4::params::BT_SPD_I>) _param_bt_spd_i,
+		(ParamFloat<px4::params::BT_SPD_IMAX>) _param_bt_spd_imax,
+		(ParamFloat<px4::params::BT_SPD_OUTLIM>) _param_bt_spd_outlim,
+		(ParamFloat<px4::params::BT_ANG_P>) _param_bt_ang_p,
+		(ParamFloat<px4::params::BT_ANG_I>) _param_bt_ang_i,
+		(ParamFloat<px4::params::BT_ANG_IMAX>) _param_bt_ang_imax,
+		(ParamFloat<px4::params::BT_ANG_OUTLIM>) _param_bt_ang_outlim
+	)
+};

src/modules/boat/BoatControl/CMakeLists.txt

@@ -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(BoatControl
+	BoatControl.cpp
+)
+
+target_link_libraries(BoatControl PUBLIC pid)
+target_include_directories(BoatControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/boat/BoatGuidance/BoatGuidance.cpp

@@ -0,0 +1,264 @@
+/****************************************************************************
+ *
+ *   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 "BoatGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+BoatGuidance::BoatGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	_currentState = GuidanceState::kDriving;
+}
+
+void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position,
+				   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);
+	}
+
+	const matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
+	const matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
+	const matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
+	const matrix::Vector2d previous_waypoint(_position_setpoint_triplet.previous.lat,
+			_position_setpoint_triplet.previous.lon);
+
+	if (!_global_local_proj_ref.isInitialized()
+	    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.timestamp)) {
+		_global_local_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
+						     vehicle_local_position.timestamp);
+	}
+
+	const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(
+				current_waypoint(0),
+				current_waypoint(1));
+	const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(
+				previous_waypoint(0),
+				previous_waypoint(1));
+	const Vector2f local_frame_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+
+	const float distance_to_next_wp = get_distance_to_next_waypoint(
+			global_position(0),
+			global_position(1),
+			current_waypoint(0),
+			current_waypoint(1));
+
+	float heading_error = 0.f;
+	float speed_interpolation = 0.f;
+	float heading_to_next_waypoint = 0.f;
+	float heading_error_to_next_waypoint = 0.f;
+	float desired_speed = _param_bt_spd_cruise.get();
+
+	// Go back to driving, when the waypoint has been reached
+	if (_current_waypoint != current_waypoint) {
+		_currentState = GuidanceState::kDriving;
+	}
+
+	// Make boat stop when it arrives at the last waypoint
+	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
+		_currentState = GuidanceState::kGoalReached;
+	}
+
+	switch (_currentState) {
+	case GuidanceState::kDriving: {
+			if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) {
+
+				float look_ahead_distance = getLookAheadDistance(
+								    current_waypoint_local_frame,
+								    previous_waypoint_local_frame,
+								    local_frame_position
+							    );
+
+				float desired_heading = calcDesiredHeading(
+								current_waypoint_local_frame,
+								previous_waypoint_local_frame,
+								local_frame_position,
+								look_ahead_distance
+							);
+
+				heading_error = matrix::wrap_pi(desired_heading - yaw);
+				_desired_angular_velocity = heading_error;
+
+				heading_to_next_waypoint = get_bearing_to_next_waypoint(
+								   previous_waypoint(0),
+								   previous_waypoint(1),
+								   current_waypoint(0),
+								   current_waypoint(1)
+							   );
+
+			} else {
+				_previous_local_position = local_frame_position;
+				_previous_position = global_position;
+				_currentState = GuidanceState::kDrivingToAPoint;
+			}
+
+			break;
+		}
+
+	// Control logic if there is no previous waypoint
+	case GuidanceState::kDrivingToAPoint: {
+			float look_ahead_distance = getLookAheadDistance(
+							    current_waypoint_local_frame,
+							    _previous_local_position,
+							    local_frame_position
+						    );
+
+			float desired_heading = calcDesiredHeading(
+							current_waypoint_local_frame,
+							_previous_local_position,
+							local_frame_position,
+							look_ahead_distance
+						);
+
+			heading_error = matrix::wrap_pi(desired_heading - yaw);
+			_desired_angular_velocity = heading_error;
+
+			heading_to_next_waypoint = get_bearing_to_next_waypoint(
+							   _previous_position(0),
+							   _previous_position(1),
+							   current_waypoint(0),
+							   current_waypoint(1)
+						   );
+			break;
+		}
+
+	case GuidanceState::kGoalReached:
+		desired_speed = 0.f;
+		heading_error = 0.f;
+		_desired_angular_velocity = 0.f;
+		break;
+	}
+
+	heading_error_to_next_waypoint = matrix::wrap_pi(heading_to_next_waypoint - yaw);
+
+	// Interpolate the speed based on the heading error
+	if (PX4_ISFINITE(heading_error_to_next_waypoint) && desired_speed > 0.1f) {
+
+		speed_interpolation = math::interpolate<float>(abs(heading_error_to_next_waypoint),
+				      _param_bt_min_heading_error.get() * M_PI_F / 180.f,
+				      _param_bt_max_heading_error.get() * M_PI_F / 180.f,
+				      _param_bt_spd_cruise.get(),
+				      _param_bt_spd_min.get());
+		desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed);
+
+	}
+
+	boat_setpoint_s output{};
+	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
+	output.yaw_rate = math::constrain(_desired_angular_velocity, -_max_angular_velocity, _max_angular_velocity);
+	output.closed_loop_speed_control = true;
+	output.closed_loop_yaw_rate_control = true;
+	output.timestamp = hrt_absolute_time();
+
+	_boat_setpoint_pub.publish(output);
+
+	_current_waypoint = current_waypoint;
+}
+
+float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+		const Vector2f &curr_pos_local)
+{
+	// Calculate crosstrack error
+	const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
+
+	if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
+		return 0.f;
+	}
+
+	const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
+	const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
+			prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
+	const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
+
+	if (crosstrack_error.length() < _param_look_ahead_distance.get()) {
+		return _param_look_ahead_distance.get();
+
+	} else {
+		return crosstrack_error.length();
+	}
+}
+
+float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+				       Vector2f const &curr_pos_local, float const &lookahead_distance)
+{
+	// Setup variables
+	const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
+	const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
+			0) - prev_wp_local(0));
+	const float a = -line_segment_slope;
+	const float c = -line_segment_rover_offset;
+	const float r = lookahead_distance;
+	const float x0 = -a * c / (a * a + 1.0f);
+	const float y0 = -c / (a * a + 1.0f);
+
+	// Calculate intersection points
+	if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
+		return 0.f;
+
+	} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
+		return atan2f(y0, x0);
+
+	} else { // Two intersetion points exist
+		const float d = r * r - c * c / (a * a + 1.0f);
+		const float mult = sqrt(d / (a * a + 1.0f));
+		const float ax = x0 + mult;
+		const float bx = x0 - mult;
+		const float ay = y0 - a * mult;
+		const float by = y0 + a * mult;
+		const Vector2f point1(ax, ay);
+		const Vector2f point2(bx, by);
+		const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
+		const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
+
+		// Return intersection point closer to current waypoint
+		if (distance1.norm_squared() < distance2.norm_squared()) {
+			return atan2f(ay, ax);
+
+		} else {
+			return atan2f(by, bx);
+		}
+	}
+}
+
+void BoatGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+}

src/modules/boat/BoatGuidance/BoatGuidance.hpp

@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ *   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/boat_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+#include <lib/pid/pid.h>
+#include <lib/l1/ECL_L1_Pos_Controller.hpp>
+
+using namespace matrix;
+/**
+ * @brief Enum class for the different states of guidance.
+ */
+enum class GuidanceState {
+	kDriving, ///< The vehicle is currently driving straight.
+	kDrivingToAPoint, ///< The vehicle is currently driving to the next waypoint.
+	kGoalReached ///< The vehicle has reached its goal.
+};
+
+/**
+ * @brief Class for boat drive guidance.
+ */
+class BoatGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for BoatGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	BoatGuidance(ModuleParams *parent);
+	~BoatGuidance() = 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, vehicle_local_position_s vehicle_local_position, float dt);
+
+	/**
+	 * @brief Calculate the lookahead distance based on the current and previous waypoints and the current position.
+	 * @param curr_wp_local The current waypoint in local coordinates.
+	 * @param prev_wp_local The previous waypoint in local coordinates.
+	 * @param curr_pos_local The current position in local coordinates.
+	 * @return The calculated lookahead distance.
+	 */
+	float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+				   const Vector2f &curr_pos_local);
+
+	/**
+	 * @brief Calculate the desired heading based on the current and previous waypoints, current position, and lookahead distance.
+	 * @param curr_wp_local The current waypoint in local coordinates.
+	 * @param prev_wp_local The previous waypoint in local coordinates.
+	 * @param curr_pos_local The current position in local coordinates.
+	 * @param lookahead_distance The lookahead distance.
+	 * @return The calculated desired heading.
+	 */
+	float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
+				 const float &lookahead_distance);
+
+	/**
+	 * @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 boat.
+	 * @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<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
+
+	position_setpoint_triplet_s _position_setpoint_triplet{};
+	vehicle_global_position_s _vehicle_global_position{};
+
+	GuidanceState _currentState;
+
+	float _desired_angular_velocity{};
+	float _max_angular_velocity{};
+	float _look_ahead_distance{};
+	float _max_speed{};
+
+	VelocitySmoothing _forwards_velocity_smoothing{};
+	PositionSmoothing _position_smoothing{};
+	MapProjection _global_local_proj_ref{};
+	matrix::Vector2d _current_waypoint{};
+	ECL_L1_Pos_Controller _l1_guidance{};
+	Vector2f _previous_local_position{};
+	Vector2d _previous_position{};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::BT_MAX_HERR>) _param_bt_max_heading_error,
+		(ParamFloat<px4::params::BT_MIN_HERR>) _param_bt_min_heading_error,
+		(ParamFloat<px4::params::BT_LOOKAHEAD>) _param_look_ahead_distance,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
+		(ParamFloat<px4::params::BT_SPD_CRUISE>) _param_bt_spd_cruise,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
+		(ParamFloat<px4::params::BT_SPD_MIN>) _param_bt_spd_min
+	)
+};

src/modules/boat/BoatGuidance/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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(BoatGuidance
+	BoatGuidance.cpp
+	BoatGuidance.hpp
+)
+
+target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/boat/BoatKinematics/BoatKinematics.cpp

@@ -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.
+ *
+ ****************************************************************************/
+
+#include "BoatKinematics.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+BoatKinematics::BoatKinematics(ModuleParams *parent) : ModuleParams(parent)
+{}
+
+void BoatKinematics::allocate()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_boat_control_output_sub.updated()) {
+		_boat_control_output_sub.copy(&_boat_control_output);
+	}
+
+	const bool setpoint_timeout = (_boat_control_output.timestamp + 100_ms) < now;
+
+	Vector2f boat_output =
+		computeInverseKinematics(_boat_control_output.speed, _boat_control_output.yaw_rate);
+
+	if (!_armed || setpoint_timeout) {
+		boat_output = {}; // stop
+	}
+
+	boat_output = matrix::constrain(boat_output, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.control[0] = boat_output(0);
+	actuator_motors.control[1] = boat_output(0);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+
+	actuator_servos_s actuator_servos{};
+	actuator_servos.control[0] = boat_output(1);
+	actuator_servos.control[1] = boat_output(1);
+	actuator_servos.timestamp = now;
+	_actuator_servos_pub.publish(actuator_servos);
+}
+
+matrix::Vector2f BoatKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
+{
+	if (_max_speed < FLT_EPSILON) {
+		return Vector2f();
+	}
+
+	// Room for more advanced dynamics, if required
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	float throttle = linear_velocity_x / _max_speed;
+	float rudder_angle = yaw_rate / _max_angular_velocity;
+
+	return Vector2f(throttle, rudder_angle);
+}

src/modules/boat/BoatKinematics/BoatKinematics.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 <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/actuator_servos.h>
+#include <uORB/topics/boat_setpoint.h>
+
+
+/**
+ * @brief Boat Drive Kinematics class for computing the kinematics of a boat drive robot.
+ *
+ * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
+ * given linear velocity and yaw rate.
+ */
+class BoatKinematics : public ModuleParams
+{
+public:
+	BoatKinematics(ModuleParams *parent);
+	~BoatKinematics() = default;
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
+
+	/**
+	 * @brief Sets the maximum speed of the robot.
+	 *
+	 * @param max_speed The maximum speed of the robot.
+	 */
+	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
+
+	/**
+	 * @brief Sets the maximum angular speed of the robot.
+	 *
+	 * @param max_angular_speed The maximum angular speed of the robot.
+	 */
+	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 boat drive.
+	 *
+	 * @param linear_velocity_x Linear velocity along the x-axis.
+	 * @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) const;
+
+private:
+	uORB::Subscription _boat_control_output_sub{ORB_ID(boat_control_output)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::PublicationMulti<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
+
+	boat_setpoint_s _boat_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
+	)
+};

src/modules/boat/BoatKinematics/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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(BoatKinematics
+	BoatKinematics.cpp
+)
+
+target_include_directories(BoatKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_functional_gtest(SRC BoatKinematicsTest.cpp LINKLIBS BoatKinematics)

src/modules/boat/CMakeLists.txt

@@ -0,0 +1,53 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(BoatControl)
+add_subdirectory(BoatGuidance)
+add_subdirectory(BoatKinematics)
+
+px4_add_module(
+	MODULE modules__boat
+	MAIN boat
+	SRCS
+		Boat.cpp
+		Boat.hpp
+	DEPENDS
+		BoatControl
+		BoatGuidance
+		BoatKinematics
+		px4_work_queue
+		l1
+		# modules__control_allocator # for parameter CA_R_REV
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/boat/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_BOAT
+	bool "boat"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of boats

src/modules/boat/module.yaml

@@ -0,0 +1,149 @@
+module_name: Boat Drive
+
+parameters:
+  - group: Boat Drive
+    definitions:
+      BT_SPD_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      BT_ANG_VEL_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      BT_SPD_CRUISE:
+        description:
+          short: Default cruise speed
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 50.0
+        decimal: 1
+        default: 7.0
+      BT_SPD_MAX:
+        description:
+          short: Maximum speed
+        type: float
+        unit: m/s
+        min: 1.0
+        max: 50.0
+        decimal: 1
+        default: 17.0
+      BT_SPD_MIN:
+        description:
+          short: Minimum speed
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 50.0
+        decimal: 1
+        default: 2.0
+      BT_SPD_P:
+        description:
+          short: Speed controller proportional gain
+        type: float
+        min: 0.0
+        max: 2.0
+        decimal: 2
+        default: 2.0
+      BT_SPD_I:
+        description:
+          short: Speed controller integral gain
+        type: float
+        min: 0.0
+        max: 20.0
+        decimal: 2
+        default: 1.0
+      BT_SPD_IMAX:
+        description:
+          short: Speed integral maximum value
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 20.0
+        decimal: 1
+        default: 20.0
+      BT_SPD_OUTLIM:
+        description:
+          short: Speed PI controller output limit
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 200.0
+        decimal: 1
+        default: 50.0
+      BT_ANG_MAX:
+        description:
+          short: Maximum angular velocity
+        type: float
+        unit: rad/s
+        min: 0.0
+        max: 20.0
+        decimal: 1
+        default: 1.0
+      BT_ANG_P:
+        description:
+          short: Angular velocity controller proportional gain
+        type: float
+        min: 0.0
+        max: 2.0
+        decimal: 2
+        default: 0.2
+      BT_ANG_I:
+        description:
+          short: Angular velocity controller integral gain
+        type: float
+        min: 0.0
+        max: 20.0
+        decimal: 2
+        default: 0.0
+      BT_ANG_IMAX:
+        description:
+          short: Angular velocity controller integral maximum value
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 20.0
+        decimal: 1
+        default: 20.0
+      BT_ANG_OUTLIM:
+        description:
+          short: Angular velocity controller PI controller output limit
+        type: float
+        unit: m/s
+        min: 0.0
+        max: 200.0
+        decimal: 1
+        default: 50.0
+      BT_MAX_HERR:
+        description:
+          short: Max heading error for slowdown
+        type: float
+        min: 0.0
+        decimal: 2
+        default: 90.0
+      BT_MIN_HERR:
+        description:
+          short: Min heading error for full speed
+        type: float
+        min: 0.0
+        decimal: 2
+        default: 30.0
+      BT_LOOKAHEAD:
+        description:
+          short: Lookahead distance for heading error
+        type: float
+        unit: m
+        min: 0.0
+        decimal: 2
+        default: 10.0

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -72,7 +72,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 
 	// 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;
+		_currentState = GuidanceState::kGoalReached;
 	}
 
 	float desired_speed = 0.f;
@@ -82,12 +82,12 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 		desired_speed = 0.f;
 
 		if (fabsf(heading_error) < 0.05f) {
-			_currentState = GuidanceState::DRIVING;
+			_currentState = GuidanceState::kDriving;
 		}
 
 		break;
 
-	case GuidanceState::DRIVING: {
+	case GuidanceState::kDriving: {
 			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);
@@ -97,7 +97,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 			break;
 		}
 
-	case GuidanceState::GOAL_REACHED:
+	case GuidanceState::kGoalReached:
 		// temporary till I find a better way to stop the vehicle
 		desired_speed = 0.f;
 		heading_error = 0.f;

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp

@@ -58,8 +58,8 @@
  */
 enum class GuidanceState {
 	TURNING, ///< The vehicle is currently turning.
-	DRIVING, ///< The vehicle is currently driving straight.
-	GOAL_REACHED ///< The vehicle has reached its goal.
+	kDriving, ///< The vehicle is currently driving straight.
+	kGoalReached ///< The vehicle has reached its goal.
 };
 
 /**