mc_pos_control: moded weathervane parameters to weathervane lib

Signed-off-by: Roman <bapstroman@gmail.com>

src/lib/WeatherVane/WeatherVane.cpp

@@ -41,7 +41,8 @@
 #include <mathlib/mathlib.h>
 
 
-WeatherVane::WeatherVane()
+WeatherVane::WeatherVane() :
+	ModuleParams(nullptr)
 {
 	_q_sp_prev = matrix::Quatf();
 }
@@ -68,15 +69,16 @@ float WeatherVane::get_weathervane_yawrate()
 	float roll_sp = -asinf(body_z_sp(1));
 
 	float roll_exceeding_treshold = 0;
+	float min_roll_rad = math::radians(_wv_min_roll.get());
 
-	if (roll_sp > _wv_min_roll_rad) {
-		roll_exceeding_treshold = roll_sp - _wv_min_roll_rad;
+	if (roll_sp > min_roll_rad) {
+		roll_exceeding_treshold = roll_sp - min_roll_rad;
 
-	} else if (roll_sp < -_wv_min_roll_rad) {
-		roll_exceeding_treshold = roll_sp + _wv_min_roll_rad;
+	} else if (roll_sp < -min_roll_rad) {
+		roll_exceeding_treshold = roll_sp + min_roll_rad;
 
 	}
 
-	return math::constrain(roll_exceeding_treshold * _wv_gain, -_wv_yawrate_max_rad,
-			       _wv_yawrate_max_rad);
+	return math::constrain(roll_exceeding_treshold * _wv_gain.get(), -math::radians(_wv_max_yaw_rate.get()),
+			       math::radians(_wv_max_yaw_rate.get()));
 }

src/lib/WeatherVane/WeatherVane.hpp

@@ -42,14 +42,15 @@
 
 #pragma once
 
+#include <px4_module_params.h>
 #include <matrix/matrix/math.hpp>
 
-class WeatherVane
+class WeatherVane : public ModuleParams
 {
 public:
 	WeatherVane();
 
-	~WeatherVane() {};
+	~WeatherVane() = default;
 
 	void activate() {_is_active = true;}
 
@@ -57,24 +58,28 @@ public:
 
 	bool is_active() {return _is_active;}
 
+	bool manual_enabled() { return _wv_manual_enabled.get(); }
+
+	bool auto_enabled() { return _wv_auto_enabled.get(); }
+
 	void update(matrix::Quatf q_sp_prev, float yaw);
 
 	float get_weathervane_yawrate();
 
-	void set_weathervane_gain(float gain) {_wv_gain = gain;}
-
-	void set_min_roll_rad(float min_roll_rad) {_wv_min_roll_rad = min_roll_rad;}
-
-	void set_yawrate_max_rad(float yawrate_max_rad) {_wv_yawrate_max_rad = yawrate_max_rad;}
+	void update_parameters() { ModuleParams::updateParams(); }
 
 private:
 	matrix::Quatf _q_sp_prev;	// previous attitude setpoint quaternion
-	float _yaw = 0.0f;					// current yaw angle
+	float _yaw = 0.0f;			// current yaw angle
 
 	bool _is_active = true;
 
-	float _wv_gain = 1.0f;		// gain that maps excessive roll angle setpoint to yawrate setoint [1/s]
-	float _wv_min_roll_rad = 0.01f;		// minimum roll angle setpoint for the controller to output a non-zero yawrate setpoint
-	float _wv_yawrate_max_rad = 1.0f;	// maximum yaw-rate the controller will output
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::WV_MAN_EN>) _wv_manual_enabled,
+		(ParamBool<px4::params::WV_AUTO_EN>) _wv_auto_enabled,
+		(ParamFloat<px4::params::WV_ROLL_MIN>) _wv_min_roll,
+		(ParamFloat<px4::params::WV_GAIN>) _wv_gain,
+		(ParamFloat<px4::params::WV_YRATE_MAX>) _wv_max_yaw_rate
+	)
 
 };

src/lib/WeatherVane/weathervane_params.c

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file weathervane_params.c
+ *
+ * Parameters defined by the weathervane lib.
+ *
+ * @author Roman Bapst <roman@auterion.com>
+ */
+
+#include <px4_config.h>
+#include <parameters/param.h>
+
+/**
+ * Enable weathervane for manual.
+ *
+ * @value 0 Disabled
+ * @value 1 Enabled
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(WV_MAN_EN, 0);
+
+/**
+ * Enable weathervane for auto.
+ *
+ * @value 0 Disabled
+ * @value 1 Enabled
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(WV_AUTO_EN, 0);
+
+/**
+ * Weather-vane yaw rate from roll gain.
+ *
+ * The desired gain to convert roll sp into yaw rate sp.
+ *
+ * @min 0.0
+ * @max 3.0
+ * @increment 0.01
+ * @decimal 3
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(WV_GAIN, 1.0f);
+
+/**
+ * Minimum roll angle setpoint for weathervane controller to demand a yaw-rate.
+ *
+ * @min 0
+ * @max 5
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(WV_ROLL_MIN, 1.0f);
+
+/**
+ * Maximum yawrate the weathervane controller is able to demand.
+ *
+ * @min 0
+ * @max 120
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(WV_YRATE_MAX, 90.0f);