fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-25 06:40:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+18 -9
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) {