From 574b482fdb18662c9eabd5a4ace79fe20a9c94bd Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sat, 28 Dec 2019 18:13:16 +0100 Subject: [PATCH] Navigator: make weather vane work in all auto modes, not just mission (#13761) - rename of flag in position sp: from allow_weather_vane to disable_weather_vane - flag now doesn't have to be set for all auto modes, meaning that weather vane is also active outside of mission - flag is set before front transition to align with wp, and unset after alignment is over Signed-off-by: Silvan Fuhrer --- msg/position_setpoint.msg | 2 +- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 2 +- src/modules/navigator/mission.cpp | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index e66ce35a66..b7857f2640 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -55,4 +55,4 @@ float32 acceptance_radius # navigation acceptance_radius if we're doing waypoi float32 cruising_speed # the generally desired cruising speed (not a hard constraint) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) -bool allow_weather_vane # VTOL: allow (in mission mode) the weather vane feature that turns the nose into the wind +bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index d53d0bbb57..fc19d0de66 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -238,7 +238,7 @@ bool FlightTaskAuto::_evaluateTriplets() if (_ext_yaw_handler != nullptr) { // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) - (_param_wv_en.get() && _sub_triplet_setpoint.get().current.allow_weather_vane) ? _ext_yaw_handler->activate() : + (_param_wv_en.get() && !_sub_triplet_setpoint.get().current.disable_weather_vane) ? _ext_yaw_handler->activate() : _ext_yaw_handler->deactivate(); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 510b59d594..62c5ee0a04 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -688,9 +688,6 @@ 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() && @@ -773,7 +770,7 @@ Mission::set_mission_items() !_navigator->get_land_detected()->landed) { /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.allow_weather_vane = false; + pos_sp_triplet->current.disable_weather_vane = true; /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ _mission_item.yaw = get_bearing_to_next_waypoint( @@ -795,6 +792,9 @@ Mission::set_mission_items() _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + /* 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; @@ -976,7 +976,7 @@ Mission::set_mission_items() && has_next_position_item) { /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.allow_weather_vane = false; + pos_sp_triplet->current.disable_weather_vane = true; new_work_item_type = WORK_ITEM_TYPE_ALIGN;