TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Convert GPS coords to angular in check_pz

This commit is contained in:
emanuel 2021-05-21 16:43:26 +01:00
parent a56d15cb1f
commit 7d16f0525e
1 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/cam.c
View File

@ -31,6 +31,16 @@
#define EARTH_RADIUS 6369000 #define EARTH_RADIUS 6369000
#define RAD_PER_DEG M_PI/180.0 #define RAD_PER_DEG M_PI/180.0
double haversine(double lat1, double lon1, double lat2, double lon2) {
double d_lat = (lat1-lat2) * RAD_PER_DEG;
double d_lon = (lon1-lon2) * RAD_PER_DEG;
double a = pow(sin(d_lat/2.0), 2) + cos(lat1 * RAD_PER_DEG) * cos(lat2 * RAD_PER_DEG) * pow(sin(d_lon/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
return EARTH_RADIUS * c;
}
const cid_ssp_bm_t CID_SSP_BM_MAP[] = { const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000}, {"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000},
{"publicTransport/publicTransportContainer", 0x4000}, {"publicTransport/publicTransportContainer", 0x4000},
@ -555,23 +565,14 @@ static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) {
it2s_tender_lock_space(epv); it2s_tender_lock_space(epv);
it2s_tender_get_space(epv); it2s_tender_get_space(epv);
long lat = epv->space.latitude; double lat = epv->space.latitude/10000000.0;
long lon = epv->space.longitude; double lon = epv->space.longitude/10000000.0;
it2s_tender_unlock_space(epv); it2s_tender_unlock_space(epv);
pthread_mutex_lock(&lightship->lock); pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->pz_len; ++i) { for (int i = 0; i < lightship->pz_len; ++i) {
double d_lat = (lat - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG; double d = haversine(lat, lon, (double) lightship->pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->pz[i]->protectedZoneLongitude/10000000.0);
double d_lon = (lon - (double) lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG;
double a = pow(sin(d_lat/2.0), 2) +
cos(lat * RAD_PER_DEG) *
cos((double) lightship->pz[i]->protectedZoneLatitude/10000000 * RAD_PER_DEG) *
pow(sin(d_lon/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
double d = EARTH_RADIUS * c;
int pz_radius = 50; int pz_radius = 50;
if (lightship->pz[i]->protectedZoneRadius) { if (lightship->pz[i]->protectedZoneRadius) {
@ -579,6 +580,7 @@ static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) {
} }
if (d < pz_radius) { if (d < pz_radius) {
is_inside = true; is_inside = true;
break;
} }
} }