- // find target vertical speed if altitude lock engaged
- if (alt_lock && use_perf_vs) {
- if (altitude_ft < tgt_altitude) {
- tgt_vs = tgt_altitude - altitude_ft;
- if (tgt_vs > performance->climb_rate)
- tgt_vs = performance->climb_rate;
- } else {
- tgt_vs = tgt_altitude - altitude_ft;
- if (tgt_vs < (-performance->descent_rate))
- tgt_vs = -performance->descent_rate;
+ else
+ {
+ // find target vertical speed if altitude lock engaged
+ if (alt_lock && use_perf_vs) {
+ if (altitude_ft < tgt_altitude) {
+ tgt_vs = tgt_altitude - altitude_ft;
+ if (tgt_vs > performance->climb_rate)
+ tgt_vs = performance->climb_rate;
+ } else {
+ tgt_vs = tgt_altitude - altitude_ft;
+ if (tgt_vs < (-performance->descent_rate))
+ tgt_vs = -performance->descent_rate;
+ }
+ }
+
+ if (alt_lock && !use_perf_vs) {
+ double max_vs = 4*(tgt_altitude - altitude);
+ double min_vs = 100;
+ if (tgt_altitude < altitude) min_vs = -100.0;
+ if ((fabs(tgt_altitude - altitude) < 1500.0) &&
+ (fabs(max_vs) < fabs(tgt_vs))) tgt_vs = max_vs;
+ if (fabs(tgt_vs) < fabs(min_vs)) tgt_vs = min_vs;
+ }