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

Variable CAM frequency

This commit is contained in:
emanuel 2021-03-10 21:34:45 +00:00
parent 74b514eff3
commit a81188976a
2 changed files with 296 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

399
src/cam.c
View File

@ -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;
@ -128,102 +270,96 @@ 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) {
@ -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;
@ -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;
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;
}
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);

10
src/cam.h
View File

@ -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;