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

more evcsnm related changes

This commit is contained in:
gilteixeira 2023-02-21 17:27:30 +00:00
parent 05e920c688
commit 5f37056d2f
7 changed files with 746 additions and 1177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.vscode/settings.json vendored
View File

@ -3,6 +3,16 @@
"packet.h": "c",
"managementrequest.h": "c",
"facilitiesindication.h": "c",
"constr_sequence.h": "c"
"constr_sequence.h": "c",
"btppacketindication.h": "c",
"transportrequest.h": "c",
"evcsnm.h": "c",
"mman.h": "c",
"space.h": "c",
"evcsnpdu.h": "c",
"asn_application.h": "c",
"itspduheader.h": "c",
"evchargingspotnotificationpoimessage.h": "c",
"vcm.h": "c"
}
}

2
src/CMakeLists.txt
View File

@ -10,6 +10,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
saem.c
tpm.c
vcm.c
evcsnm.c
)
TARGET_LINK_LIBRARIES(it2s-itss-facilities
@ -28,6 +29,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-saem
-lit2s-asn-tpm
-lit2s-asn-vcm
-lit2s-asn-evcsnm
-lit2s-asn-verco
-lit2s-tender
-lit2s-obd

3
src/config.c
View File

@ -394,6 +394,9 @@ int facilities_config() {
facilities.coordination.vcm_period_min = config->facilities.mcm.period_min;
facilities.coordination.vcm_period_max = config->facilities.mcm.period_max;
// EVCSNM
facilities.evcsnm_args.activate = config->facilities.evcsnm.activate;
// Replay
facilities.replay = config->networking.replay.activate;

563
src/evcsnm.c
View File

@ -21,71 +21,19 @@
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <it2s-obd.h>
#define EARTH_RADIUS 6369000.0
#define RAD_PER_DEG M_PI / 180.0
const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000},
{"publicTransport/publicTransportContainer", 0x4000},
{"specialTransport/specialTransportContainer", 0x2000},
{"dangerousGoods/dangerousGoodsContainer", 0x1000},
{"roadwork/roadWorksContainerBasic", 0x0800},
{"rescue/rescueContainer", 0x0400},
{"emergency/emergencyContainer", 0x0200},
{"safetyCar/safetyCarContainer", 0x0100},
{"closedLanes/RoadworksContainerBasic", 0x0080},
{"requestForRightOfWay/EmergencyContainer: EmergencyPriority", 0x0040},
{"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", 0x0020},
{"noPassing/SafetyCarContainer: TrafficRule", 0x0010},
{"noPassingForTrucks/SafetyCarContainer: TrafficRule", 0x0008},
{"speedLimit/SafetyCarContainer", 0x0004},
{"reserved0", 0x0002},
{"reserved1", 0x0001},
};
static int permissions_check(int cid, uint8_t *permissions, uint8_t permissions_len)
static UTF8String_t *create_empty_utf8_string()
{
/* evcsnm SSP scheme
*
* byte | description
* ---------------------------------
* 0 | SSP version control
* 1-2 | Service-specific parameter
* 3-30 | Reserved
*/
if (permissions_len < 3)
{
log_debug("[ca] permissions length too small");
return 0;
}
uint16_t perms_int = *(uint16_t *)(permissions + 1);
uint16_t perm_val = CID_SSP_BM_MAP[cid].bitmap_val;
perm_val = (perm_val >> 8) | (perm_val << 8);
if ((perm_val & perms_int) == perm_val)
return 1;
else
return 0;
UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t));
utf8_string->buf = calloc(1, 1);
utf8_string->size = 0;
utf8_string->buf[0] = '\0';
return utf8_string;
}
static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len)
{
int rv = 0;
int shm_fd, shm_valid = 0;
it2s_obd_data *shared_message;
lightship_t *lightship = &facilities.lightship;
if (lightship->use_obd)
{
it2s_obd_data *shared_message = malloc(sizeof(it2s_obd_data));
it2s_obd_read(shared_message);
}
EvcsnPdu_t *evcsnm = calloc(1, sizeof(EvcsnPdu_t));
evcsnm->header.protocolVersion = 2;
@ -97,40 +45,46 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len)
uint64_t now = itss_time_get();
evcsnm->evcsn.poiHeader.poiType = 1; // set to "EV charging station POI ID = 1"
asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now % 65536);
asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now);
evcsnm->evcsn.poiHeader.relayCapable = 0;
pthread_mutex_lock(&lightship->lock);
if (facilities.station_type != StationType_roadSideUnit)
{
itss_space_lock();
itss_space_get();
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
evcsnm->evcsn.evcsnData.totalNumberOfStations = 1;
evcsnm->evcsn.evcsnData.chargingStationsData.list.array = calloc(1, sizeof(void *));
evcsnm->evcsn.evcsnData.chargingStationsData.list.count = 1;
evcsnm->evcsn.evcsnData.chargingStationsData.list.size = 1;
evcsnm->evcsn.evcsnData.chargingStationsData.list.array[0] = calloc(1, sizeof(struct ItsChargingStationData));
struct ItsChargingStationData *cs0 = evcsnm->evcsn.evcsnData.chargingStationsData.list.array[0];
cs0->chargingStationID = 0;
itss_space_lock();
itss_space_get();
cs0->chargingStationLocation.latitude = epv.space.latitude;
cs0->chargingStationLocation.longitude = epv.space.longitude;
cs0->chargingStationLocation.altitude.altitudeValue = epv.space.altitude;
cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.altitude_conf;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = SemiAxisLength_unavailable;
cs0->accessibility = *create_empty_utf8_string();
cs0->pricing = *create_empty_utf8_string();
cs0->openingDaysHours = *create_empty_utf8_string();
cs0->chargingSpotsAvailable.list.array = calloc(1, sizeof(void *));
cs0->chargingSpotsAvailable.list.count = 1;
cs0->chargingSpotsAvailable.list.size = 1;
cs0->chargingSpotsAvailable.list.array[0] = calloc(1, sizeof(struct ItsChargingSpotDataElements));
struct ItsChargingSpotDataElements *cs_elem0 = cs0->chargingSpotsAvailable.list.array[0];
cs_elem0->energyAvailability = *create_empty_utf8_string();
cs_elem0->type.buf = calloc(1, sizeof(uint8_t));
cs_elem0->type.size = 1;
cs_elem0->type.bits_unused = 24;
cs_elem0->type.buf[0] = 0;
cs_elem0->typeOfReceptacle.buf = calloc(1, sizeof(uint8_t));
cs_elem0->typeOfReceptacle.size = 1;
cs_elem0->typeOfReceptacle.bits_unused = 24;
itss_space_unlock(epv);
// Reference position ellipse
//if (lat_conf > lon_conf)
//{
// bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
// bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
// bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
//}
//else
//{
// bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
// bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
// bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
//}
}
else
{
}
pthread_mutex_unlock(&lightship->lock);
itss_space_unlock(epv);
// if (facilities.station_type == StationType_roadSideUnit)
//{
// }
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EvcsnPdu, NULL, evcsnm, evcsnm_oer, 512);
if (enc.encoded == -1)
@ -141,429 +95,22 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len)
}
*evcsnm_len = (enc.encoded + 7) / 8;
if (lightship->use_obd)
free(shared_message);
cleanup:
ASN_STRUCT_FREE(asn_DEF_EvcsnPdu, evcsnm);
return rv;
}
int lightship_init()
{
lightship_t *lightship = &facilities.lightship;
lightship->protected_zones.pz = calloc(256, sizeof(void *));
pthread_mutex_init(&lightship->lock, NULL);
int shm_fd;
shm_fd = shm_open("it2s-obd", O_RDONLY, 0666);
if (shm_fd == -1)
{
log_debug("obd shmem not found\n");
lightship->use_obd = 0;
}
else
{
log_debug("obd shmem found\n");
lightship->use_obd = 1;
close(shm_fd);
}
int evcsnm_check(EvcsnPdu_t* evcsnm) {
return 0;
}
int lightship_check()
enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len)
{
int rv = 0;
lightship_t *lightship = &facilities.lightship;
uint64_t now = itss_time_get();
pthread_mutex_lock(&lightship->lock);
if (lightship->type == StationType_roadSideUnit)
{ // RSU
if (lightship->is_vehicle_near && now > lightship->next_evcsnm_min)
{
rv = 1;
}
}
else
{ // Vehicle
if (now > lightship->next_evcsnm_max)
{
rv = 1;
}
else if (now > lightship->next_evcsnm_min)
{
itss_space_lock();
itss_space_get();
// Check heading delta > 4º
int diff = epv.space.heading - lightship->last_pos.heading;
if (abs(diff) > 40)
rv = 1;
if (!rv)
{
// Check speed delta > 0.5 m/s
diff = epv.space.speed - lightship->last_pos.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 = (epv.space.speed + lightship->last_pos.speed) / 2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->t_last_evcsnm) / 1000; /* ms to s */
if (avg_speed * delta_time > 4)
rv = 1;
}
}
itss_space_unlock(epv);
}
// Remove expired PZs
for (int i = 0; i < lightship->protected_zones.pz_len; ++i)
{
uint64_t expiry;
if (lightship->protected_zones.pz[i]->expiryTime)
{
asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, (unsigned long long *)&expiry);
if (now >= expiry)
{
ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]);
for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j)
{
lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j + 1];
}
--lightship->protected_zones.pz_len;
}
}
}
}
pthread_mutex_unlock(&lightship->lock);
return rv;
}
void lightship_reset_timer()
{
lightship_t *lightship = &facilities.lightship;
uint64_t now = itss_time_get();
pthread_mutex_lock(&lightship->lock);
if (lightship->type != StationType_roadSideUnit)
{ // Vehicle
lightship->next_evcsnm_max = now + lightship->vehicle_gen_max;
lightship->next_evcsnm_min = now + lightship->vehicle_gen_min;
}
else
{ // RSU
if (now > lightship->t_last_vehicle + lightship->rsu_vehicle_permanence)
{
lightship->is_vehicle_near = false;
}
lightship->next_evcsnm_min = now + lightship->rsu_gen_min;
}
pthread_mutex_unlock(&lightship->lock);
}
enum evcsnm_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, evcsnm_t *evcsnm, uint8_t *ssp, uint32_t ssp_len)
{
int rv = 0;
lightship_t *lightship = &facilities.lightship;
uint64_t now = itss_time_get();
// Check permissions
if (ssp)
{
if (evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency &&
evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU)
{
if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container);
return rv;
}
}
if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer)
{
switch (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->present)
{
case SpecialVehicleContainer_PR_NOTHING:
break;
case SpecialVehicleContainer_PR_publicTransportContainer:
if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container);
return rv;
}
break;
case SpecialVehicleContainer_PR_specialTransportContainer:
if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container);
return rv;
}
break;
case SpecialVehicleContainer_PR_dangerousGoodsContainer:
if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container);
return rv;
}
break;
case SpecialVehicleContainer_PR_roadWorksContainerBasic:
if (!permissions_check(CID_ROADWORK, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container);
return rv;
}
if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes)
{
if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container);
return rv;
}
}
break;
case SpecialVehicleContainer_PR_rescueContainer:
if (!permissions_check(CID_RESCUE, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container);
return rv;
}
break;
case SpecialVehicleContainer_PR_emergencyContainer:
if (!permissions_check(CID_EMERGENCY, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container);
return rv;
}
if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf)
{
// TODO verify bitmap
uint8_t bm = *evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
if (bm & 0x02)
{
if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_RIGHT_OF_WAY].container);
return rv;
}
}
if (bm & 0x01)
{
if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container);
return rv;
}
}
}
break;
case SpecialVehicleContainer_PR_safetyCarContainer:
if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container);
return rv;
}
if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule)
{
switch (*evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule)
{
case TrafficRule_noPassing:
if (!permissions_check(CID_NO_PASSING, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container);
return rv;
}
break;
case TrafficRule_noPassingForTrucks:
if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container);
return rv;
}
break;
default:
break;
}
}
if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit)
{
if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len))
{
rv = evcsnm_BAD_PERMISSIONS;
log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPEED_LIMIT].container);
return rv;
}
}
break;
}
}
}
pthread_mutex_lock(&lightship->lock);
if (lightship->type == StationType_roadSideUnit)
{
// Send evcsnms if vehicles nearby
if (bpi->stationType != StationType_roadSideUnit && bpi->isNeighbour)
{
lightship->t_last_vehicle = now;
lightship->is_vehicle_near = true;
}
}
else
{
// Protected zones
if (evcsnm->evcsnm.evcsnmParameters.basicContainer.stationType == StationType_roadSideUnit &&
evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU)
{
ProtectedCommunicationZonesRSU_t *pzs = evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255)
{
bool new_pz = false;
for (int k = 0; k < pzs->list.count; ++k)
{
bool found = false;
for (int j = 0; j < lightship->protected_zones.pz_len; ++j)
{
if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude)
{
found = true;
break;
}
}
if (found)
continue;
new_pz = true;
lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;
if (pzs->list.array[k]->expiryTime)
{
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
}
if (pzs->list.array[k]->protectedZoneID)
{
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8);
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
}
if (pzs->list.array[k]->protectedZoneRadius)
{
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8);
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
}
++lightship->protected_zones.pz_len;
}
// Inform [management]
if (new_pz)
{
uint8_t b_oer[512];
ManagementRequest_t *mreq = calloc(1, sizeof(ManagementRequest_t));
mreq->present = ManagementRequest_PR_attributes;
mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones));
mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len;
mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void *);
mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void *));
for (int p = 0; p < lightship->protected_zones.pz_len; ++p)
{
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512);
oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void **)&mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded);
}
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512);
void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
itss_0send(management_socket, b_oer, enc.encoded);
log_debug("[ca]-> sending MReq.attributes.set.protectedZones to ->[management]");
uint8_t code;
itss_0recv_rt(&management_socket, &code, 1, b_oer, enc.encoded, 1000);
itss_0close(management_socket);
}
}
}
}
pthread_mutex_unlock(&lightship->lock);
return rv;
}
static int check_pz()
{
lightship_t *lightship = &facilities.lightship;
bool is_inside = false;
itss_space_lock();
itss_space_get();
double lat = epv.space.latitude / 10000000.0;
double lon = epv.space.longitude / 10000000.0;
itss_space_unlock();
pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->protected_zones.pz_len; ++i)
{
double d = itss_haversine(lat, lon, (double)lightship->protected_zones.pz[i]->protectedZoneLatitude / 10000000.0, (double)lightship->protected_zones.pz[i]->protectedZoneLongitude / 10000000.0);
int pz_radius = 50;
if (lightship->protected_zones.pz[i]->protectedZoneRadius)
{
pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
}
if (d < pz_radius)
{
is_inside = true;
break;
}
}
pthread_mutex_unlock(&lightship->lock);
return is_inside;
}
void *ca_service()
void *evcsn_service()
{
int rv = 0;
@ -584,7 +131,7 @@ void *ca_service()
bpr->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = Port_evcsnm;
bpr->destinationPort = Port_cam;
bpr->gn.trafficClass = 2;
@ -597,7 +144,7 @@ void *ca_service()
FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t *fmi = &fi->choice.message;
fmi->itsMessageType = ItsMessageType_evcsnm;
fmi->itsMessageType = messageID_evcsn;
fmi->data.buf = malloc(512);
uint8_t tr_oer[1024];
@ -606,9 +153,8 @@ void *ca_service()
fi_oer[0] = 4;
while (!facilities.exit)
{
usleep(1000 * 50);
if (lightship_check() && facilities.lightship.active)
usleep(1000 * 1000);
if (facilities.evcsnm_args.activate)
{
rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size);
if (rv)
@ -618,11 +164,6 @@ void *ca_service()
memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size);
fmi->data.size = bpr->data.size;
// Check if inside PZ
bpr->gn.communicationProfile = 0;
if (facilities.station_type != 15 && check_pz())
bpr->gn.communicationProfile = 1;
uint32_t id = itss_id(bpr->data.buf, bpr->data.size);
bpr->id = id;
fmi->id = id;
@ -645,15 +186,13 @@ void *ca_service()
itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message");
lightship_reset_timer();
// Logging
if (facilities.logging.dbms)
{
pthread_mutex_lock(&facilities.id.lock);
uint32_t station_id = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock);
itss_db_add(facilities.logging.dbms, station_id, bpr->id, true, messageID_evcsnm, NULL, bpr->data.buf, bpr->data.size);
itss_db_add(facilities.logging.dbms, station_id, bpr->id, true, messageID_evcsn, NULL, bpr->data.buf, bpr->data.size);
}
if (facilities.logging.recorder)

