/*!
* 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;
}
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(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;
}
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;
}
