fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 07:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

wip towards glide slope tracking for autoland

Browse Source
This commit is contained in:
Thomas Gubler
2013-10-31 17:15:55 +01:00
parent 7d443eb332
commit 1214cbe71f
1 changed files with 16 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
+16 -8
View File
@@ -759,10 +759,10 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_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;
}
// /* do not go down too early */
// if (wp_distance > 50.0f) {
// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
// }
_att_sp.roll_body = _l1_control.nav_roll();
@@ -777,6 +777,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
float airspeed_land = _parameters.airspeed_min;
float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param
float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad);
if (altitude_error > -4.0f) {
/* land with minimal speed */
@@ -798,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
/* minimize speed to approach speed */
/* minimize speed to approach speed, stay on glide slope */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
_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,
@@ -808,9 +811,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else {
/* normal cruise speed */
/* normal cruise speed
* intersect glide slope: if current position higher than slope sink to glide slope but continue horizontally if below slope */
float altitude_desired = _global_pos.alt;
if (_global_pos.alt > landing_slope_alt_desired) {
altitude_desired = landing_slope_alt_desired;
}
_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(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,