diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 386493ab9599..e66ce35a6602 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -54,3 +54,5 @@ 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 diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index d3b0e66c9f70..c46f59c0e84b 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -259,6 +259,12 @@ 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() : + _ext_yaw_handler->deactivate(); + } + // set heading if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { _yaw_setpoint = _yaw; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 2e32a2d2d34b..c14371dbb77d 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -120,7 +120,8 @@ class FlightTaskAuto : public FlightTask (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, - (ParamFloat) _param_mis_yaw_err // yaw-error threshold + (ParamFloat) _param_mis_yaw_err, // yaw-error threshold + (ParamBool) _param_wv_en // enable/disable weather vane (VTOL) ); private: diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 66a078e29b0e..2829be5bca3f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 47b1d0b7656a..faa3a3065a64 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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() && @@ -752,6 +755,29 @@ Mission::set_mission_items() _work_item_type == WORK_ITEM_TYPE_TAKEOFF && new_work_item_type == WORK_ITEM_TYPE_DEFAULT && _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 */ + pos_sp_triplet->current.allow_weather_vane = false; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WORK_ITEM_TYPE_ALIGN && + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { /* check if the vtol_takeoff waypoint is on top of us */ @@ -934,6 +960,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); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index fa459319a39d..efbf8faec82a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -319,7 +319,7 @@ MissionBlock::is_mission_item_reached() } } - if (_waypoint_position_reached) { + if (_waypoint_position_reached && !_waypoint_position_reached_previously) { // reached just now _time_wp_reached = now; } @@ -408,6 +408,7 @@ MissionBlock::is_mission_item_reached() } // all acceptance criteria must be met in the same iteration + _waypoint_position_reached_previously = _waypoint_position_reached; _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ac342b65e9bb..e6ca6a5febc7 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -129,6 +129,7 @@ class MissionBlock : public NavigatorMode bool _waypoint_position_reached{false}; bool _waypoint_yaw_reached{false}; + bool _waypoint_position_reached_previously{false}; hrt_abstime _time_first_inside_orbit{0}; hrt_abstime _action_start{0};