Implement a boat position controller for a bi-motor catamaran

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_boat

@@ -0,0 +1,31 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.boat_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_REV 1 # reverse right wheel

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

@@ -83,6 +83,7 @@ px4_add_romfs_files(
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
+	4012_gz_boat
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -77,6 +77,13 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	)
 endif()
 
+if(CONFIG_MODULES_BOAT_POS_CONTROL)
+	px4_add_romfs_files(
+		rc.boat_differential_apps
+		rc.boat_differential_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	px4_add_romfs_files(
 		rc.rover_differential_apps

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

@@ -0,0 +1,22 @@
+#!/bin/sh
+#
+# @name Motorized catamaran
+#
+#
+# @type Boat
+# @class Catamaran
+#
+#
+
+. ${R}etc/init.d/rc.boat_differential_defaults
+
+param set-default BAT1_N_CELLS 4
+
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+
+# Set geometry & output configration
+param set-default RBCLW_ADDRESS 128
+param set-default RBCLW_FUNC1 101
+param set-default RBCLW_FUNC2 102
+param set-default RBCLW_REV 1 # reverse right wheels

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

@@ -145,6 +145,13 @@ endif()
 if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	px4_add_romfs_files(
 		50003_aion_robotics_r1_rover
+		70001_boat_mot_catamaran
+	)
+endif()
+
+if(CONFIG_MODULES_BOAT_POS_CONTROL)
+	px4_add_romfs_files(
+		70001_boat_mot_catamaran
 	)
 endif()
 

ROMFS/px4fmu_common/init.d/rc.boat_differential_apps

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

ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults

@@ -0,0 +1,13 @@
+#!/bin/sh
+# Boat parameters.
+
+set VEHICLE_TYPE boat
+
+param set-default MAV_TYPE 11 # Surface vessel
+
+param set-default CA_AIRFRAME 13 # Boat (Differential)
+param set-default CA_R_REV 3 # Right and left motors reversible
+
+param set-default NAV_ACC_RAD 1.5 # mission acceptance radius
+
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

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_differential_apps
+fi
+
 #
 # Differential Rover setup.
 #

boards/px4/fmu-v6c/default.px4board

@@ -63,6 +63,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_BOAT_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y

boards/px4/sitl/default.px4board

@@ -8,6 +8,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
+CONFIG_MODULES_BOAT_POS_CONTROL=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

src/modules/boat_pos_control/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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_module(
+	MODULE modules__boat_pos_control
+	MAIN boat_pos_control
+	SRCS
+		boat_pos_control.cpp
+		boat_pos_control.hpp
+	DEPENDS
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+	)

src/modules/boat_pos_control/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_BOAT_POS_CONTROL
+	bool "boat_pos_control"
+	default n
+	---help---
+		Enable support for boat position controller