diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index b9dc214f34..8ffb0157f2 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -686,8 +686,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. + const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (doing_backtransition) { + _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + + } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -708,22 +712,14 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - if (!_vehicle_status.in_transition_mode) { - - // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. - // Straight landings are currently only possible in Missions, and there the previous WP - // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. - if (_position_setpoint_previous_valid) { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; - - } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; - } + // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. + // Straight landings are currently only possible in Missions, and there the previous WP + // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. + if (_position_setpoint_previous_valid) { + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO; - // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; } } else { @@ -838,7 +834,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto position_setpoint_s current_sp = pos_sp_curr; move_position_setpoint_for_vtol_transition(current_sp); - const uint8_t position_sp_type = handle_setpoint_type(current_sp); + const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next); _position_sp_type = position_sp_type; @@ -971,7 +967,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) } uint8_t -FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr, + const position_setpoint_s &pos_sp_next) { uint8_t position_sp_type = pos_sp_curr.type; @@ -986,8 +983,13 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp const float acc_rad = _npfg.switchDistance(500.0f); - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION - || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + const bool approaching_vtol_backtransition = _vehicle_status.is_vtol + && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid + && pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid; + + + // check if we should switch to loiter but only if we are not expecting a backtransition to happen + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) { float dist_xy = -1.f; float dist_z = -1.f; @@ -997,17 +999,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp _current_latitude, _current_longitude, _current_altitude, &dist_xy, &dist_z); - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - // Achieve position setpoint altitude via loiter when laterally close to WP. - // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that - // case remove the dist_xy check (not switch out of Loiter until altitude is reached). - if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) - && (dist_z > _param_nav_fw_alt_rad.get()) - && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + // Achieve position setpoint altitude via loiter when laterally close to WP. + // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that + // case remove the dist_xy check (not switch out of Loiter until altitude is reached). + if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) + && (dist_z > _param_nav_fw_alt_rad.get()) + && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { - // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER - position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; - } + // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; } } @@ -2178,6 +2178,53 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _att_sp.pitch_body = get_tecs_pitch(); } +void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr) +{ + float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); + + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + + if (_position_setpoint_previous_valid) { + Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); + navigateLine(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + + } else { + + // if we don't have a previous waypoint for line following, then create one using the current position at the + // start of the transition + if (!_lpos_where_backtrans_started.isAllFinite()) { + _lpos_where_backtrans_started = curr_pos_local; + } + + navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + } + + _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + tecs_update_pitch_throttle(control_interval, + pos_sp_curr.alt, + target_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_sinkrate_target.get(), + _param_climbrate_target.get()); + + _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + _att_sp.pitch_body = get_tecs_pitch(); +} float FixedwingPositionControl::get_tecs_pitch() { @@ -2381,7 +2428,11 @@ FixedwingPositionControl::Run() } } - _vehicle_status_sub.update(&_vehicle_status); + if (_vehicle_status_sub.update(&_vehicle_status)) { + if (!_vehicle_status.in_transition_mode) { + _lpos_where_backtrans_started = Vector2f(NAN, NAN); + } + } Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); @@ -2474,8 +2525,14 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_TRANSITON: { + control_backtransition(control_interval, ground_speed, _pos_sp_triplet.previous, + _pos_sp_triplet.current); + break; + } } + if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { if (_control_mode.flag_control_manual_enabled) { @@ -3062,6 +3119,7 @@ float FixedwingPositionControl::getLoadFactor() } + extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[]) { return FixedwingPositionControl::main(argc, argv); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 751904590b..3c5576f09d 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -101,6 +101,7 @@ #ifdef CONFIG_FIGURE_OF_EIGHT #include "figure_eight/FigureEight.hpp" #include + #endif // CONFIG_FIGURE_OF_EIGHT using namespace launchdetection; @@ -222,6 +223,8 @@ private: vehicle_local_position_s _local_pos{}; vehicle_status_s _vehicle_status{}; + Vector2f _lpos_where_backtrans_started; + bool _position_setpoint_previous_valid{false}; bool _position_setpoint_current_valid{false}; bool _position_setpoint_next_valid{false}; @@ -243,6 +246,7 @@ private: FW_POSCTRL_MODE_AUTO_PATH, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, + FW_POSCTRL_MODE_TRANSITON, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed @@ -540,7 +544,8 @@ private: * @param pos_sp_curr Current position setpoint * @return Adjusted position setpoint type */ - uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr); + uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr, + const position_setpoint_s &pos_sp_next); /* automatic control methods */ @@ -685,6 +690,18 @@ private: */ void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); + /** + * @brief Controls flying towards a transition waypoint and then transitioning to MC mode. + * + * @param control_interval Time since last position control call [s] + * @param ground_speed Local 2D ground speed of vehicle [m/s] + * @param pos_sp_prev previous position setpoint + * @param pos_sp_curr current position setpoint + */ + void control_backtransition(const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, + const position_setpoint_s &pos_sp_curr); + float get_tecs_pitch(); float get_tecs_thrust(); diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 7fc5f6b5da..f69ffe4954 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -574,10 +574,6 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; - - // make previous setpoint invalid, such that there will be no prev-current line following - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - _navigator->reset_position_setpoint(pos_sp_triplet->previous); } } else if (needs_to_land) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 27330190a0..e0006ebddf 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -110,7 +110,8 @@ MissionBlock::is_mission_item_reached_or_completed() if (int(_mission_item.params[0]) == 3) { // transition to RW requested, only accept waypoint if vehicle state has changed accordingly - return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_navigator->get_vstatus()->in_transition_mode; } else if (int(_mission_item.params[0]) == 4) { // transition to FW requested, only accept waypoint if vehicle state has changed accordingly diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 63c876b3c5..d65054ea6d 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -310,6 +310,7 @@ void RtlDirect::set_rtl_item() dest.alt = loiter_altitude; setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode); + _navigator->reset_position_setpoint(pos_sp_triplet->previous); _rtl_state = RTLState::LAND;