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

CAM pathHistory

This commit is contained in:
emanuel 2021-03-11 18:24:32 +00:00
parent a81188976a
commit 80e532aefd
2 changed files with 100 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/cam.c
View File

@ -244,13 +244,15 @@ static int get_altitude(struct it2s_gps_data* gps_data, long* altitude_value, lo
} else { } else {
*altitude_value = AltitudeValue_unavailable; *altitude_value = AltitudeValue_unavailable;
*altitude_confidence = AltitudeConfidence_unavailable; *altitude_confidence = AltitudeConfidence_unavailable;
rv = 1;
} }
return rv; 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_oer, uint32_t *cam_len) {
int rv = 0; int rv = 0;
struct timespec systemtime; struct timespec systemtime;
@ -261,22 +263,22 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
struct it2s_gps_data gps_data; struct it2s_gps_data gps_data;
it2s_gps_read(&gps_data); it2s_gps_read(&gps_data);
CAM_t *cam_tx = calloc(1, sizeof(CAM_t)); CAM_t *cam = calloc(1, sizeof(CAM_t));
cam_tx->header.protocolVersion = 2; cam->header.protocolVersion = 2;
cam_tx->header.messageID = ItsPduHeader__messageID_cam; cam->header.messageID = ItsPduHeader__messageID_cam;
pthread_mutex_lock(&facilities->lock); pthread_mutex_lock(&facilities->lock);
cam_tx->header.stationID = facilities->station_id; cam->header.stationID = facilities->station_id;
pthread_mutex_unlock(&facilities->lock); pthread_mutex_unlock(&facilities->lock);
cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type; cam->cam.camParameters.basicContainer.stationType = facilities->station_type;
long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); 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 - 1072915200000; // EPOCH -> 2004/01/01 00:00:000
generationdeltatime = generationdeltatime % 64536; generationdeltatime = generationdeltatime % 64536;
cam_tx->cam.generationDeltaTime = generationdeltatime; cam->cam.generationDeltaTime = generationdeltatime;
BasicContainer_t* bc = &cam_tx->cam.camParameters.basicContainer; BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
get_altitude(&gps_data, &bc->referencePosition.altitude.altitudeValue, &bc->referencePosition.altitude.altitudeConfidence); get_altitude(&gps_data, &bc->referencePosition.altitude.altitudeValue, &bc->referencePosition.altitude.altitudeConfidence);
@ -292,11 +294,12 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
pthread_mutex_lock(&facilities->lightship->lock); lightship_t* lightship = facilities->lightship;
pthread_mutex_lock(&lightship->lock);
if (facilities->station_type != StationType_roadSideUnit) { if (facilities->station_type != StationType_roadSideUnit) {
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
BasicVehicleContainerHighFrequency_t* bvc_hf = &cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
bvc_hf->vehicleWidth = 20; bvc_hf->vehicleWidth = 20;
bvc_hf->vehicleLength.vehicleLengthValue = 46; bvc_hf->vehicleLength.vehicleLengthValue = 46;
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
@ -316,22 +319,76 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
get_heading(&gps_data, &bvc_hf->heading.headingValue, &bvc_hf->heading.headingConfidence); get_heading(&gps_data, &bvc_hf->heading.headingValue, &bvc_hf->heading.headingConfidence);
// Save current values // Save current values
facilities->lightship->heading = bvc_hf->heading.headingValue; if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) {
facilities->lightship->lat = bc->referencePosition.latitude; free(lightship->pos_history[POS_HISTORY_MAX_LEN-1]);
facilities->lightship->lon = bc->referencePosition.longitude; --lightship->pos_history_len;
facilities->lightship->speed = bvc_hf->speed.speedValue; }
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;
struct timespec systemtime; struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime); clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
now -= 1072915200000; now -= 1072915200000;
facilities->lightship->last_cam = now; lightship->last_cam = now;
lightship->pos_history[0]->ts = now;
++lightship->pos_history_len;
// 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 { } else {
cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; cam->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->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->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
pzs->list.count = facilities->lightship->pz_len; pzs->list.count = facilities->lightship->pz_len;
pzs->list.size = facilities->lightship->pz_len * sizeof(void*); pzs->list.size = facilities->lightship->pz_len * sizeof(void*);
pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*)); pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*));
@ -361,7 +418,7 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
pthread_mutex_unlock(&facilities->lightship->lock); pthread_mutex_unlock(&facilities->lightship->lock);
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, cam_oer, 512);
if (enc.encoded == -1) { if (enc.encoded == -1) {
syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name); syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name);
rv = 1; rv = 1;
@ -370,7 +427,7 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) {
*cam_len = (enc.encoded + 7) / 8; *cam_len = (enc.encoded + 7) / 8;
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_CAM, cam_tx); ASN_STRUCT_FREE(asn_DEF_CAM, cam);
return rv; return rv;
} }
@ -382,6 +439,8 @@ lightship_t* lightship_init() {
lightship->pz = malloc(256 * sizeof(void*)); lightship->pz = malloc(256 * sizeof(void*));
pthread_mutex_init(&lightship->lock, NULL); pthread_mutex_init(&lightship->lock, NULL);
lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*));
return lightship; return lightship;
} }
@ -411,7 +470,7 @@ int lightship_check(lightship_t* lightship) {
// Check heading delta > 4º // Check heading delta > 4º
long heading_value, heading_confidence; long heading_value, heading_confidence;
if (!get_heading(&gps_data, &heading_value, &heading_confidence)) { if (!get_heading(&gps_data, &heading_value, &heading_confidence)) {
int diff = heading_value - lightship->heading; int diff = heading_value - lightship->pos_history[0]->heading;
if (abs(diff) > 40) rv = 1; if (abs(diff) > 40) rv = 1;
} }
@ -419,14 +478,14 @@ int lightship_check(lightship_t* lightship) {
// Check speed delta > 0.5 m/s // Check speed delta > 0.5 m/s
long speed_value, speed_confidence; long speed_value, speed_confidence;
if (!get_speed(&gps_data, &speed_value, &speed_confidence)) { if (!get_speed(&gps_data, &speed_value, &speed_confidence)) {
int diff = speed_value - lightship->speed; int diff = speed_value - lightship->pos_history[0]->speed;
if (abs(diff) > 50) rv = 1; if (abs(diff) > 50) rv = 1;
} }
if (!rv) { if (!rv) {
// Check position delta > 4 m // Check position delta > 4 m
// TODO make an *accurate* distance calculator using GPS coords // TODO make an *accurate* distance calculator using GPS coords
int32_t avg_speed = (speed_value + lightship->speed)/2 / 100; /* cm/s to m/s */ int32_t avg_speed = (speed_value + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */ uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */
if (avg_speed * delta_time > 4) rv = 1; if (avg_speed * delta_time > 4) rv = 1;
} }
@ -659,7 +718,7 @@ void *ca_service(void *fc) {
bdr->gnTrafficClass = 2; bdr->gnTrafficClass = 2;
bdr->data.buf = malloc(384); bdr->data.buf = malloc(512);
if (facilities->use_security) { if (facilities->use_security) {
bdr->gnSecurityProfile = malloc(sizeof(long)); bdr->gnSecurityProfile = malloc(sizeof(long));

20
src/cam.h
View File

@ -9,6 +9,9 @@
#include <camv2/CAM.h> #include <camv2/CAM.h>
#include <itss-transport/BTPDataIndication.h> #include <itss-transport/BTPDataIndication.h>
#define POS_HISTORY_MAX_LEN 24
#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1
typedef enum CID_CAM { typedef enum CID_CAM {
CID_RESERVED, CID_RESERVED,
CID_PROTECTED_ZONES, CID_PROTECTED_ZONES,
@ -33,6 +36,15 @@ typedef struct cid_ssp_bm {
const uint32_t bitmap_val; const uint32_t bitmap_val;
} cid_ssp_bm_t; } cid_ssp_bm_t;
typedef struct pos_vector {
uint64_t ts;
uint16_t heading;
int32_t lon;
int32_t lat;
int32_t alt;
uint16_t speed;
} pos_vector_t;
typedef struct lightship { typedef struct lightship {
bool active; bool active;
@ -44,10 +56,10 @@ typedef struct lightship {
uint64_t next_cam_max; uint64_t next_cam_max;
uint64_t next_cam_min; uint64_t next_cam_min;
uint16_t heading; uint64_t last_cam_lfc;
int32_t lon;
int32_t lat; pos_vector_t** pos_history;
uint16_t speed; uint16_t pos_history_len;
bool is_vehicle_near; bool is_vehicle_near;