Example #1
0
int main(int argc, char **argv)
{
  plan_tests(37);

  FlatGeoPoint p1(1, 1);
  FlatGeoPoint p2(1, 2);
  FlatGeoPoint p3(3, 10);

  // test cross()
  ok1(p1.CrossProduct(p2) == 1);
  ok1(p2.CrossProduct(p1) == -1);
  ok1(p1.CrossProduct(p3) == 7);
  ok1(p3.CrossProduct(p1) == -7);
  ok1(p2.CrossProduct(p3) == 4);
  ok1(p3.CrossProduct(p2) == -4);

  // test dot()
  ok1(p1.DotProduct(p2) == 3);
  ok1(p2.DotProduct(p1) == 3);
  ok1(p1.DotProduct(p3) == 13);
  ok1(p3.DotProduct(p1) == 13);
  ok1(p2.DotProduct(p3) == 23);
  ok1(p3.DotProduct(p2) == 23);

  // test ==
  ok1(p1 == FlatGeoPoint(1, 1));
  ok1(p2 == FlatGeoPoint(1, 2));
  ok1(p3 == FlatGeoPoint(3, 10));

  // test <
  ok1(p2.Sort(p1));
  ok1(p3.Sort(p1));
  ok1(p3.Sort(p2));

  // test * (and ==)
  ok1(p1 * fixed(2) == FlatGeoPoint(2, 2));
  ok1(p2 * fixed(2) == FlatGeoPoint(2, 4));
  ok1(p3 * fixed(2) == FlatGeoPoint(6, 20));

  // test +
  p2 = p2 + p1;
  ok1(p2.longitude == 2);
  ok1(p2.latitude == 3);

  // test -
  p2 = p2 - p1;
  ok1(p2.longitude == 1);
  ok1(p2.latitude == 2);

  // test distance_sq_to()
  ok1(p1.DistanceSquared(p2) == 1);
  ok1(p2.DistanceSquared(p1) == 1);
  ok1(p1.DistanceSquared(p3) == 85);
  ok1(p3.DistanceSquared(p1) == 85);
  ok1(p2.DistanceSquared(p3) == 68);
  ok1(p3.DistanceSquared(p2) == 68);

  // test distance_to()
  ok1(p1.Distance(p2) == 1);
  ok1(p2.Distance(p1) == 1);
  ok1(p1.Distance(p3) == 9);
  ok1(p3.Distance(p1) == 9);
  ok1(p2.Distance(p3) == 8);
  ok1(p3.Distance(p2) == 8);

  return exit_status();
}
Example #2
0
fixed
DigitEntry::GetFixedValue() const
{
  fixed value = fixed(GetPositiveInteger()) + GetPositiveFractional();
  return IsNegative() ? -value : value;
}
Example #3
0
void
WesterboerVW921Device::SentenceZero(const void *_data, size_t length,
                                    struct NMEAInfo &info)
{
  const uint8_t *data = (const uint8_t *)_data;

  /*
  Received sentence #0 with length = 45
  24 77 2d 3c 00 10 82 07   c8 6c 28 00 00 bd ff 46
  00 04 02 00 00 46 00 00   00 12 00 01 00 00 00 00
  00 3f 0b 80 62 d5 b1 7d   5a 0d cd a6 ff
  */

  // 0 - 4        Header
  // 24 77 2d 3c 00

  // 5      Byte  Flight status (Bitmask)
  // 6      Byte  Flight status 2 (Bitmask)
  // Bit 7 : External Switch (1=Climbing, 0=Cruise)

  uint8_t flight_status2 = *(data + 6);
  if ((flight_status2 & (1 << 7)) == 0) {
    info.switch_state.flight_mode = SwitchState::FlightMode::CRUISE;
  } else {
    info.switch_state.flight_mode = SwitchState::FlightMode::CIRCLING;
  }

  // 7      Byte  GPS status (Bitmask)
  // Bit 3: GPS-Status-Flag ("A"-valid position/"V"- NAV receiver warning)
  // 8      Byte  GPS status 2 (Bitmask)

  // 9     Word  Flight time (sec)
  // 11     Word  Distance to next target (0.1 km)
  // 6c 28 00 00

  // 13     Int   STD altitude (m)
  // 15     Int   QFE-Höhe h (m)
  // b8 ff 00 00

  int16_t std_altitude =
      ReadUnalignedLE16((const uint16_t *)(const void *)(data + 13));
  info.ProvidePressureAltitude(fixed(std_altitude));

  // 17     Int   Windempfehlung (km/ h)
  // 19     Int   delta speed (km/ h)
  // 21     Int   Gleitpath deviation (m)
  // 00 00 00 00 00 00

  // 23     Int   avg. Vario (0,1 m/s)
  // 25     Int   Nettovario (0,1 m/s)
  // 27     Int   Vario (0,1 m/s)
  // 00 00 11 00 f7 ff

  int16_t netto_vario =
      ReadUnalignedLE16((const uint16_t *)(const void *)(data + 25));
  info.ProvideNettoVario(fixed(netto_vario) / 10);

  int16_t vario =
      ReadUnalignedLE16((const uint16_t *)(const void *)(data + 27));
  info.ProvideTotalEnergyVario(fixed(vario) / 10);

  // 29     Int   True Air Speed TAS (0,1 m/s)
  // 31     Int   Groundspeed GS (0,1 m/s)
  // 00 00 e6 00

  // Flight tests by Kimmo Hytönen have shown that the Westerboer documentation
  // is wrong and that the TAS is actually transmitted in 0.1 km/h instead.
  int16_t tas =
      ReadUnalignedLE16((const uint16_t *)(const void *)(data + 29));
  info.ProvideTrueAirspeed(Units::ToSysUnit(fixed(tas) / 10,
                                            Unit::KILOMETER_PER_HOUR));

  int16_t ground_speed =
      ReadUnalignedLE16((const uint16_t *)(const void *)(data + 31));

  info.ground_speed = fixed(ground_speed) / 10;
  info.ground_speed_available.Update(info.clock);

  // Next 12 bytes are zero if GPS connection is bad:
  // 33     Int   Track (0.1 deg)
  // eb 03
  // 35     Float Latitude (arc, -pi/2 .. +pi/2)
  // 80 62 d6 ce = N504606
  // 39     Float Longitude (arc, -pi .. +pi)
  // 7d 59 e4 01 = E 60601
  // 43     Int   DiffAngle (0.1 deg)
  // 62 fd

