diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 56f37821e9..186c53ce06 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -802,230 +802,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - // apply full flaps for landings. this flag will also trigger the use of flaperons - // if they have been enabled using the corresponding parameter - _att_sp.apply_flaps = true; - - // save time at which we started landing and reset abort_landing - if (_time_started_landing == 0) { - _time_started_landing = hrt_absolute_time(); - - _fw_pos_ctrl_status.abort_landing = false; - } - - float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); - - /* Horizontal landing control */ - /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); - - /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ - float wp_distance_save = wp_distance; - - if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) { - wp_distance_save = 0.0f; - } - - - // we want the plane to keep tracking the desired flight path until we start flaring - // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds - if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal && - ((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) { - - if (pos_sp_prev.valid) { - /* heading hold, along the line connecting this and the last waypoint */ - _target_bearing = bearing_lastwp_currwp; - - } else { - _target_bearing = _yaw; - } - - _land_noreturn_horizontal = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold"); - } - - if (_land_noreturn_horizontal) { - // heading hold - _l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d); - - } else { - // normal navigation - _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d); - } - - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - if (_land_noreturn_horizontal) { - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f)); - } - - /* Vertical landing control */ - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; - float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; - - // default to no terrain estimation, just use landing waypoint altitude - float terrain_alt = pos_sp_curr.alt; - - if (_parameters.land_use_terrain_estimate == 1) { - if (_global_pos.terrain_alt_valid) { - // all good, have valid terrain altitude - terrain_alt = _global_pos.terrain_alt; - _t_alt_prev_valid = terrain_alt; - _time_last_t_alt = hrt_absolute_time(); - - } else if (_time_last_t_alt == 0) { - // we have started landing phase but don't have valid terrain - // wait for some time, maybe we will soon get a valid estimate - // until then just use the altitude of the landing waypoint - if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { - terrain_alt = pos_sp_curr.alt; - - } else { - // still no valid terrain, abort landing - terrain_alt = pos_sp_curr.alt; - _fw_pos_ctrl_status.abort_landing = true; - } - - } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) - || _land_noreturn_vertical) { - // use previous terrain estimate for some time and hope to recover - // if we are already flaring (land_noreturn_vertical) then just - // go with the old estimate - terrain_alt = _t_alt_prev_valid; - - } else { - // terrain alt was not valid for long time, abort landing - terrain_alt = _t_alt_prev_valid; - _fw_pos_ctrl_status.abort_landing = true; - } - } - - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_altitude_rel = 0.0f; - - if (pos_sp_prev.valid) { - L_altitude_rel = pos_sp_prev.alt - terrain_alt; - } - - float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, - bearing_lastwp_currwp, bearing_airplane_currwp); - - /* Check if we should start flaring with a vertical and a - * horizontal limit (with some tolerance) - * The horizontal limit is only applied when we are in front of the wp - */ - if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) && - (wp_distance_save < _landingslope.flare_length() + 5.0f)) || - _land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - - /* land with minimal speed */ - - /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ - // _tecs.set_speed_weight(2.0f); - - /* kill the throttle if param requests it */ - throttle_max = _parameters.throttle_max; - - /* enable direct yaw control using rudder/wheel */ - if (_land_noreturn_horizontal) { - _att_sp.yaw_body = _target_bearing; - _att_sp.fw_control_yaw = true; - } - - if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) { - throttle_max = min(throttle_max, _parameters.throttle_land_max); - - if (!_land_motor_lim) { - _land_motor_lim = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle"); - } - } - - float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, - bearing_airplane_currwp); - - /* avoid climbout */ - if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { - flare_curve_alt_rel = 0.0f; // stay on ground - _land_stayonground = true; - } - - tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), - radians(_parameters.land_flare_pitch_min_deg), - radians(_parameters.land_flare_pitch_max_deg), - 0.0f, - throttle_max, - throttle_land, - false, - _land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min), - _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); - - if (!_land_noreturn_vertical) { - // just started with the flaring phase - _att_sp.pitch_body = 0.0f; - _flare_height = _global_pos.alt - terrain_alt; - mavlink_log_info(&_mavlink_log_pub, "Landing, flaring"); - _land_noreturn_vertical = true; - - } else { - if (_global_pos.vel_d > 0.1f) { - _att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) * - constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); - } - - // otherwise continue using previous _att_sp.pitch_body - } - - _flare_curve_alt_rel_last = flare_curve_alt_rel; - - } else { - - /* intersect glide slope: - * minimize speed to approach speed - * if current position is higher than the slope follow the glide slope (sink to the - * glide slope) - * also if the system captures the slope it should stay - * on the slope (bool land_onslope) - * if current position is below the slope continue at previous wp altitude - * until the intersection with slope - * */ - float altitude_desired_rel{0.0f}; - - if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) { - /* stay on slope */ - altitude_desired_rel = landing_slope_alt_rel_desired; - - if (!_land_onslope) { - mavlink_log_info(&_mavlink_log_pub, "Landing, on slope"); - _land_onslope = true; - } - - } else { - /* continue horizontally */ - if (pos_sp_prev.valid) { - altitude_desired_rel = L_altitude_rel; - - } else { - altitude_desired_rel = _global_pos.alt - terrain_alt; - } - } - - tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, - calculate_target_airspeed(airspeed_approach), - radians(_parameters.pitch_limit_min), - radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - radians(_parameters.pitch_limit_min)); - } + control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -1427,6 +1204,253 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons return setpoint; } +void +FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +{ + /* current waypoint (the one currently heading for) */ + math::Vector<2> curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); + math::Vector<2> prev_wp{0.0f, 0.0f}; /* previous waypoint */ + + if (pos_sp_prev.valid) { + prev_wp(0) = (float)pos_sp_prev.lat; + prev_wp(1) = (float)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) = (float)pos_sp_curr.lat; + prev_wp(1) = (float)pos_sp_curr.lon; + } + + + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + + // save time at which we started landing and reset abort_landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + + _fw_pos_ctrl_status.abort_landing = false; + } + + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); + + /* Horizontal landing control */ + /* switch to heading hold for the last meters, continue heading hold after */ + float wp_distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1)); + + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) { + wp_distance_save = 0.0f; + } + + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal && + ((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) { + + if (pos_sp_prev.valid) { + /* heading hold, along the line connecting this and the last waypoint */ + _target_bearing = bearing_lastwp_currwp; + + } else { + _target_bearing = _yaw; + } + + _land_noreturn_horizontal = true; + mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold"); + } + + if (_land_noreturn_horizontal) { + // heading hold + _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed); + + } else { + // normal navigation + _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); + } + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (_land_noreturn_horizontal) { + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f)); + } + + /* Vertical landing control */ + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; + + // default to no terrain estimation, just use landing waypoint altitude + float terrain_alt = pos_sp_curr.alt; + + if (_parameters.land_use_terrain_estimate == 1) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { + terrain_alt = pos_sp_curr.alt; + + } else { + // still no valid terrain, abort landing + terrain_alt = pos_sp_curr.alt; + _fw_pos_ctrl_status.abort_landing = true; + } + + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || _land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _fw_pos_ctrl_status.abort_landing = true; + } + } + + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ + float L_altitude_rel = 0.0f; + + if (pos_sp_prev.valid) { + L_altitude_rel = pos_sp_prev.alt - terrain_alt; + } + + float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + bearing_lastwp_currwp, bearing_airplane_currwp); + + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ + if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) && + (wp_distance_save < _landingslope.flare_length() + 5.0f)) || + _land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ + // _tecs.set_speed_weight(2.0f); + + /* kill the throttle if param requests it */ + float throttle_max = _parameters.throttle_max; + + /* enable direct yaw control using rudder/wheel */ + if (_land_noreturn_horizontal) { + _att_sp.yaw_body = _target_bearing; + _att_sp.fw_control_yaw = true; + } + + if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) { + throttle_max = min(throttle_max, _parameters.throttle_land_max); + + if (!_land_motor_lim) { + _land_motor_lim = true; + mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle"); + } + } + + float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); + + /* avoid climbout */ + if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { + flare_curve_alt_rel = 0.0f; // stay on ground + _land_stayonground = true; + } + + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land), + radians(_parameters.land_flare_pitch_min_deg), + radians(_parameters.land_flare_pitch_max_deg), + 0.0f, + throttle_max, + throttle_land, + false, + _land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min), + _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + + if (!_land_noreturn_vertical) { + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + _flare_height = _global_pos.alt - terrain_alt; + mavlink_log_info(&_mavlink_log_pub, "Landing, flaring"); + _land_noreturn_vertical = true; + + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) * + constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); + } + + // otherwise continue using previous _att_sp.pitch_body + } + + _flare_curve_alt_rel_last = flare_curve_alt_rel; + + } else { + + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ + float altitude_desired_rel{0.0f}; + + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) { + /* stay on slope */ + altitude_desired_rel = landing_slope_alt_rel_desired; + + if (!_land_onslope) { + mavlink_log_info(&_mavlink_log_pub, "Landing, on slope"); + _land_onslope = true; + } + + } else { + /* continue horizontally */ + if (pos_sp_prev.valid) { + altitude_desired_rel = L_altitude_rel; + + } else { + altitude_desired_rel = _global_pos.alt - terrain_alt; + } + } + + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, + calculate_target_airspeed(airspeed_approach), + radians(_parameters.pitch_limit_min), + radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + radians(_parameters.pitch_limit_min)); + } +} + float FixedwingPositionControl::get_tecs_pitch() { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 548c73d1e8..9ae2fe268f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -440,6 +440,9 @@ private: bool control_position(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + void control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + float get_tecs_pitch(); float get_tecs_thrust();