#include "cam.h" #include "facilities.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) #define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) #define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) #ifndef NDEBUG #define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__) #else #define syslog_debug(msg, ...) #endif #define LEAP_SECONDS 5 #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}, {"specialTransport/specialTransportContainer", 0x2000}, {"dangerousGoods/dangerousGoodsContainer", 0x1000}, {"roadwork/roadWorksContainerBasic", 0x0800}, {"rescue/rescueContainer", 0x0400}, {"emergency/emergencyContainer", 0x0200}, {"safetyCar/safetyCarContainer", 0x0100}, {"closedLanes/RoadworksContainerBasic", 0x0080}, {"requestForRightOfWay/EmergencyContainer: EmergencyPriority", 0x0040}, {"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", 0x0020}, {"noPassing/SafetyCarContainer: TrafficRule", 0x0010}, {"noPassingForTrucks/SafetyCarContainer: TrafficRule", 0x0008}, {"speedLimit/SafetyCarContainer", 0x0004}, {"reserved0", 0x0002}, {"reserved1", 0x0001}, }; static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_len) { /* CAM SSP scheme * * byte | description * --------------------------------- * 0 | SSP version control * 1-2 | Service-specific parameter * 3-30 | Reserved */ if (permissions_len < 3) { syslog_debug("[facilities] [ca] permissions length too small"); return 0; } uint16_t perms_int = *(uint16_t*)(permissions+1); uint16_t perm_val = CID_SSP_BM_MAP[cid].bitmap_val; perm_val = (perm_val>>8) | (perm_val<<8); if ((perm_val & perms_int) == perm_val) return 1; else return 0; } static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) { int rv = 0; CAM_t *cam = calloc(1, sizeof(CAM_t)); cam->header.protocolVersion = 2; cam->header.messageID = ItsPduHeader__messageID_cam; pthread_mutex_lock(&facilities->id.lock); cam->header.stationID = facilities->id.value; pthread_mutex_unlock(&facilities->id.lock); cam->cam.camParameters.basicContainer.stationType = facilities->station_type; BasicContainer_t* bc = &cam->cam.camParameters.basicContainer; uint64_t now = it2s_tender_get_clock(&facilities->epv); lightship_t* lightship = facilities->lightship; pthread_mutex_lock(&lightship->lock); if (facilities->station_type != StationType_roadSideUnit) { cam->cam.generationDeltaTime = now % 65536; it2s_tender_lock_space(&facilities->epv); it2s_tender_get_space(&facilities->epv); bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude; bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf; // Set GPS coordinates bc->referencePosition.latitude = facilities->epv.space.latitude; bc->referencePosition.longitude = facilities->epv.space.longitude; uint16_t lat_conf = facilities->epv.space.latitude_conf; uint16_t lon_conf = facilities->epv.space.longitude_conf; cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; // Set speed bvc_hf->speed.speedValue = facilities->epv.space.speed; bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf; // Set heading bvc_hf->heading.headingValue = facilities->epv.space.heading; bvc_hf->heading.headingConfidence = facilities->epv.space.heading_conf; it2s_tender_unlock_space(&facilities->epv); if (lat_conf > lon_conf) { bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0; } else { bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900; } bvc_hf->vehicleWidth = facilities->vehicle.width; bvc_hf->vehicleLength.vehicleLengthValue = facilities->vehicle.length; bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; bvc_hf->driveDirection = DriveDirection_unavailable; bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable; bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable; bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable; bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable; // Save current values if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) { free(lightship->pos_history[POS_HISTORY_MAX_LEN-1]); --lightship->pos_history_len; } for (int i = lightship->pos_history_len-1; i >= 0; --i) { lightship->pos_history[i+1] = lightship->pos_history[i]; } lightship->pos_history[0] = malloc(sizeof(pos_vector_t)); lightship->pos_history[0]->heading = bvc_hf->heading.headingValue; lightship->pos_history[0]->lat = bc->referencePosition.latitude; lightship->pos_history[0]->lon = bc->referencePosition.longitude; lightship->pos_history[0]->alt = bc->referencePosition.altitude.altitudeValue; lightship->pos_history[0]->speed = bvc_hf->speed.speedValue; lightship->last_cam = now; lightship->pos_history[0]->ts = now; ++lightship->pos_history_len; // Acceleration if (lightship->pos_history_len > 1) { double delta_angle = (lightship->pos_history[0]->heading - lightship->pos_history[1]->heading) / 10.0; /* 1º */ double delta_speed = (lightship->pos_history[0]->speed - (lightship->pos_history[1]->speed * cos(delta_angle * M_PI/180))) / 10.0; /* 0.1 m/s */ int16_t long_a = (int16_t) (delta_speed / (double) ((lightship->pos_history[0]->ts - lightship->pos_history[1]->ts) / 1000)); /* 0.1 m/s^2 */ if (long_a > 160) long_a = 160; else if (long_a < -160) long_a = -160; bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = long_a; bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; } else { bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; } // Low frequency container if (now > lightship->last_cam_lfc + 500) { cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(LowFrequencyContainer_t)); cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory; ph->list.array = malloc((lightship->pos_history_len-1) * sizeof(void*)); ph->list.count = lightship->pos_history_len-1; ph->list.size = (lightship->pos_history_len-1) * sizeof(void*); for (int i = 0; i < lightship->pos_history_len-1; ++i) { ph->list.array[i] = calloc(1,sizeof(PathPoint_t)); if (lightship->pos_history[i+1]->alt != AltitudeValue_unavailable && lightship->pos_history[i]->alt != AltitudeValue_unavailable) { ph->list.array[i]->pathPosition.deltaAltitude = lightship->pos_history[i+1]->alt - lightship->pos_history[i]->alt; } else { ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; } if (lightship->pos_history[i+1]->lat != Latitude_unavailable && lightship->pos_history[i]->lat != Latitude_unavailable) { ph->list.array[i]->pathPosition.deltaLatitude = lightship->pos_history[i+1]->lat - lightship->pos_history[i]->lat; } else { ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; } if (lightship->pos_history[i+1]->lon != Longitude_unavailable && lightship->pos_history[i]->lon != Longitude_unavailable) { ph->list.array[i]->pathPosition.deltaLongitude = lightship->pos_history[i+1]->lon - lightship->pos_history[i]->lon; } else { ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; } ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); *ph->list.array[i]->pathDeltaTime = (lightship->pos_history[i]->ts - lightship->pos_history[i+1]->ts)/10; } lightship->last_cam_lfc = now; } } else { cam->cam.generationDeltaTime = now % 65536; it2s_tender_lock_space(&facilities->epv); it2s_tender_get_space(&facilities->epv); bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude; bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf; // Set GPS coordinates bc->referencePosition.latitude = facilities->epv.space.latitude; bc->referencePosition.longitude = facilities->epv.space.longitude; bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; it2s_tender_unlock_space(&facilities->epv); cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; if (facilities->lightship->protected_zones.pz_len > 0) { cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t)); ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; pzs->list.count = facilities->lightship->protected_zones.pz_len; pzs->list.size = facilities->lightship->protected_zones.pz_len * sizeof(void*); pzs->list.array = malloc(facilities->lightship->protected_zones.pz_len * sizeof(void*)); for (int i = 0; i < facilities->lightship->protected_zones.pz_len; ++i) { pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t)); pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLatitude; pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLongitude; pzs->list.array[i]->protectedZoneType = facilities->lightship->protected_zones.pz[i]->protectedZoneType; if (facilities->lightship->protected_zones.pz[i]->expiryTime) { pzs->list.array[i]->expiryTime->size = facilities->lightship->protected_zones.pz[i]->expiryTime->size; pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->protected_zones.pz[i]->expiryTime->size); memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->size); } if (facilities->lightship->protected_zones.pz[i]->protectedZoneID) { pzs->list.array[i]->protectedZoneID = malloc(8); *pzs->list.array[i]->protectedZoneID = *facilities->lightship->protected_zones.pz[i]->protectedZoneID; } if (facilities->lightship->protected_zones.pz[i]->protectedZoneRadius) { pzs->list.array[i]->protectedZoneRadius = malloc(8); *pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->protected_zones.pz[i]->protectedZoneRadius; } } } } pthread_mutex_unlock(&facilities->lightship->lock); asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam, cam_oer, 512); if (enc.encoded == -1) { syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name); rv = 1; goto cleanup; } *cam_len = (enc.encoded + 7) / 8; cleanup: ASN_STRUCT_FREE(asn_DEF_CAM, cam); return rv; } lightship_t* lightship_init() { lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t)); lightship->protected_zones.pz = calloc(256 , sizeof(void*)); pthread_mutex_init(&lightship->lock, NULL); lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*)); return lightship; } int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv) { int rv = 0; uint64_t now = it2s_tender_get_clock(epv); pthread_mutex_lock(&lightship->lock); if (lightship->type == StationType_roadSideUnit) { // RSU if (lightship->is_vehicle_near && now > lightship->next_cam_min) { rv = 1; } } else { // Vehicle if (now > lightship->next_cam_max) { rv = 1; } else if (now > lightship->next_cam_min) { it2s_tender_lock_space(epv); it2s_tender_get_space(epv); // Check heading delta > 4º int diff = epv->space.heading - lightship->pos_history[0]->heading; if (abs(diff) > 40) rv = 1; if (!rv) { // Check speed delta > 0.5 m/s diff = epv->space.speed - lightship->pos_history[0]->speed; if (abs(diff) > 50) rv = 1; if (!rv) { // Check position delta > 4 m // TODO make an *accurate* distance calculator using GPS coords int32_t avg_speed = (epv->space.speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */ uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */ if (avg_speed * delta_time > 4) rv = 1; } } it2s_tender_unlock_space(epv); } // Remove expired PZs for (int i = 0; i < lightship->protected_zones.pz_len; ++i) { uint64_t expiry; if (lightship->protected_zones.pz[i]->expiryTime) { asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, &expiry); if (now >= expiry) { ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]); for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) { lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j+1]; } --lightship->protected_zones.pz_len; } } } } pthread_mutex_unlock(&lightship->lock); return rv; } void lightship_reset_timer(lightship_t* lightship, it2s_tender_epv_t* epv) { uint64_t now = it2s_tender_get_clock(epv); pthread_mutex_lock(&lightship->lock); if (lightship->type != StationType_roadSideUnit) { // Vehicle lightship->next_cam_max = now + lightship->vehicle_gen_max; lightship->next_cam_min = now + lightship->vehicle_gen_min; } else { // RSU if (now > lightship->last_vehicle + lightship->rsu_vehicle_permanence) { lightship->is_vehicle_near = false; } lightship->next_cam_min = now + lightship->rsu_gen_min; } pthread_mutex_unlock(&lightship->lock); } enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) { int rv = 0; facilities_t* facilities = (facilities_t*) fc; lightship_t *lightship = ((facilities_t*) fc)->lightship; uint64_t now = it2s_tender_get_clock(epv); // Check permissions if (ssp) { if (cam->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency && cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) { if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container); return rv; } } if (cam->cam.camParameters.specialVehicleContainer) { switch (cam->cam.camParameters.specialVehicleContainer->present) { case SpecialVehicleContainer_PR_NOTHING: break; case SpecialVehicleContainer_PR_publicTransportContainer: if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container); return rv; } break; case SpecialVehicleContainer_PR_specialTransportContainer: if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container); return rv; } break; case SpecialVehicleContainer_PR_dangerousGoodsContainer: if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container); return rv; } break; case SpecialVehicleContainer_PR_roadWorksContainerBasic: if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container); return rv; } if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) { if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container); return rv; } } break; case SpecialVehicleContainer_PR_rescueContainer: if (!permissions_check(CID_RESCUE, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container); return rv; } break; case SpecialVehicleContainer_PR_emergencyContainer: if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container); return rv; } if (cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority && cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) { // TODO verify bitmap uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf; if (bm & 0x02) { if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_RIGHT_OF_WAY].container); return rv; } } if (bm & 0x01) { if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container); return rv; } } } break; case SpecialVehicleContainer_PR_safetyCarContainer: if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container); return rv; } if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) { switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) { case TrafficRule_noPassing: if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container); return rv; } break; case TrafficRule_noPassingForTrucks: if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container); return rv; } break; default: break; } } if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) { if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) { rv = CAM_BAD_PERMISSIONS; syslog_debug("[facilities] [ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPEED_LIMIT].container); return rv; } } break; } } } pthread_mutex_lock(&lightship->lock); if (lightship->type == StationType_roadSideUnit) { // Send CAMs if vehicles nearby if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) { lightship->last_vehicle = now; lightship->is_vehicle_near = true; } } else { // Protected zones if (cam->cam.camParameters.basicContainer.stationType == StationType_roadSideUnit && cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) { ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) { bool new_pz = false; for (int k = 0; k < pzs->list.count; ++k) { bool found = false; for (int j = 0; j < lightship->protected_zones.pz_len; ++j) { if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude && lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) { found = true; break; } } if (found) continue; new_pz = true; lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t)); lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude; lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude; lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType; if (pzs->list.array[k]->expiryTime) { lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size; lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size); memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size); } if (pzs->list.array[k]->protectedZoneID) { lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8); *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID; } if (pzs->list.array[k]->protectedZoneRadius) { lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8); *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius; } ++lightship->protected_zones.pz_len; } // Inform [management] if (new_pz) { uint8_t b_oer[512]; ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); mreq->present = ManagementRequest_PR_attributes; mreq->choice.attributes.present = ManagementRequestAttributes_PR_set; mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones)); mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len; mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void*); mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void*)); for (int p = 0; p < lightship->protected_zones.pz_len; ++p) { asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512); oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded); } asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512); void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ); int wait_ms = 1000; zmq_setsockopt(management_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int)); zmq_connect(management_socket, facilities->zmq.management_address); zmq_send(management_socket, b_oer, enc.encoded, 0); syslog_debug("[facilities] [ca]-> sending MReq.attributes.set.protectedZones to ->[management]"); uint8_t code; zmq_recv(management_socket, &code, 1, 0); zmq_close(management_socket); } } } } pthread_mutex_unlock(&lightship->lock); return rv; } static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) { bool is_inside = false; it2s_tender_lock_space(epv); it2s_tender_get_space(epv); 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->protected_zones.pz_len; ++i) { double d = haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0); int pz_radius = 50; if (lightship->protected_zones.pz[i]->protectedZoneRadius) { pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius; } if (d < pz_radius) { is_inside = true; break; } } pthread_mutex_unlock(&lightship->lock); return is_inside; } void *ca_service(void *fc) { int rv = 0; uint8_t code = 0; facilities_t *facilities = (facilities_t*) fc; BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); bdr->btpType = BTPType_btpB; bdr->gnDestinationAddress.buf = malloc(6); for (int i = 0; i < 6; ++i) { bdr->gnDestinationAddress.buf[i] = 0xff; } bdr->gnDestinationAddress.size = 6; bdr->gnPacketTransportType = PacketTransportType_shb; bdr->destinationPort = Port_cam; bdr->gnTrafficClass = 2; bdr->data.buf = malloc(512); if (facilities->use_security) { bdr->gnSecurityProfile = malloc(sizeof(long)); *bdr->gnSecurityProfile = 1; } uint8_t bdr_oer[1024]; bdr_oer[0] = 4; // Facilities while (!facilities->exit) { usleep(1000*50); if (lightship_check(facilities->lightship, &facilities->epv) && facilities->lightship->active) { rv = mk_cam(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size); if (rv) { continue; } // Check if inside PZ bdr->gnCommunicationProfile = 0; if (facilities->station_type != 15 && check_pz(facilities->lightship, &facilities->epv)) bdr->gnCommunicationProfile = 1; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 1023); if (enc.encoded == -1) { syslog_err("[facilities] encoding BTPDataRequest for cam failed"); continue; } queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); pthread_cond_signal(&facilities->tx_queue->trigger); lightship_reset_timer(facilities->lightship, &facilities->epv); } } ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr); return NULL; }