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();
}
fixed
DigitEntry::GetFixedValue() const
{
fixed value = fixed(GetPositiveInteger()) + GetPositiveFractional();
return IsNegative() ? -value : value;
}
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);
}
}
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();
}
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;
}
}
}
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;
};
}
}
/**
* 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;
}
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;
}
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;
}
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;
}
/**
* 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;
}
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;
}
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;
void
AutopilotParameters::realistic()
{
bearing_noise= fixed(20.0);
turn_speed= fixed(12.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;
}
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();
}
}
/**
* 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)
{
}
void
MapWindowProjection::SetMapScale(const fixed x)
{
SetScale(fixed(GetMapResolutionFactor()) / LimitMapScale(x));
}
/**
* 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)
{
}
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);
}
}
}
}
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));
}
fixed
LineSectorZone::ScoreAdjustment() const
{
return fixed(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);
}
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);
}
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;
}
void
AutopilotParameters::ideal()
{
bearing_noise= fixed_zero;
turn_speed= fixed(90.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;
}
constexpr
static fixed Export(int16_t x) {
return fixed(x) / 256;
}
