From 8abcf2defa557f619aecac0489cc8eb04958739f Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Tue, 4 Sep 2018 17:30:30 +0200
Subject: [PATCH] mc_pos_control: use weathervane library to make vehicle turn
into relative wind
Signed-off-by: Roman <bapstroman@gmail.com>
---
src/lib/FlightTasks/FlightTasks.hpp | 6 +++
.../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 6 +++
.../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 8 +++
.../tasks/FlightTask/FlightTask.hpp | 7 +++
.../FlightTaskManualStabilized.cpp | 6 +++
.../FlightTaskManualStabilized.hpp | 8 +++
src/lib/WeatherVane/CMakeLists.txt | 2 +-
src/lib/WeatherVane/WeatherVane.cpp | 2 +-
src/lib/WeatherVane/WeatherVane.hpp | 2 +-
src/modules/mc_pos_control/CMakeLists.txt | 3 +-
.../mc_pos_control/mc_pos_control_main.cpp | 31 +++++++++++-
.../mc_pos_control/mc_pos_control_params.c | 49 +++++++++++++++++++
12 files changed, 125 insertions(+), 5 deletions(-)
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index d60c6b4e12..f0ce8974ee 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -44,6 +44,7 @@
#include "FlightTask.hpp"
#include "SubscriptionArray.hpp"
#include "FlightTasks_generated.hpp"
+#include <lib/WeatherVane/WeatherVane.hpp>
#include <new>
@@ -125,6 +126,11 @@ public:
*/
const char *errorToString(const int error);
+ /**
+ * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
+ */
+ void set_yaw_handler(WeatherVane *ext_yaw_handler) {_current_task.task->set_yaw_handler(ext_yaw_handler);}
+
private:
/**
diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
index 630e858ac6..922201fd90 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
@@ -62,6 +62,12 @@ bool FlightTaskAutoMapper::update()
_reset();
}
+ // check if an external yaw handler is active and if yes, let it compute the yaw setpoints
+ if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+ _yaw_setpoint = _yaw;
+ _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate();
+ }
+
// The only time a thrust set-point is sent out is during
// idle. Hence, reset thrust set-point to NAN in case the
// vehicle exits idle.
diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
index f89a424087..6413370e52 100644
--- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
@@ -50,6 +50,11 @@ public:
bool activate() override;
bool update() override;
+ /**
+ * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+ */
+ void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+
protected:
float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
@@ -73,6 +78,9 @@ protected:
void updateParams() override; /**< See ModuleParam class */
private:
+ WeatherVane *_ext_yaw_handler =
+ nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
void _reset(); /**< Resets member variables to current vehicle state */
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index 4a5b665bde..c8500602a0 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -51,6 +51,7 @@
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <lib/WeatherVane/WeatherVane.hpp>
#include "SubscriptionArray.hpp"
class FlightTask : public ModuleParams
@@ -140,6 +141,12 @@ 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 set_yaw_handler(WeatherVane *ext_yaw_handler) {};
+
protected:
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp
index f2f66065ea..fb11330e31 100644
--- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp
@@ -88,6 +88,12 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
}
}
}
+
+ // check if an external yaw handler is active and if yes, let it compute the yaw setpoints
+ if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+ _yaw_setpoint = NAN;
+ _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate();
+ }
}
void FlightTaskManualStabilized::_updateThrustSetpoints()
diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
index 6640c60952..7372f70c62 100644
--- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
+++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
@@ -52,6 +52,11 @@ public:
bool updateInitialize() override;
bool update() override;
+ /**
+ * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+ */
+ void set_yaw_handler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+
protected:
virtual void _updateSetpoints(); /**< updates all setpoints*/
virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
@@ -64,6 +69,9 @@ private:
float _throttle{}; /** mapped from stick z */
+ WeatherVane *_ext_yaw_handler =
+ nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _tilt_max_man, /**< maximum tilt allowed for manual flight */
diff --git a/src/lib/WeatherVane/CMakeLists.txt b/src/lib/WeatherVane/CMakeLists.txt
index 53b0d2c1a4..9719a71cb8 100644
--- a/src/lib/WeatherVane/CMakeLists.txt
+++ b/src/lib/WeatherVane/CMakeLists.txt
@@ -31,4 +31,4 @@
#
############################################################################
-px4_add_library(WeatherVane WeatherVane.cpp)
\ No newline at end of file
+px4_add_library(WeatherVane WeatherVane.cpp)
diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp
index ff7edd0dff..8391e803c4 100644
--- a/src/lib/WeatherVane/WeatherVane.cpp
+++ b/src/lib/WeatherVane/WeatherVane.cpp
@@ -79,4 +79,4 @@ float WeatherVane::get_weathervane_yawrate()
return math::constrain(roll_exceeding_treshold * _wv_gain, -_wv_yawrate_max_rad,
_wv_yawrate_max_rad);
-}
\ No newline at end of file
+}
diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp
index d9db35f1ef..a3cf4889f9 100644
--- a/src/lib/WeatherVane/WeatherVane.hpp
+++ b/src/lib/WeatherVane/WeatherVane.hpp
@@ -77,4 +77,4 @@ private:
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
-};
\ No newline at end of file
+};
diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt
index 90fdad3a79..f60e2abe39 100644
--- a/src/modules/mc_pos_control/CMakeLists.txt
+++ b/src/modules/mc_pos_control/CMakeLists.txt
@@ -44,4 +44,5 @@ px4_add_module(
FlightTasks
git_ecl
ecl_geo
- )
\ No newline at end of file
+ WeatherVane
+ )
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 514ed0b176..e89cde34f2 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -67,6 +67,7 @@
#include <controllib/blocks.hpp>
#include <lib/FlightTasks/FlightTasks.hpp>
+#include <lib/WeatherVane/WeatherVane.hpp>
#include "PositionControl.hpp"
#include "Utility/ControlMath.hpp"
@@ -141,7 +142,12 @@ private:
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
(ParamFloat<px4::params::MPC_IDLE_TKO>) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */
- (ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID /**< enable obstacle avoidance */
+ (ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID, /**< enable obstacle avoidance */
+ (ParamBool<px4::params::MPC_WV_MAN_EN>) _wv_manual_enabled,
+ (ParamBool<px4::params::MPC_WV_AUTO_EN>) _wv_auto_enabled,
+ (ParamFloat<px4::params::MPC_WV_ROLL_MIN>) _wv_min_roll,
+ (ParamFloat<px4::params::MPC_WV_GAIN>) _wv_gain,
+ (ParamFloat<px4::params::MPC_WV_YRATE_MAX>) _wv_max_yaw_rate
);
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -172,6 +178,8 @@ private:
systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
+ WeatherVane _wv_controller;
+
/**
* Update our local parameter cache.
* Parameter update can be forced when argument is true.
@@ -371,6 +379,10 @@ 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.set_weathervane_gain(_wv_gain.get());
+ _wv_controller.set_min_roll_rad(math::radians(_wv_min_roll.get()));
+ _wv_controller.set_yawrate_max_rad(math::radians(_wv_max_yaw_rate.get()));
}
return OK;
@@ -587,6 +599,19 @@ MulticopterPositionControl::task_main()
// set dt for control blocks
setDt(dt);
+ // 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_manual_enabled.get()) {
+ _wv_controller.activate();
+
+ } else if (_control_mode.flag_control_auto_enabled && _wv_auto_enabled.get()) {
+ _wv_controller.activate();
+
+ } else {
+ _wv_controller.deactivate();
+ }
+
if (_control_mode.flag_armed) {
// as soon vehicle is armed check for flighttask
start_flight_task();
@@ -607,6 +632,8 @@ MulticopterPositionControl::task_main()
// setpoints from flighttask
vehicle_local_position_setpoint_s setpoint;
+ _flight_tasks.set_yaw_handler(&_wv_controller);
+
// update task
if (!_flight_tasks.update()) {
// FAILSAFE
@@ -721,6 +748,8 @@ MulticopterPositionControl::task_main()
_att_sp.disable_mc_yaw_control = false;
_att_sp.apply_flaps = false;
+ _wv_controller.update(matrix::Quatf(&_att_sp.q_d[0]), _local_pos.yaw);
+
if (!constraints.landing_gear) {
if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 82c4534494..546bbefac5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -672,3 +672,52 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0);
* @group Mission
*/
PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
+
+/**
+ * Enable weathervane for manual.
+ *
+ * @value 0 Disabled
+ * @value 1 Enabled
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(MPC_WV_MAN_EN, 0);
+
+/**
+ * Enable weathervane for auto.
+ *
+ * @value 0 Disabled
+ * @value 1 Enabled
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(MPC_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(MPC_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(MPC_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(MPC_WV_YRATE_MAX, 90.0f);
\ No newline at end of file
--
GitLab