diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-25 15:45:01 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-25 15:45:01 +0200 |
| commit | 0cddae02ab9c9ac52bde196b936b549f32435fb4 (patch) | |
| tree | 81f1af53719a457b88e46c583e7963cf755d29e0 /src | |
| parent | e87460b9f4d2a5dde2c0acd07fe3c727007b27c3 (diff) | |
| download | px4-firmware-0cddae02ab9c9ac52bde196b936b549f32435fb4.tar.gz px4-firmware-0cddae02ab9c9ac52bde196b936b549f32435fb4.tar.bz2 px4-firmware-0cddae02ab9c9ac52bde196b936b549f32435fb4.zip | |
fw: resolve an issue when the aircraft was climbing before landing
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 |
1 files changed, 7 insertions, 4 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 4f40682ea..f23719794 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 @@ -977,7 +977,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); + float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); @@ -1032,10 +1032,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* intersect glide slope: * minimize speed to approach speed * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below slope -10m continue at previous wp altitude + * until the intersection with slope * */ float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { + if (relative_alt > landing_slope_alt_rel_desired - 10.0f || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1044,7 +1047,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = math::max(relative_alt, L_altitude_rel); + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; } tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, |