add obd value mem alloc and assign to its cam fields

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);
     }