Merge remote-tracking branch 'px4/master' into pr-param_notify_schedule

.clang-format

@@ -1,13 +0,0 @@
-# Google C++ style as base
-Language: Cpp
-BasedOnStyle: Google
-
-# Indentation using tabs
-UseTab: ForContinuationAndIndentation
-TabWidth: 8
-IndentWidth: 8
-ContinuationIndentWidth: 8
-ConstructorInitializerIndentWidth: 8
-AccessModifierOffset: -8
-
-ColumnLimit: 120 # Allow more collumns

Makefile

@@ -375,10 +375,6 @@ format:
 	$(call colorecho,'Formatting with astyle')
 	@"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix
 
-format_clang:
-	$(call colorecho,'Formatting with clang-format')
-	@"$(SRC_DIR)"/Tools/astyle/files_to_check_code_style.sh | xargs clang-format -i -style=file
-
 # Testing
 # --------------------------------------------------------------------
 .PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -62,7 +62,7 @@ param set-default MPC_JERK_AUTO 4
 param set-default MPC_LAND_SPEED 1
 param set-default MPC_MAN_TILT_MAX 25
 param set-default MPC_MAN_Y_MAX 40
-param set-default COM_SPOOLUP_TIME 1.5
+param set-default MPC_SPOOLUP_TIME 1.5
 param set-default MPC_THR_HOVER 0.45
 param set-default MPC_TILTMAX_AIR 25
 param set-default MPC_TKO_RAMP_T 1.8

Tools/astyle/files_to_check_code_style.sh

@@ -8,24 +8,23 @@ if [ $# -gt 0 ]; then
 fi
 
 exec find boards msg src platforms test \
-    -not -path "msg/templates/urtps/*" \
-    -not -path "platforms/nuttx/NuttX/*" \
-    -not -path "platforms/qurt/dspal/*" \
-    -not -path "src/drivers/gps/devices/*" \
-    -not -path "src/drivers/uavcan/libuavcan/*" \
-    -not -path "src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/*" \
-    -not -path "src/drivers/cyphal/libcanard/*" \
-    -not -path "src/lib/crypto/monocypher/*" \
-    -not -path "src/lib/events/libevents/*" \
-    -not -path "src/lib/parameters/uthash/*" \
-    -not -path "src/modules/ekf2/EKF/*" \
-    -not -path "src/modules/gyro_fft/CMSIS_5/*" \
-    -not -path "src/modules/mavlink/mavlink/*" \
-    -not -path "src/modules/micrortps_bridge/micro-CDR/*" \
-    -not -path "src/modules/micrortps_bridge/microRTPS_client/*" \
-    -not -path "test/mavsdk_tests/catch2/*" \
-    -not -path "src/lib/crypto/monocypher/*" \
-    -not -path "src/lib/crypto/libtomcrypt/*" \
-    -not -path "src/lib/crypto/libtommath/*" \
-    -not -path "src/modules/microdds_client/Micro-XRCE-DDS-Client/*" \
+    -path msg/templates/urtps -prune -o \
+    -path platforms/nuttx/NuttX -prune -o \
+    -path platforms/qurt/dspal -prune -o \
+    -path src/drivers/uavcan/libuavcan -prune -o \
+    -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
+    -path src/drivers/cyphal/libcanard -prune -o \
+    -path src/lib/crypto/monocypher -prune -o \
+    -path src/lib/events/libevents -prune -o \
+    -path src/lib/parameters/uthash -prune -o \
+    -path src/modules/ekf2/EKF -prune -o \
+    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
+    -path src/modules/mavlink/mavlink -prune -o \
+    -path src/modules/micrortps_bridge/micro-CDR -prune -o \
+    -path src/modules/micrortps_bridge/microRTPS_client -prune -o \
+    -path test/mavsdk_tests/catch2 -prune -o \
+    -path src/lib/crypto/monocypher -prune -o \
+    -path src/lib/crypto/libtomcrypt -prune -o \
+    -path src/lib/crypto/libtommath -prune -o \
+    -path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

boards/cuav/nora/src/board_config.h

@@ -118,8 +118,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)
 
-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */
 
 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -603,15 +603,8 @@ void PAA3905::RunImpl()
 			// rotate measurements in yaw from sensor frame to body frame
 			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
 