  uint8_t gps_status = *(data + 7);
  if ((gps_status & (1 << 3)) != 0) {
    // GPS has a valid fix

    int16_t track =
        ReadUnalignedLE16((const uint16_t *)(const void *)(data + 31));

    info.track = Angle::Degrees(fixed(track) / 10);
    info.track_available.Update(info.clock);

    info.location.latitude = Angle::Radians(fixed(ReadFloat(data + 35)));
    info.location.longitude = Angle::Radians(fixed(ReadFloat(data + 39)));

    info.location_available.Update(info.clock);
  }
}
Example #4
0
int main(int argc, char **argv)
{
  plan_tests(41);

  FlatPoint p1(fixed_one, fixed_one);
  FlatPoint p2(fixed_one, fixed_two);
  FlatPoint p3(fixed(3), fixed_ten);

  // test cross()
  ok1(equals(p1.CrossProduct(p2), 1));
  ok1(equals(p2.CrossProduct(p1), -1));
  ok1(equals(p1.CrossProduct(p3), 7));
  ok1(equals(p3.CrossProduct(p1), -7));
  ok1(equals(p2.CrossProduct(p3), 4));
  ok1(equals(p3.CrossProduct(p2), -4));

  // test mul_y()
  p2.MultiplyY(fixed_two);
  ok1(equals(p2.x, 1));
  ok1(equals(p2.y, 4));

  // test sub()
  p2.Subtract(p1);
  ok1(equals(p2.x, 0));
  ok1(equals(p2.y, 3));

  // test add()
  p2.Add(p3);
  ok1(equals(p2.x, 3));
  ok1(equals(p2.y, 13));

  // test rotate()
  p2.Rotate(Angle::Degrees(fixed(-90)));
  ok1(equals(p2.x, 13));
  ok1(equals(p2.y, -3));

  p2.Rotate(Angle::Degrees(fixed(45)));
  p2.Rotate(Angle::Degrees(fixed(45)));
  ok1(equals(p2.x, 3));
  ok1(equals(p2.y, 13));

  // test d()
  ok1(equals(p2.Distance(p3), 3));
  ok1(equals(p3.Distance(p2), 3));

  // test mag_sq()
  ok1(equals(p1.MagnitudeSquared(), 2));
  ok1(equals(p2.MagnitudeSquared(), 178));
  ok1(equals(p3.MagnitudeSquared(), 109));

  // test mag()
  ok1(equals(p1.Magnitude(), 1.4142135623730950488016887242097));
  ok1(equals(p2.Magnitude(), 13.341664064126333712489436272508));
  ok1(equals(p3.Magnitude(), 10.440306508910550179757754022548));

  // test dot()
  ok1(equals(p1.DotProduct(p2), 16));
  ok1(equals(p2.DotProduct(p1), 16));
  ok1(equals(p1.DotProduct(p3), 13));
  ok1(equals(p3.DotProduct(p1), 13));
  ok1(equals(p2.DotProduct(p3), 139));
  ok1(equals(p3.DotProduct(p2), 139));

  // test ==
  ok1(p1 == p1);
  ok1(p2 == p2);
  ok1(p3 == p3);
  /*
  // Test #2 fails due to floating point inaccuracies
  ok1(p1 == FlatPoint(fixed_one, fixed_one));
  ok1(p2 == FlatPoint(fixed(3), fixed(13)));
  ok1(p3 == FlatPoint(fixed(3), fixed_ten));
  */

  // test *
  p2 = p3 * fixed(1.5);
  ok1(equals(p2.x, 4.5));
  ok1(equals(p2.y, 15));

  // test +
  p2 = p1 + p3;
  ok1(equals(p2.x, 4));
  ok1(equals(p2.y, 11));

  // test +=
  p2 += p1;
  ok1(equals(p2.x, 5));
  ok1(equals(p2.y, 12));

  // test -
  p2 = p3 - p1;
  ok1(equals(p2.x, 2));
  ok1(equals(p2.y, 9));

  return exit_status();
}
Example #5
0
void
DigitEntry::SetDigits(fixed degrees, CoordinateFormat format, bool isLatitude)
{
  // Calculate half the last digit so that we round to the nearest
  fixed roundingAdjustment = fixed(0);
  switch (format) {
  case CoordinateFormat::DD_DDDDD:
    roundingAdjustment = fixed(0.5 * (1.0 / 100000));
    break;

  case CoordinateFormat::DDMM_MMM:
    roundingAdjustment = fixed(0.5 * ( (1.0/60) / 1000));
    break;

  case CoordinateFormat::DDMMSS_S:
    roundingAdjustment = fixed(0.5 * ( (1.0/3600) / 10));
    break;

  case CoordinateFormat::DDMMSS:
    roundingAdjustment = fixed(0.5 * ( (1.0/3600) / 1));
    break;

  default:
  case CoordinateFormat::UTM:
    /// \todo support UTM format
    break;
  }

  // Apply adjustment so we show rounding rather than truncation
  degrees += roundingAdjustment;

  // Handle whole degrees
  const unsigned i_degrees = std::min(unsigned(degrees), isLatitude ? 90u : 180u);
  columns[1].value = i_degrees / 10;
  columns[2].value = i_degrees % 10;

  // Set columns according to specified format
  // Work out to an more decimal places than required for rounding
  /// \todo support UTM format
  switch (format) {
  case CoordinateFormat::DD_DDDDD: {
    // Check format is xxx.99999°
    assert(length == 10);
    assert(columns[4].type == Column::Type::DIGIT);
    assert(columns[5].type == Column::Type::DIGIT);
    assert(columns[6].type == Column::Type::DIGIT);
    assert(columns[7].type == Column::Type::DIGIT);
    assert(columns[8].type == Column::Type::DIGIT);
    // Set fractional degree columns
    unsigned remainder = unsigned(degrees * 1000000u) % 1000000u;
    columns[4].value = remainder / 100000u;  remainder %= 100000u;
    columns[5].value = remainder / 10000u;   remainder %= 10000u;
    columns[6].value = remainder / 1000u;    remainder %= 1000u;
    columns[7].value = remainder / 100u;     remainder %= 100u;
    columns[8].value = remainder / 10u;
    break;
  }

  case CoordinateFormat::DDMM_MMM: {
    // Check format is xxx°59.999"
    assert(length == 11);
    assert(columns[4].type == Column::Type::DIGIT6);
    assert(columns[5].type == Column::Type::DIGIT);
    assert(columns[7].type == Column::Type::DIGIT);
    assert(columns[8].type == Column::Type::DIGIT);
    assert(columns[9].type == Column::Type::DIGIT);
    // Set minute columns
    unsigned remainder = unsigned(degrees * 600000u) % 600000u;
    columns[4].value = remainder / 100000u; remainder %= 100000u;
    columns[5].value = remainder / 10000u;  remainder %= 10000u;
    columns[7].value = remainder / 1000u;   remainder %= 1000u;
    columns[8].value = remainder / 100u;    remainder %= 100u;
    columns[9].value = remainder / 10u;
    break;
  }

  case CoordinateFormat::DDMMSS_S: {
    // Check format is xxx°59'59.9"
    assert(length == 12);
    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);
    assert(columns[10].type == Column::Type::DIGIT);
    // Set minute and second columns
    const unsigned full_hunseconds = unsigned(degrees * 360000u) % 360000u;
    const unsigned minutes = std::min(full_hunseconds / 6000u, 59u);
    columns[4].value = minutes / 10;
    columns[5].value = minutes % 10;
    unsigned remainder = full_hunseconds % 6000u;
    columns[7].value = remainder / 1000;  remainder %= 1000;
    columns[8].value = remainder / 100;   remainder %= 100;
    columns[10].value = remainder / 10;   remainder %= 10;
    break;
  }

  case CoordinateFormat::UTM: /// \todo support UTM format
  case CoordinateFormat::DDMMSS: {
    // Check format is xxx°59'59"
    assert(length == 10);
    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);
    // Set minute and second columns
    const unsigned full_hunseconds = unsigned(degrees * 360000u) % 360000u;
    const unsigned minutes = std::min(full_hunseconds / 6000u, 59u);
    columns[4].value = minutes / 10;
    columns[5].value = minutes % 10;
    unsigned remainder = full_hunseconds % 6000u;
    columns[7].value = remainder / 1000;  remainder %= 1000;
    columns[8].value = remainder / 100;   remainder %= 100;
    break;
  }
  }
}
Example #6
0
static void GetValues(void) {
  WndProperty* wp;
  bool sign = false;
  int dd = 0;
  double num=0, mm = 0, ss = 0; // mm,ss are numerators (division) so don't want to lose decimals

  sign = GetFormValueInteger(*wf, _T("prpLongitudeSign")) == 1;
  dd = GetFormValueInteger(*wf, _T("prpLongitudeD"));

  switch (Units::GetCoordinateFormat()) {
  case 0: // ("DDMMSS");
  case 1: // ("DDMMSS.ss");
    mm = GetFormValueInteger(*wf, _T("prpLongitudeM"));
    ss = GetFormValueInteger(*wf, _T("prpLongitudeS"));
    num = dd+mm/60.0+ss/3600.0;
    break;
  case 2: // ("DDMM.mmm");
    mm = GetFormValueInteger(*wf, _T("prpLongitudeM"));
    ss = GetFormValueInteger(*wf, _T("prpLongitudemmm"));
    num = dd+(mm+ss/1000.0)/60.0;
    break;
  case 3: // ("DD.dddd");
    mm = GetFormValueInteger(*wf, _T("prpLongitudeDDDD"));
    num = dd+mm/10000;
    break;
  }
  if (!sign) {
    num = -num;
  }

  global_wpt->Location.Longitude = Angle::degrees(fixed(num));

  sign = GetFormValueInteger(*wf, _T("prpLatitudeSign")) == 1;
  dd = GetFormValueInteger(*wf, _T("prpLatitudeD"));

  switch (Units::GetCoordinateFormat()) {
  case 0: // ("DDMMSS");
  case 1: // ("DDMMSS.ss");
    mm = GetFormValueInteger(*wf, _T("prpLatitudeM"));
    ss = GetFormValueInteger(*wf, _T("prpLatitudeS"));
    num = dd+mm/60.0+ss/3600.0;
    break;
  case 2: // ("DDMM.mmm");
    mm = GetFormValueInteger(*wf, _T("prpLatitudeM"));
    ss = GetFormValueInteger(*wf, _T("prpLatitudemmm"));
    num = dd+(mm+ss/1000.0)/60.0;
    break;
  case 3: // ("DD.dddd");
    mm = GetFormValueInteger(*wf, _T("prpLatitudeDDDD"));
    num = dd+mm/10000;
    break;
  }
  if (!sign) {
    num = -num;
  }

  global_wpt->Location.Latitude = Angle::degrees(fixed(num));

  ss = GetFormValueInteger(*wf, _T("prpAltitude"));
  global_wpt->Altitude = ss == 0 && terrain != NULL
    ? fixed(WayPointFile::AltitudeFromTerrain(global_wpt->Location, *terrain))
    : Units::ToSysUnit(fixed(ss), Units::AltitudeUnit);

  wp = (WndProperty*)wf->FindByName(_T("prpFlags"));
  if (wp) {
    int myflag = wp->GetDataField()->GetAsInteger();
    switch(myflag) {
    case 1:
      global_wpt->Flags.TurnPoint = true;
      global_wpt->Flags.Airport = true;
      break;
    case 2:
      global_wpt->Flags.LandPoint = true;
      break;
    default:
      global_wpt->Flags.TurnPoint = true;
      global_wpt->Flags.Airport = false;
      global_wpt->Flags.LandPoint = false;
    };
  }
}
Example #7
0
/**
 * Returns the weighted mean windvector over the stored values, or 0
 * if no valid vector could be calculated (for instance: too little or
 * too low quality data).
 */
