194 lines
8.4 KiB
C
194 lines
8.4 KiB
C
#include "mcm.h"
|
|
#include "facilities.h"
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <stdint.h>
|
|
|
|
#include <it2s-gnss.h>
|
|
#include <it2s-tender/epv.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/recorder.h>
|
|
#include <it2s-tender/packet.h>
|
|
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
|
|
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
|
|
#include <it2s-asn/etsi-its-v2/mcm/EI2_MCM.h>
|
|
|
|
static void mcm_forward_to_app(EI2_MCM_t* mcm){
|
|
const uint16_t buf_len = 4096;
|
|
uint8_t buf[buf_len];
|
|
EIS_FacilitiesIndication_t *fi = NULL;
|
|
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_MCM, NULL, mcm, buf, buf_len);
|
|
if (enc.encoded == -1){
|
|
log_error("[mc] MCM encode failure (%s)", enc.failed_type->name);
|
|
goto cleanup;
|
|
}
|
|
uint16_t mcm_len = (enc.encoded + 7) / 8;
|
|
|
|
fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
|
|
fi->present = EIS_FacilitiesIndication_PR_message;
|
|
fi->choice.message.id = itss_id(buf, mcm_len);
|
|
fi->choice.message.itsMessageType = EIS_Port_mcm;
|
|
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){
|
|
log_error("[mc] TR 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_APPLICATIONS, itss_id(buf, mcm_len), "FI.message"));
|
|
|
|
cleanup:
|
|
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;
|
|
|
|
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));
|
|
|
|
// Header
|
|
EI2_ItsPduHeader_t* header = &mcm->header;
|
|
header->protocolVersion = 2;
|
|
header->messageId = EI2_MessageId_mcm;
|
|
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 = 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;
|
|
EI2_VehiclemaneuverContainer_t* vmcContainer = &mcmContainer->choice.vehiclemaneuverContainer;
|
|
vmcContainer->mcmType = EI2_McmType_intent;
|
|
vmcContainer->maneuverId = 0;
|
|
vmcContainer->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking;
|
|
vmcContainer->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal;
|
|
vmcContainer->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception;
|
|
vmcContainer->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started;
|
|
vmcContainer->trajectoryId = 0;
|
|
vmcContainer->vehicleTrajectory.present = EI2_vehicleTrajectory_PR_wgs84Trajectory;
|
|
|
|
EI2_VehicleCurrentState_t* vcs = &vmcContainer->vehicleCurrentState;
|
|
EI2_McmGenericCurrentState_t* mgcs = &vcs->mcmGenericCurrentState;
|
|
mgcs->mcmType = EI2_McmType_intent;
|
|
mgcs->maneuverId = 0;
|
|
mgcs->maneuverCoordinationConcept = EI2_ManeuverCoordinationConcept_agreementSeeking;
|
|
mgcs->maneuverCoordinationRational.present = EI2_ManeuverCoordinationRational_PR_maneuverCooperationGoal;
|
|
mgcs->maneuverCoordinationRational.choice.maneuverCooperationGoal = EI2_ManeuverCooperationGoal_vehicleInterception;
|
|
mgcs->maneuverExecutionStatus = EI2_ManeuverExecutionStatus_started;
|
|
EI2_VehicleAutomationState_t* vas = &vcs->vehicleAutomationState;
|
|
vas->humanDrivingLongitudinalAutomated = false;
|
|
vas->humanDrivenLateralAutomated = false;
|
|
vas->automatedDriving = false;
|
|
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 = traj_len;
|
|
trajectory->trajectoryPoints.list.size = sizeof(EI2_Wgs84TrajectoryPoint_t*);
|
|
trajectory->trajectoryPoints.list.array = calloc(trajectory->trajectoryPoints.list.count, trajectory->trajectoryPoints.list.size);
|
|
|
|
for (int i = 0; i < trajectory->trajectoryPoints.list.count; i++){
|
|
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 = 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->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 = traj_points[i].deltaTime._0;
|
|
}
|
|
|
|
mcm_forward_to_app(mcm);
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_EI2_MCM, mcm);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void *mc_service(){
|
|
int rv;
|
|
mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord;
|
|
|
|
while (!facilities.exit){
|
|
sleep(1);
|
|
|
|
pthread_mutex_lock(&mcm_coord->lock);
|
|
rv = mk_mcm();
|
|
if (rv)
|
|
continue;
|
|
pthread_mutex_unlock(&mcm_coord->lock);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void mcm_coord_init(){
|
|
mcm_coord_t *coord = &facilities.mcm_coord;
|
|
pthread_mutex_init(&coord->lock, NULL);
|
|
}
|