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

experiment with landing slope hight for more exact landing

Browse Source
This commit is contained in:
Thomas Gubler
2013-11-05 16:50:46 +01:00
parent 4beca20de2
commit a34252d18f
1 changed files with 9 additions and 8 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
+9 -8
View File
@@ -288,7 +288,7 @@ private:
/**
* Get Altitude on the landing glide slope
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad);
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt);
/**
* Control position.
@@ -642,9 +642,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
}
}
float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad)
float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt)
{
return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude;
return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative
}
bool
@@ -801,16 +801,17 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
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);
float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad);
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);
if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out
if (altitude_error > flare_relative_alt || land_noreturn) { //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);
// /* 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);