const Vector
WindMeasurementList::getWind(unsigned now, fixed alt, bool &found) const
{
  //relative weight for each factor
  static constexpr unsigned REL_FACTOR_QUALITY = 100;
  static constexpr unsigned REL_FACTOR_ALTITUDE = 100;
  static constexpr unsigned REL_FACTOR_TIME = 200;

  static constexpr unsigned altRange = 1000;
  static constexpr unsigned timeRange = 3600; // one hour

  static constexpr fixed k(0.0025);

  unsigned int total_quality = 0;

  Vector result(fixed(0), fixed(0));

  found = false;

  fixed override_time(1.1);
  bool overridden = false;

  for (unsigned i = 0; i < measurements.size(); i++) {
    const WindMeasurement &m = measurements[i];

    if (now < m.time)
      /* time warp - usually, this shouldn't happen, because time
         warps "should" be filtered at a higher level already */
      continue;

    fixed timediff = fixed(now - m.time) / timeRange;
    if (timediff >= fixed(1))
      continue;

    fixed altdiff = fabs(alt - m.altitude) / altRange;
    if (altdiff >= fixed(1))
      continue;

    // measurement quality
    unsigned int q_quality = std::min(5u, m.quality) * REL_FACTOR_QUALITY / 5u;

    // factor in altitude difference between current altitude and
    // measurement.  Maximum alt difference is 1000 m.
    unsigned int a_quality =
      uround(((fixed(2) / (sqr(altdiff) + fixed(1))) - fixed(1))
             * REL_FACTOR_ALTITUDE);

    // factor in timedifference. Maximum difference is 1 hours.
    unsigned int t_quality =
      uround(k * (fixed(1) - timediff) / (sqr(timediff) + k)
             * REL_FACTOR_TIME);

    if (m.quality == 6) {
      if (timediff < override_time) {
        // over-ride happened, so re-set accumulator
        override_time = timediff;
        total_quality = 0;
        result.x = fixed(0);
        result.y = fixed(0);
        overridden = true;
      } else {
        // this isn't the latest over-ride or obtained fix, so ignore
        continue;
      }
    } else {
      if (timediff < override_time) {
        // a more recent fix was obtained than the over-ride, so start using
        // that one
        override_time = timediff;
        if (overridden) {
          // re-set accumulators
          overridden = false;
          total_quality = 0;
          result.x = fixed(0);
          result.y = fixed(0);
        }
      }
    }

    unsigned int quality = q_quality * (a_quality * t_quality);
    result.x += m.vector.x * quality;
    result.y += m.vector.y * quality;
    total_quality += quality;
  }

  if (total_quality > 0) {
    found = true;
    result = Vector(result.x / (int)total_quality,
                    result.y / (int)total_quality);
  }

  return result;
}
Example #8
0
static void
ReadAltitude(const TCHAR *Text_, AIRSPACE_ALT *Alt)
{
  TCHAR Text[128];
  bool fHasUnit = false;

  _tcsncpy(Text, Text_, sizeof(Text) / sizeof(Text[0]));
  Text[sizeof(Text) / sizeof(Text[0]) - 1] = '\0';

  _tcsupr(Text);

  Alt->Altitude = fixed_zero;
  Alt->FL = fixed_zero;
  Alt->AGL = fixed_zero;
  Alt->Base = abUndef;

  const TCHAR *p = Text;
  while (true) {
    while (*p == _T(' '))
      ++p;

    if (_istdigit(*p)) {
      TCHAR *endptr;
      fixed d = fixed(_tcstod(p, &endptr));

      if (Alt->Base == abFL)
        Alt->FL = d;
      else if (Alt->Base == abAGL)
        Alt->AGL = d;
      else
        Alt->Altitude = d;

      p = endptr;
    } else if (_tcsncmp(p, _T("GND"), 3) == 0) {
      // JMW support XXXGND as valid, equivalent to XXXAGL
      Alt->Base = abAGL;
      if (Alt->Altitude > fixed_zero) {
        Alt->AGL = Alt->Altitude;
        Alt->Altitude = fixed_zero;
      } else {
        Alt->FL = fixed_zero;
        Alt->Altitude = fixed_zero;
        Alt->AGL = fixed_minus_one;
        fHasUnit = true;
      }

      p += 3;
    } else if (_tcsncmp(p, _T("SFC"), 3) == 0) {
      Alt->Base = abAGL;
      Alt->FL = fixed_zero;
      Alt->Altitude = fixed_zero;
      Alt->AGL = fixed_minus_one;
      fHasUnit = true;

      p += 3;
    } else if (_tcsncmp(p, _T("FL"), 2) == 0) {
      // this parses "FL=150" and "FL150"
      Alt->Base = abFL;
      fHasUnit = true;

      p += 2;
    } else if (*p == _T('F')) {
      Alt->Altitude = Units::ToSysUnit(Alt->Altitude, unFeet);
      fHasUnit = true;

      ++p;
      if (*p == _T('T'))
        ++p;
    } else if (_tcsncmp(p, _T("MSL"), 3) == 0) {
      Alt->Base = abMSL;

      p += 3;
    } else if (*p == _T('M')) {
      // JMW must scan for MSL before scanning for M
      fHasUnit = true;

      ++p;
    } else if (_tcsncmp(p, _T("AGL"), 3) == 0) {
      Alt->Base = abAGL;
      Alt->AGL = Alt->Altitude;
      Alt->Altitude = fixed_zero;

      p += 3;
    } else if (_tcsncmp(p, _T("STD"), 3) == 0) {
      if (Alt->Base != abUndef) {
        // warning! multiple base tags
      }
      Alt->Base = abFL;
      Alt->FL = Units::ToUserUnit(Alt->Altitude, unFlightLevel);

      p += 3;
    } else if (_tcsncmp(p, _T("UNL"), 3) == 0) {
      // JMW added Unlimited (used by WGC2008)
      Alt->Base = abMSL;
      Alt->AGL = fixed_minus_one;
      Alt->Altitude = fixed(50000);

      p += 3;
    } else if (*p == _T('\0'))
      break;
    else
      ++p;
  }

  if (!fHasUnit && (Alt->Base != abFL)) {
    // ToDo warning! no unit defined use feet or user alt unit
    // Alt->Altitude = Units::ToSysAltitude(Alt->Altitude);
    Alt->Altitude = Units::ToSysUnit(Alt->Altitude, unFeet);
    Alt->AGL = Units::ToSysUnit(Alt->AGL, unFeet);
  }

  if (Alt->Base == abUndef)
    // ToDo warning! no base defined use MSL
    Alt->Base = abMSL;
}
Example #9
0
static bool
ReadCoords(const TCHAR *Text, GeoPoint &point)
{
  // Format: 53:20:41 N 010:24:41 E

  TCHAR *Stop;

  // ToDo, add more error checking and making it more tolerant/robust

  long deg = _tcstol(Text, &Stop, 10);
  if ((Text == Stop) || (*Stop == '\0'))
    return false;

  Stop++;
  long min = _tcstol(Stop, &Stop, 10);
  if (*Stop == '\0')
    return false;

  long sec = 0;
  if (*Stop == ':') {
    Stop++;
    if (*Stop == '\0')
      return false;

    sec = _tcstol(Stop, &Stop, 10);
    if (sec < 0 || sec >= 60) {
      // ToDo
    }
  }

  point.Latitude = Angle::dms(fixed(deg), fixed(min), fixed(sec));

  if (*Stop == ' ')
    Stop++;

  if (*Stop == '\0')
    return false;

  if ((*Stop == 'S') || (*Stop == 's'))
    point.Latitude.flip();

  Stop++;
  if (*Stop == '\0')
    return false;

  deg = _tcstol(Stop, &Stop, 10);
  Stop++;
  min = _tcstol(Stop, &Stop, 10);
  if (*Stop == ':') {
    Stop++;
    if (*Stop == '\0')
      return false;

    sec = _tcstol(Stop, &Stop, 10);
  }

  point.Longitude = Angle::dms(fixed(deg), fixed(min), fixed(sec));

  if (*Stop == ' ')
    Stop++;

  if (*Stop == '\0')
    return false;

  if ((*Stop == 'W') || (*Stop == 'w'))
    point.Longitude.flip();
  point.Longitude = point.Longitude.as_bearing();
  return true;
}
Example #10
0
 void set_default_location(const GeoPoint& default_location) {
   location_start = default_location;
   location_previous = default_location;
   location_previous.Latitude-= Angle::degrees(fixed(1.0));
 }