-			// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
-			static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
-			static constexpr float INCHES_PER_METER = 39.3701f;
-
-			// CPI/m -> radians
-			static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
-
-			report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
-			report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+			report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians
+			report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians
 		}
 
 		report.timestamp = hrt_absolute_time();

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -823,15 +823,8 @@ void PAW3902::RunImpl()
 			// rotate measurements in yaw from sensor frame to body frame
 			const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
 
-			// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
-			static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
-			static constexpr float INCHES_PER_METER = 39.3701f;
-
-			// CPI/m -> radians
-			static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
-
-			report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
-			report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
+			report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians
+			report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians
 		}
 
 		report.timestamp = hrt_absolute_time();

src/examples/fake_gyro/Kconfig

@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_FAKE_GYRO
+	bool "fake_gyro"
+	default n
+	---help---
+		Enable support for fake_gyro

src/examples/fixedwing_control/main.cpp

@@ -278,14 +278,25 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	 * These structs contain the system state and things
 	 * like attitude, position, the current waypoint, etc.
 	 */
-	vehicle_attitude_s att{};
-	vehicle_rates_setpoint_s rates_sp{};
-	manual_control_setpoint_s manual_control_setpoint{};
-	vehicle_status_s vstatus{};
-	position_setpoint_s global_sp{};
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	struct manual_control_setpoint_s manual_control_setpoint;
+	memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
+	struct vehicle_status_s vstatus;
+	memset(&vstatus, 0, sizeof(vstatus));
+	struct position_setpoint_s global_sp;
+	memset(&global_sp, 0, sizeof(global_sp));
 
 	/* output structs - this is what is sent to the mixer */
-	actuator_controls_s actuators{};
+	struct actuator_controls_s actuators;
+	memset(&actuators, 0, sizeof(actuators));
+
 
 	/* publish actuator controls with zero values */
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {

src/lib/motion_planning/PositionSmoothing.cpp

@@ -84,6 +84,26 @@ bool PositionSmoothing::_isTurning(const Vector3f &target) const
 		&& pos_to_target.longerThan(_target_acceptance_radius));
 }
 
+
+
+/* Constrain some value vith a constrain depending on the sign of the constraint
+ * Example: 	- if the constrain is -5, the value will be constrained between -5 and 0
+ * 		- if the constrain is 5, the value will be constrained between 0 and 5
+ */
+inline float _constrainOneSide(float val, float constraint)
+{
+	const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
+	const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
+
+	return math::constrain(val, min, max);
+}
+
+inline float _constrainAbs(float val, float max)
+{
+	return matrix::sign(val) * math::min(fabsf(val), fabsf(max));
+}
+
+
 float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const
 {
 	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),

src/lib/parameters/param_translation.cpp

