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

Add confidence values to GPS coords, calculate CAM pos confidence

This commit is contained in:
emanuel 2021-07-05 18:26:59 +01:00
parent a22a723fca
commit c75624d8ca
3 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/cam.c
View File

@ -116,9 +116,8 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
// Set GPS coordinates
bc->referencePosition.latitude = facilities->epv.space.latitude;
bc->referencePosition.longitude = facilities->epv.space.longitude;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
uint16_t lat_conf = facilities->epv.space.latitude_conf;
uint16_t lon_conf = facilities->epv.space.longitude_conf;
cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
@ -133,6 +132,16 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
it2s_tender_unlock_space(&facilities->epv);
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;
}
bvc_hf->vehicleWidth = facilities->vehicle.width;
bvc_hf->vehicleLength.vehicleLengthValue = facilities->vehicle.length;
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;

2
src/config.c
View File

@ -342,7 +342,9 @@ int facilities_config(void* facilities_s) {
mrep->attributes->clockOffset &&
mrep->attributes->gpsType) {
facilities->epv.space.latitude = mrep->attributes->coordinates->latitude;
facilities->epv.space.latitude_conf = mrep->attributes->coordinates->latitudeConfidence;
facilities->epv.space.longitude = mrep->attributes->coordinates->longitude;
facilities->epv.space.longitude_conf = mrep->attributes->coordinates->longitudeConfidence;
facilities->epv.space.altitude = mrep->attributes->altitude->altitudeValue;
facilities->epv.space.altitude_conf = mrep->attributes->altitude->altitudeConfidence;
facilities->epv.space.heading = mrep->attributes->heading->headingValue;

2
src/facilities.c
View File

@ -402,7 +402,9 @@ static int management_indication(facilities_t* facilities, void* responder, uint
if (mi->present == ManagementIndication_PR_attributes) {
it2s_tender_lock_space(&facilities->epv);
facilities->epv.space.latitude = mi->choice.attributes.coordinates.latitude;
facilities->epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence;
facilities->epv.space.longitude = mi->choice.attributes.coordinates.longitude;
facilities->epv.space.longitude_conf = mi->choice.attributes.coordinates.longitudeConfidence;
facilities->epv.space.speed = mi->choice.attributes.speed.speedValue;
facilities->epv.space.speed_conf = mi->choice.attributes.speed.speedConfidence;
facilities->epv.space.heading = mi->choice.attributes.heading.headingValue;