From 935fa8c43866ad64eb285e2c7316d8981be87ee8 Mon Sep 17 00:00:00 2001 From: David Rocha Date: Tue, 18 Jan 2022 21:49:23 +0000 Subject: [PATCH] add obd value mem alloc and assign to its cam fields --- src/cam.c | 85 ++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 56 insertions(+), 29 deletions(-) diff --git a/src/cam.c b/src/cam.c index 25a23ab..0885d73 100644 --- a/src/cam.c +++ b/src/cam.c @@ -93,6 +93,16 @@ static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) { int rv = 0; int shm_fd, shm_valid = 0; + decoded_can_values_shm* shared_message; + + if(facilities->lightship->use_obd){ + shm_fd = shm_open("it2s-bluetooth-decoded", O_RDONLY, 0666); + if(shm_fd == -1) + syslog_err("[facilities] failed to open CAN shared memory\n"); + shared_message = (decoded_can_values_shm *)mmap(0, sizeof(decoded_can_values_shm), PROT_READ, MAP_SHARED, shm_fd, 0); + if(shared_message == MAP_FAILED) + syslog_err("[facilities] failed to map CAN shared memory\n"); + } CAM_t *cam = calloc(1, sizeof(CAM_t)); @@ -111,33 +121,6 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) pthread_mutex_lock(&lightship->lock); if (facilities->station_type != StationType_roadSideUnit) { - int shm_fd; - decoded_can_values_shm* shared_message; - if(facilities->lightship->use_obd){ - shm_fd = shm_open("it2s-bluetooth-decoded", O_RDONLY, 0666); - if(shm_fd == -1) - syslog_err("[facilities] failed to open CAN shared memory\n"); - - shared_message = (decoded_can_values_shm *)mmap(0, sizeof(decoded_can_values_shm), PROT_READ, MAP_SHARED, shm_fd, 0); - if(shared_message == MAP_FAILED) - syslog_err("[facilities] failed to map CAN shared memory\n"); - - // process and fill fields here, still need to alloc mem for optional - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = shared_message->speed_value; - //cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights = shared_message->lights_value; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.steeringWheelAngle->steeringWheelAngleValue = shared_message->w_angle_value; - - /*if(shared_message->b_pedal_value) - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl = 0; - else if(shared_message->s_pedal_value) - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl = 1; - */ - - munmap(shared_message, sizeof(can_values_shm)); - if(close(shm_fd) == -1) - syslog_err("[facilities] failed to close CAN shared memory\n"); - } - cam->cam.generationDeltaTime = now % 65536; it2s_tender_lock_space(&facilities->epv); @@ -154,6 +137,30 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + if(facilities->lightship->use_obd){ + uint8_t ac = 0x00; + + // Speed (already getting set bellow) + // bvc_hf.speed.speedValue = ceil(shared_message->speed_value รท 0.036); + + // Steering Wheel Angle + bvc_hf->steeringWheelAngle = malloc(sizeof(SteeringWheelAngle_t)); + bvc_hf->steeringWheelAngle->steeringWheelAngleValue = shared_message->w_angle_value; + + // still missing temperature; where is de_temperature ? + + // Acceleration Control + if(shared_message->b_pedal_value) + ac = ac | 0x01; + if(shared_message->s_pedal_value) + ac = ac | 0x02; + bvc_hf->accelerationControl = malloc(sizeof(AccelerationControl_t)); + bvc_hf->accelerationControl->buf = malloc(sizeof(uint8_t)); + memcpy(bvc_hf->accelerationControl->buf, &ac, 1); + bvc_hf->accelerationControl->size = 1; + bvc_hf->accelerationControl->bits_unused = 0; + } + // Set speed bvc_hf->speed.speedValue = facilities->epv.space.speed; bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf; @@ -226,10 +233,24 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) // Low frequency container if (now > lightship->last_cam_lfc + 500) { - cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(LowFrequencyContainer_t)); cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; + + BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency; + + if(facilities->lightship->use_obd){ + uint8_t el = 0x00; + + // Exterior Lights + if(shared_message->lights_value) + el = el | 0x01; + bvc_lf->exteriorLights.buf = malloc(sizeof(uint8_t)); + memcpy(bvc_lf->exteriorLights.buf, &el, 1); + bvc_lf->exteriorLights.size = 1; + bvc_lf->exteriorLights.bits_unused = 0; + } + PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory; ph->list.array = malloc((lightship->pos_history_len-1) * sizeof(void*)); @@ -322,6 +343,12 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) } *cam_len = (enc.encoded + 7) / 8; + if(facilities->lightship->use_obd){ + munmap(shared_message, sizeof(can_values_shm)); + if(close(shm_fd) == -1) + syslog_err("[facilities] failed to close CAN shared memory\n"); + } + cleanup: ASN_STRUCT_FREE(asn_DEF_CAM, cam); @@ -345,7 +372,7 @@ lightship_t* lightship_init() { lightship->use_obd = 0; } else{ - lightship->use_obd = 0; // TODO + lightship->use_obd = 1; // TODO close(shm_fd); }