Added demo for LP55231

Remove the entire external yaw handler, dynamic memory allocation,
pointer passing logic. Directly instanciate the weather vane instance
in the flight tasks that support it.

boards/px4/fmu-v5/default.px4board

@@ -43,6 +43,7 @@ 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,4 +24,5 @@ else
 fi
 
 rgbled_pwm start
+rgbled_lp55231 -X start
 safety_button start

src/drivers/drv_sensor.h

@@ -206,6 +206,7 @@
 #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,4 +34,6 @@
 #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

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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

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

src/drivers/lights/rgbled_lp55231/rgbled_lp55231.cpp

@@ -0,0 +1,385 @@
+/****************************************************************************
+ *
+ *   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,8 +603,15 @@ 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};
 
-			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
+			// 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.timestamp = hrt_absolute_time();

src/drivers/optical_flow/paw3902/PAW3902.cpp

@@ -823,8 +823,15 @@ 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};
 
-			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
+			// 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.timestamp = hrt_absolute_time();

src/lib/motion_planning/PositionSmoothing.cpp

@@ -84,26 +84,6 @@ 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/weather_vane/WeatherVane.cpp

@@ -41,24 +41,39 @@
 #include <mathlib/mathlib.h>
 
 
-WeatherVane::WeatherVane() :
-	ModuleParams(nullptr)
+WeatherVane::WeatherVane(ModuleParams *parent) :
+	ModuleParams(parent)
 { }
 
-void WeatherVane::update(const matrix::Vector3f &dcm_z_sp_prev, float yaw)
+void WeatherVane::update()
 {
-	_dcm_z_sp_prev = dcm_z_sp_prev;
-	_yaw = 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));
 }
 
-float WeatherVane::get_weathervane_yawrate()
+float WeatherVane::getWeathervaneYawrate()
 {
 	// direction of desired body z axis represented in earth frame
-	matrix::Vector3f body_z_sp(_dcm_z_sp_prev);
+	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
 
 	// 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.
-	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -_yaw);
+	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);
 	body_z_sp = R_yaw * body_z_sp;
 	body_z_sp.normalize();
 

src/lib/weather_vane/WeatherVane.hpp

@@ -44,33 +44,39 @@
 
 #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();
+	WeatherVane(ModuleParams *parent);
 
 	~WeatherVane() = default;
 
-	void activate() {_is_active = true;}
+	void setNavigatorForceDisabled(bool navigator_force_disabled) { _navigator_force_disabled = navigator_force_disabled; };
 
-	void deactivate() {_is_active = false;}
+	bool isActive() {return _is_active;}
 
-	bool is_active() {return _is_active;}
+	void update();
 
-	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(); }
+	float getWeathervaneYawrate();
 
 private:
-	matrix::Vector3f _dcm_z_sp_prev; ///< previous attitude setpoint body z axis
-	float _yaw = 0.0f; ///< current yaw angle
+	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)};
 
-	bool _is_active = true;
+	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};
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::WV_EN>) _param_wv_en,

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -60,7 +60,6 @@ FlightModeManager::~FlightModeManager()
 		_current_task.task->~FlightTask();
 	}
 
-	delete _wv_controller;
 	perf_free(_loop_perf);
 }
 
@@ -107,33 +106,7 @@ void FlightModeManager::Run()
 		_home_position_sub.update();
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_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);
-		}
+		_vehicle_status_sub.update();
 
 		start_flight_task();
 
@@ -157,10 +130,6 @@ void FlightModeManager::updateParams()
 	if (isAnyTaskActive()) {
 		_current_task.task->handleParameterUpdate();
 	}
-
-	if (_wv_controller != nullptr) {
-		_wv_controller->update_parameters();
-	}
 }
 
 void FlightModeManager::start_flight_task()
@@ -461,8 +430,6 @@ 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,7 +131,6 @@ 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)
+target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane)
 target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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

@@ -40,14 +40,6 @@
 
 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);
@@ -257,9 +249,7 @@ 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
-		const bool weather_vane_active = (_ext_yaw_handler != nullptr) && _ext_yaw_handler->is_active();
-
-		if (!weather_vane_active || fabsf(_sticks.getPositionExpo()(3)) > FLT_EPSILON) {
+		if (!_weathervane.isActive() || 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);
 		}
@@ -455,11 +445,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_next_was_valid = _sub_triplet_setpoint.get().next.valid;
 	}
 
-	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();
-	}
+	// 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);
 
 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -476,13 +463,15 @@ 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,
-				_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type);
+				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
 	}
 
 	// set heading
-	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_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)
@@ -492,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yawspeed_setpoint = 0.0f;
 
 		} else {
-			_yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint = _weathervane.getWeathervaneYawrate();
 		}
 
 

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

@@ -46,6 +46,7 @@
 #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"
@@ -84,18 +85,13 @@ enum class State {
 class FlightTaskAuto : public FlightTask
 {
 public:
-	FlightTaskAuto();
-
+	FlightTaskAuto() = default;
 	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:
@@ -143,12 +139,12 @@ protected:
 	AlphaFilter<float> _yawspeed_filter;
 	bool _yaw_sp_aligned{false};
 
-	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
+	ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */
 
 	PositionSmoothing _position_smoothing;
 	Vector3f _unsmoothed_velocity_setpoint;
