fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 12:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: VTOL: disable weather vane during yaw aligning before front transition

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-08-21 10:08:04 +02:00
committed by Roman Bapst
parent af0d63250c
commit ab28f1e4f7
5 changed files with 26 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -5
View File
@@ -535,12 +535,14 @@ MulticopterPositionControl::run()
if (_wv_controller != nullptr) {
// in manual mode we just want to use weathervane if position is controlled as well
if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled
|| _control_mode.flag_control_position_enabled)) {
_wv_controller->activate();
// in mission, enabling wv is done in flight task
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) {
_wv_controller->activate();
} else {
_wv_controller->deactivate();
} else {
_wv_controller->deactivate();
}
}
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
src/modules/navigator/mission.cpp
+9
View File
@@ -673,6 +673,9 @@ Mission::set_mission_items()
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// allow weather vane in mission
pos_sp_triplet->current.allow_weather_vane = true;
/* do takeoff before going to setpoint if needed and not already in takeoff */
/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
if (do_need_vertical_takeoff() &&
@@ -754,6 +757,9 @@ Mission::set_mission_items()
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_navigator->get_land_detected()->landed) {
/* disable weathervane before front transition for allowing yaw to align */
!_navigator->get_land_detected()->landed) {
/* check if the vtol_takeoff waypoint is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
@@ -934,6 +940,9 @@ Mission::set_mission_items()
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.allow_weather_vane = false;
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);