diff options
| author | Thomas Gubler <thomasgubler@student.ethz.ch> | 2013-10-31 17:15:55 +0100 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@student.ethz.ch> | 2013-10-31 17:15:55 +0100 |
| commit | 1214cbe71f57e980e26d520dda693c089265e1e3 (patch) | |
| tree | a5ae61d97f70bdc34bf131f5b7c58ac727b28b43 /src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | |
| parent | 7d443eb3325cfff469c88864fdc96b68612d36c0 (diff) | |
| download | px4-firmware-1214cbe71f57e980e26d520dda693c089265e1e3.tar.gz px4-firmware-1214cbe71f57e980e26d520dda693c089265e1e3.tar.bz2 px4-firmware-1214cbe71f57e980e26d520dda693c089265e1e3.zip | |
wip towards glide slope tracking for autoland
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 24 |
1 files changed, 16 insertions, 8 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 ffa7915a7..000fa5fa3 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 @@ -759,10 +759,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_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 ¤t_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 ¤t_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 ¤t_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, |