/**
* This function creates some simulated traffic for FLARM debugging
* @param GPS_INFO Pointer to the NMEA_INFO struct
*/
void NMEAParser::TestRoutine(NMEA_INFO *GPS_INFO) {
static int i = 90;
i++;
if (i > 255)
i = 0;
if (i > 80)
return;
const Angle angle = Angle::degrees(fixed((i * 360) / 255)).as_bearing();
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
int h1;
int n1;
int e1;
int t1;
unsigned l;
h1 = (angle.ifastsine()) / 7;
n1 = (angle.ifastsine()) / 2 - 200;
e1 = (angle.ifastcosine()) / 1.5;
t1 = -angle.as_bearing().value_degrees();
l = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1));
int h2;
int n2;
int e2;
int t2;
Angle dangle = (angle + Angle::degrees(fixed(120))).as_bearing();
Angle hangle = dangle; hangle.flip(); hangle = hangle.as_bearing();
h2 = (angle.ifastcosine()) / 10;
n2 = (dangle.ifastsine()) / 1.20 + 300;
e2 = (dangle.ifastcosine()) + 500;
t2 = hangle.value_degrees();
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>,<RelativeVertical>,
// <IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>,<ClimbRate>,<AcftType>
char t_laa1[50];
sprintf(t_laa1, "%d,%d,%d,%d,2,DDA85C,%d,0,0,0,1", l, n1, e1, h1, t1);
char t_laa2[50];
sprintf(t_laa2, "0,%d,%d,%d,2,AA9146,%d,0,0,0,1", n2, e2, h2, t2);
char t_lau[50];
sprintf(t_lau, "2,1,2,1,%d", l);
GPS_INFO->flarm.FLARM_Available = true;
NMEAInputLine line(t_lau);
PFLAU(line, GPS_INFO->flarm);
line = NMEAInputLine(t_laa1);
PFLAA(line, GPS_INFO);
line = NMEAInputLine(t_laa2);
PFLAA(line, GPS_INFO);
}
/**
* Shifts and rotates the given polygon and also sizes it via FastScale()
* @param poly Points specifying the polygon
* @param n Number of points of the polygon
* @param xs Pixels to shift in the x-direction
* @param ys Pixels to shift in the y-direction
* @param angle Angle of rotation
*/
void
PolygonRotateShift(RasterPoint *poly, const int n, const int xs, const int ys,
Angle angle)
{
static Angle lastangle = Angle::native(-fixed_one);
static int cost = 1024, sint = 0;
angle = angle.as_bearing();
if (angle != lastangle) {
lastangle = angle;
cost = Layout::FastScale(angle.ifastcosine());
sint = Layout::FastScale(angle.ifastsine());
}
const int xxs = (xs << 10) + 512;
const int yys = (ys << 10) + 512;
RasterPoint *p = poly;
const RasterPoint *pe = poly + n;
while (p < pe) {
int x = p->x;
int y = p->y;
p->x = (x * cost - y * sint + xxs) >> 10;
p->y = (y * cost + x * sint + yys) >> 10;
p++;
}
}
static void
CalculateSector(const TCHAR *Text, TempAirspaceType &temp_area)
{
fixed Radius;
TCHAR *Stop;
GeoPoint TempPoint;
static const fixed fixed_75 = fixed(7.5);
const Angle BearingStep = Angle::degrees(temp_area.Rotation * fixed(5));
Radius = Units::ToSysUnit(fixed(_tcstod(&Text[2], &Stop)), unNauticalMiles);
Angle StartBearing = Angle::degrees(fixed(_tcstod(&Stop[1], &Stop)));
Angle EndBearing = Angle::degrees(fixed(_tcstod(&Stop[1], &Stop)));
if (EndBearing < StartBearing)
EndBearing += Angle::degrees(fixed_360);
while ((EndBearing - StartBearing).magnitude_degrees() > fixed_75) {
StartBearing = StartBearing.as_bearing();
FindLatitudeLongitude(temp_area.Center, StartBearing, Radius, &TempPoint);
temp_area.points.push_back(TempPoint);
StartBearing += BearingStep;
}
FindLatitudeLongitude(temp_area.Center, EndBearing, Radius, &TempPoint);
temp_area.points.push_back(TempPoint);
}
/**
* Shifts and rotates the given polygon and also sizes it via FastScale()
* @param poly Points specifying the polygon
* @param n Number of points of the polygon
* @param xs Pixels to shift in the x-direction
* @param ys Pixels to shift in the y-direction
* @param angle Angle of rotation
*/
void
PolygonRotateShift(RasterPoint *poly, const int n, const int xs, const int ys,
Angle angle, const bool scale)
{
static Angle lastangle = Angle::native(-fixed_one);
static int cost = 1024, sint = 0;
static bool last_scale = false;
angle = angle.as_bearing();
if ((angle != lastangle) || (last_scale != scale)) {
lastangle = angle;
last_scale = scale;
if (scale) {
cost = Layout::FastScale(angle.ifastcosine());
sint = Layout::FastScale(angle.ifastsine());
} else {
cost = Layout::FastScale(angle.ifastcosine()/2);
sint = Layout::FastScale(angle.ifastsine()/2);
}
}
RasterPoint *p = poly;
const RasterPoint *pe = poly + n;
while (p < pe) {
int x = p->x;
int y = p->y;
p->x = roundshift(x * cost - y * sint ) + xs;
p->y = roundshift(y * cost + x * sint ) + ys;
p++;
}
}
/**
* Sets the TrackBearing to val
*
* not in use
* @param val New TrackBearing
*/
void
DeviceBlackboard::SetTrack(Angle val)
{
ScopeLock protect(mutexBlackboard);
simulator_data.track = val.as_bearing();
Merge();
}
void
FlarmTrafficControl::UpdateTaskDirection(bool show_task_direction, Angle bearing)
{
if (!show_task_direction)
task_direction = Angle::degrees(fixed_minus_one);
else
task_direction = bearing.as_bearing();
}
bool
Estimate(fixed &V_westb, Angle &theta_westb, fixed &error)
{
int i;
bool scanned[NUM_V_POINTS];
for (i = 0; i < NUM_V_POINTS; i++)
scanned[i] = false;
// scan for 6 points around current best estimate.
// if a better estimate is found, keep scanning around
// that point, and don't repeat scans
bool improved = false;
bool continue_search = true;
bool full_search = false;
while (continue_search) {
continue_search = false;
int ib = VtoI(V_west_best);
int il, ih;
if (full_search) {
il = 0;
ih = NUM_V_POINTS - 1;
} else {
il = min(NUM_V_POINTS - 1, max(0, ib - 3));
ih = min(NUM_V_POINTS - 1, max(0, ib + 3));
}
for (i = il; i <= ih; i++) {
if (scanned[i]) {
continue;
} else {
scanned[i] = true;
// see if we can find a better estimate
fixed V_west = ItoV(i);
if (UpdateSearch(V_west)) {
improved = true;
continue_search = true; // earnt more search
}
}
}
if (!continue_search && !full_search && (error_best > 100)) {
full_search = true;
continue_search = true;
// if no improvement and still large error,
// try searching all speeds that haven't been checked yet.
}
}
// return true if estimate was improved
V_westb = V_west_best;
theta_westb = theta_west_best.as_bearing();
error = fixed(error_best) / 10;
return improved;
}
/**
* 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)).as_bearing();
Angle dangle = (angle + Angle::degrees(fixed(120))).as_bearing();
Angle hangle = dangle.flipped().as_bearing();
int alt = (angle.ifastsine()) / 7;
int north = (angle.ifastsine()) / 2 - 200;
int east = (angle.ifastcosine()) / 1.5;
int track = -angle.as_bearing().value_degrees();
unsigned alarm_level = (i % 30 > 13 ? 0 : (i % 30 > 5 ? 2 : 1));
NMEAParser parser;
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);
AppendNMEAChecksum(buffer);
parser.ParseNMEAString_Internal(buffer, basic);
alt = (angle.ifastcosine()) / 10;
north = (dangle.ifastsine()) / 1.20 + 300;
east = (dangle.ifastcosine()) + 500;
track = hangle.value_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);
AppendNMEAChecksum(buffer);
parser.ParseNMEAString_Internal(buffer, basic);
// PFLAU,<RX>,<TX>,<GPS>,<Power>,<AlarmLevel>,<RelativeBearing>,<AlarmType>,
// <RelativeVertical>,<RelativeDistance>(,<ID>)
sprintf(buffer, "$PFLAU,2,1,2,1,%d", alarm_level);
AppendNMEAChecksum(buffer);
parser.ParseNMEAString_Internal(buffer, basic);
}
static void
CalculateArc(const TCHAR *Text, TempAirspaceType &temp_area)
{
GeoPoint Start;
GeoPoint End;
Angle StartBearing;
fixed Radius;
const TCHAR *Comma = NULL;
GeoPoint TempPoint;
static const fixed fixed_75 = fixed(7.5);
const Angle BearingStep = Angle::degrees(temp_area.Rotation * fixed(5));
ReadCoords(&Text[3], Start);
Comma = _tcschr(Text, ',');
if (!Comma)
return;
ReadCoords(&Comma[1], End);
DistanceBearing(temp_area.Center, Start, &Radius, &StartBearing);
Angle EndBearing = Bearing(temp_area.Center, End);
TempPoint.Latitude = Start.Latitude;
TempPoint.Longitude = Start.Longitude;
temp_area.points.push_back(TempPoint);
while ((EndBearing - StartBearing).magnitude_degrees() > fixed_75) {
StartBearing += BearingStep;
StartBearing = StartBearing.as_bearing();
FindLatitudeLongitude(temp_area.Center, StartBearing, Radius, &TempPoint);
temp_area.points.push_back(TempPoint);
}
TempPoint = End;
temp_area.points.push_back(TempPoint);
}