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();
}
//------------------------------------------------------------------------------
inline Angle
Canvas::normalize(const Angle& a) const
{
double r = fmod(a.Radians(), 2.0 * M_PI);
Angle ra = Angle::Radians(r);
#ifndef NDEBUG
double ad = a.Degrees();
double rd = ra.Degrees();
std::cerr << __LINE__ << ": " << ad << ", " << rd << std::endl;
#endif
return (ra);
}
//------------------------------------------------------------------------------
void
Canvas::DrawAnnulus(int x, int y,
unsigned inner_r, unsigned outer_r,
Angle start, Angle end)
{
QPainterPath p;
QRectF ri(x - inner_r, y - inner_r, 2 * inner_r, 2 * inner_r);
QRectF ro(x - outer_r, y - outer_r, 2 * outer_r, 2 * outer_r);
// Draw the inner radius of the annulus.
p.arcMoveTo(ri, start.Degrees());
p.arcTo(ri, start.Degrees(), end.Degrees() - start.Degrees());
if (start != end)
{ // Only draw the end caps when needed.
// The currentPosition() will be at the end of the inner circle. Draw
// one side of the annulus.
// \todo This doesn't work because Angle(360) != Angle(0)!
double xx = (outer_r - inner_r) * cos(end.Radians()) +
p.currentPosition().rx();
double yy = (outer_r - inner_r) * -sin(end.Radians()) +
p.currentPosition().ry();
p.lineTo(xx, yy);
}
else
p.arcMoveTo(ro, end.Degrees()); // Set up for the outer circle.
// The currentPosition() will be at the 'end' of the outer circle. Draw the
// outer to the start.
p.arcTo(ro, end.Degrees(), start.Degrees() - end.Degrees());
if (start != end)
{// And close it off to finish up.
p.closeSubpath();
}
this->pushObject(p, this->pen(), this->brush());
}
//------------------------------------------------------------------------------
void
Canvas::DrawSegment(int x,
int y,
unsigned radius,
Angle start,
Angle end,
bool horizon)
{
QPainterPath p;
QRectF r(x - radius, y - radius, 2 * radius, 2 * radius);
p.arcMoveTo(r, start.Degrees());
p.arcTo(r, start.Degrees(), end.Degrees() - start.Degrees());
this->pushObject(p, this->pen(), this->brush());
}
void
DigitEntry::SetLongitude(Angle value)
{
// TODO: support all CoordinateFormats
value = value.AsBearing();
assert(length == 9);
assert(columns[0].type == Column::Type::EAST_WEST);
assert(columns[1].type == Column::Type::DIGIT19);
assert(columns[2].type == Column::Type::DIGIT);
assert(columns[4].type == Column::Type::DIGIT6);
assert(columns[5].type == Column::Type::DIGIT);
assert(columns[7].type == Column::Type::DIGIT6);
assert(columns[8].type == Column::Type::DIGIT);
columns[0].value = negative(value.Native());
const fixed degrees = fabs(value.Degrees());
const unsigned i_degrees = std::min(unsigned(degrees), 180u);
const unsigned full_seconds = unsigned(degrees * 3600u) % 3600u;
const unsigned minutes = std::min(full_seconds / 60u, 59u);
const unsigned seconds = full_seconds % 60u;
columns[1].value = i_degrees / 10;
columns[2].value = i_degrees % 10;
columns[4].value = minutes / 10;
columns[5].value = minutes % 10;
columns[7].value = seconds / 10;
columns[8].value = seconds % 10;
Invalidate();
}
/**
* Paints an arrow into the direction of the current task leg
* @param canvas The canvas to paint on
*/
void
FlarmTrafficControl::PaintTaskDirection(Canvas &canvas) const
{
if (negative(task_direction.Degrees()))
return;
canvas.Select(look.radar_pen);
canvas.SelectHollowBrush();
RasterPoint triangle[4];
triangle[0].x = 0;
triangle[0].y = -radius / Layout::FastScale(1) + 15;
triangle[1].x = 7;
triangle[1].y = triangle[0].y + 30;
triangle[2].x = -triangle[1].x;
triangle[2].y = triangle[1].y;
triangle[3].x = triangle[0].x;
triangle[3].y = triangle[0].y;
PolygonRotateShift(triangle, 4, radar_mid,
task_direction - (enable_north_up ?
Angle::Zero() : heading));
// Draw the arrow
canvas.DrawPolygon(triangle, 4);
}
gcc_const
static char
CalculateZoneLetter(const Angle latitude)
{
static constexpr char letters[] = "CDEFGHJKLMNPQRSTUVWXX";
unsigned index = (unsigned)((latitude.Degrees() + fixed(80)) / 8);
return (index < ARRAY_SIZE(letters)) ? letters[index] : '\0';
}
static void
Main()
{
Angle value = Angle::Zero();
if (!AngleEntryDialog(_T("The caption"), value))
return;
printf("%u\n", uround(value.Degrees()));
}
Angle
SunEphemeris::GetHourAngleTwilight(Angle lat, Angle declin)
{
Angle df1 = Angle::Degrees(6);
// Correction: different sign at southern hemisphere
if (negative(lat.Degrees()))
df1.Flip();
fixed fi = (declin + df1).tan() * lat.tan();
return Angle::asin(fi) + Angle::QuarterCircle();
}
Angle
SunEphemeris::GetHourAngle(Angle lat, Angle declin)
{
Angle dfo = Angle::Degrees(SUN_DIAMETER / 2 + AIR_REFRACTION);
// Correction: different sign at southern hemisphere
if (negative(lat.Degrees()))
dfo.Flip();
fixed fo = (declin + dfo).tan() * lat.tan();
return Angle::asin(fo) + Angle::QuarterCircle();
}
void
Canvas::DrawSegment(PixelScalar x, PixelScalar y, UPixelScalar radius,
Angle start, Angle end, bool horizon)
{
// XXX horizon
x += x_offset;
y += y_offset;
if (!brush.IsHollow())
::filledPieColor(surface, x, y, radius,
(int)start.Degrees() - 90,
(int)end.Degrees() - 90,
brush.GetColor().GFXColor());
if (pen_over_brush())
::pieColor(surface, x, y, radius,
(int)start.Degrees() - 90,
(int)end.Degrees() - 90,
pen.GetColor().GFXColor());
}
CanvasRotateShift(const RasterPoint pos, Angle angle,
const int scale = 100) {
#ifdef USE_GLSL
glm::mat4 matrix = glm::rotate(glm::translate(glm::mat4(),
glm::vec3(pos.x, pos.y, 0)),
GLfloat(angle.Degrees()),
glm::vec3(0, 0, 1));
float gl_scale = scale / 100.f;
if (Layout::ScaleSupported())
gl_scale *= Layout::scale_1024 / 1024.f;
matrix = glm::scale(matrix, glm::vec3(gl_scale));
glUniformMatrix4fv(OpenGL::solid_modelview, 1, GL_FALSE,
glm::value_ptr(matrix));
#else
glPushMatrix();
#ifdef HAVE_GLES
glTranslatex((GLfixed)pos.x << 16, (GLfixed)pos.y << 16, 0);
GLfixed fixed_angle = angle.Degrees() * (1<<16);
glRotatex(fixed_angle, 0, 0, 1<<16);
#else
glTranslatef(pos.x, pos.y, 0.);
glRotatef((GLfloat)angle.Degrees(), 0., 0., 1.);
#endif
#ifdef HAVE_GLES
GLfixed gl_scale = ((GLfixed) scale << 16) / 100;
if (Layout::ScaleSupported())
gl_scale = (gl_scale * Layout::scale_1024) >> 10;
glScalex(gl_scale, gl_scale, (GLfixed)1 << 16);
#else
float gl_scale = scale / 100.f;
if (Layout::ScaleSupported())
gl_scale *= Layout::scale_1024 / 1024.f;
glScalef(gl_scale, gl_scale, 1.);
#endif
#endif /* USE_GLSL */
};
Angle
SunEphemeris::GetHourAngleTwilight(Angle lat, Angle declin)
{
Angle df1 = Angle::Degrees(fixed(6));
// Correction: different sign at southern hemisphere
if (negative(lat.Degrees()))
df1.Flip();
fixed fi = (declin + df1).tan() * lat.tan();
fi = asin(fi) + fixed_half_pi;
return Angle::Radians(fi);
}
char
UTM::CalculateZoneLetter(const Angle latitude)
{
fixed degrees = latitude.Degrees();
if (degrees >= fixed(84))
return '\0';
if (degrees >= fixed(72))
return 'X';
if (degrees >= fixed(64))
return 'W';
if (degrees >= fixed(56))
return 'V';
if (degrees >= fixed(48))
return 'U';
if (degrees >= fixed(40))
return 'T';
if (degrees >= fixed(32))
return 'S';
if (degrees >= fixed(24))
return 'R';
if (degrees >= fixed(16))
return 'Q';
if (degrees >= fixed(8))
return 'P';
if (degrees >= fixed(0))
return 'N';
if (degrees >= fixed(-8))
return 'M';
if (degrees >= fixed(-16))
return 'L';
if (degrees >= fixed(-24))
return 'K';
if (degrees >= fixed(-32))
return 'J';
if (degrees >= fixed(-40))
return 'H';
if (degrees >= fixed(-48))
return 'G';
if (degrees >= fixed(-56))
return 'F';
if (degrees >= fixed(-64))
return 'E';
if (degrees >= fixed(-72))
return 'D';
if (degrees >= fixed(-80))
return 'C';
return '\0';
}
int main(int argc, char **argv)
{
Args args(argc, argv, "DRIVER FILE");
std::unique_ptr<DebugReplay> replay(CreateDebugReplay(args));
if (!replay)
return EXIT_FAILURE;
args.ExpectEnd();
printf("# time quality wind_bearing (deg) wind_speed (m/s)\n");
CirclingSettings circling_settings;
circling_settings.SetDefaults();
CirclingComputer circling_computer;
CirclingWind circling_wind;
while (replay->Next()) {
const bool last_circling = replay->Calculated().circling;
circling_computer.TurnRate(replay->SetCalculated(),
replay->Basic(),
replay->Calculated().flight);
circling_computer.Turning(replay->SetCalculated(),
replay->Basic(),
replay->Calculated().flight,
circling_settings);
if (replay->Calculated().circling != last_circling)
circling_wind.NewFlightMode(replay->Calculated());
CirclingWind::Result result = circling_wind.NewSample(replay->Basic());
if (result.quality > 0) {
fixed mag = result.wind.Magnitude();
Angle bearing;
if (result.wind.y == fixed(0) && result.wind.x == fixed(0))
bearing = Angle::Zero();
else
bearing = Angle::FromXY(result.wind.x, result.wind.y).AsBearing();
TCHAR time_buffer[32];
FormatTime(time_buffer, replay->Basic().time);
_tprintf(_T("%s %d %d %g\n"),
time_buffer, result.quality, (int)bearing.Degrees(), (double)mag);
}
}
}
void
DigitEntry::SetValue(Angle value)
{
assert(length == 3);
assert(columns[0].type == Column::Type::DIGIT36);
assert(columns[1].type == Column::Type::DIGIT);
assert(columns[2].type == Column::Type::DEGREES);
int degrees = iround(value.Degrees());
columns[0].value = degrees / 10;
columns[1].value = degrees % 10;
Invalidate();
}
int main(int argc, char **argv)
{
Args args(argc, argv, "DRIVER FILE");
DebugReplay *replay = CreateDebugReplay(args);
if (replay == NULL)
return EXIT_FAILURE;
args.ExpectEnd();
printf("# time quality wind_bearing (deg) wind_speed (m/s)\n");
CirclingComputer circling_computer;
CirclingWind circling_wind;
while (replay->Next()) {
circling_computer.TurnRate(replay->SetCalculated(),
replay->Basic(), replay->LastBasic(),
replay->Calculated(), replay->LastCalculated());
circling_computer.Turning(replay->SetCalculated(),
replay->Basic(), replay->LastBasic(),
replay->Calculated(), replay->LastCalculated(),
replay->GetComputerSettings());
if ((replay->LastCalculated().turn_mode == CirclingMode::POSSIBLE_CLIMB &&
replay->Calculated().turn_mode == CirclingMode::CLIMB) ||
(replay->LastCalculated().turn_mode == CirclingMode::POSSIBLE_CRUISE &&
replay->Calculated().turn_mode == CirclingMode::CRUISE))
circling_wind.NewFlightMode(replay->Calculated());
CirclingWind::Result result = circling_wind.NewSample(replay->Basic());
if (result.quality > 0) {
fixed mag = hypot(result.wind.x, result.wind.y);
Angle bearing;
if (result.wind.y == fixed_zero && result.wind.x == fixed_zero)
bearing = Angle::Zero();
else
bearing = Angle::Radians(atan2(result.wind.y, result.wind.x)).AsBearing();
printf("%d %d %d %g\n",
(int)replay->Basic().time,
result.quality, (int)bearing.Degrees(), (double)mag);
}
}
delete replay;
}
/**
* This function creates some simulated traffic for FLARM debugging
* @param GPS_INFO Pointer to the NMEA_INFO struct
*/
void
Simulator::GenerateFLARMTraffic(NMEAInfo &basic)
{
static int i = 90;
i++;
if (i > 255)
i = 0;
if (i > 80)
return;
const Angle angle = Angle::Degrees(fixed((i * 360) / 255)).AsBearing();
Angle dangle = (angle + Angle::Degrees(fixed(120))).AsBearing();
Angle hangle = dangle.Flipped().AsBearing();
int alt = (angle.ifastsine()) / 7;
int north = (angle.ifastsine()) / 2 - 200;
int east = (angle.ifastcosine()) / 1.5;
int track = -(int)angle.AsBearing().Degrees();
unsigned alarm_level = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1));
NMEAParser parser(true);
char buffer[50];
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
sprintf(buffer, "$PFLAA,%d,%d,%d,%d,2,DDA85C,%d,0,35,0,1",
alarm_level, north, east, alt, track);
parser.ParseLine(buffer, basic);
alt = (angle.ifastcosine()) / 10;
north = (dangle.ifastsine()) / 1.20 + 300;
east = (dangle.ifastcosine()) + 500;
track = (int)hangle.Degrees();
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
sprintf(buffer, "$PFLAA,0,%d,%d,%d,2,AA9146,,,,,1",
north, east, alt);
parser.ParseLine(buffer, basic);
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
sprintf(buffer, "$PFLAU,2,1,2,1,%d", alarm_level);
parser.ParseLine(buffer, basic);
}
void
DigitEntry::SetLongitude(Angle value, CoordinateFormat format)
{
// Longitude in floating point degrees
value = value.AsDelta();
const auto degrees = fabs(value.Degrees());
// Check the first three columns here
assert(columns[0].type == Column::Type::EAST_WEST);
assert(columns[1].type == Column::Type::DIGIT19);
assert(columns[2].type == Column::Type::DIGIT);
columns[0].value = value.IsNegative();
// Set up and check the remaining digits
SetDigits(degrees, format, false);
Invalidate();
}
void
DigitEntry::SetLatitude(Angle value, CoordinateFormat format)
{
// Latitude in floating point degrees
value = value.AsDelta();
const fixed degrees = fabs(value.Degrees());
// Check the first three columns
assert(columns[0].type == Column::Type::NORTH_SOUTH);
assert(columns[1].type == Column::Type::DIGIT);
assert(columns[2].type == Column::Type::DIGIT);
columns[0].value = negative(value.Native());
// Set up and check the remaining digits
SetDigits(degrees, format, true);
Invalidate();
}
void
TaskAutoPilot::update_cruise_bearing(const TaskAccessor& task,
const AircraftState& state,
const fixed timestep)
{
const ElementStat &stat = task.leg_stats();
Angle bct = stat.solution_remaining.cruise_track_bearing;
Angle bearing;
if (current_has_target(task)) {
bearing = stat.solution_remaining.vector.bearing;
if (parms.enable_bestcruisetrack && (stat.solution_remaining.vector.distance>fixed_1000)) {
bearing = bct;
}
} else {
bearing = state.location.Bearing(target(task));
}
if (positive(state.wind.norm) && positive(state.true_airspeed)) {
const fixed sintheta = (state.wind.bearing-bearing).sin();
if (fabs(sintheta)>fixed(0.0001)) {
bearing +=
Angle::Radians(asin(sintheta * state.wind.norm / state.true_airspeed));
}
}
Angle diff = (bearing-heading).AsDelta();
fixed d = diff.Degrees();
fixed max_turn = parms.turn_speed*timestep;
heading += Angle::Degrees(max(-max_turn, min(max_turn, d)));
if (positive(parms.bearing_noise)) {
heading += heading_deviation()*timestep;
}
heading = heading.AsBearing();
}
void
DataNode::SetAttribute(const TCHAR *name, Angle value)
{
SetAttribute(name, value.Degrees());
}
static inline void WriteAngle(TextWriter &writer, Angle value) {
WriteFixed(writer, value.Degrees());
}
static inline bool
equals(const Angle a, double b)
{
return equals(a.Degrees(), b);
}
static inline bool
equals(const Angle a, int b)
{
return equals(a.Degrees(), double(b));
}
void Push(lua_State *L, Angle value) {
Push(L, value.Degrees());
}
static void
TestAATPoint()
{
OrderedTask task(task_behaviour);
task.Append(StartPoint(new CylinderZone(wp1->location, 500),
WaypointPtr(wp1),
task_behaviour,
ordered_task_settings.start_constraints));
task.Append(AATPoint(new CylinderZone(wp2->location, 10000),
WaypointPtr(wp2),
task_behaviour));
task.Append(FinishPoint(new CylinderZone(wp3->location, 500),
WaypointPtr(wp3),
task_behaviour,
ordered_task_settings.finish_constraints));
task.SetActiveTaskPoint(1);
task.UpdateGeometry();
ok1(task.CheckTask());
AATPoint &ap = (AATPoint &)task.GetPoint(1);
ok1(!ap.IsTargetLocked());
ok1(equals(ap.GetTargetLocation(), wp2->location));
ap.LockTarget(true);
ok1(ap.IsTargetLocked());
ok1(equals(ap.GetTargetLocation(), wp2->location));
GeoPoint target = MakeGeoPoint(0, 45.31);
ap.SetTarget(target);
ok1(ap.IsTargetLocked());
ok1(equals(ap.GetTargetLocation(), wp2->location));
ap.SetTarget(target, true);
ok1(ap.IsTargetLocked());
ok1(equals(ap.GetTargetLocation(), target));
RangeAndRadial rar = ap.GetTargetRangeRadial();
ok1(equals(rar.range, 0.1112, 1000));
ok1(equals(rar.radial.Degrees(), 0, 200));
target = MakeGeoPoint(0, 45.29);
ap.SetTarget(target, true);
rar = ap.GetTargetRangeRadial();
ok1(equals(rar.range, -0.1112, 1000));
ok1(equals(rar.radial.Degrees(), 180, 200) ||
equals(rar.radial.Degrees(), -180, 200));
target = MakeGeoPoint(-0.05, 45.3);
ap.SetTarget(target, true);
rar = ap.GetTargetRangeRadial();
ok1(equals(rar.range, 0.39217));
ok1(equals(rar.radial.Degrees(), -89.98));
target = MakeGeoPoint(0.05, 45.3);
ap.SetTarget(target, true);
rar = ap.GetTargetRangeRadial();
ok1(equals(rar.range, 0.39217));
ok1(equals(rar.radial.Degrees(), 89.98));
for (int radial = -170; radial <= 170; radial += 10) {
const Angle radial2 = Angle::Degrees(radial);
for (int range = 10; range <= 100; range += 10) {
const double range2((radial >= -90 && radial <= 90
? range : -range) / 100.);
ap.SetTarget(RangeAndRadial{range2, radial2}, task.GetTaskProjection());
rar = ap.GetTargetRangeRadial();
ok1(equals(rar.range, range2, 100));
ok1(equals(rar.radial.Degrees(), radial2.Degrees(), 100));
}
}
}
void
RowFormWidget::LoadValue(unsigned i, Angle value)
{
LoadValue(i, value.Degrees());
}
gcc_const
static int32_t
AngleToLX(Angle value)
{
return ToBE32((int32_t)(value.Degrees() * 60000));
}
void
FlarmComputer::Process(FlarmData &flarm, const FlarmData &last_flarm,
const NMEAInfo &basic)
{
// Cleanup old calculation instances
if (basic.time_available)
flarm_calculations.CleanUp(basic.time);
// if (FLARM data is available)
if (!flarm.IsDetected())
return;
double north_to_latitude(0);
double east_to_longitude(0);
if (basic.location_available) {
// Precalculate relative east and north projection to lat/lon
// for Location calculations of each target
constexpr Angle delta_lat = Angle::Degrees(0.01);
constexpr Angle delta_lon = Angle::Degrees(0.01);
GeoPoint plat = basic.location;
plat.latitude += delta_lat;
GeoPoint plon = basic.location;
plon.longitude += delta_lon;
double dlat = basic.location.DistanceS(plat);
double dlon = basic.location.DistanceS(plon);
if (fabs(dlat) > 0 && fabs(dlon) > 0) {
north_to_latitude = delta_lat.Degrees() / dlat;
east_to_longitude = delta_lon.Degrees() / dlon;
}
}
// for each item in traffic
for (auto &traffic : flarm.traffic.list) {
// if we don't know the target's name yet
if (!traffic.HasName()) {
// lookup the name of this target's id
const TCHAR *fname = FlarmDetails::LookupCallsign(traffic.id);
if (fname != NULL)
traffic.name = fname;
}
// Calculate distance
traffic.distance = hypot(traffic.relative_north, traffic.relative_east);
// Calculate Location
traffic.location_available = basic.location_available;
if (traffic.location_available) {
traffic.location.latitude =
Angle::Degrees(traffic.relative_north * north_to_latitude) +
basic.location.latitude;
traffic.location.longitude =
Angle::Degrees(traffic.relative_east * east_to_longitude) +
basic.location.longitude;
}
// Calculate absolute altitude
traffic.altitude_available = basic.gps_altitude_available;
if (traffic.altitude_available)
traffic.altitude = traffic.relative_altitude + RoughAltitude(basic.gps_altitude);
// Calculate average climb rate
traffic.climb_rate_avg30s_available = traffic.altitude_available;
if (traffic.climb_rate_avg30s_available)
traffic.climb_rate_avg30s =
flarm_calculations.Average30s(traffic.id, basic.time, traffic.altitude);
// The following calculations are only relevant for targets
// where information is missing
if (traffic.track_received && traffic.turn_rate_received &&
traffic.speed_received && traffic.climb_rate_received)
continue;
// Check if the target has been seen before in the last seconds
const FlarmTraffic *last_traffic =
last_flarm.traffic.FindTraffic(traffic.id);
if (last_traffic == NULL || !last_traffic->valid)
continue;
// Calculate the time difference between now and the last contact
double dt = traffic.valid.GetTimeDifference(last_traffic->valid);
if (dt > 0) {
// Calculate the immediate climb rate
if (!traffic.climb_rate_received)
traffic.climb_rate =
(traffic.relative_altitude - last_traffic->relative_altitude) / dt;
} else {
// Since the time difference is zero (or negative)
// we can just copy the old values
if (!traffic.climb_rate_received)
traffic.climb_rate = last_traffic->climb_rate;
}
if (dt > 0 &&
traffic.location_available &&
last_traffic->location_available) {
// Calculate the GeoVector between now and the last contact
GeoVector vec = last_traffic->location.DistanceBearing(traffic.location);
if (!traffic.track_received)
traffic.track = vec.bearing;
// Calculate the turn rate
if (!traffic.turn_rate_received) {
Angle turn_rate = traffic.track - last_traffic->track;
traffic.turn_rate =
turn_rate.AsDelta().Degrees() / dt;
}
// Calculate the speed [m/s]
if (!traffic.speed_received)
traffic.speed = vec.distance / dt;
} else {
// Since the time difference is zero (or negative)
// we can just copy the old values
if (!traffic.track_received)
traffic.track = last_traffic->track;
if (!traffic.turn_rate_received)
traffic.turn_rate = last_traffic->turn_rate;
if (!traffic.speed_received)
traffic.speed = last_traffic->speed;
}
}
}