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

boards/px4/fmu-v5/default.px4board

@@ -43,7 +43,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
-CONFIG_DRIVERS_LIGHTS_RGBLED_LP55231=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y

boards/px4/fmu-v5/init/rc.board_defaults

@@ -24,5 +24,4 @@ else
 fi
 
 rgbled_pwm start
-rgbled_lp55231 -X start
 safety_button start

src/drivers/drv_sensor.h

@@ -206,7 +206,6 @@
 #define DRV_TRNS_DEVTYPE_MXS 0xB0
 
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1
-#define DRV_LED_DEVTYPE_RGBLED_LP55231 0xB2
 
 #define DRV_FLOW_DEVTYPE_MAVLINK 0xB2
 #define DRV_FLOW_DEVTYPE_PMW3901 0xB3

src/drivers/lights/CMakeLists.txt

@@ -34,6 +34,4 @@
 #add_subdirectory(neopixel) # requires board support (BOARD_HAS_N_S_RGB_LED)
 add_subdirectory(rgbled)
 add_subdirectory(rgbled_ncp5623c)
-add_subdirectory(rgbled_lp55231)
-
 #add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)

src/drivers/lights/rgbled_lp55231/CMakeLists.txt

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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 drivers__rgbled_lp55231
-	MAIN rgbled_lp55231
-	SRCS
-		rgbled_lp55231.cpp
-	DEPENDS
-		drivers__device
-		led
-	)

src/drivers/lights/rgbled_lp55231/Kconfig

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

src/drivers/lights/rgbled_lp55231/rgbled_lp55231.cpp