double SlaveBoundaryVertices::loop_over_mesh( Mesh* mesh, 
                                              MeshDomain* domain, 
                                              const Settings* settings,
                                              MsqError& err )
{
  if (settings->get_slaved_ho_node_mode() != Settings::SLAVE_CALCULATED) {
    MSQ_SETERR(err)("Request to calculate higher-order node slaved status "
                    "when Settings::get_get_slaved_ho_node_mode() "
                    "!= SLAVE_CALCUALTED", MsqError::INVALID_STATE);
    return 0.0;
  }
  
    // If user said that we should treat fixed vertices as the
    // boundary, but specified that fixed vertices are defined
    // by the dimension of their geometric domain, then just
    // do distance from the geometric domain.
  int dim = this->domainDoF;
  if (dim >= 4 && settings->get_fixed_vertex_mode() != Settings::FIXED_FLAG)
    dim = settings->get_fixed_vertex_mode();
  
    // Create a map to contain vertex depth.  Intiliaze all to 
    // elemDepth+1.
  std::vector<Mesh::VertexHandle> vertices;
  mesh->get_all_vertices( vertices, err );  MSQ_ERRZERO(err);
  if (vertices.empty())
    return 0.0;
  std::sort( vertices.begin(), vertices.end() );
  std::vector<unsigned short> depth( vertices.size(), elemDepth+1 );
  BoolArr fixed( vertices.size() );
  mesh->vertices_get_fixed_flag( &vertices[0], fixed.mArray, vertices.size(), err );
  MSQ_ERRZERO(err);
  
    // Initialize map with boundary vertices.
  if (dim >= 4) {
    for(size_t i = 0; i < vertices.size(); ++i)
      if (fixed[i])
        depth[i] = 0;
  }
  else {
    if (!domain) {
      MSQ_SETERR(err)("Request to calculate higher-order node slaved status "
                      "by distance from bounding domain without a domain.",
                      MsqError::INVALID_STATE);
      return 0.0;
    }
    
    std::vector<unsigned short> dof( vertices.size() );
    domain->domain_DoF( &vertices[0], &dof[0], vertices.size(), err ); MSQ_ERRZERO(err);
    for (size_t i = 0; i < vertices.size(); ++i)
      if (dof[i] <= dim)
        depth[i] = 0;
  }
  
    // Now iterate over elements repeatedly until we've found all of the 
    // elements near the boundary.  This could be done much more efficiently
    // using vertex-to-element adjacencies, but it is to common for 
    // applications not to implement that unless doing relaxation smoothing.
    // This is O(elemDepth * elements.size() * ln(vertices.size()));
  std::vector<Mesh::ElementHandle> elements;
  std::vector<Mesh::ElementHandle>::const_iterator j, k;
  std::vector<Mesh::VertexHandle> conn;
  std::vector<size_t> junk(2);
  mesh->get_all_elements( elements, err );  MSQ_ERRZERO(err);
  if (elements.empty())
    return 0.0;
  bool some_changed;
  do {
    some_changed = false;
    for (j = elements.begin(); j != elements.end(); ++j) {
      conn.clear(); junk.clear();
      mesh->elements_get_attached_vertices( &*j, 1, conn, junk, err ); MSQ_ERRZERO(err);
      unsigned short elem_depth = elemDepth+1;
      for (k = conn.begin(); k != conn.end(); ++k) {
        size_t i = std::lower_bound( vertices.begin(), vertices.end(), *k ) - vertices.begin();
        if (i == vertices.size()) {
          MSQ_SETERR(err)("Invalid vertex handle in element connectivity list.", 
                          MsqError::INVALID_MESH);
          return 0.0;
        }
        if (depth[i] < elem_depth)
          elem_depth = depth[i];
      }
      if (elem_depth == elemDepth+1)
        continue;
      
      ++elem_depth;
      for (k = conn.begin(); k != conn.end(); ++k) {
        size_t i = std::lower_bound( vertices.begin(), vertices.end(), *k ) - vertices.begin();
        if (depth[i] > elem_depth) {
          depth[i] = elem_depth;
          some_changed = true;
        }
      }
    } // for(elements)
  } while (some_changed);
  
    // Now remove any corner vertices from the slaved set
  std::vector<Mesh::VertexHandle>::iterator p;
  std::vector<EntityTopology> types(elements.size());
  mesh->elements_get_topologies( &elements[0], &types[0], elements.size(), err ); MSQ_ERRZERO(err);
  for (j = elements.begin(); j != elements.end(); ++j) {
    const unsigned corners = TopologyInfo::corners(types[j-elements.begin()]);
    conn.clear(); junk.clear();
    mesh->elements_get_attached_vertices( &*j, 1, conn, junk, err ); MSQ_ERRZERO(err);
    for (unsigned i = 0; i < corners; ++i) {
      p = std::lower_bound( vertices.begin(), vertices.end(), conn[i] );
      depth[p-vertices.begin()] = 0;
    }
  }
  
    // Now mark all vertices *not* within specified depth as slave vertices.
  std::vector<unsigned char> bytes( vertices.size() );
  mesh->vertices_get_byte( &vertices[0], &bytes[0], vertices.size(), err ); MSQ_ERRZERO(err);
  for (size_t i = 0; i < vertices.size(); ++i) {
    if (depth[i] <= elemDepth || fixed[i])
      bytes[i] &= ~MsqVertex::MSQ_DEPENDENT;
    else
      bytes[i] |= MsqVertex::MSQ_DEPENDENT;
  }
  
  mesh->vertices_set_byte( &vertices[0], &bytes[0], vertices.size(), err ); MSQ_ERRZERO(err);
  return 0.0;
}
Example #12
0
/** 
 * Initialises waypoints with random and non-random waypoints
 * for testing
 *
 * @param waypoints waypoints class to add waypoints to
 */
