WeatherVane lib: address review comments

src/lib/WeatherVane/WeatherVane.cpp

@@ -66,7 +66,7 @@ float WeatherVane::get_weathervane_yawrate()
 
 	float roll_sp = -asinf(body_z_sp(1));
 
-	float roll_exceeding_treshold = 0;
+	float roll_exceeding_treshold = 0.0f;
 	float min_roll_rad = math::radians(_wv_min_roll.get());
 
 	if (roll_sp > min_roll_rad) {

src/lib/WeatherVane/weathervane_params.c

@@ -39,14 +39,10 @@
  * @author Roman Bapst <roman@auterion.com>
  */
 
-#include <px4_config.h>
-#include <parameters/param.h>
-
 /**
  * Enable weathervane for manual.
  *
- * @value 0 Disabled
+ * @boolean
- * @value 1 Enabled
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(WV_MAN_EN, 0);
@@ -54,14 +50,13 @@ PARAM_DEFINE_INT32(WV_MAN_EN, 0);
 /**
  * Enable weathervane for auto.
  *
- * @value 0 Disabled
+ * @boolean
- * @value 1 Enabled
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(WV_AUTO_EN, 0);
 
 /**
- * Weather-vane yaw rate from roll gain.
+ * Weather-vane roll angle to yawrate.
  *
  * The desired gain to convert roll sp into yaw rate sp.
  *
@@ -78,15 +73,17 @@ PARAM_DEFINE_FLOAT(WV_GAIN, 1.0f);
  *
  * @min 0
  * @max 5
+ * @unit deg
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(WV_ROLL_MIN, 1.0f);
 
 /**
- * Maximum yawrate the weathervane controller is able to demand.
+ * Maximum yawrate the weathervane controller is allowed to demand.
  *
  * @min 0
  * @max 120
+ * @unit deg/s
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(WV_YRATE_MAX, 90.0f);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -173,7 +173,7 @@ private:
 
 	systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
 
-	WeatherVane _wv_controller;
+	WeatherVane *_wv_controller{nullptr};
 
 	/**
 	 * Update our local parameter cache.
@@ -317,6 +317,10 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 MulticopterPositionControl::~MulticopterPositionControl()
 {
+	if (_wv_controller != nullptr) {
+		delete _wv_controller;
+	}
+
 	if (_control_task != -1) {
 		// task wakes up every 100ms or so at the longest
 		_task_should_exit = true;
@@ -375,7 +379,7 @@ MulticopterPositionControl::parameters_update(bool force)
 		// set trigger time for arm hysteresis
 		_arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * 1000000.0f));
 
-		_wv_controller.update_parameters();
+		_wv_controller->update_parameters();
 	}
 
 	return OK;
@@ -400,6 +404,11 @@ MulticopterPositionControl::poll_subscriptions()
 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
 		}
+
+		// if vehicle is a VTOL we want to enable weathervane capabilities
+		if (_wv_controller == nullptr && _vehicle_status.is_vtol) {
+			_wv_controller = new WeatherVane();
+		}
 	}
 
 	orb_check(_vehicle_land_detected_sub, &updated);
@@ -595,14 +604,14 @@ MulticopterPositionControl::task_main()
 		// 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 (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled
-		    && _wv_controller.manual_enabled()) {
+		    && _wv_controller->manual_enabled()) {
-			_wv_controller.activate();
+			_wv_controller->activate();
 
-		} else if (_control_mode.flag_control_auto_enabled && _wv_controller.auto_enabled()) {
+		} else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
-			_wv_controller.activate();
+			_wv_controller->activate();
 
 		} else {
-			_wv_controller.deactivate();
+			_wv_controller->deactivate();
 		}
 
 		if (_control_mode.flag_armed) {
@@ -625,7 +634,7 @@ MulticopterPositionControl::task_main()
 			// setpoints from flighttask
 			vehicle_local_position_setpoint_s setpoint;
 
-			_flight_tasks.setYawHandler(&_wv_controller);
+			_flight_tasks.setYawHandler(_wv_controller);
 
 			// update task
 			if (!_flight_tasks.update()) {
@@ -741,7 +750,7 @@ MulticopterPositionControl::task_main()
 			_att_sp.disable_mc_yaw_control = false;
 			_att_sp.apply_flaps = false;
 
-			_wv_controller.update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
+			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
 
 			if (!constraints.landing_gear) {
 				if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {