diff --git a/src/cam.c b/src/cam.c
index a68a966..45b36e3 100644
--- a/src/cam.c
+++ b/src/cam.c
@@ -31,6 +31,16 @@
 #define EARTH_RADIUS 6369000
 #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[] = {
     {"CenDsrcTollingZone/ProtectedCommunicationZonesRSU",                            0x8000},
     {"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_get_space(epv);
-    long lat = epv->space.latitude;
-    long lon = epv->space.longitude;
+    double lat = epv->space.latitude/10000000.0;
+    double lon = epv->space.longitude/10000000.0;
     it2s_tender_unlock_space(epv);
 
     pthread_mutex_lock(&lightship->lock);
 
     for (int i = 0; i < lightship->pz_len; ++i) {
-        double d_lat = (lat - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG;
-        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;
+        double d = haversine(lat, lon, (double) lightship->pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->pz[i]->protectedZoneLongitude/10000000.0);
 
         int pz_radius = 50;
         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) {
             is_inside = true;
+            break;
         } 
     }