bool setup_waypoints(Waypoints &waypoints, const unsigned n) 
{

  Waypoint wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
                                          Angle::Degrees(fixed_zero)));
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed(0.25);
  waypoints.Append(wp);

  wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero), 
                                 Angle::Degrees(fixed_one)));
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed(0.25);
  waypoints.Append(wp);

  wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one), 
                                 Angle::Degrees(fixed_one)));
  wp.name = _T("Hello");
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed_half;
  waypoints.Append(wp);

  wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(0.8)), 
                                 Angle::Degrees(fixed(0.5))));
  wp.name = _T("Unk");
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed(0.25);
  waypoints.Append(wp);

  wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one), 
                                 Angle::Degrees(fixed_zero)));
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed(0.25);
  waypoints.Append(wp);

  wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero), 
                                 Angle::Degrees(fixed(0.23))));
  wp.type = Waypoint::Type::AIRFIELD;
  wp.altitude = fixed(0.25);
  waypoints.Append(wp);

  for (unsigned i=0; i<(unsigned)std::max((int)n-6,0); i++) {
    int x = rand()%1200-100;
    int y = rand()%1200-100;
    double z = rand()% std::max(terrain_height,1);
    wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(x/1000.0)), 
                                   Angle::Degrees(fixed(y/1000.0))));
    wp.type = Waypoint::Type::NORMAL;
    wp.altitude = fixed(z);
    waypoints.Append(wp);
  }
  waypoints.Optimise();

  if (verbose) {
    std::ofstream fin("results/res-wp-in.txt");
    for (unsigned i=1; i<=waypoints.size(); i++) {
      const Waypoint *wp = waypoints.LookupId(i);
      if (wp != NULL)
        fin << *wp;
    }
  }
  return true;
}
Example #13
0
static bool
ParseRPYL(NMEAInputLine &line, NMEAInfo &info)
{
  // $RPYL,Roll,Pitch,MagnHeading,SideSlip,YawRate,G,errorcode,

  int roll;
  if (!line.ReadChecked(roll)) return false;

  int pitch;
  if (!line.ReadChecked(pitch)) return false;

  int heading;
  if (!line.ReadChecked(heading)) return false;

  int sideslip;
  if (!line.ReadChecked(sideslip)) return false;

  int yawrate;
  if (!line.ReadChecked(yawrate)) return false;

  int G;
  if (!line.ReadChecked(G)) return false;

  int errorcode;
  if (!line.ReadChecked(errorcode)) return false;

  /* Error bits:
   *   0: Roll gyro test failed  
   *   1: Roll gyro test failed 
   *   2: Roll gyro test failed 
   *   3: Acc X test failed 
   *   4: Acc Y test failed 
   *   5: Acc Z test failed 
   *   6: Watchdog test failed
   *   7: Ram test failed
   *   8: EEPROM access test failed
   *   9: EEPROM checksum test failed
   *  10: Flash checksum test failed
   *  11: Low voltage error
   *  12: High temperature error (>60 C)
   *  13: Inconsistent roll data between gyro and acc.
   *  14: Inconsistent pitch data between gyro and acc.
   *  15: Inconsistent yaw data between gyro and acc.
   */
  if (errorcode && !error_reported) {
    ErrorMessage(errorcode);
    error_reported = true;
  }

  info.attitude.bank_angle_available.Update(info.clock);
  info.attitude.bank_angle = Angle::Degrees(fixed(roll) / 10);

  info.attitude.pitch_angle_available.Update(info.clock);
  info.attitude.pitch_angle = Angle::Degrees(fixed(pitch) / 10);

  info.attitude.heading_available.Update(info.clock);
  info.attitude.heading = Angle::Degrees(fixed(heading) / 10);

  info.acceleration.ProvideGLoad(fixed(G) / 1000, true);

  return true;
}
Example #14
0
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
}
*/

#ifndef XCSOAR_GLIDE_RATIO_HPP
#define XCSOAR_GLIDE_RATIO_HPP

#include "Util/NonCopyable.hpp"
#include "Math/fixed.hpp"
#include "Compiler.h"

static const fixed INVALID_GR = fixed(999);

struct ComputerSettings;

class GlideRatioCalculator : private NonCopyable {
  struct Record {
    unsigned distance;
    int altitude;
  };

  /**
   * Rotary array with a predefined max capacity.
   */
  Record records[180];

