float AP_MavlinkCommand::distanceTo(const AP_MavlinkCommand & next) const {
float sinDeltaLat2 = sin((getLat() - next.getLat()) / 2);
float sinDeltaLon2 = sin((getLon() - next.getLon()) / 2);
float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
next.getLat()) * sinDeltaLon2 * sinDeltaLon2;
float c = 2 * atan2(sqrt(a), sqrt(1 - a));
return rEarth * c;
}
float AP_MavlinkCommand::bearingTo(int32_t latDegInt, int32_t lonDegInt) const {
// have to be careful to maintain the precision of the gps coordinate
float deltaLon = (lonDegInt - getLon_degInt()) * degInt2Rad;
float nextLat = latDegInt * degInt2Rad;
float bearing = atan2(
sin(deltaLon) * cos(nextLat),
cos(getLat()) * sin(nextLat) - sin(getLat()) * cos(nextLat)
* cos(deltaLon));
if (bearing < 0)
bearing += 2 * M_PI;
return bearing;
}
float AP_MavlinkCommand::bearingTo(const AP_MavlinkCommand & next) const {
float deltaLon = next.getLon() - getLon();
/*
Serial.print("Lon: "); Serial.println(getLon());
Serial.print("nextLon: "); Serial.println(next.getLon());
Serial.print("deltaLonDeg * 1e7: "); Serial.println(deltaLon*rad2DegInt);
*/
float bearing = atan2(
sin(deltaLon) * cos(next.getLat()),
cos(getLat()) * sin(next.getLat()) - sin(getLat()) * cos(
next.getLat()) * cos(deltaLon));
return bearing;
}
/*!
* Two house numbers are considered to be equal if
* * country, city, postcode, suburb, street, housenumber, name, and shop equal
* * or housenumber, street, name, and shop equal, and country, city, and postcode do not differ (ignoring empty values),
* and lat/lon difference is less than DISTANCE_THRESHOLD
*/
bool HouseNumber::isSameAddress(HouseNumber const& rhs) const {
if(getName().toLower()!=rhs.getName().toLower() ||
getShop().toLower()!=rhs.getShop().toLower() ||
getNumber().toLower()!=rhs.getNumber().toLower() ||
getStreet().toLower()!=rhs.getStreet().toLower() ||
getNumber()=="" || getStreet()=="") {
return false;
}
if(getPostcode().toLower()==rhs.getPostcode().toLower() && getPostcode()!="" &&
getCity().toLower()==rhs.getCity().toLower() && getCity()!="" &&
getSuburb().toLower()==rhs.getSuburb().toLower() &&
getCountry().toLower()==rhs.getCountry().toLower() && getCountry()!="") {
return true;
}
// consider two house numbers with similar address information and little distance to each other to be equal
if(myAbs(getLat()-rhs.getLat())>DISTANCE_THRESHOLD ||
myAbs(getLon()-rhs.getLon())>DISTANCE_THRESHOLD)
return false;
if(getPostcode()!="" && rhs.getPostcode()!="" && getPostcode().toLower()!=rhs.getPostcode().toLower())
return false;
if(getCity()!="" && rhs.getCity()!="" && getCity().toLower()!=rhs.getCity().toLower())
return false;
if(getSuburb()!="" && rhs.getSuburb()!="" && getSuburb()!=rhs.getSuburb())
return false;
if(getCountry()!="" && rhs.getCountry()!="" && getCountry().toLower()!=rhs.getCountry().toLower())
return false;
return true;
}
//**********************************************************************
// void CLatLongDistanceLayer::build()
// build
//**********************************************************************
void CLatLongDistanceLayer::build() {
try {
// Get our Layers and validate values
pLatLayer = CLayerManager::Instance()->getNumericLayer(sLatLayer);
pLongLayer = CLayerManager::Instance()->getNumericLayer(sLongLayer);
for (int i = 0; i < iHeight; ++i) {
for (int j = 0; j < iWidth; ++j) {
double dLong = getLong(i, j);
double dLat = getLat(i, j);
if (dLat < -90)
CError::errorLessThan(PARAM_LAT_LAYER,"-90");
if (dLat > 90)
CError::errorGreaterThan(PARAM_LAT_LAYER,"90");
if (dLong < 0)
CError::errorLessThan(PARAM_LONG_LAYER,"0");
if (dLong > 360)
CError::errorGreaterThan(PARAM_LONG_LAYER,"360");
}
}
rebuild();
} catch (string &Ex) {
Ex = "CLatLongDistanceLayer.build(" + getLabel() + ")->" + Ex;
throw Ex;
}
}
int MAX::renew()
{
BufIn(GPSBuf);
int i = getPhrase(GPSBuf, GPSPhr);
if(i == 1) return 0; //not renewed
else{
String tempLat = getLat(GPSPhr);
String tempLng = getLong(GPSPhr);
String temphgt = getHigh(GPSPhr);
int a = spellCheck(tempLat);
int b = spellCheck(tempLng);
int c = spellCheck(temphgt);
if(a == 0) {sLatitude = tempLat;}
if(b == 0) sLongitude = tempLng;
if(c == 0) sHeight = temphgt;
int r = StoF();
if(r==0){
count ++;
return 1;
}
else if(r==-1) {return 0;}
}
}
void GpsFixDataRecord::printRecordLn(Stream* stream, const char* separator) const
{
if (stream) {
fieldPrintWithSeparator(stream, separator, getPreviousFix());
fieldPrintWithSeparator(stream, separator, getLat());
fieldPrintWithSeparator(stream, separator, getLong());
fieldPrintWithSeparator(stream, "\n", getTimestamp());
}
}
//**********************************************************************
// void CLatLongDistanceLayer::rebuild()
// build
//**********************************************************************
void CLatLongDistanceLayer::rebuild() {
try {
for (int i = 0; i < iHeight; ++i) {
for (int j = 0; j < iWidth; ++j) {
for (int k = 0; k < iHeight; ++k) {
for (int l = 0; l < iWidth; ++l) {
double dLong1 = getLong(i, j);
double dLat1 = getLat(i, j);
double dLong2 = getLong(k, l);
double dLat2 = getLat(k, l);
mGrid[i][j][k][l] = haversine(dLong1, dLat1, dLong2, dLat2);
}
}
}
}
} catch (string &Ex) {
Ex = "CLatLongDistanceLayer.rebuild(" + getLabel() + ")->" + Ex;
throw Ex;
}
}
float AP_MavlinkCommand::distanceTo(int32_t lat_degInt, int32_t lon_degInt) const {
float sinDeltaLat2 = sin(
(lat_degInt - getLat_degInt()) * degInt2Rad / 2);
float sinDeltaLon2 = sin(
(lon_degInt - getLon_degInt()) * degInt2Rad / 2);
float a = sinDeltaLat2 * sinDeltaLat2 + cos(getLat()) * cos(
lat_degInt * degInt2Rad) * sinDeltaLon2 * sinDeltaLon2;
float c = 2 * atan2(sqrt(a), sqrt(1 - a));
/*
Serial.print("wp lat_degInt: "); Serial.println(getLat_degInt());
Serial.print("wp lon_degInt: "); Serial.println(getLon_degInt());
Serial.print("lat_degInt: "); Serial.println(lat_degInt);
Serial.print("lon_degInt: "); Serial.println(lon_degInt);
Serial.print("sinDeltaLat2: "); Serial.println(sinDeltaLat2);
Serial.print("sinDeltaLon2: "); Serial.println(sinDeltaLon2);
*/
return rEarth * c;
}
int main(int argc, char *argv[])
{
fprintf(stderr,"Starting Location Service Tester!\n");
if ( startLocationServices() )
{
unsigned int sampleNumber=0;
while (1)
{
if ( pollLocationServices())
{
fprintf(stderr,"Sample %u => %0.2f %0.2f \n",sampleNumber++,getLat(),getLon());
}
usleep(10000);
fprintf(stderr,".");
}
stopLocationServices();
}
fprintf(stderr,"Done..\n\n");
return 0;
}
