fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

WeatherVane lib: address review comments

Browse Source
This commit is contained in:
Roman 2018-09-18 09:52:06 +02:00 committed by Lorenz Meier
parent ce7272a39c
commit ccaeb58708
3 changed files with 25 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/WeatherVane/WeatherVane.cpp
View File

@ -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) {

15
src/lib/WeatherVane/weathervane_params.c
View File

@ -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);

27
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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) {