fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 12:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

introduce experimental flare trajectory

Browse Source
This commit is contained in:
Thomas Gubler
2013-11-06 13:10:14 +01:00
parent dbee676367
commit bdbe64026b
2 changed files with 61 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+60 -32
View File
@@ -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;
@@ -292,7 +295,7 @@ private:
/**
* Get Altitude on the landing glide slope
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt);
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement);
/**
* Control position.
@@ -353,7 +356,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;
@@ -374,7 +380,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_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_SLL");
_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");
@@ -648,9 +654,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
}
}
float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt)
float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement)
{
return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative
return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
}
bool
@@ -762,7 +768,8 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* switch to heading hold for the last meters, continue heading hold after */
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 */
@@ -771,15 +778,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else {
if (!land_noreturn) //set target_bearing in first occurrence
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);
land_noreturn = true;
land_noreturn_horizontal = true;
} else {
@@ -791,6 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* 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) {
@@ -806,13 +814,19 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle);
float landingslope_length = _parameters.land_slope_length;
float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen")
float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length;
float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt);
float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt);
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 (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out
if (_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 */
@@ -820,18 +834,34 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
// _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;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
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,
math::radians(flare_angle_rad), math::radians(15.0f));
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));
warnx("Landing: land with minimal speed");
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);
flare_curve_alt_last = flare_curve_alt;
} else if (wp_distance < L_wp_distance) {
@@ -841,26 +871,22 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
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: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance);
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 {
/* intersect glide slope:
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m and above altitude at L (see documentation) continue horizontally
* if current position is below altitude at L, climb to altitude of L */
* 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: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance);
} else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) {
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_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection
warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude);
} else {
/* climb to L_altitude */
altitude_desired = L_altitude;
warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance);
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, altitude_desired, calculate_target_airspeed(airspeed_approach),
@@ -957,7 +983,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_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()) {
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+1 -1
View File
@@ -115,4 +115,4 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);