@@ -42,15 +42,6 @@
 
 bool param_modify_on_import(bson_node_t node)
 {
-	// migrate MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME (2020-12-03). This can be removed after the next release (current release=1.11)
-	if (node->type == BSON_DOUBLE) {
-		if (strcmp("MPC_SPOOLUP_TIME", node->name) == 0) {
-			strcpy(node->name, "COM_SPOOLUP_TIME");
-			PX4_INFO("param migrating MPC_SPOOLUP_TIME (removed) -> COM_SPOOLUP_TIME: value=%.3f", node->d);
-			return true;
-		}
-	}
-
 	// migrate COM_ARM_AUTH -> COM_ARM_AUTH_ID, COM_ARM_AUTH_MET and COM_ARM_AUTH_TO (2020-11-06). This can be removed after the next release (current release=1.11)
 	if (node->type == BSON_INT32) {
 		if (strcmp("COM_ARM_AUTH", node->name) == 0) {

src/lib/parameters/parameters.cpp

@@ -95,6 +95,10 @@ static struct work_s autosave_work {};
 static px4::atomic_bool autosave_scheduled{false};
 static bool autosave_disabled = false;
 
+// notify worker
+static struct work_s notify_work {};
+static px4::atomic<uint8_t> notify_delay_count{0};
+
 static px4::AtomicBitset<param_info_count> params_active;  // params found
 static px4::AtomicBitset<param_info_count> params_changed; // params non-default
 static px4::Bitset<param_info_count> params_custom_default; // params with runtime default value
@@ -664,6 +668,31 @@ param_control_autosave(bool enable)
 	param_unlock_writer();
 }
 
+static void notify_worker(void *arg)
+{
+	param_notify_changes();
+	notify_delay_count.store(0);
+}
+
+static void schedule_notify_changes()
+{
+#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(CONFIG_ARCH_BOARD_PX4_SITL)
+	// on SITL call notify_worker directly to avoid issues with lockstep and SITL tests
+	(void)notify_work;
+	notify_worker(nullptr);
+#else
+
+	if (notify_delay_count.fetch_add(1) < 100) {
+		// delay notification by 20 ms, but no more than 100 times
+		work_queue(LPWORK, &notify_work, &notify_worker, nullptr, USEC2TICK(20'000));
+
+	} else {
+		// do nothing until notify_delay_count is cleared by the notify worker
+	}
+
+#endif
+}
+
 static int
 param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
 {
@@ -763,7 +792,7 @@ out:
 	 * a thing has been set.
 	 */
 	if ((result == PX4_OK) && param_changed && notify_changes) {
-		param_notify_changes();
+		schedule_notify_changes();
 	}
 
 	return result;
@@ -908,7 +937,7 @@ int param_set_default_value(param_t param, const void *val)
 
 	if ((result == PX4_OK) && param_used(param)) {
 		// send notification if param is already in use
-		param_notify_changes();
+		schedule_notify_changes();
 	}
 
 	return result;
@@ -942,7 +971,7 @@ static int param_reset_internal(param_t param, bool notify = true)
 	param_unlock_writer();
 
 	if (s != nullptr && notify) {
-		param_notify_changes();
+		schedule_notify_changes();
 	}
 
 	return (!param_found);

src/lib/weather_vane/WeatherVane.cpp

@@ -41,39 +41,24 @@
 #include <mathlib/mathlib.h>
 
 
-WeatherVane::WeatherVane(ModuleParams *parent) :
-	ModuleParams(parent)
+WeatherVane::WeatherVane() :
+	ModuleParams(nullptr)
 { }
 
-void WeatherVane::update()
+void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw)
 {
-	vehicle_control_mode_s vehicle_control_mode;
-
-	if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
-		_flag_control_manual_enabled = vehicle_control_mode.flag_control_manual_enabled;
-		_flag_control_position_enabled = vehicle_control_mode.flag_control_position_enabled;
-	}
-
-	// Weathervane needs to be enabled by parameter
-	// in manual we use weathervane just if position is controlled as well
-	// in mission we use weathervane except for when navigator disables it
-	_is_active = _param_wv_en.get()
-		     && ((_flag_control_manual_enabled && _flag_control_position_enabled)
-			 || (!_flag_control_manual_enabled && !_navigator_force_disabled));
+	_dcm_z_sp_prev = dcm_z_sp_prev;
+	_yaw = yaw;
 }
 
-float WeatherVane::getWeathervaneYawrate()
+float WeatherVane::get_weathervane_yawrate()
 {
 	// direction of desired body z axis represented in earth frame
-	vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
-	_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
-	matrix::Vector3f body_z_sp(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z()); // attitude setpoint body z axis
+	matrix::Vector3f body_z_sp(_dcm_z_sp_prev);
 
 	// rotate desired body z axis into new frame which is rotated in z by the current
 	// heading of the vehicle. we refer to this as the heading frame.
-	vehicle_local_position_s vehicle_local_position{};
-	_vehicle_local_position_sub.copy(&vehicle_local_position);
-	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -vehicle_local_position.heading);
+	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -_yaw);
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
 

src/lib/weather_vane/WeatherVane.hpp

@@ -44,39 +44,33 @@
 
 #include <px4_platform_common/module_params.h>
 #include <matrix/matrix/math.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
 
 class WeatherVane : public ModuleParams
 {
 public:
-	WeatherVane(ModuleParams *parent);
+	WeatherVane();
 
 	~WeatherVane() = default;
 
-	void setNavigatorForceDisabled(bool navigator_force_disabled) { _navigator_force_disabled = navigator_force_disabled; };
+	void activate() {_is_active = true;}
 
-	bool isActive() {return _is_active;}
+	void deactivate() {_is_active = false;}
 
-	void update();
+	bool is_active() {return _is_active;}
 
-	float getWeathervaneYawrate();
+	bool weathervane_enabled() { return _param_wv_en.get(); }
+
+	void update(const matrix::Vector3f &dcm_z_sp_prev, float yaw);
+
+	float get_weathervane_yawrate();
+
+	void update_parameters() { ModuleParams::updateParams(); }
 
 private:
-	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis
+	float _yaw = 0.0f; ///< current yaw angle
 
-	bool _is_active{false};
-
-	// local copies of status such that we don't need to copy uORB messages all the time
-	bool _flag_control_manual_enabled{false};
-	bool _flag_control_position_enabled{false};
-	bool _navigator_force_disabled{false};
+	bool _is_active = true;
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::WV_EN>) _param_wv_en,

src/modules/commander/Commander.cpp

@@ -2721,8 +2721,8 @@ Commander::run()
 
 			if (_arm_state_machine.isArmed()) {
 				if (fd_status_flags.arm_escs) {
-					// Checks have to pass within the spool up time
-					if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) {
+					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
+					if (hrt_elapsed_time(&_vehicle_status.armed_time) < 500_ms) {
 						disarm(arm_disarm_reason_t::failure_detector);
 						mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t");
 						events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request");

src/modules/commander/Commander.hpp

@@ -258,9 +258,7 @@ private:
 		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
 
 		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
-		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
-
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
+		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
 	)
 
 	// optional parameters

src/modules/commander/commander_params.c

@@ -1023,22 +1023,6 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
 
-/**
- * Enforced delay between arming and further navigation
- *
- * The minimal time from arming the motors until moving the vehicle is possible is COM_SPOOLUP_TIME seconds.
- * Goal:
- * - Motors and propellers spool up to idle speed before getting commanded to spin faster
- * - Timeout for ESCs and smart batteries to successfulyy do failure checks
- *   e.g. for stuck rotors before the vehicle is off the ground
- *
- * @group Commander
- * @min 0
- * @max 5
- * @unit s
- */
-PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
-
 /**
  * Wind speed warning threshold
  *

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -269,7 +269,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
 		bool is_esc_failure = !is_all_escs_armed;
 
 		for (int i = 0; i < limited_esc_count; i++) {
-			is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
+			is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
 		}
 
 		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -60,6 +60,7 @@ FlightModeManager::~FlightModeManager()
 		_current_task.task->~FlightTask();
 	}
 
+	delete _wv_controller;
 	perf_free(_loop_perf);
 }
 
@@ -106,7 +107,33 @@ void FlightModeManager::Run()
 		_home_position_sub.update();
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
-		_vehicle_status_sub.update();
+
+		if (_vehicle_status_sub.update()) {
+			if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) {
+				// if vehicle is a VTOL we want to enable weathervane capabilities
+				_wv_controller = new WeatherVane();
+			}
+		}
+
+		// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
+		// that turns the nose of the vehicle into the wind
+		if (_wv_controller != nullptr) {
+
+			// in manual mode we just want to use weathervane if position is controlled as well
+			// in mission, enabling wv is done in flight task
+			if (_vehicle_control_mode_sub.get().flag_control_manual_enabled) {
+				if (_vehicle_control_mode_sub.get().flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
+					_wv_controller->activate();
+
+				} else {
+					_wv_controller->deactivate();
+				}
+			}
+
+			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+			_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint);
+			_wv_controller->update(matrix::Quatf(vehicle_attitude_setpoint.q_d).dcm_z(), vehicle_local_position.heading);
+		}
 
 		start_flight_task();
 
@@ -130,6 +157,10 @@ void FlightModeManager::updateParams()
 	if (isAnyTaskActive()) {
 		_current_task.task->handleParameterUpdate();
 	}
+
+	if (_wv_controller != nullptr) {
+		_wv_controller->update_parameters();
+	}
 }
 
 void FlightModeManager::start_flight_task()
@@ -430,6 +461,8 @@ void FlightModeManager::handleCommand()
 void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		const vehicle_local_position_s &vehicle_local_position)
 {
+	_current_task.task->setYawHandler(_wv_controller);
+
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
 	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -131,6 +131,7 @@ private:
 		FlightTaskIndex index{FlightTaskIndex::None};
 	} _current_task{};
 
+	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED};
 	int _task_failure_count{0};

src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto
 	FlightTaskAuto.cpp
 )
 
-target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane)
+target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility)
 target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -40,6 +40,14 @@
 
 using namespace matrix;
 
+FlightTaskAuto::FlightTaskAuto() :
+	_obstacle_avoidance(this),
+	_sticks(this),
+	_stick_acceleration_xy(this)
+{
+
+}
+
 bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
@@ -249,7 +257,9 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		vertical_speed *= (1 + _sticks.getPositionExpo()(2));
 
 		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-		if (!_weathervane.isActive() || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
+		const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active();
+
+		if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
 			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
 						       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
 		}
@@ -445,8 +455,11 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_next_was_valid = _sub_triplet_setpoint.get().next.valid;
 	}
 
-	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
+	if (_ext_yaw_handler != nullptr) {
+		// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
+		(_param_wv_en.get() && !_sub_triplet_setpoint.get().current.disable_weather_vane) ?	_ext_yaw_handler->activate() :
+		_ext_yaw_handler->deactivate();
+	}
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -463,15 +476,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 				_triplet_next_wp,
 				_sub_triplet_setpoint.get().next.yaw,
 				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
-				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
+				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
 	}
 
 	// set heading
-	_weathervane.update();
-
-	if (_weathervane.isActive()) {
+	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
 		_yaw_setpoint = NAN;
 		// use the yawrate setpoint from WV only if not moving lateral (velocity setpoint below half of _param_mpc_xy_cruise)
 		// otherwise, keep heading constant (as output from WV is not according to wind in this case)
@@ -481,7 +492,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yawspeed_setpoint = 0.0f;
 
 		} else {
-			_yawspeed_setpoint = _weathervane.getWeathervaneYawrate();
+			_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
 		}
 
 

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -46,7 +46,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/geo/geo.h>
-#include <lib/weather_vane/WeatherVane.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include "Sticks.hpp"
@@ -85,13 +84,18 @@ enum class State {
 class FlightTaskAuto : public FlightTask
 {
 public:
-	FlightTaskAuto() = default;
+	FlightTaskAuto();
+
 	virtual ~FlightTaskAuto() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
 	void overrideCruiseSpeed(const float cruise_speed_m_s) override;
 
 protected:
@@ -139,12 +143,12 @@ protected:
 	AlphaFilter<float> _yawspeed_filter;
 	bool _yaw_sp_aligned{false};
 
-	ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */
+	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
 
 	PositionSmoothing _position_smoothing;
 	Vector3f _unsmoothed_velocity_setpoint;
-	Sticks _sticks{this};
-	StickAccelerationXY _stick_acceleration_xy{this};
+	Sticks _sticks;
+	StickAccelerationXY _stick_acceleration_xy;
 	StickYaw _stick_yaw;
 	matrix::Vector3f _land_position;
 	float _land_heading;
@@ -160,6 +164,7 @@ protected:
 					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
 					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
 					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
+					(ParamBool<px4::params::WV_EN>) _param_wv_en, // enable/disable weather vane (VTOL)
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
@@ -203,7 +208,8 @@ private:
 	float _reference_altitude{NAN}; /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 
-	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
 
 	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
 	bool _evaluateTriplets(); /**< Checks and sets triplets. */

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -55,6 +55,7 @@
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/home_position.h>
 #include <lib/geo/geo.h>
+#include <lib/weather_vane/WeatherVane.hpp>
 
 struct ekf_reset_counters_s {
 	uint8_t xy;
@@ -162,6 +163,11 @@ public:
 		updateParams();
 	}
 
+	/**
+	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
+	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
+	 */
+	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
 	virtual void overrideCruiseSpeed(const float cruise_speed_m_s) {}
 
 	void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp,

src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt

@@ -36,4 +36,4 @@ px4_add_library(FlightTaskManualAcceleration
 )
 target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility WeatherVane)
+target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility)

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -39,6 +39,10 @@
 
 using namespace matrix;
 
+FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
+	_stick_acceleration_xy(this)
+{};
+
 bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
@@ -74,15 +78,13 @@ bool FlightTaskManualAcceleration::update()
 	_constraints.want_takeoff = _checkTakeoff();
 
 	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
-	_weathervane.update();
-
-	if (_weathervane.isActive()) {
+	if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
 		_yaw_setpoint = NAN;
 
 		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
 		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
 			// vehicle is steady
-			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
+			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
 		}
 	}
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -43,22 +43,26 @@
 #include "FlightTaskManualAltitudeSmoothVel.hpp"
 #include "StickAccelerationXY.hpp"
 #include "StickYaw.hpp"
-#include <lib/weather_vane/WeatherVane.hpp>
 
 class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 {
 public:
-	FlightTaskManualAcceleration() = default;
+	FlightTaskManualAcceleration();
 	virtual ~FlightTaskManualAcceleration() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
+
 private:
 	void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
 
-	StickAccelerationXY _stick_acceleration_xy{this};
+	StickAccelerationXY _stick_acceleration_xy;
 	StickYaw _stick_yaw;
 
-	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt

@@ -39,7 +39,6 @@ px4_add_library(FlightTaskManualPosition
 target_link_libraries(FlightTaskManualPosition
 	PRIVATE
 		CollisionPrevention
-		WeatherVane
 	PUBLIC
 		FlightTaskManualAltitude
 )

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -41,6 +41,11 @@
 
 using namespace matrix;
 
+FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
+{
+
+}
+
 bool FlightTaskManualPosition::updateInitialize()
 {
 	bool ret = FlightTaskManualAltitude::updateInitialize();
@@ -136,15 +141,14 @@ void FlightTaskManualPosition::_updateSetpoints()
 
 	_updateXYlock(); // check for position lock
 
-	_weathervane.update();
-
-	if (_weathervane.isActive()) {
+	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
+	if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
 		_yaw_setpoint = NAN;
 
 		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
 		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
 			// vehicle is steady
-			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
+			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
 		}
 	}
 }

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -41,17 +41,23 @@
 #pragma once
 
 #include <lib/collision_prevention/CollisionPrevention.hpp>
-#include <lib/weather_vane/WeatherVane.hpp>
 #include "FlightTaskManualAltitude.hpp"
 
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualPosition() = default;
+	FlightTaskManualPosition();
+
 	virtual ~FlightTaskManualPosition() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
+
+
 protected:
 	void _updateXYlock(); /**< applies position lock based on stick and velocity */
 	void _updateSetpoints() override;
@@ -65,6 +71,8 @@ protected:
 private:
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
 
-	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
-	CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */
+	WeatherVane *_weathervane_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
 };

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -50,6 +50,7 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 {
 public:
 	FlightTaskManualPositionSmoothVel() = default;
+
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -131,15 +131,15 @@ private:
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
-	actuator_controls_s			_actuator_controls{};
-	manual_control_setpoint_s		_manual_control_setpoint{};
-	vehicle_attitude_setpoint_s		_att_sp{};
-	vehicle_control_mode_s			_vcontrol_mode{};
-	vehicle_local_position_s		_local_pos{};
-	vehicle_rates_setpoint_s		_rates_sp{};
-	vehicle_status_s			_vehicle_status{};
+	actuator_controls_s			_actuator_controls {};		/**< actuator control inputs */
+	manual_control_setpoint_s		_manual_control_setpoint {};	/**< r/c channel data */
+	vehicle_attitude_setpoint_s		_att_sp {};			/**< vehicle attitude setpoint */
+	vehicle_control_mode_s			_vcontrol_mode {};		/**< vehicle control mode */
+	vehicle_local_position_s		_local_pos {};			/**< local position */
+	vehicle_rates_setpoint_s		_rates_sp {};			/* attitude rates setpoint */
+	vehicle_status_s			_vehicle_status {};		/**< vehicle status */
 
-	perf_counter_t _loop_perf;
+	perf_counter_t	_loop_perf;						/**< loop performance counter */
 
 	hrt_abstime _last_run{0};
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -185,12 +185,12 @@ private:
 	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
-	manual_control_setpoint_s _manual_control_setpoint{};
-	position_setpoint_triplet_s _pos_sp_triplet{};
-	vehicle_attitude_setpoint_s _att_sp{};
-	vehicle_control_mode_s _control_mode{};
-	vehicle_local_position_s _local_pos{};
-	vehicle_status_s _vehicle_status{};
+	manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
+	position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
+	vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
+	vehicle_control_mode_s _control_mode {};
+	vehicle_local_position_s _local_pos {};	// vehicle local position
+	vehicle_status_s _vehicle_status {}; // vehicle status
 
 	double _current_latitude{0};
 	double _current_longitude{0};
@@ -212,8 +212,8 @@ private:
 
 	float _min_current_sp_distance_xy{FLT_MAX};
 
-	position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started
-	position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies
+	position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
+	position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
 
 	// [us] Last absolute time position control has been called
 	hrt_abstime _last_time_position_control_called{0};

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -234,7 +234,7 @@ void MulticopterPositionControl::parameters_update(bool force)
 		_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
 		_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
 
-		_takeoff.setSpoolupTime(_param_com_spoolup_time.get());
+		_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
 		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
 		_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
 	}

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -147,7 +147,7 @@ private:
 		(ParamBool<px4::params::MPC_USE_HTE>)       _param_mpc_use_hte,
 
 		// Takeoff / Land
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
+		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>)   _param_mpc_tko_ramp_t,   /**< time constant for smooth takeoff ramp */
 		(ParamFloat<px4::params::MPC_TKO_SPEED>)    _param_mpc_tko_speed,
 		(ParamFloat<px4::params::MPC_LAND_SPEED>)   _param_mpc_land_speed,

