From 7e22b47b85489de2073d807e8c85a9a7066054bb Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Dec 2023 16:50:13 +0100 Subject: [PATCH] Navigator/FlightTaskAuto yaw handling improvements/simplifications (#22532) * PositionSetpoint: remove yaw_valid field * Navigator: set yaw setpoint to NAN for Takeoff Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset. The yaw setpoint generation is handled by FlightTaskAuto. * PositionSetpoint.msg: remove disable_weather_vane and instead only use the yaw field Strictly follow the concept that if the position_setpoint.yaw is set, then follow it the controller, and otherwise let the controller set it as it thinks it's best. * Navigator: remove logic that sets yaw to be accepted in TAKEOFF No longer needed as during Takeoff we anyway don't set a yaw setpoint. * PositionSetpoint.msg: remove yawspeed_valid * PositionSetpoint.msg: remove yawspeed * Navigator: set yaw setpoint to NAN instead of current In set_takeoff and set_land_item, as well as for VTOL transition. The flight tasks then set the yaw corresponding to the current yaw. * Navigator: change get_yaw_acceptance into a bool * PositionSetpoint.msg: improve comment for yaw * MissionBlock: remove unnecessary code from set_vtol_transition_item * Navigator: clean up calculate_breaking_stop(), set yaw to NAN * Navigator: set yaw to NAN in variouls places where not specifc setpoint is desired * Navigator: set yaw to NAN in reset_position_setpoint() --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Matthias Grob --- msg/PositionSetpoint.msg | 8 +---- .../tasks/Auto/FlightTaskAuto.cpp | 12 ++----- src/modules/navigator/mission.cpp | 17 ++------- src/modules/navigator/mission_base.cpp | 9 ++--- src/modules/navigator/mission_block.cpp | 34 +++++++----------- src/modules/navigator/navigator.h | 9 +++-- src/modules/navigator/navigator_main.cpp | 36 ++++++++----------- src/modules/navigator/takeoff.cpp | 1 - src/modules/navigator/vtol_takeoff.cpp | 2 -- 9 files changed, 38 insertions(+), 90 deletions(-) diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 268e4b937d..035d35205e 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -22,11 +22,7 @@ float32 vz # local velocity setpoint in m/s in NED float64 lat # latitude, in deg float64 lon # longitude, in deg float32 alt # altitude AMSL, in m -float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw -bool yaw_valid # true if yaw setpoint valid - -float32 yawspeed # yawspeed (only for multirotors, in rad/s) -bool yawspeed_valid # true if yawspeed setpoint valid +float32 yaw # yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task float32 loiter_radius # loiter major axis radius in m float32 loiter_minor_radius # loiter minor axis radius (used for non-circular loiter shapes) in m @@ -39,5 +35,3 @@ float32 acceptance_radius # navigation acceptance_radius if we're doing waypoi float32 cruising_speed # the generally desired cruising speed (not a hard constraint) bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover - -bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 61af3f124d..1c7b4d82af 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets() } // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) - _weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane); + _weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)); // Calculate the current vehicle state and check if it has updated. State previous_state = _current_state; @@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets() _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, _triplet_next_wp, _sub_triplet_setpoint.get().next.yaw, - _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN, + (float)NAN, _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); _obstacle_avoidance.checkAvoidanceProgress( _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); @@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = NAN; _yawspeed_setpoint = 0.f; - } else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) - && _sub_triplet_setpoint.get().current.yaw_valid) { - // Use the yaw computed in Navigator except during takeoff because - // Navigator is not handling the yaw reset properly. - // But: use if from Navigator during takeoff if disable_weather_vane is true, - // because we're then aligning to the transition waypoint. - // TODO: fix in navigator + } else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) { _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; _yawspeed_setpoint = NAN; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f954ef762b..cea25c30d8 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -322,8 +322,7 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex _mission_item.lat = _global_pos_sub.get().lat; _mission_item.lon = _global_pos_sub.get().lon; - /* hold heading for takeoff items */ - _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly _mission_item.altitude = _mission_init_climb_altitude_amsl; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -366,9 +365,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_land_detected_sub.get().landed) { - /* disable weathervane before front transition for allowing yaw to align */ - 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( _global_pos_sub.get().lat, _global_pos_sub.get().lon, @@ -389,16 +385,13 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_land_detected_sub.get().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 = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; } set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.yaw = NAN; // keep current setpoints (FW position controller generates wp to track during transition) pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; @@ -428,9 +421,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite && !_land_detected_sub.get().landed && (num_found_items > 0u)) { - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; set_align_mission_item(&_mission_item, &next_mission_items[0u]); @@ -445,9 +435,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - pos_sp_triplet->previous = pos_sp_triplet->current; // keep current setpoints (FW position controller generates wp to track during transition) pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index e16c26b02e..7fc5f6b5da 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -654,9 +654,6 @@ bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const p (fabs(p1->lon - p2->lon) < DBL_EPSILON) && (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && - (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && @@ -773,13 +770,11 @@ MissionBase::heading_sp_update() _mission_item.yaw = yaw; pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; } else { - if (!pos_sp_triplet->current.yaw_valid) { - _mission_item.yaw = _navigator->get_local_position()->heading; + if (!PX4_ISFINITE(pos_sp_triplet->current.yaw)) { + _mission_item.yaw = NAN; pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; } } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 40de1418e2..357c3c72ad 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -411,7 +411,7 @@ MissionBlock::is_mission_item_reached_or_completed() if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw)) + && _navigator->get_yaw_to_be_accepted(_mission_item.yaw) && _navigator->get_local_position()->heading_good_for_control) { const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading); @@ -423,14 +423,6 @@ MissionBlock::is_mission_item_reached_or_completed() _waypoint_yaw_reached = true; } - // Always accept yaw during takeoff - // TODO: Ideally Navigator would handle a yaw reset and adjust its yaw setpoint, making the - // following no longer necessary. - // FlightTaskAuto is currently also ignoring the yaw setpoint during takeoff and thus "handling" it. - if (_mission_item.nav_cmd == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { - _waypoint_yaw_reached = true; - } - /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_navigator->get_yaw_timeout() >= FLT_EPSILON) && @@ -668,7 +660,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; - sp->yaw_valid = PX4_ISFINITE(item.yaw); sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; @@ -704,6 +695,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi } else { sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + + // Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset. + // The yaw setpoint generation is handled by FlightTaskAuto. + sp->yaw = NAN; } break; @@ -746,6 +741,7 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it item->altitude = pos_sp_triplet->current.alt; item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ? -pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius; + item->yaw = pos_sp_triplet->current.yaw; } void @@ -765,8 +761,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) } item->altitude = loiter_altitude_amsl; - item->loiter_radius = _navigator->get_loiter_radius(); + item->yaw = NAN; } void @@ -774,10 +770,11 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s { setLoiterItemCommonFields(item); - _navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw); + _navigator->calculate_breaking_stop(item->lat, item->lon); item->altitude = _navigator->get_global_position()->alt; item->loiter_radius = _navigator->get_loiter_radius(); + item->yaw = NAN; } void @@ -801,7 +798,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) /* use current position */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; - item->yaw = _navigator->get_local_position()->heading; + item->yaw = NAN; item->altitude = abs_altitude; item->altitude_is_relative = false; @@ -831,7 +828,7 @@ MissionBlock::set_land_item(struct mission_item_s *item) // set land item to current position item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; - item->yaw = _navigator->get_local_position()->heading; + item->yaw = NAN; item->altitude = 0; item->altitude_is_relative = false; @@ -863,14 +860,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_ { item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION; item->params[0] = (float) new_mode; - item->params[1] = 0.0f; - - // Keep yaw from previous mission item if valid, as that is containing the transition heading. - // If not valid use current yaw as yaw setpoint - if (!PX4_ISFINITE(item->yaw)) { - item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading - } - + item->params[1] = 0.0f; // not immediate transition item->autocontinue = true; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f52271dd95..2215a2b23d 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -239,14 +239,13 @@ public: void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; } /** - * Get the yaw acceptance given the current mission item + * Get if the yaw acceptance is required at the current mission item * * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite * - * @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint - * should be ignored + * @return true if the yaw acceptance is required, false if not required */ - float get_yaw_acceptance(float mission_item_yaw); + bool get_yaw_to_be_accepted(float mission_item_yaw); orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } @@ -279,7 +278,7 @@ public: void release_gimbal_control(); void set_gimbal_neutral(); - void calculate_breaking_stop(double &lat, double &lon, float &yaw); + void calculate_breaking_stop(double &lat, double &lon); void stop_capturing_images(); void disable_camera_trigger(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ee611e7a02..1c8dbe42e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -309,11 +309,9 @@ void Navigator::run() // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; - rep->current.yaw_valid = true; } else { rep->current.yaw = NAN; - rep->current.yaw_valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { @@ -348,8 +346,7 @@ void Navigator::run() if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { - calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw); - rep->current.yaw_valid = true; + calculate_breaking_stop(rep->current.lat, rep->current.lon); } else { // For fixedwings we can use the current vehicle's position to define the loiter point @@ -446,7 +443,6 @@ void Navigator::run() rep->current.cruising_throttle = get_cruising_throttle(); rep->current.acceptance_radius = get_acceptance_radius(); rep->current.yaw = NAN; - rep->current.yaw_valid = false; // Position is not changing, thus we keep the setpoint rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat; @@ -458,8 +454,7 @@ void Navigator::run() if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { - calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw); - rep->current.yaw_valid = true; + calculate_breaking_stop(rep->current.lat, rep->current.lon); } if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { @@ -599,19 +594,18 @@ void Navigator::run() rep->current.cruising_speed = -1.f; // reset to default if (home_global_position_valid()) { - // Only set yaw if we know the true heading - // We assume that the heading is valid when the global position is valid because true heading - // is required to fuse NE (e.g.: GNSS) data. // TODO: we should be more explicit here - rep->current.yaw = cmd.param4; rep->previous.valid = true; rep->previous.timestamp = hrt_absolute_time(); } else { - rep->current.yaw = get_local_position()->heading; rep->previous.valid = false; } + // Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset. + // The yaw setpoint generation is handled by FlightTaskAuto. + rep->current.yaw = NAN; + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = cmd.param5; rep->current.lon = cmd.param6; @@ -1039,8 +1033,7 @@ void Navigator::geofence_breach_check() } rep->current.timestamp = hrt_absolute_time(); - rep->current.yaw = get_local_position()->heading; - rep->current.yaw_valid = true; + rep->current.yaw = NAN; rep->current.lat = loiter_latitude; rep->current.lon = loiter_longitude; rep->current.alt = loiter_altitude_amsl; @@ -1157,13 +1150,13 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp) sp.timestamp = hrt_absolute_time(); sp.lat = static_cast(NAN); sp.lon = static_cast(NAN); + sp.yaw = NAN; sp.loiter_radius = get_loiter_radius(); sp.acceptance_radius = get_default_acceptance_radius(); sp.cruising_speed = get_cruising_speed(); sp.cruising_throttle = get_cruising_throttle(); sp.valid = false; sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE; - sp.disable_weather_vane = false; sp.loiter_direction_counter_clockwise = false; } @@ -1193,7 +1186,7 @@ float Navigator::get_acceptance_radius() return acceptance_radius; } -float Navigator::get_yaw_acceptance(float mission_item_yaw) +bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; @@ -1205,7 +1198,7 @@ float Navigator::get_yaw_acceptance(float mission_item_yaw) yaw = pos_ctrl_status.yaw_acceptance; } - return yaw; + return PX4_ISFINITE(yaw); } void Navigator::load_fence_from_file(const char *filename) @@ -1470,21 +1463,20 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) return true; } -void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw) +void Navigator::calculate_breaking_stop(double &lat, double &lon) { // For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back - float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx); + const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx); // predict braking distance const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); - float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs, - _param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto); + const float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs, + _param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto); waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground, multirotor_braking_distance, &lat, &lon); - yaw = get_local_position()->heading; } void Navigator::mode_completed(uint8_t nav_state, uint8_t result) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index cb467819fa..c9e59fec1e 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -130,7 +130,6 @@ Takeoff::set_takeoff_position() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; if (rep->current.valid) { diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index a6acd64f8c..7b36201856 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -75,7 +75,6 @@ VtolTakeoff::on_active() _mission_item.lon, _loiter_location(0), _loiter_location(1))); _mission_item.force_heading = true; mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->current.disable_weather_vane = true; pos_sp_triplet->current.cruising_speed = -1.f; _navigator->set_position_setpoint_triplet_updated(); @@ -182,7 +181,6 @@ VtolTakeoff::set_takeoff_position() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->previous.valid = false; - pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated();