diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index bdf5a86832..a15f1e8f71 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -714,7 +714,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. if (doing_backtransition) { - _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -766,7 +766,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) if (doing_backtransition) { // we handle loss of position control during backtransition as a special case - _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) && !_vehicle_status.in_transition_mode) { @@ -2356,18 +2356,49 @@ FixedwingPositionControl::control_manual_position(const float control_interval, attitude_setpoint.copyTo(_att_sp.q_d); } -void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, +void FixedwingPositionControl::control_backtransition_heading_hold() +{ + if (!PX4_ISFINITE(_backtrans_heading)) { + _backtrans_heading = _local_pos.heading; + } + + float true_airspeed = _airspeed_eas * _eas2tas; + + if (!_airspeed_valid) { + true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas; + } + + // we can achieve heading control by setting airspeed and groundspeed vector equal + const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed; + const Vector2f &ground_speed = airspeed_vector; + + _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT; + + navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f)); + + const float roll_body = getCorrectedNpfgRollSetpoint(); + + const float yaw_body = _backtrans_heading; + + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + +} + +void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - const bool is_low_height = checkLowHeightConditions(); - - 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.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); // Set the position where the backtransition started the first ime we pass through here. @@ -2376,30 +2407,14 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _lpos_where_backtrans_started = curr_pos_local; } - float roll_body{0.0f}; + navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + const float roll_body = getCorrectedNpfgRollSetpoint(); - if (_control_mode.flag_control_position_enabled) { - navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - roll_body = getCorrectedNpfgRollSetpoint(); - } + const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - - float 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(), - is_low_height); - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - const float pitch_body = get_tecs_pitch(); + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); attitude_setpoint.copyTo(_att_sp.q_d); @@ -2607,6 +2622,7 @@ FixedwingPositionControl::Run() if (!_vehicle_status.in_transition_mode) { // reset position of backtransition start if not in transition _lpos_where_backtrans_started = Vector2f(NAN, NAN); + _backtrans_heading = NAN; } } @@ -2703,8 +2719,13 @@ FixedwingPositionControl::Run() break; } - case FW_POSCTRL_MODE_TRANSITON: { - control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current); + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW: { + control_backtransition_line_follow(ground_speed, _pos_sp_triplet.current); + break; + } + + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD: { + control_backtransition_heading_hold(); break; } } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5600e71fcd..e56263c5e3 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -260,7 +260,8 @@ private: FW_POSCTRL_MODE_AUTO_PATH, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, - FW_POSCTRL_MODE_TRANSITON, + FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW, + FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed @@ -409,6 +410,7 @@ private: // VTOL / TRANSITION bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; + float _backtrans_heading{NAN}; // used to lock the initial heading for backtransition with no position control // ESTIMATOR RESET COUNTERS uint8_t _xy_reset_counter{0}; @@ -707,15 +709,20 @@ private: */ void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); + /** + * @brief Holds the initial heading during the course of a transition to hover. Used when there is no local + * position to do line following. + */ + void control_backtransition_heading_hold(); + /** * @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_curr current position setpoint */ - void control_backtransition(const float control_interval, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_curr); + void control_backtransition_line_follow(const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); float get_tecs_pitch(); float get_tecs_thrust(); diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index f0c8683386..2e2005997a 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -34,8 +34,8 @@ /** * Path navigation roll slew rate limit. * - * The maximum change in roll angle setpoint per second. - * This limit is applied in all Auto modes, plus manual Position and Altitude modes. + * Maximum change in roll angle setpoint per second. + * Applied in all Auto modes, plus manual Position & Altitude modes. * * @unit deg/s * @min 0 @@ -48,7 +48,7 @@ PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f); /** * NPFG period * - * Period of the NPFG control law. + * Period of NPFG control law. * * @unit s * @min 1.0 @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f); /** * NPFG damping ratio * - * Damping ratio of the NPFG control law. + * Damping ratio of NPFG control law. * * @min 0.10 * @max 1.00 @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f); * Enable automatic lower bound on the NPFG period * * Avoids limit cycling from a too aggressively tuned period/damping combination. - * If set to false, also disables the upper bound NPFG_PERIOD_UB. + * If false, also disables upper bound NPFG_PERIOD_UB. * * @boolean * @group FW NPFG Control @@ -339,8 +339,6 @@ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f); * * This is critical for detecting when to flare, and should be enabled if possible. * - * NOTE: terrain estimate is currently solely derived from a distance sensor. - * * If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing * will be aborted, depending on the criteria set in FW_LND_ABORT. * @@ -428,12 +426,12 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); /** * Low-height threshold for tighter altitude tracking * - * Defines the height (distance to bottom) threshold below which tighter altitude + * Height above ground threshold below which tighter altitude * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC * to FW_LND_THRTC_SC*FW_T_ALT_TC. * - * If equal to -1, low-height traking is disabled. + * -1 to disable. * * @unit m * @min -1 @@ -451,8 +449,6 @@ PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * * @unit m/s * @min 1.0 * @max 15.0 @@ -579,15 +575,12 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); /** - * Speed <--> Altitude priority + * Speed <--> Altitude weight * * Adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Set to 2 for gliders. + * applies to speed vs height errors. + * 0 -> control height only + * 2 -> control speed only (gliders) * * @min 0.0 * @max 2.0