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

update mcm implementation

This commit is contained in:
dmtar 2025-03-11 14:20:39 +00:00
parent 0f3190e1be
commit 5978b02a9c
1 changed files with 65 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
src/mcm.c
View File

@ -16,10 +16,9 @@
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-v2/mcm/EI2_MCM.h>
static void tx_mcm(EI2_MCM_t* mcm){
static void mcm_forward_to_app(EI2_MCM_t* mcm){
const uint16_t buf_len = 4096;
uint8_t buf[buf_len];
EIS_NetworkingRequest_t *nr = NULL;
EIS_FacilitiesIndication_t *fi = NULL;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_MCM, NULL, mcm, buf, buf_len);
@ -27,55 +26,15 @@ static void tx_mcm(EI2_MCM_t* mcm){
log_error("[mc] MCM encode failure (%s)", enc.failed_type->name);
goto cleanup;
}
uint16_t mcm_len = (enc.encoded + 7) / 8;
nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
nr->present = EIS_NetworkingRequest_PR_packet;
EIS_NetworkingPacketRequest_t *npr = &nr->choice.packet;
npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;
npr->network.choice.gn.trafficClass = 2;
npr->network.choice.gn.destinationAddress.buf = malloc(6);
for (int i = 0; i < 6; ++i){
npr->network.choice.gn.destinationAddress.buf[i] = 0xff;
}
npr->network.choice.gn.destinationAddress.size = 6;
npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
npr->network.choice.gn.securityProfile.sign = true;
npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;
npr->transport.choice.btp.btpType = EIS_BTPType_btpB;
npr->transport.choice.btp.destinationPort = EIS_Port_mcm;
if (facilities.edm.enabled && mcm->payload.mcmContainer.present == EI2_McmContainer_PR_vehiclemaneuverContainer){
edm_encap(buf, &mcm_len, buf_len, EIS_Port_mcm);
npr->transport.choice.btp.destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t));
npr->transport.choice.btp.destinationPortInfo->size = 2;
npr->transport.choice.btp.destinationPortInfo->buf = malloc(2);
*(uint16_t *)npr->transport.choice.btp.destinationPortInfo->buf = 0xed;
}
npr->id = itss_id(buf, mcm_len);
npr->data.buf = malloc(mcm_len);
memcpy(npr->data.buf, buf, mcm_len);
npr->data.size = mcm_len;
buf[0] = ITSS_FACILITIES;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_NetworkingRequest, nr, buf + 1, buf_len - 1);
if (enc.encoded == -1){
log_error("[mc] NR MCM.reply encode failure (%s)", enc.failed_type->name);
goto cleanup;
}
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_NETWORKING, npr->id, "NR.packet.btp"));
fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
fi->present = EIS_FacilitiesIndication_PR_message;
fi->choice.message.id = npr->id;
fi->choice.message.id = itss_id(buf, mcm_len);
fi->choice.message.itsMessageType = EIS_Port_mcm;
fi->choice.message.data.size = npr->data.size;
fi->choice.message.data.buf = malloc(npr->data.size);
memcpy(fi->choice.message.data.buf, npr->data.buf, npr->data.size);
fi->choice.message.data.size = mcm_len;
fi->choice.message.data.buf = malloc(mcm_len);
memcpy(fi->choice.message.data.buf, buf, mcm_len);
buf[0] = ITSS_FACILITIES;
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_EIS_FacilitiesIndication, fi, buf + 1, buf_len - 1);
if (enc.encoded == -1){
@ -83,16 +42,36 @@ static void tx_mcm(EI2_MCM_t* mcm){
goto cleanup;
}
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_APPLICATIONS, npr->id, "FI.message"));
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buf, enc.encoded + 1, ITSS_APPLICATIONS, itss_id(buf, mcm_len), "FI.message"));
cleanup:
ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);
ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
}
static int mk_mcm(){
uint64_t gdt, traj_len, lat, lon, alt, speed, heading, lat_conf, lon_conf, alt_conf, speed_conf, heading_conf;
struct GNSSDeltaPosition traj_points[16];
int rv = 0;
uint64_t gdt = itss_time_get() % 65536;
itss_time_lock();
gdt = itss_time_get() % 65536;
itss_time_unlock();
itss_space_lock();
itss_space_get();
lat = epv.space.data.latitude.value;
lat_conf = epv.space.data.latitude.confidence;
lon = epv.space.data.longitude.value;
lon_conf = epv.space.data.longitude.confidence;
alt = epv.space.data.altitude.value;
alt_conf = epv.space.data.altitude.confidence;
speed = epv.space.data.speed.value;
speed_conf = epv.space.data.speed.confidence;
heading = epv.space.data.heading.value;
heading_conf = epv.space.data.heading.confidence;
traj_len = epv.space.data.ptraj_len;
memcpy(traj_points, &epv.space.data.ptraj, sizeof(struct GNSSDeltaPosition)*traj_len);
itss_space_unlock();
// MCM
EI2_MCM_t* mcm = calloc(1, sizeof(EI2_MCM_t));
@ -101,17 +80,28 @@ static int mk_mcm(){
EI2_ItsPduHeader_t* header = &mcm->header;
header->protocolVersion = 2;
header->messageId = EI2_MessageId_mcm;
header->stationId = 0;
header->stationId = facilities.id.station_id;
// Payload
EI2_WrappedMcmInformationBlocks_t* payload = &mcm->payload;
EI2_McmBasicContainer_t* basicContainer = &payload->basicContainer;
asn_uint642INTEGER(&basicContainer->generationDeltaTime, gdt);
basicContainer->stationType = 0;
basicContainer->deltaReferencePosition.deltaLatitude = 0;
basicContainer->deltaReferencePosition.deltaLongitude = 0;
basicContainer->deltaReferencePosition.deltaAltitude = 0;
basicContainer->stationType = facilities.station_type;
basicContainer->referencePosition.latitude = lat;
basicContainer->referencePosition.longitude = lon;
basicContainer->referencePosition.altitude.altitudeValue = alt;
basicContainer->referencePosition.altitude.altitudeConfidence = alt_conf;
if (lat_conf > lon_conf) {
basicContainer->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lon_conf;
basicContainer->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lat_conf;
basicContainer->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
}
else {
basicContainer->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lon_conf;
basicContainer->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lat_conf;
basicContainer->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
}
EI2_McmContainer_t* mcmContainer = &payload->mcmContainer;
mcmContainer->present = EI2_McmContainer_PR_vehiclemaneuverContainer;
@ -137,17 +127,19 @@ static int mk_mcm(){
vas->humanDrivingLongitudinalAutomated = false;
vas->humanDrivenLateralAutomated = false;
vas->automatedDriving = false;
vcs->speed.speedValue = 0;
vcs->speed.speedConfidence = 1;
vcs->heading.headingValue = 0;
vcs->heading.headingConfidence = 1;
vcs->longitudinalAcceleration.longitudinalAccelerationValue = 0;
vcs->longitudinalAcceleration.longitudinalAccelerationConfidence = 0;
vcs->speed.speedValue = speed;
vcs->speed.speedConfidence = speed_conf;
vcs->heading.headingValue = heading;
vcs->heading.headingConfidence = heading_conf;
vcs->longitudinalAcceleration.longitudinalAccelerationValue = EI2_AccelerationValue_unavailable;
vcs->longitudinalAcceleration.longitudinalAccelerationConfidence = EI2_AccelerationConfidence_unavailable;
vcs->vehicleSize.vehicleLenth = 0;
vcs->vehicleSize.vehicleWidth = 0;
if (traj_len > 10)
traj_len = 10;
EI2_Wgs84Trajectory_t* trajectory = &vmcContainer->vehicleTrajectory.choice.wgs84Trajectory;
trajectory->trajectoryPoints.list.count = 2;
trajectory->trajectoryPoints.list.count = traj_len;
trajectory->trajectoryPoints.list.size = sizeof(EI2_Wgs84TrajectoryPoint_t*);
trajectory->trajectoryPoints.list.array = calloc(trajectory->trajectoryPoints.list.count, trajectory->trajectoryPoints.list.size);
@ -155,26 +147,22 @@ static int mk_mcm(){
trajectory->trajectoryPoints.list.array[i] = calloc(1, sizeof(EI2_Wgs84TrajectoryPoint_t));
EI2_Wgs84TrajectoryPoint_t* trajectory_point = trajectory->trajectoryPoints.list.array[i];
trajectory_point->intermediatePoint.present = EI2_IntermediatePoint_PR_reference;
trajectory_point->longitudePosition = 0;
trajectory_point->latitudePosition = 0;
trajectory_point->speed.speedValue = 0;
trajectory_point->speed.speedConfidence = 1;
trajectory_point->longitudePosition = lon+traj_points[i].deltaLongitude._0;
trajectory_point->latitudePosition = lat+traj_points[i].deltaLatitude._0;
trajectory_point->speed.speedValue = EI2_SpeedValue_unavailable;
trajectory_point->speed.speedConfidence = EI2_SpeedConfidence_unavailable;
EI2_IntermediatePointReference_t* ipr = &trajectory_point->intermediatePoint.choice.reference;
ipr->referenceStartingPosition.latitude = 0;
ipr->referenceStartingPosition.longitude = 0;
ipr->referenceStartingPosition.positionConfidenceEllipse.semiMajorAxisLength = 0;
ipr->referenceStartingPosition.positionConfidenceEllipse.semiMinorAxisLength = 0;
ipr->referenceStartingPosition.positionConfidenceEllipse.semiMajorAxisOrientation = 0;
ipr->referenceStartingPosition.altitude.altitudeValue = 0;
ipr->referenceStartingPosition.altitude.altitudeConfidence = 1;
ipr->referenceHeading.headingValue = 0;
ipr->referenceHeading.headingConfidence = 1;
ipr->lane.lanePosition = 0;
ipr->deltaReferencePosition.deltaLatitude = traj_points[i].deltaLongitude._0;
ipr->deltaReferencePosition.deltaLongitude = traj_points[i].deltaLatitude._0;
ipr->deltaReferencePosition.deltaAltitude = traj_points[i].deltaAltitude._0;
ipr->referenceHeading.headingValue = EI2_HeadingValue_unavailable;
ipr->referenceHeading.headingConfidence = EI2_HeadingConfidence_unavailable;
ipr->lane.lanePosition = EI2_LanePosition_innerHardShoulder;
ipr->lane.laneCount = 1;
ipr->timeOfPos = 0;
ipr->timeOfPos = traj_points[i].deltaTime._0;
}
tx_mcm(mcm);
mcm_forward_to_app(mcm);
cleanup:
ASN_STRUCT_FREE(asn_DEF_EI2_MCM, mcm);