diff --git a/src/lib/WeatherVane/WeatherVane.cpp b/src/lib/WeatherVane/WeatherVane.cpp index 8391e803c4..e6bdc66774 100644 --- a/src/lib/WeatherVane/WeatherVane.cpp +++ b/src/lib/WeatherVane/WeatherVane.cpp @@ -41,7 +41,8 @@ #include -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())); } diff --git a/src/lib/WeatherVane/WeatherVane.hpp b/src/lib/WeatherVane/WeatherVane.hpp index a3cf4889f9..d732f64749 100644 --- a/src/lib/WeatherVane/WeatherVane.hpp +++ b/src/lib/WeatherVane/WeatherVane.hpp @@ -42,14 +42,15 @@ #pragma once +#include #include -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) _wv_manual_enabled, + (ParamBool) _wv_auto_enabled, + (ParamFloat) _wv_min_roll, + (ParamFloat) _wv_gain, + (ParamFloat) _wv_max_yaw_rate + ) }; diff --git a/src/lib/WeatherVane/weathervane_params.c b/src/lib/WeatherVane/weathervane_params.c new file mode 100644 index 0000000000..2847defa32 --- /dev/null +++ b/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 + */ + +#include +#include + +/** + * 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); \ 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 e89cde34f2..d6ceb28feb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -142,12 +142,7 @@ private: (ParamInt) MPC_POS_MODE, (ParamInt) MPC_ALT_MODE, (ParamFloat) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */ - (ParamInt) MPC_OBS_AVOID, /**< enable obstacle avoidance */ - (ParamBool) _wv_manual_enabled, - (ParamBool) _wv_auto_enabled, - (ParamFloat) _wv_min_roll, - (ParamFloat) _wv_gain, - (ParamFloat) _wv_max_yaw_rate + (ParamInt) MPC_OBS_AVOID /**< enable obstacle avoidance */ ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -380,9 +375,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.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())); + _wv_controller.update_parameters(); } return OK; @@ -602,10 +595,10 @@ 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_manual_enabled.get()) { + && _wv_controller.manual_enabled()) { _wv_controller.activate(); - } else if (_control_mode.flag_control_auto_enabled && _wv_auto_enabled.get()) { + } else if (_control_mode.flag_control_auto_enabled && _wv_controller.auto_enabled()) { _wv_controller.activate(); } else { 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 546bbefac5..82c4534494 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -672,52 +672,3 @@ 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