int main(int argc, char **argv)
{
#ifdef USE_WGS84
plan_tests(9 + 36);
#else
plan_tests(9 + 36 + 18);
#endif
const GeoPoint a(Angle::Degrees(7.7061111111111114),
Angle::Degrees(51.051944444444445));
const GeoPoint b(Angle::Degrees(7.599444444444444),
Angle::Degrees(51.099444444444444));
const GeoPoint c(Angle::Degrees(4.599444444444444),
Angle::Degrees(47.099444444444444));
fixed distance = Distance(a, b);
#ifdef USE_WGS84
ok1(distance > fixed(9150) && distance < fixed(9160));
#else
ok1(distance > fixed(9130) && distance < fixed(9140));
#endif
Angle bearing = Bearing(a, b);
ok1(bearing.Degrees() > fixed(304));
ok1(bearing.Degrees() < fixed(306));
bearing = Bearing(b, a);
ok1(bearing.Degrees() > fixed(124));
ok1(bearing.Degrees() < fixed(126));
distance = ProjectedDistance(a, b, a);
ok1(is_zero(distance));
distance = ProjectedDistance(a, b, b);
#ifdef USE_WGS84
ok1(distance > fixed(9150) && distance < fixed(9180));
#else
ok1(distance > fixed(9120) && distance < fixed(9140));
#endif
const GeoPoint middle(a.longitude.Fraction(b.longitude, fixed(0.5)),
a.latitude.Fraction(b.latitude, fixed(0.5)));
distance = ProjectedDistance(a, b, middle);
#ifdef USE_WGS84
ok1(distance > fixed(9150/2) && distance < fixed(9180/2));
#else
ok1(distance > fixed(9100/2) && distance < fixed(9140/2));
#endif
fixed big_distance = Distance(a, c);
ok1(big_distance > fixed(494000) && big_distance < fixed(495000));
TestLinearDistance();
return exit_status();
}
int main(int argc, char **argv)
{
plan_tests(9 + 36 + 18);
const GeoPoint a(Angle::degrees(fixed(7.7061111111111114)),
Angle::degrees(fixed(51.051944444444445)));
const GeoPoint b(Angle::degrees(fixed(7.599444444444444)),
Angle::degrees(fixed(51.099444444444444)));
const GeoPoint c(Angle::degrees(fixed(4.599444444444444)),
Angle::degrees(fixed(47.099444444444444)));
fixed distance = Distance(a, b);
ok1(distance > fixed(9130) && distance < fixed(9140));
Angle bearing = Bearing(a, b);
ok1(bearing.value_degrees() > fixed(304));
ok1(bearing.value_degrees() < fixed(306));
bearing = Bearing(b, a);
ok1(bearing.value_degrees() > fixed(124));
ok1(bearing.value_degrees() < fixed(126));
distance = ProjectedDistance(a, b, a);
ok1(is_zero(distance));
distance = ProjectedDistance(a, b, b);
ok1(distance > fixed(9120) && distance < fixed(9140));
const GeoPoint middle(a.Longitude.Fraction(b.Longitude, fixed_half),
a.Latitude.Fraction(b.Latitude, fixed_half));
distance = ProjectedDistance(a, b, middle);
ok1(distance > fixed(9100/2) && distance < fixed(9140/2));
fixed big_distance = Distance(a, c);
ok1(big_distance > fixed(494000) && big_distance < fixed(495000));
TestLinearDistance();
return exit_status();
}
double AATDistance::distance_achieved(int taskwaypoint, int jbest,
double longitude, double latitude) {
double achieved = Dmax[taskwaypoint][jbest];
double d0a;
DistanceBearing(lat_points[taskwaypoint][jbest],
lon_points[taskwaypoint][jbest],
latitude,
longitude,
&d0a, NULL);
legdistance_achieved[taskwaypoint] = 0;
if (d0a>0) {
// Calculates projected distance from P3 along line P1-P2
legdistance_achieved[taskwaypoint] =
ProjectedDistance(lon_points[taskwaypoint][jbest],
lat_points[taskwaypoint][jbest],
Task[taskwaypoint+1].AATTargetLon,
Task[taskwaypoint+1].AATTargetLat,
longitude, latitude, NULL, NULL);
achieved += legdistance_achieved[taskwaypoint];
}
return achieved;
}
void TaskStatistics(NMEA_INFO *Basic, DERIVED_INFO *Calculated,
const double this_maccready)
{
if (!ValidTaskPoint(ActiveWayPoint) ||
((ActiveWayPoint>0) && !ValidTaskPoint(ActiveWayPoint-1))) {
Calculated->LegSpeed = 0;
Calculated->LegDistanceToGo = 0;
Calculated->LegDistanceCovered = 0;
Calculated->LegTimeToGo = 0;
if (!AATEnabled) {
Calculated->AATTimeToGo = 0;
}
// Calculated->TaskSpeed = 0;
Calculated->TaskDistanceToGo = 0;
Calculated->TaskDistanceCovered = 0;
Calculated->TaskTimeToGo = 0;
Calculated->LKTaskETE = 0;
Calculated->TaskTimeToGoTurningNow = -1;
Calculated->TaskAltitudeRequired = 0;
Calculated->TaskAltitudeDifference = 0;
Calculated->TaskAltitudeDifference0 = 0;
Calculated->TaskAltitudeArrival = 0;
Calculated->TerrainWarningLatitude = 0.0;
Calculated->TerrainWarningLongitude = 0.0;
Calculated->GRFinish = INVALID_GR;
Calculated->FinalGlide = false;
CheckGlideThroughTerrain(Basic, Calculated); // BUGFIX 091123
// no task selected, so work things out at current heading
GlidePolar::MacCreadyAltitude(this_maccready, 100.0,
Basic->TrackBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
&(Calculated->BestCruiseTrack),
&(Calculated->VMacCready),
(Calculated->FinalGlide==true),
NULL, 1.0e6, CRUISE_EFFICIENCY);
return;
}
// LockFlightData();
LockTaskData();
// Calculate Task Distances
// First calculate distances for this waypoint
double LegCovered, LegToGo=0, LegXTD=0, LegCurrentCourse;
double LegDistance, LegBearing=0;
bool calc_turning_now;
double w1lat;
double w1lon;
double w0lat;
double w0lon;
if (AATEnabled && (ActiveWayPoint>0) && (ValidTaskPoint(ActiveWayPoint))) {
w1lat = Task[ActiveWayPoint].AATTargetLat;
w1lon = Task[ActiveWayPoint].AATTargetLon;
} else {
w1lat = WayPointList[TASKINDEX].Latitude;
w1lon = WayPointList[TASKINDEX].Longitude;
}
DistanceBearing(Basic->Latitude,
Basic->Longitude,
w1lat,
w1lon,
&LegToGo, &LegBearing);
if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1)
&& Calculated->IsInSector && (this_maccready>0.1) ) {
calc_turning_now = true;
} else {
calc_turning_now = false;
}
if (ActiveWayPoint<1) {
LegCovered = 0;
LegCurrentCourse=LegBearing;
if (ValidTaskPoint(ActiveWayPoint+1)) { // BUGFIX 091221
LegToGo=0;
}
} else {
if (AATEnabled) {
LKASSERT((ActiveWayPoint-1)>=0);
// TODO accuracy: Get best range point to here...
w0lat = Task[ActiveWayPoint-1].AATTargetLat;
w0lon = Task[ActiveWayPoint-1].AATTargetLon;
} else {
LKASSERT((ActiveWayPoint-1)>=0);
LKASSERT(ValidTaskPoint(ActiveWayPoint-1));
w0lat = WayPointList[Task[ActiveWayPoint-1].Index].Latitude;
w0lon = WayPointList[Task[ActiveWayPoint-1].Index].Longitude;
}
DistanceBearing(w1lat,
w1lon,
w0lat,
w0lon,
&LegDistance, NULL);
LegCovered = ProjectedDistance(w0lon, w0lat,
w1lon, w1lat,
Basic->Longitude,
Basic->Latitude,
&LegXTD, &LegCurrentCourse);
if ((StartLine==0) && (ActiveWayPoint==1)) {
// Correct speed calculations for radius
// JMW TODO accuracy: legcovered replace this with more accurate version
// LegDistance -= StartRadius;
LegCovered = max(0.0, LegCovered-StartRadius);
}
}
Calculated->LegDistanceToGo = LegToGo;
Calculated->LegDistanceCovered = LegCovered;
Calculated->LegCrossTrackError = LegXTD;
Calculated->LegActualTrueCourse = LegCurrentCourse;
Calculated->TaskDistanceCovered = LegCovered;
if (Basic->Time > Calculated->LegStartTime) {
if (flightstats.LegStartTime[ActiveWayPoint]<0) {
flightstats.LegStartTime[ActiveWayPoint] = Basic->Time;
}
Calculated->LegSpeed = Calculated->LegDistanceCovered
/ (Basic->Time - Calculated->LegStartTime);
}
// Now add distances for start to previous waypoint
if (!AATEnabled) {
for(int i=0;i< ActiveWayPoint-1; i++)
{
if (!ValidTaskPoint(i) || !ValidTaskPoint(i+1)) continue;
w1lat = WayPointList[Task[i].Index].Latitude;
w1lon = WayPointList[Task[i].Index].Longitude;
w0lat = WayPointList[Task[i+1].Index].Latitude;
w0lon = WayPointList[Task[i+1].Index].Longitude;
DistanceBearing(w1lat,
w1lon,
w0lat,
w0lon,
&LegDistance, NULL);
Calculated->TaskDistanceCovered += LegDistance;
}
} else if (ActiveWayPoint>0) {
// JMW added correction for distance covered
Calculated->TaskDistanceCovered =
aatdistance.DistanceCovered(Basic->Longitude,
Basic->Latitude,
ActiveWayPoint);
}
CheckTransitionFinalGlide(Basic, Calculated);
// accumulators
double TaskAltitudeRequired = 0;
double TaskAltitudeRequired0 = 0;
Calculated->TaskDistanceToGo = 0;
Calculated->TaskTimeToGo = 0;
Calculated->LKTaskETE = 0;
Calculated->TaskTimeToGoTurningNow = 0;
Calculated->TaskAltitudeArrival = 0;
double LegTime0;
// Calculate Final Glide To Finish
int FinalWayPoint = getFinalWaypoint();
double final_height = FAIFinishHeight(Basic, Calculated, -1);
double total_energy_height = Calculated->NavAltitude + Calculated->EnergyHeight;
double height_above_finish = total_energy_height - final_height;
if (ISPARAGLIDER) {
TaskAltitudeRequired = final_height;
TaskAltitudeRequired0 = final_height;
}
// Now add it for remaining waypoints
int task_index= FinalWayPoint;
double StartBestCruiseTrack = -1;
while ((task_index>ActiveWayPoint) && (ValidTaskPoint(task_index))) {
double this_LegTimeToGo;
bool this_is_final = (task_index==FinalWayPoint)
|| ForceFinalGlide;
this_is_final = true; // JMW CHECK FGAMT
if (AATEnabled) {
w1lat = Task[task_index].AATTargetLat;
w1lon = Task[task_index].AATTargetLon;
w0lat = Task[task_index-1].AATTargetLat;
w0lon = Task[task_index-1].AATTargetLon;
} else {
w1lat = WayPointList[Task[task_index].Index].Latitude;
w1lon = WayPointList[Task[task_index].Index].Longitude;
w0lat = WayPointList[Task[task_index-1].Index].Latitude;
w0lon = WayPointList[Task[task_index-1].Index].Longitude;
}
double NextLegDistance, NextLegBearing;
DistanceBearing(w0lat,
w0lon,
w1lat,
w1lon,
&NextLegDistance, &NextLegBearing);
double LegAltitude = GlidePolar::
MacCreadyAltitude(this_maccready,
NextLegDistance, NextLegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
this_is_final,
&this_LegTimeToGo,
height_above_finish, CRUISE_EFFICIENCY);
double LegAltitude0 = GlidePolar::
MacCreadyAltitude(0,
NextLegDistance, NextLegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
true,
&LegTime0, 1.0e6, CRUISE_EFFICIENCY
);
if (LegTime0>=0.9*ERROR_TIME) {
// can't make it, so assume flying at current mc
LegAltitude0 = LegAltitude;
}
TaskAltitudeRequired += LegAltitude;
TaskAltitudeRequired0 += LegAltitude0;
if(ISPARAGLIDER) {
// if required altitude is less than previous turpoint altitude,
// use previous turn point altitude
double w0Alt = FAIFinishHeight(Basic, Calculated, task_index-1);
if(TaskAltitudeRequired < w0Alt) {
Calculated->TaskAltitudeArrival += w0Alt - TaskAltitudeRequired;
TaskAltitudeRequired = w0Alt;
}
if(TaskAltitudeRequired0 < w0Alt) {
TaskAltitudeRequired0 = w0Alt;
}
}
Calculated->TaskDistanceToGo += NextLegDistance;
Calculated->TaskTimeToGo += this_LegTimeToGo;
if (task_index==1) {
StartBestCruiseTrack = NextLegBearing;
}
if (calc_turning_now) {
if (task_index == ActiveWayPoint+1) {
double NextLegDistanceTurningNow, NextLegBearingTurningNow;
double this_LegTimeToGo_turningnow=0;
DistanceBearing(Basic->Latitude,
Basic->Longitude,
w1lat,
w1lon,
&NextLegDistanceTurningNow,
&NextLegBearingTurningNow);
GlidePolar::
MacCreadyAltitude(this_maccready,
NextLegDistanceTurningNow,
NextLegBearingTurningNow,
Calculated->WindSpeed,
Calculated->WindBearing,
0, 0,
this_is_final,
&this_LegTimeToGo_turningnow,
height_above_finish, CRUISE_EFFICIENCY);
Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo_turningnow;
} else {
Calculated->TaskTimeToGoTurningNow += this_LegTimeToGo;
}
}
height_above_finish-= LegAltitude;
task_index--;
}
// current waypoint, do this last!
if (AATEnabled && (ActiveWayPoint>0) && ValidTaskPoint(ActiveWayPoint+1) && Calculated->IsInSector) {
if (Calculated->WaypointDistance<AATCloseDistance()*3.0) {
LegBearing = AATCloseBearing(Basic, Calculated);
}
}
#ifdef BCT_ALT_FIX
// Don't calculate BCT yet. LegAltitude will be used to calculate
// task altitude difference, which will then be used to calculate BCT.
#endif
double LegAltitude =
GlidePolar::MacCreadyAltitude(this_maccready,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
#ifdef BCT_ALT_FIX
0,
#else
&(Calculated->BestCruiseTrack),
#endif
&(Calculated->VMacCready),
// (Calculated->FinalGlide==1),
true, // JMW CHECK FGAMT
&(Calculated->LegTimeToGo),
height_above_finish, CRUISE_EFFICIENCY);
double LegAltitude0 =
GlidePolar::MacCreadyAltitude(0,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
0,
0,
true,
&LegTime0, 1.0e6, CRUISE_EFFICIENCY
);
#ifndef BCT_ALT_FIX
// fix problem of blue arrow wrong in task sector
if (StartBestCruiseTrack>=0) // use it only if assigned, workaround
if (Calculated->IsInSector && (ActiveWayPoint==0)) {
// set best cruise track to first leg bearing when in start sector
Calculated->BestCruiseTrack = StartBestCruiseTrack;
}
#endif
// JMW TODO accuracy: Use safetymc where appropriate
if (LegTime0>= 0.9*ERROR_TIME) {
// can't make it, so assume flying at current mc
LegAltitude0 = LegAltitude;
}
TaskAltitudeRequired += LegAltitude;
TaskAltitudeRequired0 += LegAltitude0;
Calculated->TaskDistanceToGo += LegToGo;
Calculated->TaskTimeToGo += Calculated->LegTimeToGo;
#ifndef BCT_ALT_FIX
height_above_finish-= LegAltitude;
#endif
if (calc_turning_now) {
Calculated->TaskTimeToGoTurningNow +=
Basic->Time-Calculated->TaskStartTime;
} else {
Calculated->TaskTimeToGoTurningNow = -1;
}
if (ISPARAGLIDER) {
Calculated->TaskAltitudeRequired = TaskAltitudeRequired;
} else {
Calculated->TaskAltitudeRequired = TaskAltitudeRequired + final_height;
TaskAltitudeRequired0 += final_height;
}
Calculated->TaskAltitudeDifference = total_energy_height - Calculated->TaskAltitudeRequired;
Calculated->TaskAltitudeDifference0 = total_energy_height - TaskAltitudeRequired0;
Calculated->NextAltitudeDifference0 = total_energy_height - Calculated->NextAltitudeRequired0;
Calculated->TaskAltitudeArrival += Calculated->TaskAltitudeDifference;
Calculated->GRFinish= CalculateGlideRatio(Calculated->TaskDistanceToGo, Calculated->NavAltitude - final_height);
if (Calculated->TaskSpeedAchieved >0)
Calculated->LKTaskETE = Calculated->TaskDistanceToGo/Calculated->TaskSpeedAchieved;
else
Calculated->LKTaskETE=0;
#ifdef BCT_ALT_FIX
// This MCA call's only purpose is to update BestCruiseTrack (BCT).
// It must occur after TaskAltitudeDifference (TAD) is updated,
// since BCT depends on TAD.
GlidePolar::MacCreadyAltitude(this_maccready,
LegToGo,
LegBearing,
Calculated->WindSpeed,
Calculated->WindBearing,
&(Calculated->BestCruiseTrack),
0,
true,
0,
height_above_finish,
CRUISE_EFFICIENCY,
Calculated->TaskAltitudeDifference);
// fix problem of blue arrow wrong in task sector
if (StartBestCruiseTrack>=0) // use it only if assigned, workaround
if (Calculated->IsInSector && (ActiveWayPoint==0)) {
// set best cruise track to first leg bearing when in start sector
Calculated->BestCruiseTrack = StartBestCruiseTrack;
}
height_above_finish-= LegAltitude;
#endif
CheckGlideThroughTerrain(Basic, Calculated);
CheckForceFinalGlide(Basic, Calculated);
UnlockTaskData();
}
static double
EffectiveMacCready_internal(const NMEA_INFO *Basic, const DERIVED_INFO *Calculated,
bool cruise_efficiency_mode)
{
if (Calculated->ValidFinish) return 0;
if (task.getActiveIndex()<=0) return 0; // no e mc before start
if (!Calculated->ValidStart) return 0;
if (Calculated->TaskStartTime<0) return 0;
if (!task.Valid()
|| !task.ValidTaskPoint(task.getActiveIndex()-1)) return 0;
if (Calculated->TaskDistanceToGo<=0) {
return 0;
}
double mc_setting = GlidePolar::GetMacCready();
double start_speed = Calculated->TaskStartSpeed;
double V_bestld = GlidePolar::Vbestld;
double energy_height_start =
max(0, start_speed*start_speed-V_bestld*V_bestld)/(9.81*2.0);
double telapsed = Basic->Time-Calculated->TaskStartTime;
double height_below_start =
Calculated->TaskStartAltitude + energy_height_start
- Calculated->NavAltitude - Calculated->EnergyHeight;
double LegDistances[MAXTASKPOINTS];
double LegBearings[MAXTASKPOINTS];
// JMW TODO remove dist/bearing: this is already done inside the task!
for (unsigned i=0; i<task.getActiveIndex(); i++) {
GEOPOINT w1 = task.getTargetLocation(i+1);
GEOPOINT w0 = task.getTargetLocation(i);
DistanceBearing(w0, w1,
&LegDistances[i], &LegBearings[i]);
if (i+1==task.getActiveIndex()) {
LegDistances[i] = ProjectedDistance(w0, w1, Basic->Location);
}
if ((task.getSettings().StartType==START_CIRCLE) && (i==0)) {
// Correct speed calculations for radius
// JMW TODO accuracy: leg distance replace this with more accurate version
// leg_distance -= StartRadius;
LegDistances[0] = max(0.1,LegDistances[0]-task.getSettings().StartRadius);
}
}
// OK, distance/bearings calculated, now search for Mc
double value_found;
if (cruise_efficiency_mode) {
value_found = 1.5; // max
} else {
value_found = 10.0; // max
}
for (double value_scan=0.01; value_scan<1.0; value_scan+= 0.01) {
double height_remaining = height_below_start;
double time_total=0;
double mc_effective;
double cruise_efficiency;
if (cruise_efficiency_mode) {
mc_effective = mc_setting;
if (Calculated->FinalGlide && (Calculated->timeCircling>0)) {
mc_effective = Calculated->TotalHeightClimb / Calculated->timeCircling;
}
cruise_efficiency = 0.5+value_scan;
} else {
mc_effective = value_scan*10.0;
cruise_efficiency = 1.0;
}
// Now add times from start to this waypoint,
// allowing for final glide where possible if aircraft height is below
// start
for(int i=task.getActiveIndex()-1;i>=0; i--) {
double time_this;
double height_used_this =
GlidePolar::MacCreadyAltitude(mc_effective,
LegDistances[i],
LegBearings[i],
Calculated->WindSpeed,
Calculated->WindBearing,
0, NULL,
(height_remaining>0),
&time_this,
height_remaining,
cruise_efficiency);
height_remaining -= height_used_this;
if (time_this>=0) {
time_total += time_this;
} else {
// invalid! break out of loop early
time_total= time_this;
i= -1;
continue;
}
}
if (time_total<0) {
// invalid
continue;
}
if (time_total>telapsed) {
// already too slow
continue;
}
// add time for climb from start height to height above start
if (height_below_start<0) {
time_total -= height_below_start/mc_effective;
}
// now check time..
if (time_total<telapsed) {
if (cruise_efficiency_mode) {
value_found = cruise_efficiency;
} else {
value_found = mc_effective;
}
break;
}
}
return value_found;
}