Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
mc_pos_control: moded weathervane parameters to weathervane lib
Signed-off-by: Roman <bapstroman@gmail.com>
  • Loading branch information
RomanBapst authored and LorenzMeier committed Sep 19, 2018
1 parent 8abcf2d commit 4ad1148
Show file tree
Hide file tree
Showing 5 changed files with 121 additions and 78 deletions.
16 changes: 9 additions & 7 deletions src/lib/WeatherVane/WeatherVane.cpp
Expand Up @@ -41,7 +41,8 @@
#include <mathlib/mathlib.h>


WeatherVane::WeatherVane()
WeatherVane::WeatherVane() :
ModuleParams(nullptr)
{
_q_sp_prev = matrix::Quatf();
}
Expand All @@ -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()));
}
27 changes: 16 additions & 11 deletions src/lib/WeatherVane/WeatherVane.hpp
Expand Up @@ -42,39 +42,44 @@

#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;}

void deactivate() {_is_active = false;}

bool is_active() {return _is_active;}

void update(matrix::Quatf q_sp_prev, float yaw);
bool manual_enabled() { return _wv_manual_enabled.get(); }

float get_weathervane_yawrate();
bool auto_enabled() { return _wv_auto_enabled.get(); }

void set_weathervane_gain(float gain) {_wv_gain = gain;}
void update(matrix::Quatf q_sp_prev, float yaw);

void set_min_roll_rad(float min_roll_rad) {_wv_min_roll_rad = min_roll_rad;}
float get_weathervane_yawrate();

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
)

};
92 changes: 92 additions & 0 deletions 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);
15 changes: 4 additions & 11 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
Expand Up @@ -142,12 +142,7 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara
(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 */
(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
(ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID /**< enable obstacle avoidance */
);

control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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 {
Expand Down
49 changes: 0 additions & 49 deletions src/modules/mc_pos_control/mc_pos_control_params.c
Expand Up @@ -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);

0 comments on commit 4ad1148

Please sign in to comment.