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

Add heading to VCM vehs

This commit is contained in:
emanuel 2023-06-14 23:54:18 +01:00
parent 06bcca6f47
commit 514201526f
1 changed files with 31 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/vcm.c
View File

@ -80,7 +80,7 @@ static void tx_vcm(VCM_t* vcm) {
FacilitiesIndication_t* fi = NULL; FacilitiesIndication_t* fi = NULL;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm, buf, buf_len); asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm, buf, buf_len);
if (enc.encoded == -1) { if (enc.encoded == -1) {
log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name); log_error("[vc] VCM encode failure (%s)", enc.failed_type->name);
goto cleanup; goto cleanup;
} }
uint16_t vcm_len = (enc.encoded + 7) / 8; uint16_t vcm_len = (enc.encoded + 7) / 8;
@ -172,10 +172,14 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) {
pthread_mutex_unlock(&facilities.id.lock); pthread_mutex_unlock(&facilities.id.lock);
int32_t lat, lon; int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock(); itss_space_lock();
itss_space_get(); itss_space_get();
lat = epv.space.latitude; lat = epv.space.latitude;
lon = epv.space.longitude; lon = epv.space.longitude;
heading = epv.space.heading;
heading = epv.space.heading_conf;
itss_space_unlock(); itss_space_unlock();
uint64_t now = itss_time_get(); uint64_t now = itss_time_get();
@ -221,6 +225,8 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) {
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
mvc_rep->heading.headingValue = heading;
mvc_rep->heading.headingConfidence = heading_conf;
// Vehicle DimensionsDimensions // Vehicle DimensionsDimensions
mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length; mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
@ -313,10 +319,14 @@ static bool commit() {
pthread_mutex_unlock(&facilities.id.lock); pthread_mutex_unlock(&facilities.id.lock);
int32_t lat, lon; int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock(); itss_space_lock();
itss_space_get(); itss_space_get();
lat = epv.space.latitude; lat = epv.space.latitude;
lon = epv.space.longitude; lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
itss_space_unlock(); itss_space_unlock();
vcm_com->vcm.currentPosition.latitude = lat; vcm_com->vcm.currentPosition.latitude = lat;
vcm_com->vcm.currentPosition.longitude = lon; vcm_com->vcm.currentPosition.longitude = lon;
@ -327,6 +337,8 @@ static bool commit() {
vcm_com->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_com->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm_com->vcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm_com->vcm.maneuverContainer.choice.vehicle;
mvc->heading.headingValue = heading;
mvc->heading.headingConfidence = heading_conf;
// Vehicle Dimensions // Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length; mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
@ -453,10 +465,14 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
if (h) --h; if (h) --h;
int32_t lat, lon; int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock(); itss_space_lock();
itss_space_get(); itss_space_get();
lat = epv.space.latitude; lat = epv.space.latitude;
lon = epv.space.longitude; lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
itss_space_unlock(); itss_space_unlock();
double dreq = itss_haversine( double dreq = itss_haversine(
@ -546,6 +562,8 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
mvc_rep->heading.headingValue = heading;
mvc_rep->heading.headingConfidence = heading_conf;
// Vehicle DimensionsDimensions // Vehicle DimensionsDimensions
mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length; mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
@ -736,10 +754,14 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
int32_t lat, lon; int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
itss_space_lock(); itss_space_lock();
itss_space_get(); itss_space_get();
lat = epv.space.latitude; lat = epv.space.latitude;
lon = epv.space.longitude; lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
itss_space_unlock(); itss_space_unlock();
itss_trajectory_lock(); itss_trajectory_lock();
@ -787,6 +809,8 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle;
mvc->heading.headingValue = heading;
mvc->heading.headingConfidence = heading_conf;
// Vehicle Dimensions // Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length; mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
@ -1060,11 +1084,15 @@ static int mk_vcm() {
pthread_mutex_unlock(&facilities.id.lock); pthread_mutex_unlock(&facilities.id.lock);
int32_t lat, lon; int32_t lat, lon;
uint16_t heading;
uint8_t heading_conf;
uint16_t trajectory_len = 0; uint16_t trajectory_len = 0;
itss_space_lock(); itss_space_lock();
itss_space_get(); itss_space_get();
lat = epv.space.latitude; lat = epv.space.latitude;
lon = epv.space.longitude; lon = epv.space.longitude;
heading = epv.space.heading;
heading_conf = epv.space.heading_conf;
itss_space_unlock(); itss_space_unlock();
itss_trajectory_lock(); itss_trajectory_lock();
@ -1102,6 +1130,8 @@ static int mk_vcm() {
} else { } else {
vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle; ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
mvc->heading.headingValue = heading;
mvc->heading.headingConfidence = heading_conf;
// Vehicle Dimensions // Vehicle Dimensions
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length; mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;