From a81188976af2cf7bec0522469b92e18c166821c6 Mon Sep 17 00:00:00 2001 From: emanuel Date: Wed, 10 Mar 2021 21:34:45 +0000 Subject: [PATCH] Variable CAM frequency --- src/cam.c | 407 ++++++++++++++++++++++++++++++++++++++---------------- src/cam.h | 10 +- 2 files changed, 296 insertions(+), 121 deletions(-) diff --git a/src/cam.c b/src/cam.c index 9317dd8..f875cfb 100644 --- a/src/cam.c +++ b/src/cam.c @@ -107,6 +107,148 @@ static SpeedConfidence_t getSpeedConfidence(uint32_t conf) { return conf; } +static int get_speed(struct it2s_gps_data* gps_data, long* speed_value, long* speed_confidence) { + int rv = 0; + + /* + * SpeedValue ::= INTEGER { + * standstill(0), + * oneCentimeterPerSec(1), + * unavailable(16383) + * } (0..16383) + * + * SpeedConfidence ::= INTEGER { + * equalOrWithinOneCentimeterPerSec(1), + * equalOrWithinOneMeterPerSec(100), + * outOfRange(126), + * unavailable(127) + * } (1..127) + */ + + if (!isnan(gps_data->gps_speed)) { + *speed_value = ((uint16_t)(gps_data->gps_speed * 100)); // cm/s + *speed_confidence = getSpeedConfidence((uint8_t)(gps_data->gps_eps * 100)); + } else { + *speed_value = SpeedValue_unavailable; + *speed_confidence = SpeedConfidence_unavailable; + + rv = 1; + } + + return rv; +} + +static int get_heading(struct it2s_gps_data* gps_data, long* heading_value, long* heading_confidence) { + int rv = 0; + + /* + * HeadingValue ::= INTEGER { + * wgs84North(0), + * wgs84East(900), + * wgs84South(1800), + * wgs84West(2700), + * unavailable(3601) + * } (0..3601) + * + * HeadingConfidence ::= INTEGER { + * equalOrWithinZeroPointOneDegree (1), + * equalOrWithinOneDegree (10), + * outOfRange(126), + * unavailable(127) + * } (1..127) + * + */ + + if (!isnan(gps_data->gps_track)) { + *heading_value = ((uint16_t)(gps_data->gps_track * 10)); + *heading_confidence = getHeadingConfidence((uint8_t)(gps_data->gps_epd * 10)); + } else { + *heading_value = HeadingValue_unavailable; + *heading_confidence = HeadingConfidence_unavailable; + + rv = 1; + } + + return rv; +} + +static int get_wgs84_coordinates(struct it2s_gps_data* gps_data, long* latitude, long* longitude) { + int rv = 0; + + /* + * Latitude ::= INTEGER { + * oneMicrodegreeNorth (10), + * oneMicrodegreeSouth (-10), + * unavailable(900000001) + * } (-900000000..900000001) + * Longitude ::= INTEGER { + * oneMicrodegreeEast (10), + * oneMicrodegreeWest (-10), + * unavailable(1800000001) + * } (-1800000000..1800000001) + * + */ + + if (!isnan(gps_data->gps_latitude)) { + *latitude = (int32_t)((gps_data->gps_latitude) * 10000000); + } else { + *latitude = Latitude_unavailable; + + rv = 1; + } + + if (!isnan(gps_data->gps_longitude)) { + *longitude = (int32_t)((gps_data->gps_longitude) * 10000000); + } else { + *longitude = Longitude_unavailable; + + rv = 1; + } + + return rv; +} + +static int get_altitude(struct it2s_gps_data* gps_data, long* altitude_value, long* altitude_confidence) { + int rv = 0; + + /* + * AltitudeValue ::= INTEGER { + * referenceEllipsoidSurface(0), + * oneCentimeter(1), + * unavailable(800001) + * } (-100000..800001) + * + * AltitudeConfidence ::= ENUMERATED { + * alt-000-01 (0), + * alt-000-02 (1), + * alt-000-05 (2), + * alt-000-10 (3), + * alt-000-20 (4), + * alt-000-50 (5), + * alt-001-00 (6), + * alt-002-00 (7), + * alt-005-00 (8), + * alt-010-00 (9), + * alt-020-00 (10), + * alt-050-00 (11), + * alt-100-00 (12), + * alt-200-00 (13), + * outOfRange (14), + * unavailable (15) + * } + */ + + if (!isnan(gps_data->gps_altitude)) { + *altitude_value = (int32_t) ((gps_data->gps_altitude) * 10); + *altitude_confidence = (uint8_t) getAltitudeConfidence(gps_data->gps_epv); + } else { + *altitude_value = AltitudeValue_unavailable; + *altitude_confidence = AltitudeConfidence_unavailable; + } + + return rv; +} + static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) { int rv = 0; @@ -120,7 +262,7 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) { it2s_gps_read(&gps_data); CAM_t *cam_tx = calloc(1, sizeof(CAM_t)); - + cam_tx->header.protocolVersion = 2; cam_tx->header.messageID = ItsPduHeader__messageID_cam; pthread_mutex_lock(&facilities->lock); @@ -128,103 +270,97 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) { pthread_mutex_unlock(&facilities->lock); cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type; - if (facilities->station_type != StationType_roadSideUnit) { - cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; - - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable; - - if (!isnan(gps_data.gps_track)) { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10)); - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10)); - } else { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable; - - } - - if (!isnan(gps_data.gps_speed)) { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100)); // cm/s - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100)); - } else { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable; - } - } else { - cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; - if (facilities->lightship->pz_len > 0) { - cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t)); - ProtectedCommunicationZonesRSU_t *pzs = cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; - pzs->list.count = facilities->lightship->pz_len; - pzs->list.size = facilities->lightship->pz_len * sizeof(void*); - pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*)); - for (int i = 0; i < facilities->lightship->pz_len; ++i) { - pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t)); - pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude; - pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude; - pzs->list.array[i]->protectedZoneType = facilities->lightship->pz[i]->protectedZoneType; - - if (facilities->lightship->pz[i]->expiryTime) { - pzs->list.array[i]->expiryTime->size = facilities->lightship->pz[i]->expiryTime->size; - pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->pz[i]->expiryTime->size); - memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->size); - } - if (facilities->lightship->pz[i]->protectedZoneID) { - pzs->list.array[i]->protectedZoneID = malloc(8); - *pzs->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID; - } - if (facilities->lightship->pz[i]->protectedZoneRadius) { - pzs->list.array[i]->protectedZoneRadius = malloc(8); - *pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius; - } - } - } - - } long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); generationdeltatime = generationdeltatime - 1072915200000; // EPOCH -> 2004/01/01 00:00:000 generationdeltatime = generationdeltatime % 64536; cam_tx->cam.generationDeltaTime = generationdeltatime; - if (!isnan(gps_data.gps_altitude)) { - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data.gps_altitude) * 10); - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data.gps_epv); - } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable; - } + BasicContainer_t* bc = &cam_tx->cam.camParameters.basicContainer; + get_altitude(&gps_data, &bc->referencePosition.altitude.altitudeValue, &bc->referencePosition.altitude.altitudeConfidence); + + // Set GPS coordinates if (!facilities->gps_fixed) { - if (!isnan(gps_data.gps_latitude)) { - cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000); - } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable; - } - - if (!isnan(gps_data.gps_longitude)) { - cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000); - } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable; - } + get_wgs84_coordinates(&gps_data, &bc->referencePosition.latitude, &bc->referencePosition.longitude); } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000); - cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000); - + bc->referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000); + bc->referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000); } - cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; - + bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; + + pthread_mutex_lock(&facilities->lightship->lock); + + if (facilities->station_type != StationType_roadSideUnit) { + cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + BasicVehicleContainerHighFrequency_t* bvc_hf = &cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + bvc_hf->vehicleWidth = 20; + bvc_hf->vehicleLength.vehicleLengthValue = 46; + 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; + + // Set speed + get_speed(&gps_data, &bvc_hf->speed.speedValue, &bvc_hf->speed.speedConfidence); + + // Set heading + get_heading(&gps_data, &bvc_hf->heading.headingValue, &bvc_hf->heading.headingConfidence); + + // Save current values + facilities->lightship->heading = bvc_hf->heading.headingValue; + facilities->lightship->lat = bc->referencePosition.latitude; + facilities->lightship->lon = bc->referencePosition.longitude; + facilities->lightship->speed = bvc_hf->speed.speedValue; + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + facilities->lightship->last_cam = now; + + } else { + cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; + + if (facilities->lightship->pz_len > 0) { + cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t)); + ProtectedCommunicationZonesRSU_t *pzs = cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; + pzs->list.count = facilities->lightship->pz_len; + pzs->list.size = facilities->lightship->pz_len * sizeof(void*); + pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*)); + for (int i = 0; i < facilities->lightship->pz_len; ++i) { + pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t)); + pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude; + pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude; + pzs->list.array[i]->protectedZoneType = facilities->lightship->pz[i]->protectedZoneType; + + if (facilities->lightship->pz[i]->expiryTime) { + pzs->list.array[i]->expiryTime->size = facilities->lightship->pz[i]->expiryTime->size; + pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->pz[i]->expiryTime->size); + memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->size); + } + if (facilities->lightship->pz[i]->protectedZoneID) { + pzs->list.array[i]->protectedZoneID = malloc(8); + *pzs->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID; + } + if (facilities->lightship->pz[i]->protectedZoneRadius) { + pzs->list.array[i]->protectedZoneRadius = malloc(8); + *pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius; + } + } + } + } + + pthread_mutex_unlock(&facilities->lightship->lock); + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256); if (enc.encoded == -1) { syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name); @@ -261,13 +397,43 @@ int lightship_check(lightship_t* lightship) { pthread_mutex_lock(&lightship->lock); if (lightship->type == StationType_roadSideUnit) { // RSU - if (lightship->is_vehicle_near && now > lightship->next_cam) { + if (lightship->is_vehicle_near && now > lightship->next_cam_min) { rv = 1; } } else { // Vehicle - if (now > lightship->next_cam) { + if (now > lightship->next_cam_max) { rv = 1; + } else if (now > lightship->next_cam_min) { + + struct it2s_gps_data gps_data; + it2s_gps_read(&gps_data); + + // Check heading delta > 4ยบ + long heading_value, heading_confidence; + if (!get_heading(&gps_data, &heading_value, &heading_confidence)) { + int diff = heading_value - lightship->heading; + if (abs(diff) > 40) rv = 1; + } + + if (!rv) { + // Check speed delta > 0.5 m/s + long speed_value, speed_confidence; + if (!get_speed(&gps_data, &speed_value, &speed_confidence)) { + int diff = speed_value - lightship->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 = (speed_value + lightship->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; + } + } } + + // Remove expired PZs for (int i = 0; i < lightship->pz_len; ++i) { uint64_t expiry; @@ -286,7 +452,7 @@ int lightship_check(lightship_t* lightship) { pthread_mutex_unlock(&lightship->lock); - + return rv; } @@ -299,12 +465,13 @@ void lightship_reset_timer(lightship_t* lightship) { pthread_mutex_lock(&lightship->lock); if (lightship->type != StationType_roadSideUnit) { // Vehicle - lightship->next_cam = now + lightship->vehicle_gen_max; + 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 = now + lightship->rsu_gen_min; + lightship->next_cam_min = now + lightship->rsu_gen_min; } pthread_mutex_unlock(&lightship->lock); @@ -349,15 +516,15 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, ServiceSpecificPer case SpecialVehicleContainer_PR_emergencyContainer: if (!permissions_check(CID_EMERGENCY, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; 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->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} - } - if (bm & 0x01) { - if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} - } + 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->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } + if (bm & 0x01) { + if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp->choice.bitmapSsp.buf, ssp->choice.bitmapSsp.size)) {rv = 1; return rv;} + } } break; case SpecialVehicleContainer_PR_safetyCarContainer: @@ -393,15 +560,15 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, ServiceSpecificPer } 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; + 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->pz_len < 256) { for (int k = 0; k < pzs->list.count; ++k) { bool found = false; for (int j = 0; j < lightship->pz_len; ++j) { if (lightship->pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude && - lightship->pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) { + lightship->pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) { found = true; break; } @@ -445,24 +612,24 @@ static int check_pz(lightship_t *lightship) { pthread_mutex_lock(&lightship->lock); for (int i = 0; i < lightship->pz_len; ++i) { - double d_lat = (gps_data.gps_latitude - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG; - double d_lon = (gps_data.gps_longitude - (double) lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG; + double d_lat = (gps_data.gps_latitude - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG; + double d_lon = (gps_data.gps_longitude - (double) lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG; - double a = pow(sin(d_lat/2.0), 2) + - cos(gps_data.gps_latitude * RAD_PER_DEG) * - cos((double) lightship->pz[i]->protectedZoneLatitude/10000000 * RAD_PER_DEG) * - pow(sin(d_lon/2.0), 2); + double a = pow(sin(d_lat/2.0), 2) + + cos(gps_data.gps_latitude * 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; - if (lightship->pz[i]->protectedZoneRadius) { - pz_radius = *lightship->pz[i]->protectedZoneRadius; - } - if (d < pz_radius) { - is_inside = true; - } + double c = 2 * atan2(sqrt(a), sqrt(1-a)); + double d = EARTH_RADIUS * c; + + int pz_radius = 50; + if (lightship->pz[i]->protectedZoneRadius) { + pz_radius = *lightship->pz[i]->protectedZoneRadius; + } + if (d < pz_radius) { + is_inside = true; + } } pthread_mutex_unlock(&lightship->lock); diff --git a/src/cam.h b/src/cam.h index 82026b4..492f2a2 100644 --- a/src/cam.h +++ b/src/cam.h @@ -40,7 +40,15 @@ typedef struct lightship { uint8_t type; - uint64_t next_cam; + uint64_t last_cam; + uint64_t next_cam_max; + uint64_t next_cam_min; + + uint16_t heading; + int32_t lon; + int32_t lat; + uint16_t speed; + bool is_vehicle_near; uint64_t last_vehicle;