  unsigned totaldistance;
Example #15
0
void
AutopilotParameters::realistic()
{
  bearing_noise= fixed(20.0);
  turn_speed= fixed(12.0);
}
Example #16
0
static bool
ParseLine(Airspaces &airspace_database, const TCHAR *line,
          TempAirspaceType &temp_area)
{
  const TCHAR *value;

  // Only return expected lines
  switch (line[0]) {
  case _T('A'):
  case _T('a'):
    switch (line[1]) {
    case _T('C'):
    case _T('c'):
      value = value_after_space(line + 2);
      if (value == NULL)
        break;

      if (!temp_area.Waiting)
        temp_area.AddPolygon(airspace_database);

      temp_area.reset();

      temp_area.Type = ParseType(value);
      temp_area.Waiting = false;
      break;

    case _T('N'):
    case _T('n'):
      value = value_after_space(line + 2);
      if (value != NULL)
        temp_area.Name = value;
      break;

    case _T('L'):
    case _T('l'):
      value = value_after_space(line + 2);
      if (value != NULL)
        ReadAltitude(value, &temp_area.Base);
      break;

    case _T('H'):
    case _T('h'):
      value = value_after_space(line + 2);
      if (value != NULL)
        ReadAltitude(value, &temp_area.Top);
      break;

    default:
      return true;
    }

    break;

  case _T('D'):
  case _T('d'):
    switch (line[1]) {
    case _T('A'):
    case _T('a'):
      CalculateSector(line, temp_area);
      break;

    case _T('B'):
    case _T('b'):
      CalculateArc(line, temp_area);
      break;

    case _T('C'):
    case _T('c'):
      temp_area.Radius = Units::ToSysUnit(fixed(_tcstod(&line[2], NULL)),
                                          unNauticalMiles);
      temp_area.AddCircle(airspace_database);
      temp_area.reset();
      break;

    case _T('P'):
    case _T('p'):
      value = value_after_space(line + 2);
      if (value == NULL)
        break;

    {
      GeoPoint TempPoint;

      if (!ReadCoords(value, TempPoint))
        return false;

      temp_area.points.push_back(TempPoint);
      break;
    }
    default:
      return true;
    }

    break;

  case _T('V'):
  case _T('v'):
    // Need to set these while in count mode, or DB/DA will crash
    if (string_after_prefix_ci(&line[2], _T("X="))) {
      if (!ReadCoords(&line[4],temp_area.Center))
        return false;
    } else if (string_after_prefix_ci(&line[2], _T("D=-"))) {
      temp_area.Rotation = -1;
    } else if (string_after_prefix_ci(&line[2], _T("D=+"))) {
      temp_area.Rotation = +1;
    }
  }

  return true;
}
Example #17
0
static void SetValues(void) {
  WndProperty* wp;
  bool sign;
  int dd,mm,ss;

  Units::LongitudeToDMS(global_wpt->Location.Longitude,
			&dd, &mm, &ss, &sign);

  wp = (WndProperty*)wf->FindByName(_T("prpLongitudeSign"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    dfe->addEnumText((_T("W")));
    dfe->addEnumText((_T("E")));
    dfe->Set(sign);
    wp->RefreshDisplay();
  }

  LoadFormProperty(*wf, _T("prpLongitudeD"), dd);

  switch (Units::GetCoordinateFormat()) {
  case 0: // ("DDMMSS");
  case 1: // ("DDMMSS.ss");
    LoadFormProperty(*wf, _T("prpLongitudeM"), mm);
    LoadFormProperty(*wf, _T("prpLongitudeS"), ss);
    break;
  case 2: // ("DDMM.mmm");
    LoadFormProperty(*wf, _T("prpLongitudeM"), mm);
    LoadFormProperty(*wf, _T("prpLongitudemmm"), 1000 * fixed(ss) / 60);
    break;
  case 3: // ("DD.dddd");
    LoadFormProperty(*wf, _T("prpLongitudeDDDD"),
                     10000 * (fixed)(mm + ss) / 3600);
    break;
  }

  Units::LatitudeToDMS(global_wpt->Location.Latitude,
		       &dd, &mm, &ss, &sign);

  LoadFormProperty(*wf, _T("prpLatitudeD"), dd);

  wp = (WndProperty*)wf->FindByName(_T("prpLatitudeSign"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    dfe->addEnumText((_T("S")));
    dfe->addEnumText((_T("N")));
    dfe->Set(sign);
    wp->RefreshDisplay();
  }
  wp = (WndProperty*)wf->FindByName(_T("prpLatitudeD"));
  if (wp) {
    wp->GetDataField()->SetAsInteger(dd);
    wp->RefreshDisplay();
  }

  switch (Units::GetCoordinateFormat()) {
  case 0: // ("DDMMSS");
  case 1: // ("DDMMSS.ss");
    LoadFormProperty(*wf, _T("prpLatitudeM"), mm);
    LoadFormProperty(*wf, _T("prpLatitudeS"), ss);
    break;
  case 2: // ("DDMM.mmm");
    LoadFormProperty(*wf, _T("prpLatitudeM"), mm);
    LoadFormProperty(*wf, _T("prpLatitudemmm"), 1000 * fixed(ss) / 60);
    break;
  case 3: // ("DD.dddd");
    LoadFormProperty(*wf, _T("prpLatitudeDDDD"),
                     10000 * (fixed)(mm + ss) / 3600);
    break;
  }

  wp = (WndProperty*)wf->FindByName(_T("prpAltitude"));
  if (wp) {
    wp->GetDataField()->SetAsInteger(iround(
        Units::ToUserUnit(global_wpt->Altitude, Units::AltitudeUnit)));
    wp->GetDataField()->SetUnits(Units::GetAltitudeName());
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(_T("prpFlags"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();

    dfe->addEnumText(_T("Turnpoint"));
    dfe->addEnumText(_T("Airport"));
    dfe->addEnumText(_T("Landpoint"));

    if (global_wpt->Flags.Airport) {
      dfe->Set(1);
    } else if (global_wpt->Flags.LandPoint) {
      dfe->Set(2);
    } else {
      dfe->Set(0);
    }

    wp->RefreshDisplay();
  }
}
Example #18
0
 /**
  * Constructor.
  *
  * @param _polar Glide polar to optimise
  * @param vmin Minimum speed to search (m/s)
  * @param vmax Maximum speed to search (m/s)
  *
  * @return Initialised object (no search yet)
  */
 GlidePolarVopt(const GlidePolar &_polar, const fixed vmin, const fixed vmax):
   ZeroFinder(vmin, vmax, fixed(TOLERANCE_POLAR_BESTLD)),
   polar(_polar)
 {
 }
Example #19
0
void
MapWindowProjection::SetMapScale(const fixed x)
{
  SetScale(fixed(GetMapResolutionFactor()) / LimitMapScale(x));
}
Example #20
0
 /**
  * Constructor.
  *
  * @param _polar Glide polar to optimise
  * @param vmax Maximum speed to search (m/s)
  *
  * @return Initialised object (no search yet)
  */
 GlidePolarMinSink(const GlidePolar &_polar, const fixed vmax):
   ZeroFinder(fixed(1), vmax, fixed(TOLERANCE_POLAR_MINSINK)),
   polar(_polar)
 {
 }
Example #21
0
void
WaypointInfoWidget::Prepare(ContainerWindow &parent, const PixelRect &rc)
{
  RowFormWidget::Prepare(parent, rc);

  const MoreData &basic = CommonInterface::Basic();
  const DerivedInfo &calculated = CommonInterface::Calculated();
  const ComputerSettings &settings = CommonInterface::GetComputerSettings();

  StaticString<64> buffer;

  if (!waypoint.comment.empty())
    AddMultiLine(waypoint.comment.c_str());

  if (waypoint.radio_frequency.IsDefined() &&
      waypoint.radio_frequency.Format(buffer.buffer(),
                                      buffer.MAX_SIZE) != nullptr) {
    buffer += _T(" MHz");
    AddReadOnly(_("Radio frequency"), nullptr, buffer);
  }

  if (waypoint.runway.IsDirectionDefined())
    buffer.UnsafeFormat(_T("%02u"), waypoint.runway.GetDirectionName());
  else
    buffer.clear();

  if (waypoint.runway.IsLengthDefined()) {
    if (!buffer.empty())
      buffer += _T("; ");

    TCHAR length_buffer[16];
    FormatSmallUserDistance(length_buffer,
                                   fixed(waypoint.runway.GetLength()));
    buffer += length_buffer;
  }

  if (!buffer.empty())
    AddReadOnly(_("Runway"), nullptr, buffer);

  if (FormatGeoPoint(waypoint.location,
                     buffer.buffer(), buffer.MAX_SIZE) != nullptr)
    AddReadOnly(_("Location"), nullptr, buffer);

  FormatUserAltitude(waypoint.elevation,
                            buffer.buffer(), buffer.MAX_SIZE);
  AddReadOnly(_("Elevation"), nullptr, buffer);

  if (basic.time_available && basic.date_available) {
    const SunEphemeris::Result sun =
      SunEphemeris::CalcSunTimes(waypoint.location, basic.date_time_utc,
                                 settings.utc_offset);

    const BrokenTime sunrise = BreakHourOfDay(sun.time_of_sunrise);
    const BrokenTime sunset = BreakHourOfDay(sun.time_of_sunset);

    buffer.UnsafeFormat(_T("%02u:%02u - %02u:%02u"),
                        sunrise.hour, sunrise.minute,
                        sunset.hour, sunset.minute);
    AddReadOnly(_("Daylight time"), nullptr, buffer);
  }

  if (basic.location_available) {
    const GeoVector vector = basic.location.DistanceBearing(waypoint.location);

    TCHAR distance_buffer[32];
    FormatUserDistanceSmart(vector.distance, distance_buffer,
                                   ARRAY_SIZE(distance_buffer));

    FormatBearing(buffer.buffer(), buffer.MAX_SIZE,
                  vector.bearing, distance_buffer);
    AddReadOnly(_("Bearing and Distance"), nullptr, buffer);
  }

  if (basic.location_available && basic.NavAltitudeAvailable() &&
      settings.polar.glide_polar_task.IsValid()) {
    const GlideState glide_state(basic.location.DistanceBearing(waypoint.location),
                                 waypoint.elevation + settings.task.safety_height_arrival,
                                 basic.nav_altitude,
                                 calculated.GetWindOrZero());

    GlidePolar gp0 = settings.polar.glide_polar_task;
    gp0.SetMC(fixed(0));
    AddGlideResult(_("Alt. diff. MC 0"),
                   MacCready::Solve(settings.task.glide,
                                    gp0, glide_state));

    AddGlideResult(_("Alt. diff. MC safety"),
                   MacCready::Solve(settings.task.glide,
                                    calculated.glide_polar_safety,
                                    glide_state));

    AddGlideResult(_("Alt. diff. MC current"),
                   MacCready::Solve(settings.task.glide,
                                    settings.polar.glide_polar_task,
                                    glide_state));
  }

  if (basic.location_available && basic.NavAltitudeAvailable()) {
    const TaskBehaviour &task_behaviour =
      CommonInterface::GetComputerSettings().task;

    const fixed safety_height = task_behaviour.safety_height_arrival;
    const fixed target_altitude = waypoint.elevation + safety_height;
    const fixed delta_h = basic.nav_altitude - target_altitude;
    if (positive(delta_h)) {
      const fixed distance = basic.location.Distance(waypoint.location);
      const fixed gr = distance / delta_h;
      if (GradientValid(gr)) {
        buffer.UnsafeFormat(_T("%.1f"), (double)gr);
        AddReadOnly(_("Required glide ratio"), nullptr, buffer);
      }
    }
  }
}
Example #22
0
static fixed
FRiskFunction(const fixed x, const fixed k)
{
  return fixed(2) / (fixed(1) + exp(-x * k)) - fixed(1);
}
void 
AirspaceIntersectSort::add(const fixed t, const GeoPoint &p)
{
  if (t >= fixed(0))
    m_q.push(std::make_pair(t, p));
}
Example #24
0
fixed
LineSectorZone::ScoreAdjustment() const
{
  return fixed(0);
}
Example #25
0
Angle
DigitEntry::GetGeoAngle(CoordinateFormat format) const
{

  if (!valid)
    return Angle::FullCircle();

  // The first three columns are common to all formats
  // N99... and E*9...
  assert(columns[0].type == Column::Type::NORTH_SOUTH ||
         columns[0].type == Column::Type::EAST_WEST);
  assert(columns[1].type == Column::Type::DIGIT ||
         columns[1].type == Column::Type::DIGIT19);
  assert(columns[2].type == Column::Type::DIGIT);
  fixed degrees = fixed(columns[1].value * 10 + columns[2].value);

  // Read columns according to specified format
  /// \todo support UTM format
  switch (format) {
  case CoordinateFormat::DD_DDDDD:
    // Check format is E*9.99999°
    assert(length == 10);
    assert(columns[4].type == Column::Type::DIGIT);
    assert(columns[5].type == Column::Type::DIGIT);
    assert(columns[6].type == Column::Type::DIGIT);
    assert(columns[7].type == Column::Type::DIGIT);
    assert(columns[8].type == Column::Type::DIGIT);
    // Read fractional degree columns
    degrees += (columns[4].value * 10000 +
                columns[5].value * 1000 +
                columns[6].value * 100 +
                columns[7].value * 10  +
                columns[8].value        ) * fixed(1 / 100000.);
    break;

  case CoordinateFormat::DDMM_MMM:
    // Check format is E*9°59.999"
    assert(length == 11);
    assert(columns[4].type == Column::Type::DIGIT6);
    assert(columns[5].type == Column::Type::DIGIT);
    assert(columns[7].type == Column::Type::DIGIT);
    assert(columns[8].type == Column::Type::DIGIT);
    assert(columns[9].type == Column::Type::DIGIT);
    // Read minute columns
    degrees += (columns[4].value * 10 + columns[5].value) * fixed(1 / 60.)
      +  (columns[7].value * 100 +
          columns[8].value * 10  +
          columns[9].value        ) * fixed(1 / 60000.);
    break;

  case CoordinateFormat::DDMMSS_S:
    // Check format is E*9°59'59.9"
    assert(length == 12);
    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);
    assert(columns[10].type == Column::Type::DIGIT);
    // Read minute and decimal second columns
    degrees += (columns[4].value * 10 + columns[5].value) * fixed(1 / 60.)
      +  (columns[7].value * 10 + columns[8].value) * fixed(1 / 3600.)
      +  (columns[10].value                       ) * fixed(1 / 36000.);
    break;

  case CoordinateFormat::UTM: /// \todo support UTM format
  case CoordinateFormat::DDMMSS:
    // Check format is E*9°59'59"
    assert(length == 10);
    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);
    // Read minute and second columns
    degrees += (columns[4].value * 10 + columns[5].value) * fixed(1 / 60.)
      +  (columns[7].value * 10 + columns[8].value) * fixed(1 / 3600.);
    break;
  }

  if (columns[0].IsNegative())
    degrees = -degrees;

  return Angle::Degrees(degrees);
}
Example #26
0
extern void
articulate(void)
{
   node *stn, *stnStart;
   int i;
   long cFixed;

   component_list = NULL;
   articulation_list = NULL;
   artlist = NULL;
   fixedlist = NULL;

   /* find articulation points and components */
   colour = 0;
   stnStart = NULL;
   cMaxVisits = 0;
   FOR_EACH_STN(stn, stnlist) {
      if (fixed(stn)) {
	 remove_stn_from_list(&stnlist, stn);
	 add_stn_to_list(&fixedlist, stn);
	 colour++;
	 stn->colour = -colour;
#ifdef DEBUG_ARTIC
	 printf("Putting stn ");
	 print_prefix(stn->name);
	 printf(" on fixedlist\n");
#endif
      } else {
	 cMaxVisits++;
	 stn->colour = 0;
      }
   }
   dirn_stack = osmalloc(cMaxVisits);
   min_stack = osmalloc(cMaxVisits * sizeof(long));

   /* fixedlist can be NULL here if we've had a *solve followed by survey
    * which is all hanging. */
   cFixed = colour;
   while (fixedlist) {
      int c;
      stnStart = fixedlist;
      stn = stnStart;

      /* see if this is a fresh component - it may not be, we may be
       * processing the other way from a fixed point cut-line */
      if (stn->colour < 0) {
#ifdef DEBUG_ARTIC
	 printf("new component\n");
#endif
	 stn->colour = -stn->colour; /* fixed points are negative until we colour from them */
	 cComponents++;

	 /* FIXME: logic to count components isn't the same as the logic
	  * to start a new one - we should start a new one for a fixed point
	  * cut-line (see below) */
	 if (artlist) {
	     component *comp;
	     articulation *art;

	     art = osnew(articulation);
	     art->stnlist = artlist;
	     art->next = articulation_list;
	     articulation_list = art;
	     artlist = NULL;

	     comp = osnew(component);
	     comp->next = component_list;
	     comp->artic = articulation_list;
	     component_list = comp;
	     articulation_list = NULL;
	 }

#ifdef DEBUG_ARTIC
	 print_prefix(stn->name);
	 printf(" [%p] is root of component %ld\n", stn, cComponents);
	 printf(" and colour = %d/%d\n", stn->colour, cFixed);
#endif
      }

      c = 0;
      for (i = 0; i <= 2 && stn->leg[i]; i++) {
	 node *stn2 = stn->leg[i]->l.to;
	 if (stn2->colour < 0) {
	    stn2->colour = -stn2->colour;
	 } else if (stn2->colour == 0) {
	    /* Special case to check if start station is an articulation point
	     * which it is iff we have to colour from it in more than one dirn
	     *
	     * We're looking for articulation legs - these are those where
	     * colouring from here doesn't reach a fixed point (including
	     * stn - the fixed point we've started from)
	     *
	     * FIXME: this is a "fixed point cut-line" case where we could
	     * start a new component.
	     */
	    long col = visit(stn2, reverse_leg_dirn(stn->leg[i]));
#ifdef DEBUG_ARTIC
	    print_prefix(stn->name);
	    printf(" -> ");
	    print_prefix(stn2->name);
	    printf(" col %d cFixed %d\n", col, cFixed);
#endif
	    if (col > cFixed) {
		/* start new articulation - FIXME - overeager */
		articulation *art = osnew(articulation);
		art->stnlist = artlist;
		art->next = articulation_list;
		articulation_list = art;
		artlist = NULL;
		c |= 1 << i;
	    }
	 }
      }

      switch (c) {
       /* had to colour in 2 or 3 directions from start point */
       case 3: case 5: case 6: case 7:
#ifdef DEBUG_ARTIC
	 print_prefix(stn->name);
	 printf(" is a special case start articulation point [%d]\n", c);
#endif
	 for (i = 0; i <= 2 && stn->leg[i]; i++) {
	    if (TSTBIT(c, i)) {
	       /* flag leg as an articulation for loop error reporting */
	       stn->leg[i]->l.reverse |= FLAG_ARTICULATION;
#ifdef DEBUG_ARTIC
	       print_prefix(stn->leg[i]->l.to->name);
	       putnl();
#endif
	       reverse_leg(stn->leg[i])->l.reverse |= FLAG_ARTICULATION;
	    }
	 }
      }

#ifdef DEBUG_ARTIC
      printf("Putting FIXED stn ");
      print_prefix(stn->name);
      printf(" on artlist\n");
#endif
      remove_stn_from_list(&fixedlist, stn);
      add_stn_to_list(&artlist, stn);

      if (stnStart->colour == 1) {
#ifdef DEBUG_ARTIC
	 printf("%ld components\n",cComponents);
#endif
	 break;
      }
   }

   osfree(dirn_stack);
   dirn_stack = NULL;
   osfree(min_stack);
   min_stack = NULL;

   if (artlist) {
      articulation *art = osnew(articulation);
      art->stnlist = artlist;
      art->next = articulation_list;
      articulation_list = art;
      artlist = NULL;
   }
   if (articulation_list) {
      component *comp = osnew(component);
      comp->next = component_list;
      comp->artic = articulation_list;
      component_list = comp;
      articulation_list = NULL;
   }

   if (stnlist) {
      /* Any stations still in stnlist are unfixed, which is means we have
       * one or more hanging surveys.
       *
       * The cause of the problem is pretty likely to be a typo, so run the
       * checks which report errors and warnings about issues which such a
       * typo is likely to result in.
       */
      check_node_stats();

      /* Actually this error is fatal, but we want to list the survey
       * stations which aren't connected, so we report it as an error
       * and die after listing them...
       */
      bool fNotAttached = fFalse;
      error(/*Survey not all connected to fixed stations*/45);
      FOR_EACH_STN(stn, stnlist) {
	 /* Anonymous stations must be at the end of a trailing traverse (since
	  * the same anonymous station can't be referred to more than once),
	  * and trailing traverses have been removed at this point.
	  */
	 SVX_ASSERT(!TSTBIT(stn->name->sflags, SFLAGS_ANON));
	 if (stn->name->ident) {
	    if (!fNotAttached) {
	       fNotAttached = fTrue;
	       puts(msg(/*The following survey stations are not attached to a fixed point:*/71));
	    }
	    puts(sprint_prefix(stn->name));
	 }
      }
      exit(EXIT_FAILURE);
   }
Example #27
0
bool run_flight(TaskManager &task_manager,
                const AutopilotParameters &parms,
                const int n_wind,
                const double speed_factor) 
{
  DirectTaskAccessor ta(task_manager);
  PrintTaskAutoPilot autopilot(parms);
  AircraftSim aircraft;

  autopilot.set_default_location(GeoPoint(Angle::degrees(fixed(1.0)), Angle::degrees(fixed(0.0))));

  unsigned print_counter=0;
  if (n_wind)
    aircraft.set_wind(wind_to_mag(n_wind), wind_to_dir(n_wind));

  autopilot.set_speed_factor(fixed(speed_factor));

  std::ofstream f4("results/res-sample.txt");
  std::ofstream f5("results/res-sample-filtered.txt");

  bool do_print = verbose;
  bool first = true;

  time_elapsed = 0.0;
  time_planned = 1.0;
  time_remaining = 0;
  calc_cruise_efficiency = 1.0;
  calc_effective_mc = 1.0;

  static const fixed fixed_10(10);

  AirspaceAircraftPerformanceGlide perf(task_manager.get_glide_polar());

  if (aircraft_filter)
    aircraft_filter->Reset(aircraft.get_state());

  autopilot.Start(ta);
  aircraft.Start(autopilot.location_start, autopilot.location_previous,
                 parms.start_alt);

  if (airspaces) {
    airspace_warnings =
        new AirspaceWarningManager(*airspaces, task_manager);
    airspace_warnings->reset(aircraft.get_state());
  }

  do {

    if ((task_manager.getActiveTaskPointIndex() == 1) &&
        first && (task_manager.get_stats().total.time_elapsed > fixed_10)) {
      time_remaining = task_manager.get_stats().total.time_remaining;
      first = false;

      time_planned = task_manager.get_stats().total.time_planned;

      if (verbose > 1) {
        printf("# time remaining %g\n", time_remaining);
        printf("# time planned %g\n", time_planned);
      }
    }

    if (do_print) {
      PrintHelper::taskmanager_print(task_manager, aircraft.get_state());

      const AircraftState state = aircraft.get_state();
      f4 << state.time << " "
         <<  state.location.Longitude << " "
         <<  state.location.Latitude << " "
         <<  state.altitude << "\n";

      f4.flush();
      if (aircraft_filter) {
        f5 << aircraft_filter->GetSpeed() << " " 
           << aircraft_filter->GetBearing() << " " 
           << aircraft_filter->GetClimbRate() << "\n"; 
        f5.flush();
      }
    }

    if (airspaces) {
      scan_airspaces(aircraft.get_state(), *airspaces, perf,
                     do_print, 
                     autopilot.target(ta));
    }
    if (airspace_warnings) {
      if (verbose > 1) {
        bool warnings_updated = airspace_warnings->update(aircraft.get_state(),
                                                          false, 1);
        if (warnings_updated) {
          printf("# airspace warnings updated, size %d\n",
                 (int)airspace_warnings->size());
          print_warnings();
          wait_prompt();
        }
      }
    }

    n_samples++;

    do_print = (++print_counter % output_skip == 0) && verbose;

    if (aircraft_filter)
      aircraft_filter->Update(aircraft.get_state());

    autopilot.update_state(ta, aircraft.get_state());
    aircraft.Update(autopilot.heading);

    {
      const AircraftState state = aircraft.get_state();
      const AircraftState state_last = aircraft.get_state_last();
      task_manager.update(state, state_last);
      task_manager.update_idle(state);
      task_manager.update_auto_mc(state, fixed_zero);
    }

  } while (autopilot.update_autopilot(ta, aircraft.get_state(), aircraft.get_state_last()));

  aircraft.Stop();
  autopilot.Stop();

  if (verbose) {
    PrintHelper::taskmanager_print(task_manager, aircraft.get_state());

    const AircraftState state = aircraft.get_state();
    f4 << state.time << " "
       <<  state.location.Longitude << " "
       <<  state.location.Latitude << " "
       <<  state.altitude << "\n";
    f4 << "\n";
    f4.flush();
    task_report(task_manager, "end of task\n");
  }
  wait_prompt();

  time_elapsed = task_manager.get_stats().total.time_elapsed;
  time_planned = task_manager.get_stats().total.time_planned;
  calc_cruise_efficiency = task_manager.get_stats().cruise_efficiency;
  calc_effective_mc = task_manager.get_stats().effective_mc;

  if (verbose)
    distance_counts();

  if (airspace_warnings)
    delete airspace_warnings;

  return true;
}
Example #28
0
void
AutopilotParameters::ideal()
{
  bearing_noise= fixed_zero;
  turn_speed= fixed(90.0);
}
Example #29
0
void
dlgBasicSettingsShowModal()
{
  const ComputerSettings &settings = CommonInterface::GetComputerSettings();

  glide_polar = settings.glide_polar_task;

  wf = LoadDialog(CallBackTable, XCSoarInterface::main_window,
                      _T("IDR_XML_BASICSETTINGS"));
  if (wf == NULL)
    return;

  changed = false;

  wf->SetTimerNotify(OnTimerNotify);
  OnTimerNotify(*wf);

  SetButtons();

  SetBallast();
  LoadFormProperty(*wf, _T("prpBugs"), (fixed_one - glide_polar.GetBugs()) * 100);
  LoadFormProperty(*wf, _T("prpQNH"), Units::ToUserPressure(settings.pressure.GetHectoPascal()));

  WndProperty* wp;
  wp = (WndProperty*)wf->FindByName(_T("prpQNH"));
  if (wp) {
    DataFieldFloat &df = *(DataFieldFloat *)wp->GetDataField();

    df.SetMin(Units::ToUserPressure(Units::ToSysUnit(fixed(850), unHectoPascal)));
    df.SetMax(Units::ToUserPressure(Units::ToSysUnit(fixed(1300), unHectoPascal)));
    df.SetStep(Units::ToUserPressure(Units::ToSysUnit(fixed_one, unHectoPascal)));
    df.SetUnits(Units::GetPressureName());
    df.SetStep(Units::PressureStep());
    df.SetFormat( Units::GetFormatUserPressure());
    wp->RefreshDisplay();
}
  wp = (WndProperty*)wf->FindByName(_T("prpTemperature"));
  if (wp) {
    DataFieldFloat &df = *(DataFieldFloat *)wp->GetDataField();
    df.SetMin(Units::ToUserTemperature(Units::ToSysUnit(fixed(-50), unGradCelcius)));
    df.SetMax(Units::ToUserTemperature(Units::ToSysUnit(fixed(60), unGradCelcius)));
    df.SetUnits(Units::GetTemperatureName());
    df.Set(Units::ToUserTemperature(settings.forecast_temperature));
    wp->RefreshDisplay();
  }

  if (wf->ShowModal() == mrOK) {
    ComputerSettings &settings = CommonInterface::SetComputerSettings();

    if (changed) {
      settings.glide_polar_task = glide_polar;

      if (protected_task_manager != NULL)
        protected_task_manager->SetGlidePolar(glide_polar);
    }

    SaveFormProperty(*wf, _T("prpTemperature"),
                     ugTemperature, settings.forecast_temperature);
  }

  delete wf;
}
Example #30
0
 constexpr
 static fixed Export(int16_t x) {
   return fixed(x) / 256;
 }