예제 #1
0
파일: Parser.cpp 프로젝트: Plantain/XCSoar
/**
 * 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);
}
예제 #2
0
파일: Screen.cpp 프로젝트: galippi/xcsoar
/**
 * 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++;
  }
}
예제 #3
0
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);
}
예제 #4
0
파일: Screen.cpp 프로젝트: Mrdini/XCSoar
/**
 * 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++;
  }
}
예제 #5
0
/**
 * 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();
}
예제 #6
0
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();
}
예제 #7
0
  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;
  }
예제 #8
0
파일: Simulator.cpp 프로젝트: macsux/XCSoar
/**
 * 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);
}
예제 #9
0
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);
}