#include "cpm.h" #include "facilities.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) #define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) #define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) #ifndef NDEBUG #define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__) #else #define syslog_debug(msg, ...) #endif #define PI 3.141592654 /* Variables */ float roadRotationSin; float roadRotationCos; S_ETHERNET_CONNECTION_T s_socket; S_OBJECT_CONTROL_T s_objectControl; S_OBJECTS_T as_objects[NOF_OBJECTS]; bool waitingIncomingConnection(void){ // Listening to the socket (Waiting for incoming connection) unsigned int len = sizeof(s_socket.s_client); if(listen(s_socket.i32_socket,1)<0){ syslog_err("Waiting for incoming requests failed..."); return false; } if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ syslog_err("Client disconnected..."); return false; } syslog_debug("Radar connected"); return true; } bool interface_connection(char* radar_ip,char* radar_port){ // 1 - Create socket s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); if(s_socket.i32_socket < 0){ syslog_err("Initializing socket failed ..."); return false; } // 2 - Bind it to server address and port bzero(&s_socket.s_server, sizeof(s_socket.s_server)); s_socket.s_server.sin_family = AF_INET; s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip); s_socket.s_server.sin_port = htons(atoi(radar_port)); if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){ syslog_err("Binding socket to address error ..."); return false; } if(!waitingIncomingConnection()) return false; return true; } void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) { u_int8_t u8_pvrMessagePart = 0; u_int8_t tmp = 0; //static long last = 0; S_PVR_T s_pvr; S_SENSOR_CONTROL_T s_sensorControl; switch (u32_can_id) // Interpret the different types of CAN messages { case 0x785: s_pvr.u8_numberOfCountedObjects = 0; s_pvr.u32_UnixTime = 0; s_pvr.u16_Milliseconds = 0; s_pvr.u8_SensorNetworkID = 0; u8_pvrMessagePart = (au8_can_data[0] & 0x1); if (u8_pvrMessagePart == 0) { s_pvr.u8_numberOfCountedObjects = (au8_can_data[0] & 0xFE) >> 1; s_pvr.u32_UnixTime |= au8_can_data[4] << 24; s_pvr.u32_UnixTime |= au8_can_data[3] << 16; s_pvr.u32_UnixTime |= au8_can_data[2] << 8; s_pvr.u32_UnixTime |= au8_can_data[1]; s_pvr.u16_Milliseconds |= (au8_can_data[6] & 0x3) << 8; s_pvr.u16_Milliseconds |= au8_can_data[5]; s_pvr.u8_SensorNetworkID = (au8_can_data[6] & 0xC) >> 2; // printf("Unix Time: %u, Ms: %u\n", s_pvr.u32_UnixTime, s_pvr.u16_Milliseconds); long current = s_pvr.u32_UnixTime * 1000 + s_pvr.u16_Milliseconds; //printf("Unix Time Epoch Fixed: %ld\n", (long)(current - 1072915200000)); //printf("Diff: %ld\n", current - last); //last = current; } else if (u8_pvrMessagePart == 1) { s_pvr.u8_ObjectNumber = 0; s_pvr.u8_ObjectID = 0; s_pvr.i16_speed = 0; s_pvr.u8_class = 0; s_pvr.u8_mLineNumber = 0; s_pvr.u8_laneNumber = 0; s_pvr.u8_ObjectNumber = (au8_can_data[0] & 0xFE) >> 1; s_pvr.u8_ObjectID = au8_can_data[1]; s_pvr.i16_speed |= (au8_can_data[3] & 0x7) << 8; s_pvr.i16_speed |= au8_can_data[2]; s_pvr.i16_speed -= 1024; // Speed Offset s_pvr.u8_class = (au8_can_data[3] & 0x38) >> 3; s_pvr.u8_mLineNumber = (au8_can_data[3] & 0xC0) >> 6; s_pvr.u8_laneNumber = (au8_can_data[4] & 0x7) >> 3; } break; case 0x500: memset(&s_sensorControl, 0, sizeof(s_sensorControl)); s_sensorControl.u8_sensorStatus = au8_can_data[0]; s_sensorControl.u8_InterfaceMode = (au8_can_data[1] & 0xF); s_sensorControl.u8_networkID = (au8_can_data[1] & 0xF0) >> 4; s_sensorControl.u8_diagnose = au8_can_data[2]; s_sensorControl.u32_time |= au8_can_data[7] << 24; s_sensorControl.u32_time |= au8_can_data[6] << 16; s_sensorControl.u32_time |= au8_can_data[5] << 8; s_sensorControl.u32_time |= au8_can_data[4]; break; case 0x501: memset(&s_objectControl, 0, sizeof(s_objectControl)); s_objectControl.u8_numberOfObjects = au8_can_data[0]; s_objectControl.u8_numberOfMessages = au8_can_data[1]; s_objectControl.u8_cycleDuration = au8_can_data[2]; s_objectControl.u8_objectData0Format = au8_can_data[3] & 0xF; s_objectControl.u8_objectData1Format = (au8_can_data[3] & 0xF0) >> 4; s_objectControl.u32_cycleCount |= au8_can_data[7] << 24; s_objectControl.u32_cycleCount |= au8_can_data[6] << 16; s_objectControl.u32_cycleCount |= au8_can_data[5] << 8; s_objectControl.u32_cycleCount |= au8_can_data[4]; break; } if ((u32_can_id >= 0x502) && (u32_can_id <= 0x57F)) { u_int16_t u16_objectIndex = (u_int16_t)u32_can_id - 0x502; as_objects[u16_objectIndex].u8_modeSignal = au8_can_data[0] & 0x1; if (s_objectControl.u8_objectData0Format == 005) // without Update Flag { as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7; as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1; as_objects[u16_objectIndex].f_xPoint -= 4096; as_objects[u16_objectIndex].f_xPoint *= 0.128; as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10; as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2; as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6; as_objects[u16_objectIndex].f_yPoint -= 4096; as_objects[u16_objectIndex].f_yPoint *= 0.128; as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5); as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3); as_objects[u16_objectIndex].f_xSpeed -= 1024; as_objects[u16_objectIndex].f_xSpeed *= 0.1; as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10; as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2); as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6); as_objects[u16_objectIndex].f_ySpeed -= 1024.0f; as_objects[u16_objectIndex].f_ySpeed *= 0.1; as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; as_objects[u16_objectIndex].u8_objectID = au8_can_data[7]; } else if (s_objectControl.u8_objectData0Format == 4) { as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7; as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1; as_objects[u16_objectIndex].f_xPoint -= 4096; as_objects[u16_objectIndex].f_xPoint *= 0.128; as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10; as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2; as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6; as_objects[u16_objectIndex].f_yPoint -= 4096; as_objects[u16_objectIndex].f_yPoint *= 0.128; as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5); as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3); as_objects[u16_objectIndex].f_xSpeed -= 1024; as_objects[u16_objectIndex].f_xSpeed *= 0.1; as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10; as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2); as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6); as_objects[u16_objectIndex].f_ySpeed -= 1024.0f; as_objects[u16_objectIndex].f_ySpeed *= 0.1; as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7; } } } void parse_input(u_int8_t* u8_input_buffer, int i32_len) { static enum state_t s_state = IDLE; u_int8_t u8_input = 0; int i32_i; static int i32_can_len_counter = 0, i32_can_len = 0, i32_can_id = 0, i32_stop_search = 0; static unsigned char au8_can_data[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; static int i32_xor = 0; for (i32_i = 0; i32_i < i32_len; i32_i++) { u8_input = u8_input_buffer[i32_i]; switch (i32_stop_search) { case 0: if (u8_input == 0xea) { i32_stop_search = 1; } break; case 1: if (u8_input == 0xeb) { i32_stop_search = 2; } else { i32_stop_search = 0; } break; case 2: if (u8_input == 0xec) { i32_stop_search = 3; } else { i32_stop_search = 0; } break; case 3: if (u8_input == 0xed) { s_state = IDLE; } i32_stop_search = 0; break; } switch (s_state) { case IDLE: if (u8_input == 0xca) { s_state = START_1; } break; case START_1: if (u8_input == 0xcb) { s_state = START_2; } break; case START_2: if (u8_input == 0xcc) { s_state = START_3; } break; case START_3: if (u8_input == 0xcd) { s_state = START_4; } break; case START_4: s_state = CAN_ID_H; break; case CAN_ID_H: s_state = CAN_ID_L; break; case CAN_ID_L: s_state = CAN_LEN; break; case CAN_LEN: s_state = CAN_PAYLOAD; break; case CAN_PAYLOAD: if (i32_can_len_counter >= i32_can_len) { s_state = CAN_ID_H; } break; default: printf("Something probably went wrong, this code is likely unreachable"); } switch (s_state) { case START_1: i32_xor = 0; break; case CAN_ID_H: i32_can_id = 0; i32_can_id |= u8_input << 8; i32_xor ^= u8_input; break; case CAN_ID_L: i32_can_id |= u8_input; i32_xor ^= u8_input; break; case CAN_LEN: i32_can_len = u8_input; i32_can_len_counter = 0; i32_xor ^= u8_input; break; case CAN_PAYLOAD: if ((i32_can_len_counter < 16) && (i32_can_len_counter <= i32_can_len)) { //if (i32_can_len - i32_can_len_counter - 1 > 15) // printf("%d", i32_can_len - i32_can_len_counter - 1); if (i32_can_len - i32_can_len_counter - 1 < 16) au8_can_data[i32_can_len - i32_can_len_counter - 1] = u8_input; } i32_can_len_counter++; if (i32_can_len_counter >= i32_can_len) { parse_can_data_tm(i32_can_id, i32_can_len, au8_can_data); } i32_xor ^= u8_input; break; default:; } } } long rotate_x(long x, long y) { return (long)(x * roadRotationCos - y * roadRotationSin); } long rotate_y(long x, long y) { return (long)(x * roadRotationSin + y * roadRotationCos); } void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) { cpm_tx->header.protocolVersion = PROTOCOL_VERSION; cpm_tx->header.messageID = MESSAGE_ID; cpm_tx->header.stationID = STATION_ID; uint64_t generationDeltaTime = it2s_tender_get_clock(&facilities->epv) % 65536; // generationDeltaTime = TimestampIts mod 65 536 cpm_tx->cpm.generationDeltaTime = generationDeltaTime; cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = 420309860; //A3 // 412400078; (A24) cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = -86515778; //A3 // -86950224; (A24) cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = 4000; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00; cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t)); cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t)); cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t)); cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t)); (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t)); (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890; cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t)); (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = s_objectControl.u8_numberOfObjects; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc( cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count, sizeof(PerceivedObject_t*)); for (int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; i++) { cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i] = calloc(1, sizeof(PerceivedObject_t)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectID = (long)as_objects[i].u8_objectID; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->timeOfMeasurement = 0; //sem informaçao do radar cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectConfidence = 95; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.value = rotate_x( (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.confidence = 102; // printf("->%f<-\n", as_objects[i].f_xPoint); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.value = rotate_y( (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.confidence = 102; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.value = rotate_x( (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.confidence = 40; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.value = rotate_y( (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.confidence = 40; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectRefPoint = ObjectRefPoint_bottomMid; } cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; } int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) { memset(out_message_data, 0, 1500); asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500); if (retval_enc.encoded == -1) { syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name); return 1; } *out_message_size = ((retval_enc.encoded + 7) / 8); return 0; } dissemination_t* dissemination_init(){ /* Mutex init and dissemination memory allocation */ dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t)); pthread_mutex_init(&dissemination->lock, NULL); return dissemination; } int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv) { int rv = 0; uint64_t now = it2s_tender_get_clock(epv); /* mutex is used to lock shared resources */ pthread_mutex_lock(&dissemination->lock); // Both cases for OBU and RSU (BASIC: generation interval 1s) if (now >= dissemination->next_cpm_max) rv = 1; pthread_mutex_unlock(&dissemination->lock); return rv; } void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv){ uint64_t now = it2s_tender_get_clock(epv); /* Both cases for RSU and OBU */ pthread_mutex_lock(&dissemination->lock); dissemination->next_cpm_min = now + dissemination->T_GenCpmMin; dissemination->next_cpm_max = now + dissemination->T_GenCpmMax; pthread_mutex_unlock(&dissemination->lock); } void *cp_service(void *fc){ /* Variables */ int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; bool is_radar_connected; uint8_t bdr_oer[2048]; uint8_t fdi_oer[2048]; bdr_oer[0] = 4; //Facilities fdi_oer[0] = 4; facilities_t *facilities = (facilities_t *) fc; BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); FacilitiesDataIndication_t *fdi = calloc(1, sizeof(FacilitiesDataIndication_t)); roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); /*--- Fill mandatory BTP Data Request parameters ---*/ bdr->gnDestinationAddress.buf = malloc(6); bdr->gnDestinationAddress.size = 6; for(int i = 0; i < 6; i++) bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport //The former doesn't have a source port and the latter have bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport bdr->data.buf = malloc(1500); //CPM Data to be sent to the Transport layer if(facilities->use_security) { bdr->gnSecurityProfile = malloc(sizeof(long)); *bdr->gnSecurityProfile = 1; } is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket /*--- Fill mandatory Facilities Data Indication parameters ---*/ fdi->itsMessageType = ItsMessageType_cpm; fdi->data.buf = malloc(1500); struct timespec* systemtime; systemtime = (struct timespec*)malloc(sizeof(struct timespec)); if (systemtime == NULL) { printf("memory allocation failed: %m"); exit(1); } while(!facilities->exit){ usleep(1000*50); /* Receive Data from radar interface */ CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags) if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){ if(is_radar_connected){ if(i32_recv_bytes <= 0){ syslog_debug("No data received from radar ..."); is_radar_connected = false; continue; } /* Information parsing */ parse_input(au8_readBuffer,i32_recv_bytes); /* CPM build and encoding to bdr and fdi (Could be merged if needed) */ mk_cpm(facilities,cpm_tx,systemtime); if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); continue; } if(encode_cpm(cpm_tx, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1){ ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); continue; } ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); /* Encode BTPDataRequest */ asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); if(enc_bdr.encoded == -1){ syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); continue; } /* Encode FacilitiesDataIndication */ asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047); if(enc_fdi.encoded == -1){ syslog_err("[facilities] encoding FacilitiesDataIndication for cpm failed"); continue; } /* Create thread to send packet to the Transport Layer (=3) */ queue_add(facilities->tx_queue, bdr_oer, enc_bdr.encoded+1, 3); pthread_cond_signal(&facilities->tx_queue->trigger); /* Create thread to send packet to the Applications Layer (=5) */ queue_add(facilities->tx_queue, fdi_oer, enc_fdi.encoded+1, 5); pthread_cond_signal(&facilities->tx_queue->trigger); /*Reset Timer for dissemination control */ dissemination_reset_timer(facilities->dissemination, &facilities->epv); } else { /* Waiting for Radar to reconnect */ is_radar_connected = waitingIncomingConnection(); } } } ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi); return NULL; }