src/modules/mc_pos_control/Takeoff/Takeoff.hpp

@@ -94,7 +94,7 @@ public:
 private:
 	TakeoffState _takeoff_state = TakeoffState::disarmed;
 
-	systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true COM_SPOOLUP_TIME seconds after the vehicle was armed
+	systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed
 
 	float _takeoff_ramp_time{0.f};
 	float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -780,6 +780,22 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f);
  */
 PARAM_DEFINE_INT32(MPC_POS_MODE, 4);
 
+/**
+ * Enforced delay between arming and takeoff
+ *
+ * For altitude controlled modes the time from arming the motors until
+ * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds
+ * to ensure the motors and propellers can sppol up and reach idle speed before
+ * getting commanded to spin faster. This delay is particularly useful for vehicles
+ * with slow motor spin-up e.g. because of large propellers.
+ *
+ * @min 0
+ * @max 10
+ * @unit s
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
+
 /**
  * Yaw mode.
  *

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -117,13 +117,13 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
 
-	actuator_controls_s _actuators{};
-	manual_control_setpoint_s _manual_control_setpoint{};
-	vehicle_attitude_setpoint_s _attitude_setpoint{};
-	vehicle_rates_setpoint_s _rates_setpoint{};
-	vehicle_control_mode_s _vcontrol_mode{};
+	actuator_controls_s _actuators {}; /**< actuator control inputs */
+	manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
+	vehicle_attitude_setpoint_s _attitude_setpoint {}; /**< vehicle attitude setpoint */
+	vehicle_rates_setpoint_s _rates_setpoint {}; /**< vehicle bodyrates setpoint */
+	vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
 
