diff options
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 162 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 3 |
2 files changed, 122 insertions, 43 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207..b4edb0b36 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -206,6 +209,9 @@ private: float throttle_land_max; float loiter_hold_radius; + + float land_slope_angle; + float land_slope_length; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +246,9 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t land_slope_angle; + param_t land_slope_length; } _parameter_handles; /**< handles for interesting parameters */ @@ -280,6 +289,11 @@ private: void vehicle_setpoint_poll(); /** + * Get Altitude on the landing glide slope + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); + + /** * Control position. */ bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, @@ -338,7 +352,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -358,6 +375,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -435,6 +455,11 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -623,6 +648,11 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) +{ + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) @@ -662,7 +692,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ math::Vector2f prev_wp; @@ -700,7 +730,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -715,7 +745,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + _l1_control.navigate_loiter(curr_wp, current_position, global_triplet.current.loiter_radius, global_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -728,11 +758,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -741,75 +772,118 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -5.0f) - land_noreturn = true; + land_noreturn_horizontal = true; } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); - } - - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); } - - _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now + +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float flare_angle_rad = -math::radians(5.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.airspeed_min; - float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - - if (altitude_error > -4.0f) { + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; + + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + + if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || 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 implicitely controlled now */ - _tecs.set_speed_weight(2.0f); - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, _parameters.throttle_max, throttle_land, - math::radians(-10.0f), math::radians(15.0f)); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; + + // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + // } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - /* minimize speed to approach speed */ + flare_curve_alt_last = flare_curve_alt; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + } else if (wp_distance < L_wp_distance) { + + /* minimize speed to approach speed, stay on landing slope */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { - /* normal cruise speed */ + /* intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ + altitude_desired = landing_slope_alt_desired; + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { + /* continue horizontally */ + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -818,12 +892,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), @@ -903,7 +977,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 3bb872405..4d7400ce6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -111,3 +111,6 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); |