126
src/evcsnm.h
View File

@ -1,12 +1,12 @@
#ifndef FACILITIES_CAM_H
#define FACILITIES_CAM_H
#ifndef FACILITIES_EVCSNM_H
#define FACILITIES_EVCSNM_H
#include <stdint.h>
#include <stdlib.h>
#include <pthread.h>
#include <stdbool.h>
#include <it2s-asn/camv2/CAM.h>
#include <it2s-asn/evcsnm/EvcsnPdu.h>
#include <it2s-asn/itss-transport/BTPPacketIndication.h>
#include <it2s-tender/epv.h>
@ -14,117 +14,35 @@
#define POS_HISTORY_MAX_LEN 24
#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1
typedef enum CID_CAM {
CID_PROTECTED_ZONES,
CID_PUBLIC_TRANSPORT,
CID_SPECIAL_TRANSPORT,
CID_DANGEROUS_GOODS,
CID_ROADWORK,
CID_RESCUE,
CID_EMERGENCY,
CID_SAFETY_CAR,
CID_CLOSED_LANES,
CID_REQUEST_FOR_RIGHT_OF_WAY,
CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT,
CID_NO_PASSING,
CID_NO_PASSING_FOR_TRUCKS,
CID_SPEED_LIMIT,
CID_RESERVED0,
CID_RESERVED1,
} CID_CAM_e;
typedef struct cid_ssp_bm {
const char* container;
const uint32_t bitmap_val;
} cid_ssp_bm_t;
enum CAM_CHECK_R {
CAM_OK,
CAM_INVALID,
CAM_BAD_PERMISSIONS
enum EVCSNM_CHECK_R {
EVCSNM_OK,
EVCSNM_INVALID,
EVCSNM_BAD_PERMISSIONS
};
typedef struct pos_point {
uint64_t ts;
uint16_t heading;
int32_t lon;
int32_t lat;
int32_t alt;
uint16_t speed;
} pos_point_t;
typedef struct lightship {
bool active;
pthread_mutex_t lock;
uint8_t type;
pos_point_t last_pos;
uint64_t t_last_cam;
uint64_t next_cam_max;
uint64_t next_cam_min;
uint64_t t_last_cam_lfc;
pos_point_t concise_points[3];
uint8_t concise_points_len;
bool concise_keep_start;
pos_point_t* path_history[PATH_HISTORY_MAX_LEN];
uint16_t path_history_len;
bool is_vehicle_near;
uint64_t t_last_vehicle;
uint32_t vehicle_gen_min;
uint32_t vehicle_gen_max;
uint32_t rsu_gen_min;
uint32_t rsu_vehicle_permanence;
bool use_obd;
struct {
ProtectedCommunicationZone_t ** pz;
uint16_t pz_len;
} protected_zones;
} lightship_t;
/*
* @brief Initializes the main CA struct (lightship)
* @brief Analyzes a received EVCSNM
*
* @return Always zero
* @return A EVCSNM check code
*/
int lightship_init();
/*
* @brief Checks if a CAM must be sent
*
* @return True if CAM must be sent, false otherwise
*/
int lightship_check();
/*
* @brief Resets the CAM sending timer
*
* @return Nothing
*/
void lightship_reset_timer();
/*
* @brief Analyzes a received CAM
*
* @return A CAM check code
*/
enum CAM_CHECK_R check_cam(BTPPacketIndication_t* bpi, CAM_t* cam,uint8_t* ssp, uint32_t ssp_len);
enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len);
/*
* @brief Main CA service
*
* @return NULL
*/
void* ca_service();
void* evcsn_service();
/**
* Analyses a VCM
* @param vcm The VCM to be analyzed
* @return 0 on success, other value otherwise
*/
int evcsnm_check(EvcsnPdu_t* evcsnm);
typedef struct evcsnm_args {
bool activate;
} evcsnm_args_t;
#endif

1212
src/facilities.c
View File

File diff suppressed because it is too large Load Diff

5
src/facilities.h
View File

@ -12,6 +12,7 @@
#include "saem.h"
#include "tpm.h"
#include "vcm.h"
#include "evcsnm.h"
#include <it2s-tender/epv.h>
#include <it2s-tender/database.h>
@ -34,6 +35,7 @@ typedef struct facilities {
pthread_t cp_service;
pthread_t sa_service;
pthread_t vc_service;
pthread_t evcsn_service;
// ZMQ
struct {
@ -70,6 +72,9 @@ typedef struct facilities {
// PC
coordination_t coordination;
// EVCSN
evcsnm_args_t evcsnm_args;
// Logging
struct {
bool recorder;