From 80e532aefddc2e4774cdcbc9e3cb434976a840b7 Mon Sep 17 00:00:00 2001 From: emanuel Date: Thu, 11 Mar 2021 18:24:32 +0000 Subject: [PATCH] CAM pathHistory --- src/cam.c | 109 +++++++++++++++++++++++++++++++++++++++++------------- src/cam.h | 20 ++++++++-- 2 files changed, 100 insertions(+), 29 deletions(-) diff --git a/src/cam.c b/src/cam.c index f875cfb..bd2d4a6 100644 --- a/src/cam.c +++ b/src/cam.c @@ -244,13 +244,15 @@ static int get_altitude(struct it2s_gps_data* gps_data, long* altitude_value, lo } else { *altitude_value = AltitudeValue_unavailable; *altitude_confidence = AltitudeConfidence_unavailable; + + rv = 1; } 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; 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; 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_tx->header.messageID = ItsPduHeader__messageID_cam; + cam->header.protocolVersion = 2; + cam->header.messageID = ItsPduHeader__messageID_cam; pthread_mutex_lock(&facilities->lock); - cam_tx->header.stationID = facilities->station_id; + cam->header.stationID = facilities->station_id; 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); generationdeltatime = generationdeltatime - 1072915200000; // EPOCH -> 2004/01/01 00:00:000 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); @@ -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.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) { - cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - BasicVehicleContainerHighFrequency_t* bvc_hf = &cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; bvc_hf->vehicleWidth = 20; bvc_hf->vehicleLength.vehicleLengthValue = 46; 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); // 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; + 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; 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; + 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 { - cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; + cam->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; + 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->pz_len; pzs->list.size = 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); - 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) { syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name); 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; cleanup: - ASN_STRUCT_FREE(asn_DEF_CAM, cam_tx); + ASN_STRUCT_FREE(asn_DEF_CAM, cam); return rv; } @@ -382,6 +439,8 @@ lightship_t* lightship_init() { lightship->pz = malloc(256 * sizeof(void*)); pthread_mutex_init(&lightship->lock, NULL); + lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*)); + return lightship; } @@ -411,7 +470,7 @@ int lightship_check(lightship_t* lightship) { // Check heading delta > 4ยบ long 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; } @@ -419,14 +478,14 @@ int lightship_check(lightship_t* lightship) { // 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; + int diff = speed_value - 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 = (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 */ if (avg_speed * delta_time > 4) rv = 1; } @@ -659,7 +718,7 @@ void *ca_service(void *fc) { bdr->gnTrafficClass = 2; - bdr->data.buf = malloc(384); + bdr->data.buf = malloc(512); if (facilities->use_security) { bdr->gnSecurityProfile = malloc(sizeof(long)); diff --git a/src/cam.h b/src/cam.h index 492f2a2..f2a87f6 100644 --- a/src/cam.h +++ b/src/cam.h @@ -9,6 +9,9 @@ #include #include +#define POS_HISTORY_MAX_LEN 24 +#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 + typedef enum CID_CAM { CID_RESERVED, CID_PROTECTED_ZONES, @@ -33,6 +36,15 @@ typedef struct cid_ssp_bm { const uint32_t bitmap_val; } 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 { bool active; @@ -44,10 +56,10 @@ typedef struct lightship { uint64_t next_cam_max; uint64_t next_cam_min; - uint16_t heading; - int32_t lon; - int32_t lat; - uint16_t speed; + uint64_t last_cam_lfc; + + pos_vector_t** pos_history; + uint16_t pos_history_len; bool is_vehicle_near;