From 031f7f831b02d83b67dc538d49d2d0b475ed0929 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 8 Nov 2021 11:21:08 +0100 Subject: [PATCH] Fixedwing Pos Control: Handle vehicle transition waypoints outside controllers (#18503) * Handle VTOL transition waypoints outside FW auto control modes --- .../FixedwingPositionControl.cpp | 150 +++++++----------- 1 file changed, 56 insertions(+), 94 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 7628e953bb..d526c606b6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -745,9 +745,34 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c publishOrbitStatus(pos_sp_curr); } - _position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr); + position_setpoint_s current_sp = pos_sp_curr; - switch (_position_sp_type) { + if (_vehicle_status.in_transition_to_fw) { + + if (!PX4_ISFINITE(_transition_waypoint(0))) { + double lat_transition, lon_transition; + // create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track + // during the transition + waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, + &lon_transition); + + _transition_waypoint(0) = lat_transition; + _transition_waypoint(1) = lon_transition; + } + + + current_sp.lat = _transition_waypoint(0); + current_sp.lon = _transition_waypoint(1); + + } else { + /* reset transition waypoint, will be set upon entering front transition */ + _transition_waypoint(0) = static_cast(NAN); + _transition_waypoint(1) = static_cast(NAN); + } + + const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp); + + switch (position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: _att_sp.thrust_body[0] = 0.0f; _att_sp.roll_body = 0.0f; @@ -755,19 +780,19 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c break; case position_setpoint_s::SETPOINT_TYPE_POSITION: - control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, current_sp); break; case position_setpoint_s::SETPOINT_TYPE_LOITER: - control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr, pos_sp_next); + control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); break; case position_setpoint_s::SETPOINT_TYPE_LAND: - control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + control_auto_landing(now, curr_pos, ground_speed, pos_sp_prev, current_sp); break; case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: - control_auto_takeoff(now, dt, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + control_auto_takeoff(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp); break; } @@ -898,28 +923,9 @@ uint8_t FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr) { Vector2d curr_wp{0, 0}; - Vector2d prev_wp{0, 0}; - if (_vehicle_status.in_transition_to_fw) { - - if (!PX4_ISFINITE(_transition_waypoint(0))) { - double lat_transition, lon_transition; - // create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track - // during the transition - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, - &lon_transition); - - _transition_waypoint(0) = lat_transition; - _transition_waypoint(1) = lon_transition; - } - - - curr_wp = prev_wp = _transition_waypoint; - - } else { - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - } + /* current waypoint (the one currently heading for) */ + curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); const float acc_rad = _l1_control.switch_distance(500.0f); @@ -979,45 +985,24 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve Vector2d curr_wp{0, 0}; Vector2d prev_wp{0, 0}; - if (_vehicle_status.in_transition_to_fw) { - if (!PX4_ISFINITE(_transition_waypoint(0))) { - double lat_transition, lon_transition; - // create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track - // during the transition - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, - &lon_transition); + /* current waypoint (the one currently heading for) */ + curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - _transition_waypoint(0) = lat_transition; - _transition_waypoint(1) = lon_transition; - } - - - curr_wp = prev_wp = _transition_waypoint; + if (pos_sp_prev.valid) { + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; } else { - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - if (pos_sp_prev.valid) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; - } - - - /* reset transition waypoint, will be set upon entering front transition */ - _transition_waypoint(0) = static_cast(NAN); - _transition_waypoint(1) = static_cast(NAN); + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; } + float mission_airspeed = _param_fw_airspd_trim.get(); if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && @@ -1109,43 +1094,20 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect Vector2d curr_wp{0, 0}; Vector2d prev_wp{0, 0}; - if (_vehicle_status.in_transition_to_fw) { + /* current waypoint (the one currently heading for) */ + curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - if (!PX4_ISFINITE(_transition_waypoint(0))) { - double lat_transition, lon_transition; - // create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track - // during the transition - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, - &lon_transition); - - _transition_waypoint(0) = lat_transition; - _transition_waypoint(1) = lon_transition; - } - - - curr_wp = prev_wp = _transition_waypoint; + if (pos_sp_prev.valid) { + prev_wp(0) = pos_sp_prev.lat; + prev_wp(1) = pos_sp_prev.lon; } else { - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - if (pos_sp_prev.valid) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; - } - - - /* reset transition waypoint, will be set upon entering front transition */ - _transition_waypoint(0) = static_cast(NAN); - _transition_waypoint(1) = static_cast(NAN); + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp(0) = pos_sp_curr.lat; + prev_wp(1) = pos_sp_curr.lon; } float mission_airspeed = _param_fw_airspd_trim.get();