@@ -1,385 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- *
- * Driver for RGB LP55231 LED controller connected via I2C.
- */
-
-#include <drivers/device/i2c.h>
-#include <lib/led/led.h>
-#include <lib/parameters/param.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <px4_platform_common/module.h>
-
-using namespace time_literals;
-
-static constexpr uint8_t ADDRESS = 0x32;
-
-// register stuff
-static constexpr uint8_t REG_CNTRL1 = 0x00;
-static constexpr uint8_t REG_CNTRL2 = 0x01;
-static constexpr uint8_t REG_RATIO_MSB = 0x02;
-static constexpr uint8_t REG_RATIO_LSB = 0x03;
-static constexpr uint8_t REG_OUTPUT_ONOFF_MSB = 0x04;
-static constexpr uint8_t REG_OUTPUT_ONOFF_LSB = 0x05;
-
-// Per LED control channels - fader channel assig, log dimming enable, temperature compensation
-static constexpr uint8_t REG_D1_CTRL = 0x06;
-static constexpr uint8_t REG_D2_CTRL = 0x07;
-static constexpr uint8_t REG_D3_CTRL = 0x08;
-static constexpr uint8_t REG_D4_CTRL = 0x09;
-static constexpr uint8_t REG_D5_CTRL = 0x0a;
-static constexpr uint8_t REG_D6_CTRL = 0x0b;
-static constexpr uint8_t REG_D7_CTRL = 0x0c;
-static constexpr uint8_t REG_D8_CTRL = 0x0d;
-static constexpr uint8_t REG_D9_CTRL = 0x0e;
-
-// 0x0f to 0x15 reserved
-
-// Direct PWM control registers
-static constexpr uint8_t REG_D1_PWM  = 0x16;
-static constexpr uint8_t REG_D2_PWM  = 0x17;
-static constexpr uint8_t REG_D3_PWM  = 0x18;
-static constexpr uint8_t REG_D4_PWM  = 0x19;
-static constexpr uint8_t REG_D5_PWM  = 0x1a;
-static constexpr uint8_t REG_D6_PWM  = 0x1b;
-static constexpr uint8_t REG_D7_PWM  = 0x1c;
-static constexpr uint8_t REG_D8_PWM  = 0x1d;
-static constexpr uint8_t REG_D9_PWM  = 0x1e;
-
-// 0x1f to 0x25 reserved
-
-// Drive current registers
-static constexpr uint8_t REG_D1_I_CTL = 0x26;
-static constexpr uint8_t REG_D2_I_CTL  = 0x27;
-static constexpr uint8_t REG_D3_I_CTL  = 0x28;
-static constexpr uint8_t REG_D4_I_CTL  = 0x29;
-static constexpr uint8_t REG_D5_I_CTL  = 0x2a;
-static constexpr uint8_t REG_D6_I_CTL  = 0x2b;
-static constexpr uint8_t REG_D7_I_CTL  = 0x2c;
-static constexpr uint8_t REG_D8_I_CTL  = 0x2d;
-static constexpr uint8_t REG_D9_I_CTL  = 0x2e;
-
-// 0x2f to 0x35 reserved
-
-static constexpr uint8_t REG_MISC     = 0x36;
-static constexpr uint8_t REG_PC1      = 0x37;
-static constexpr uint8_t REG_PC2      = 0x38;
-static constexpr uint8_t REG_PC3      = 0x39;
-static constexpr uint8_t REG_STATUS_IRQ = 0x3A;
-static constexpr uint8_t REG_INT_GPIO   = 0x3B;
-static constexpr uint8_t REG_GLOBAL_VAR = 0x3C;
-static constexpr uint8_t REG_RESET      = 0x3D;
-static constexpr uint8_t REG_TEMP_CTL   = 0x3E;
-static constexpr uint8_t REG_TEMP_READ  = 0x3F;
-static constexpr uint8_t REG_TEMP_WRITE = 0x40;
-static constexpr uint8_t REG_TEST_CTL   = 0x41;
-static constexpr uint8_t REG_TEST_ADC   = 0x42;
-
-// 0x43 to 0x44 reserved
-
-static constexpr uint8_t REG_ENGINE_A_VAR = 0x45;
-static constexpr uint8_t REG_ENGINE_B_VAR = 0x46;
-static constexpr uint8_t REG_ENGINE_C_VAR = 0x47;
-
-static constexpr uint8_t REG_MASTER_FADE_1 = 0x48;
-static constexpr uint8_t REG_MASTER_FADE_2 = 0x49;
-static constexpr uint8_t REG_MASTER_FADE_3 = 0x4A;
-
-// 0x4b Reserved
-
-static constexpr uint8_t REG_PROG1_START = 0x4C;
-static constexpr uint8_t REG_PROG2_START = 0x4D;
-static constexpr uint8_t REG_PROG3_START = 0x4E;
-static constexpr uint8_t REG_PROG_PAGE_SEL = 0x4f;
-
-// Memory is more confusing - there are 6 pages, sel by addr 4f
-static constexpr uint8_t REG_PROG_MEM_BASE = 0x50;
-static constexpr uint8_t REG_PROG_MEM_END  = 0x6f;
-
-static constexpr uint8_t REG_ENG1_MAP_MSB = 0x70;
-static constexpr uint8_t REG_ENG1_MAP_LSB = 0x71;
-static constexpr uint8_t REG_ENG2_MAP_MSB = 0x72;
-static constexpr uint8_t REG_ENG2_MAP_LSB = 0x73;
-static constexpr uint8_t REG_ENG3_MAP_MSB = 0x74;
-static constexpr uint8_t REG_ENG3_MAP_LSB = 0x75;
-
-static constexpr uint8_t REG_GAIN_CHANGE = 0x76;
-
-// Colors on eval board
-static constexpr uint8_t CHANNEL_L0_RED = 0;
-static constexpr uint8_t CHANNEL_L0_GREEN = 3;
-static constexpr uint8_t CHANNEL_L0_BLUE = 4;
-static constexpr uint8_t CHANNEL_L1_RED = 1;
-static constexpr uint8_t CHANNEL_L1_GREEN = 5;
-static constexpr uint8_t CHANNEL_L1_BLUE = 6;
-static constexpr uint8_t CHANNEL_L2_RED = 2;
-static constexpr uint8_t CHANNEL_L2_GREEN = 7;
-static constexpr uint8_t CHANNEL_L2_BLUE = 8;
-
-class RGBLED_LP55231 : public device::I2C, public I2CSPIDriver<RGBLED_LP55231>
-{
-public:
-	RGBLED_LP55231(const I2CSPIDriverConfig &config);
-	virtual ~RGBLED_LP55231() override;
-
-	static void print_usage();
-
-	int init() override;
-	int probe() override;
-
-	void RunImpl();
-
-private:
-	void print_status() override;
-
-	int write(uint8_t address, uint8_t value);
-	int read(uint8_t address, uint8_t &value);
-
-	int setChannelPWM(uint8_t channel, uint8_t value);
-
-	int setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
-		   const LedControlDataSingle &led);
-
-	int reset();
-	int enable();
-
-	LedController		_led_controller;
-
-	// uint8_t _channel = 0;
-};
-
-RGBLED_LP55231::RGBLED_LP55231(const I2CSPIDriverConfig &config) :
-	I2C(config),
-	I2CSPIDriver(config)
-{
-}
-
-RGBLED_LP55231::~RGBLED_LP55231()
-{
-	reset();
-}
-
-
-int RGBLED_LP55231::write(uint8_t address, uint8_t value)
-{
-	uint8_t data[2] = {address, value};
-	return transfer(data, sizeof(data), nullptr, 0);
-}
-
-int RGBLED_LP55231::read(uint8_t address, uint8_t &value)
-{
-	return transfer(&address, 1, (uint8_t *)&value, 1);
-}
-
-
-int RGBLED_LP55231::enable()
-{
-	int ret = write(REG_CNTRL1, 0x40);
-	// enable charge pump, internal oscillator, auto increment
-	ret = write(REG_MISC, 0x53);
-	return ret;
-}
-
-int RGBLED_LP55231::reset()
-{
-	return write(REG_RESET, 0xFF);
-}
-
-
-int RGBLED_LP55231::init()
-{
-	int ret = I2C::init();
-
-	if (ret != OK) {
-		return ret;
-	}
-
-	ret = reset();
-	ret = enable();
-
-	// kick off work queue
-	ScheduleNow();
-
-	return OK;
-}
-
-int RGBLED_LP55231::setChannelPWM(uint8_t channel, uint8_t value)
-{
-	assert(channel < 9);
-	return write(REG_D1_PWM + channel, value);
-}
-
-int RGBLED_LP55231::setLed(uint8_t channel_red, uint8_t channel_green, uint8_t channel_blue,
-			   const LedControlDataSingle &led)
-{
-	uint8_t r{0}, g{0}, b{0};
-	uint8_t brightness = led.brightness;
-
-	switch (led.color) {
-	case led_control_s::COLOR_RED:
-		r = brightness; g = 0; b = 0;
-		break;
-
-	case led_control_s::COLOR_GREEN:
-		r = 0; g = brightness; b = 0;
-		break;
-
-	case led_control_s::COLOR_BLUE:
-		r = 0; g = 0; b = brightness;
-		break;
-
-	case led_control_s::COLOR_AMBER: //make it the same as yellow
-	case led_control_s::COLOR_YELLOW:
-		r = brightness / 2; g = brightness / 2; b = 0;
-		break;
-
-	case led_control_s::COLOR_PURPLE:
-		r = brightness / 2; g = 0; b = brightness / 2;
-		break;
-
-	case led_control_s::COLOR_CYAN:
-		r = 0; g = brightness / 2; b = brightness / 2;
-		break;
-
-	case led_control_s::COLOR_WHITE:
-		r = brightness / 3; g = brightness / 3; b = brightness / 3;
-		break;
-
-	default: // led_control_s::COLOR_OFF
-		r = 0; g = 0; b = 0;
-		break;
-	}
-
-	int ret = 0;
-	ret = setChannelPWM(channel_red, r);
-	ret = setChannelPWM(channel_green, g);
-	ret = setChannelPWM(channel_blue, b);
-	return ret;
-}
-
-int RGBLED_LP55231::probe()
-{
-	// try read output on / off (should be 0xff)
-	uint8_t on_off;
-	int ret = read(REG_OUTPUT_ONOFF_LSB, on_off);
-
-	if (on_off != 0xFF) {
-		return -1;
-	}
-
-	// try read current control (should be 0xAF)
-	uint8_t current_control;
-	ret = read(REG_D1_I_CTL, current_control);
-
-	if (current_control != 0xAF) {
-		return -1;
-	}
-
-	_retries = 1;
-
-	return ret;
-}
-
-void
-RGBLED_LP55231::print_status()
-{
-	PX4_INFO("No status implemented");
-}
-
-
-
-void RGBLED_LP55231::RunImpl()
-{
-	LedControlData led_control_data;
-
-	if (_led_controller.update(led_control_data) == 1) {
-		setLed(CHANNEL_L0_RED, CHANNEL_L0_GREEN, CHANNEL_L0_BLUE, led_control_data.leds[0]);
-		setLed(CHANNEL_L1_RED, CHANNEL_L1_GREEN, CHANNEL_L1_BLUE, led_control_data.leds[1]);
-		setLed(CHANNEL_L2_RED, CHANNEL_L2_GREEN, CHANNEL_L2_BLUE, led_control_data.leds[2]);
-	}
-
-	/* re-queue ourselves to run again later */
-	ScheduleDelayed(_led_controller.maximum_update_interval());
-
-	// setChannelPWM(_channel, 0);
-	// _channel++;
-	// _channel = _channel % 9;
-	// setChannelPWM(_channel, 255);
-	// ScheduleDelayed(500_ms);
-}
-
-
-void
-RGBLED_LP55231::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("rgbled_lp55231", "driver");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x32);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-extern "C" __EXPORT int rgbled_lp55231_main(int argc, char *argv[])
-{
-	using ThisDriver = RGBLED_LP55231;
-	BusCLIArguments cli{true, false};
-	cli.default_i2c_frequency = 100000;
-	cli.i2c_address = ADDRESS;
-
-	const char *verb = cli.parseDefaultArguments(argc, argv);
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP55231);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}

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/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/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/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;