Skip to content
Browse files

STF wind usage fixed

  • Loading branch information...
1 parent b3c21e9 commit 11a40a128f9d98b1ae597e05c1e1188eb40f3707 @mpusz committed
Showing with 45 additions and 53 deletions.
  1. +45 −53 Common/Source/Calc/Calculations.cpp
View
98 Common/Source/Calc/Calculations.cpp
@@ -345,65 +345,57 @@ double LowPassFilter(double y_last, double x_in, double fact) {
// Sollfarh / Dolphin Speed calculator
//
void SpeedToFly(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
- double n;
+ double gload;
// get load factor
if (Basic->AccelerationAvailable) {
- n = fabs(Basic->Gload);
- } else {
- n = fabs(Calculated->Gload);
+ gload = fabs(Basic->Gload);
}
-
- double delta_mc;
+ else {
+ gload = fabs(Calculated->Gload);
+ }
+
double current_mc = MACCREADY;
+ double delta_mc = current_mc-Calculated->NettoVario;
- delta_mc = current_mc-Calculated->NettoVario;
-
- // TODO FIX should we use this approach?
- if (1 || (Calculated->Vario <= current_mc)) {
- // airmass value is worse than mc threshold, so find opt cruise speed
-
- double VOptnew;
-
- if (!ValidTaskPoint(ActiveWayPoint) || !Calculated->FinalGlide) {
- // calculate speed as if cruising, wind has no effect on opt speed
- GlidePolar::MacCreadyAltitude(delta_mc,
- 100.0, // dummy value
- Basic->TrackBearing,
- 0.0,
- 0.0,
- NULL,
- &VOptnew,
- false,
- NULL, 0, CRUISE_EFFICIENCY);
- } else {
- GlidePolar::MacCreadyAltitude(delta_mc,
- 100.0, // dummy value
- Basic->TrackBearing,
- Calculated->WindSpeed,
- Calculated->WindBearing,
- 0,
- &VOptnew,
- true,
- NULL, 1.0e6, CRUISE_EFFICIENCY);
- }
-
- // put low pass filter on VOpt so display doesn't jump around
- // too much
- if (Calculated->Vario <= current_mc) {
- Calculated->VOpt = max(Calculated->VOpt,
- GlidePolar::Vminsink*sqrt(n));
- } else {
- Calculated->VOpt = max(Calculated->VOpt,
- (double)GlidePolar::Vminsink);
- }
- Calculated->VOpt = LowPassFilter(Calculated->VOpt,VOptnew, 0.6);
-
- } else {
- // this air mass is better than maccready, so fly at minimum sink speed
- // calculate speed of min sink adjusted for load factor
- Calculated->VOpt = GlidePolar::Vminsink*sqrt(n);
+ double VOptnew;
+ if (!ValidTaskPoint(ActiveWayPoint) || !Calculated->FinalGlide) {
+ // calculate speed as if cruising, wind has no effect on opt speed
+ GlidePolar::MacCreadyAltitude(delta_mc,
+ 100.0, // dummy value
+ Basic->TrackBearing,
+ 0.0,
+ 0.0,
+ NULL,
+ &VOptnew,
+ false,
+ NULL, 0, CRUISE_EFFICIENCY);
}
-
+ else {
+ double final_height = FAIFinishHeight(Basic, Calculated, -1);
+ double total_energy_height = Calculated->NavAltitude + Calculated->EnergyHeight;
+ double height_above_finish = total_energy_height - final_height;
+ GlidePolar::MacCreadyAltitude(delta_mc,
+ Calculated->TaskDistanceToGo,
+ Basic->TrackBearing,
+ Calculated->WindSpeed,
+ Calculated->WindBearing,
+ 0,
+ &VOptnew,
+ true,
+ NULL, height_above_finish, CRUISE_EFFICIENCY);
+ }
+
+ // put low pass filter on VOpt so display doesn't jump around
+ // too much
+ if (Calculated->Vario <= current_mc) {
+ Calculated->VOpt = max(Calculated->VOpt,
+ GlidePolar::Vminsink*sqrt(gload));
+ }
+ else {
+ Calculated->VOpt = max(Calculated->VOpt,
+ (double)GlidePolar::Vminsink);
+ }
+ Calculated->VOpt = LowPassFilter(Calculated->VOpt,VOptnew, 0.6);
}

0 comments on commit 11a40a1

Please sign in to comment.
Something went wrong with that request. Please try again.