From ccaeb58708c8fc5f2ad4e62b4d3cdc41e04ce017 Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 18 Sep 2018 09:52:06 +0200
Subject: [PATCH] WeatherVane lib: address review comments
---
src/lib/WeatherVane/WeatherVane.cpp | 2 +-
src/lib/WeatherVane/weathervane_params.c | 15 +++++------
.../mc_pos_control/mc_pos_control_main.cpp | 27 ++++++++++++-------
3 files changed, 25 insertions(+), 19 deletions(-)
diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp
index f5b2343d68..9e9856ddd2 100644
--- a/src/lib/WeatherVane/WeatherVane.cpp
+++ b/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) {
diff --git a/src/lib/WeatherVane/weathervane_params.c b/src/lib/WeatherVane/weathervane_params.c
index 2847defa32..aee3cfdaa6 100644
--- a/src/lib/WeatherVane/weathervane_params.c
+++ b/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
- * @value 1 Enabled
+ * @boolean
* @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
- * @value 1 Enabled
+ * @boolean
* @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);
\ No newline at end of file
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 9255993923..aa72c731e4 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/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.activate();
+ && _wv_controller->manual_enabled()) {
+ _wv_controller->activate();
- } else if (_control_mode.flag_control_auto_enabled && _wv_controller.auto_enabled()) {
- _wv_controller.activate();
+ } else if (_control_mode.flag_control_auto_enabled && _wv_controller->auto_enabled()) {
+ _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) {
--
GitLab