-	perf_counter_t	_loop_perf;
+	perf_counter_t	_loop_perf; /**< loop performance counter */
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,

src/modules/uuv_pos_control/uuv_pos_control.cpp

@@ -195,6 +195,16 @@ void UUVPOSControl::Run()
 		}
 	}
 
+	/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
+	if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
+		// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
+		// returning immediately and this loop will eat up resources.
+		if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
+			/* manual/direct control */
+		}
+
+	}
+
 	/* Only publish if any of the proper modes are enabled */
 	if (_vcontrol_mode.flag_control_manual_enabled ||
 	    _vcontrol_mode.flag_control_attitude_enabled) {

src/modules/uuv_pos_control/uuv_pos_control.hpp

@@ -60,6 +60,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -102,17 +103,20 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};	/**< current vehicle attitude */
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};	/**< notification of manual control updates */
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle status subscription */
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
-	vehicle_attitude_s _vehicle_attitude{};
-	trajectory_setpoint_s _trajectory_setpoint{};
-	vehicle_control_mode_s _vcontrol_mode{};
+	//actuator_controls_s _actuators {}; /**< actuator control inputs */
+	manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
+	vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */
+	trajectory_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
+	vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
 
-	perf_counter_t	_loop_perf;
+	perf_counter_t	_loop_perf; /**< loop performance counter */
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_GAIN_X_P>) _param_pose_gain_x,