From 5978b02a9c66628ede5b9658905068ff01f3ccce Mon Sep 17 00:00:00 2001 From: dmtar Date: Tue, 11 Mar 2025 14:20:39 +0000 Subject: [PATCH] update mcm implementation --- src/mcm.c | 142 +++++++++++++++++++++++++----------------------------- 1 file changed, 65 insertions(+), 77 deletions(-) diff --git a/src/mcm.c b/src/mcm.c index b754175..b36076d 100644 --- a/src/mcm.c +++ b/src/mcm.c @@ -16,10 +16,9 @@ #include #include -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);