void
PolygonRotateShift(RasterPoint *poly, const int n,
const PixelScalar xs, const PixelScalar ys,
Angle angle, const int scale)
{
static Angle lastangle = Angle::Native(fixed(-1));
static int cost = 1024, sint = 0;
static int last_scale = 0;
angle = angle.AsBearing();
if ((angle != lastangle) || (last_scale != scale)) {
lastangle = angle;
last_scale = scale;
if (scale == 100) {
cost = FastScale(angle.ifastcosine());
sint = FastScale(angle.ifastsine());
} else {
cost = FastScale(angle.ifastcosine() * scale) / 100;
sint = FastScale(angle.ifastsine() * scale) / 100;
}
}
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++;
}
}
/**
* 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++;
}
}
/**
* 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);
}
/**
* 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);
}
/**
* 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++;
}
}
/**
* 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::FullCircle() * i / 255;
Angle dangle = (angle + Angle::Degrees(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);
}