-	Sticks _sticks;
-	StickAccelerationXY _stick_acceleration_xy;
+	Sticks _sticks{this};
+	StickAccelerationXY _stick_acceleration_xy{this};
 	StickYaw _stick_yaw;
 	matrix::Vector3f _land_position;
 	float _land_heading;
@@ -164,7 +160,6 @@ 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,
@@ -208,8 +203,7 @@ private:
 	float _reference_altitude{NAN}; /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 
-	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
-
+	WeatherVane _weathervane{this}; /**< 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,7 +55,6 @@
 #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;
@@ -163,11 +162,6 @@ 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)
+target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility WeatherVane)

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

@@ -39,10 +39,6 @@
 
 using namespace matrix;
 
-FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
-	_stick_acceleration_xy(this)
-{};
-
 bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
@@ -78,13 +74,15 @@ bool FlightTaskManualAcceleration::update()
 	_constraints.want_takeoff = _checkTakeoff();
 
 	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
-	if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_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_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
 		}
 	}
 

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

@@ -43,26 +43,22 @@
 #include "FlightTaskManualAltitudeSmoothVel.hpp"
 #include "StickAccelerationXY.hpp"
 #include "StickYaw.hpp"
+#include <lib/weather_vane/WeatherVane.hpp>
 
 class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 {
 public:
-	FlightTaskManualAcceleration();
+	FlightTaskManualAcceleration() = default;
 	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;
+	StickAccelerationXY _stick_acceleration_xy{this};
 	StickYaw _stick_yaw;
 
-	WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+	WeatherVane _weathervane{this}; /**< 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,6 +39,7 @@ px4_add_library(FlightTaskManualPosition
 target_link_libraries(FlightTaskManualPosition
 	PRIVATE
 		CollisionPrevention
+		WeatherVane
 	PUBLIC
 		FlightTaskManualAltitude
 )

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

@@ -41,11 +41,6 @@
 
 using namespace matrix;
 
-FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
-{
-
-}
-
 bool FlightTaskManualPosition::updateInitialize()
 {
 	bool ret = FlightTaskManualAltitude::updateInitialize();
@@ -141,14 +136,15 @@ void FlightTaskManualPosition::_updateSetpoints()
 
 	_updateXYlock(); // check for position lock
 
-	// 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()) {
+	_weathervane.update();
+
+	if (_weathervane.isActive()) {
 		_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_yaw_handler->get_weathervane_yawrate();
+			_yawspeed_setpoint += _weathervane.getWeathervaneYawrate();
 		}
 	}
 }

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

@@ -41,23 +41,17 @@
 #pragma once
 
 #include <lib/collision_prevention/CollisionPrevention.hpp>
+#include <lib/weather_vane/WeatherVane.hpp>
 #include "FlightTaskManualAltitude.hpp"
 
 class FlightTaskManualPosition : public FlightTaskManualAltitude
 {
 public:
-	FlightTaskManualPosition();
-
+	FlightTaskManualPosition() = default;
 	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;
@@ -71,8 +65,6 @@ protected:
 private:
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
 
-	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 */
+	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 */
 };

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

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