int compareWith(const GeographyPointValue& rhs) const {
// Caller guarantees that neither side is null
assert(! isNull());
assert(! rhs.isNull());
Coord lhsLong = getLongitude();
Coord rhsLong = rhs.getLongitude();
if (lhsLong < rhsLong) {
return VALUE_COMPARE_LESSTHAN;
}
if (lhsLong > rhsLong) {
return VALUE_COMPARE_GREATERTHAN;
}
// latitude is equal; compare longitude
Coord lhsLat = getLatitude();
Coord rhsLat = rhs.getLatitude();
if (lhsLat < rhsLat) {
return VALUE_COMPARE_LESSTHAN;
}
if (lhsLat > rhsLat) {
return VALUE_COMPARE_GREATERTHAN;
}
return VALUE_COMPARE_EQUAL;
}
//------------------------------------------------------------------------------
// updateData() -- update background data here
//------------------------------------------------------------------------------
void IlsRadio::updateData(const LCreal dt)
{
//Must have ownship
if (getOwnship() == 0) return;
// Set our position to that of our ownship vehicle
setPosition();
//Do not update data every time...Hard code delay length...
if(timerCounter==0){
//Get Glideslope and Runway orientation:
glideSlopeValid = findILSGlideslopeByFreq(getFrequency());
localizerValid = findLocalizerByFreq(getFrequency());
//Get test results - make sure dest LL do not change as plane flies
Basic::Nav::gbd2ll(getLatitude(),getLongitude(),bearing,grdrange,&destLatitude,&destLongitude);
//Test for bad result...
if((glideSlopeValid==false)|(localizerValid==false)){
//std::cerr << "No ILS In Range..." << std::endl;
}
}
else if(timerCounter==30){
timerCounter=0; //Make sure the lookup occurs on the next cycle
}
else{
timerCounter++; //Do not do another DB lookup - wait for some time
}
BaseClass::updateData(dt);
}
void modCalcAzel::slotComputeCoords()
{
SkyPoint sp;
double epoch0 = getEpoch( epochName->text() );
KStarsDateTime dt;
dt.setFromEpoch( epoch0 );
long double jd = getDateTime().djd();
long double jd0 = dt.djd();
dms LST( getDateTime().gst().Degrees() + getLongitude().Degrees() );
if(radioApCoords->isChecked()) {
sp = getEquCoords();
sp.apparentCoord(jd0, jd);
dms lat(getLatitude());
sp.EquatorialToHorizontal( &LST, &lat );
showHorCoords( sp );
} else {
sp = getHorCoords();
dms lat(getLatitude());
sp.HorizontalToEquatorial( &LST, &lat );
showEquCoords( sp );
}
}
float Geocoordinate::getDistanceBetween(Geocoordinate to) {
double twoPointsDistance = 0.0;
float mtsDistance = 0;
twoPointsDistance = sqrt(sq(to.getLongitude().toFloat() - getLongitude().toFloat()) + sq(to.getLatitude().toFloat() - getLatitude().toFloat()));
mtsDistance = (-270185.843 * twoPointsDistance) + 797.67;
return abs(mtsDistance);
}
QRectF OsmAndMapView::getTileRect() const {
int z = (int) zoom;
float tileX = (float) OsmAnd::Utilities::getTileNumberX(z, getLongitude());
float tileY = (float) OsmAnd::Utilities::getTileNumberY(z, getLatitude());
float w = getCenterPointX();
float h = getCenterPointY();
QRect pixRect(0, 0, width, height);
QRectF tilesRect;
calculateTileRectangle(pixRect, w, h, tileX, tileY, tilesRect);
return tilesRect;
}
int main(int argc, char** argv) {
initTruck();
int start_time = timeInSecs();
long double start_lat = getLatitude();
long double start_lon = getLongitude();
int start_heading = getHeading();
int phase = 0;
setSteering(0);
setThrottle(50);
while(TRUE){
int target_heading;
if(phase == 0){
if(distance(start_lat, getLatitude(), start_lon, getLongitude())>5){
setThrottle(30);
setSteering(60);
phase = 1;
target_heading = newHeading(90);
}
}
else if (phase == 1){
if(getHeading()==target_heading){
setSteering(-60);
target_heading=start_heading;
phase = 2;
}
}
else if (phase == 2){
if(getHeading()==target_heading){
setSteering(0);
setThrottle(0);
while(TRUE);
}
}
background();
}
return (EXIT_SUCCESS);
}
static int writeGPSChannels(SampleRecord *sampleRecord, size_t currentTicks, GPSConfig *config){
int rate = SAMPLE_DISABLED;
{
size_t sr = config->dateCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_DateSample.intValue = getDate();
}
}
}
{
size_t sr = config->timeCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_TimeSample.floatValue = getUTCTime();
}
}
}
{
//latitude sample rate is a stand-in for position sample rate
size_t sr = config->latitudeCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_LatitueSample.floatValue = getLatitude();
sampleRecord->GPS_LongitudeSample.floatValue = getLongitude();
}
}
}
{
size_t sr = config->satellitesCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_SatellitesSample.intValue = getSatellitesUsedForPosition();
}
}
}
{
size_t sr = config->speedCfg.sampleRate;
if (sr != SAMPLE_DISABLED){
if ((currentTicks % sr) == 0){
rate = HIGHER_SAMPLE_RATE(sr, rate);
sampleRecord->GPS_SpeedSample.floatValue = getGPSSpeed() * 0.621371192; //convert to MPH
}
}
}
return rate;
}
//------------------------------------------------------------------------------
//Find ILS Glideslope By Freq - ALWAYS FIND GLIDESLOPE FIRST THEN LOCALIZER!!!
//------------------------------------------------------------------------------
bool IlsRadio::findILSGlideslopeByFreq(LCreal freq)
{
//Reset glideSlopeValid so only the first valid glide slope is returned...
glideSlopeValid = false;
if (getAirportLoader() != 0 && freq > 0) {
if (getAirportLoader()->requestDbInUse()) {
//Set the active area:
getAirportLoader()->setArea(getLatitude(), getLongitude(), getMaxDetectRange());
//See what the results are:
int found = getAirportLoader()->queryIlsByFreq(static_cast<float>(freq));
//Sort through the results and check the type - assume the closest ones are correct
//"found" list is already sorted from closest to farthest away:
for (int i = 0; i < found; i++) {
Dafif::Ils* p = getAirportLoader()->getIls(i);
//Debug Prints:
//p->printRecord(std::cout);
//Get Glideslope data here:
if((!glideSlopeValid)&&(p->isIlsType(Dafif::Ils::GLIDESLOPE))){
//Glideslope should not affect the bearing data from the localizer
currentMagVar = p->magVariance();
float ilsGS(0), acGS(0), delGS(0);
p->getGlideSlopeData(getLatitude(),getLongitude(),getAltitude(),&ilsGS,&acGS,&delGS);
ilsGlideSlope = ilsGS;
acGlideSlope= acGS;
deltaGlideSlope = delGS;
glideSlopeValid = true;
}
//Toast the object:
p->unref();
}
getAirportLoader()->clearDbInUse();
}
}
return glideSlopeValid;
}
float AvcNav::crossTrackError () {
int multiplier = 1;
// this will tell me if the car is on the right or left of the line
float plusminus = ((dLat - sLat) * (sLon - getLongitude()) - (sLat - getLatitude()) * (dLon - sLon));
if (plusminus < 0) {
multiplier = -1;
}
float a = TinyGPS::distance_between(toFloat(sLat), 0.0f, toFloat(dLat), toFloat(sLon - dLon));
float b = TinyGPS::distance_between(toFloat(getLatitude()), 0.0f, toFloat(dLat), toFloat(getLongitude() - dLon));
float c = TinyGPS::distance_between(toFloat(getLatitude()), 0.0f, toFloat(sLat), toFloat(getLongitude() - sLon));
float cosc = (sq(a) + sq(b) - sq(c)) / (2 * a * b);
float C = acos(cosc);
float d = sin(C) * b;
return float(multiplier) * d;
}
bool CIRCDDBGatewayGatewaySet::Validate()
{
wxString callsign = getCallsign();
if (callsign.IsEmpty()) {
wxMessageDialog dialog(this, _("The Gateway Callsign is not valid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
unsigned int port = getIcomPort();
if (port == 0U || port > 65535U) {
wxMessageDialog dialog(this, _("The Icom Port is not valid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
port = getHBPort();
if (port == 0U || port > 65535U) {
wxMessageDialog dialog(this, _("The Homebrew Port is not valid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
double latitude = getLatitude();
if (latitude < -90.0 || latitude > 90.0) {
wxMessageDialog dialog(this, _("The Latitude is invalid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
double longitude = getLongitude();
if (longitude < -180.0 || longitude > 180.0) {
wxMessageDialog dialog(this, _("The Longitude is invalid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
return true;
}
bool CRepeaterInfoSet::Validate()
{
double latitude = getLatitude();
if (latitude < -90.0 || latitude > 90.0) {
wxMessageDialog dialog(this, _("The Latitude is invalid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
double longitude = getLongitude();
if (longitude < -180.0 || longitude > 180.0) {
wxMessageDialog dialog(this, _("The Longitude is invalid"), m_title + _(" Error"), wxICON_ERROR);
dialog.ShowModal();
return false;
}
return true;
}
void Gprmc::printGpsData()
{
//Serial.print("Time: ");
Serial.println(getTime());
//Serial.print("Status: ");
Serial.println(getStatus());
//Serial.print("Breitengrad: ");
Serial.println(getLatitude());
//Serial.print("Längengrad: ");
Serial.println(getLongitude());
//Serial.print("Geschwindigkeit: ");
Serial.println(getSpeedOverGround());
//Serial.print("Datum:");
Serial.println(getDate());
}
int writeDatalink(float frequency){
if (time - lastTime > frequency) {
lastTime = time;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
statusData->debugChar = debugChar;
statusData->debugInt = debugInt;
statusData->debugFloat = debugFloat;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (time < lastTime){
lastTime = time;
return 0;
}
return 0;
}
GPSPosition::WaypointStruct GPSPosition::getWaypointStructData()
{
WaypointStruct tempWaypoint;
memset(&tempWaypoint,0,sizeof(WaypointStruct));
tempWaypoint.Position.Latitude = (quint32) (getLatitude()*(double)10000000);
tempWaypoint.Position.Longitude = (quint32) (getLongitude()*(double)10000000);
tempWaypoint.Position.Altitude = (qint32) (getAltitude()*(double)1000);
tempWaypoint.Position.Status = GPS_NEWDATA;
tempWaypoint.ToleranceRadius = GPS_TOLERANCE_RADIUS_DEFAULT;
tempWaypoint.HoldTime = GPS_HOLD_TIME_DEFAULT;
tempWaypoint.Index = 0;
tempWaypoint.pointType = GPS_POINT_TYPE_WPT;
/*tempWaypoint.Name[0] = getName()[0].toLatin1();
tempWaypoint.Name[1] = getName()[1].toLatin1();
tempWaypoint.Name[2] = getName()[2].toLatin1();
tempWaypoint.Name[3] = getName()[3].toLatin1();*/
return tempWaypoint;
}
void AvcNav::sample (AvcLcd *lcd) {
if (fixTime == previousFixTime || hdop < .0001) {
return;
}
if (samples >= MAX_SAMPLES) {
long lat = tempWaypoint->getLatitude();
long lon = tempWaypoint->getLongitude();
AvcEeprom::writeLatLon (numWaypointsSet - 1, &lat, &lon);
if (numWaypointsSet == 1) {
dLat = lat;
dLon = lon;
} else if (numWaypointsSet > 2) {
sLat = lat;
sLon = lon;
}
delete tempWaypoint;
sampling = false;
lcd->resetMode();
return;
}
samples++;
tempWaypoint->sample(getLatitude(), getLongitude(), getHdop());
}
/**
* Return a point which is this point translated by the
* given offset.
*
* This is mostly used in testing. We want to move points slightly,
* so that we can test approximate equality of polygons.
*/
GeographyPoint translate(const GeographyPoint &offset) {
return GeographyPoint(getLongitude() + offset.getLongitude(),
getLatitude() + offset.getLatitude());
}
void serializeTo(Serializer& output) const {
output.writeDouble(getLongitude());
output.writeDouble(getLatitude());
}
S2Point toS2Point() const {
return S2LatLng::FromDegrees(getLatitude(), getLongitude()).ToPoint();
}
std::string Coordinate::toString() const
{
std::stringstream ss;
ss << getLatitude() << "," << getLongitude();
return ss.str();
}
GeoPoint* populateGeoPoint(GeoPoint *gp) {
gp->latitude = getLatitude();
gp->longitude = getLongitude();
return gp;
}
void BasicSubmarine::UI::updateUI(double dt, SubWindow& subWindow)
{
auto vessel = mVessel.lock();
if (vessel)
{
auto vesselPosition = vessel->getState().getLocation();
double lat = vesselPosition.getLatitude();
double lng = vesselPosition.getLongitude();
double alt = vesselPosition.getAltitude();
std::stringstream locationText;
locationText << "Position and depth: " << std::endl;
locationText << std::setprecision(6) << std::fixed << lat << " deg" << std::endl << std::setprecision(6) << std::fixed << lng << " deg" << std::endl;
locationText << "Depth: " << std::setprecision(2) << alt << " m";
mLocation->SetText(locationText.str());
auto ocean = Ocean::getOcean();
ocean->lockAccess();
vessel->setHeading(mHeading->GetValue());
vessel->setPitch(mPitch->GetValue());
vessel->setVelocity(mVelocity->GetValue() * 0.514444444); //Convert to m/s.
ocean->unlockAccess();
BroadbandState state(0.3f, 0.3f, 5.0f);
std::shared_ptr<GameManager> gameManager = GameManager::getCurrent().lock();
BOOST_ASSERT_MSG(gameManager, "GameManager null");
auto eigenrayMap = gameManager->getUsmlManager()->getEigenrayMap();
//Iterate through all contacts we can hear.
for (auto& contactKV : eigenrayMap)
{
//If the azimuth angle is NaN, that means that there were no eigenrays for this contact.
if (!isnan(contactKV.second.source_az))
{
//Average the intensities across all frequencies.
//TODO: don't weight them all the same, and figure out what the numbers USML gives us even mean.
double intensitySum = 0;
for (auto intensity : contactKV.second.intensity)
{
if (intensity > 299.9999f)
continue; //TODO: What causes this?
intensitySum += intensity;
}
double intensityAverage = intensitySum / contactKV.second.intensity.size();
float adjustedIntensity = (float)(intensityAverage / 300);
state.pushContact(BroadbandState::BroadbandContact((float)contactKV.second.source_az, adjustedIntensity, 5.0f));
}
}
mWaterfall->SetState(state);
}
else
{
subWindow.switchToScreen<MainMenu>();
}
}