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

291
src/cam.c
View File

@ -107,6 +107,148 @@ static SpeedConfidence_t getSpeedConfidence(uint32_t conf) {
return 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) { static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
int rv = 0; int rv = 0;
@ -128,38 +270,65 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
pthread_mutex_unlock(&facilities->lock); pthread_mutex_unlock(&facilities->lock);
cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type; cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type;
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;
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) {
get_wgs84_coordinates(&gps_data, &bc->referencePosition.latitude, &bc->referencePosition.longitude);
} else {
bc->referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000);
bc->referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000);
}
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) { if (facilities->station_type != StationType_roadSideUnit) {
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46; bvc_hf->vehicleWidth = 20;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; bvc_hf->vehicleLength.vehicleLengthValue = 46;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; bvc_hf->driveDirection = DriveDirection_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable; bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable; bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable; bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable; bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
if (!isnan(gps_data.gps_track)) { // Set speed
cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10)); get_speed(&gps_data, &bvc_hf->speed.speedValue, &bvc_hf->speed.speedConfidence);
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;
} // 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;
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 { } else {
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (facilities->lightship->pz_len > 0) { if (facilities->lightship->pz_len > 0) {
cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t)); cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t));
ProtectedCommunicationZonesRSU_t *pzs = cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; ProtectedCommunicationZonesRSU_t *pzs = cam_tx->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
@ -187,43 +356,10 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
} }
} }
} }
} }
long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); pthread_mutex_unlock(&facilities->lightship->lock);
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;
}
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;
}
} 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);
}
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;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256); asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256);
if (enc.encoded == -1) { if (enc.encoded == -1) {
@ -261,13 +397,43 @@ int lightship_check(lightship_t* lightship) {
pthread_mutex_lock(&lightship->lock); pthread_mutex_lock(&lightship->lock);
if (lightship->type == StationType_roadSideUnit) { // RSU 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; rv = 1;
} }
} else { // Vehicle } else { // Vehicle
if (now > lightship->next_cam) { if (now > lightship->next_cam_max) {
rv = 1; 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 // Remove expired PZs
for (int i = 0; i < lightship->pz_len; ++i) { for (int i = 0; i < lightship->pz_len; ++i) {
uint64_t expiry; uint64_t expiry;
@ -299,12 +465,13 @@ void lightship_reset_timer(lightship_t* lightship) {
pthread_mutex_lock(&lightship->lock); pthread_mutex_lock(&lightship->lock);
if (lightship->type != StationType_roadSideUnit) { // Vehicle 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 } else { // RSU
if (now > lightship->last_vehicle + lightship->rsu_vehicle_permanence) { if (now > lightship->last_vehicle + lightship->rsu_vehicle_permanence) {
lightship->is_vehicle_near = false; 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); pthread_mutex_unlock(&lightship->lock);

10
src/cam.h
View File

@ -40,7 +40,15 @@ typedef struct lightship {
uint8_t type; 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; bool is_vehicle_near;
uint64_t last_vehicle; uint64_t last_vehicle;