diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index afaa75a..6dd30e3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,4 +1,5 @@ ADD_EXECUTABLE(it2s-itss-facilities + cam.c facilities.c ) diff --git a/src/cam.c b/src/cam.c new file mode 100644 index 0000000..c3ffb76 --- /dev/null +++ b/src/cam.c @@ -0,0 +1,162 @@ +#include "cam.h" + +#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 LEAP_SECONDS 5 + +static AltitudeConfidence_t getAltitudeConfidence(double conf) { + if (conf >= 200) return AltitudeConfidence_outOfRange; + if (conf >= 100) return AltitudeConfidence_alt_200_00; + if (conf >= 50) return AltitudeConfidence_alt_100_00; + if (conf >= 20) return AltitudeConfidence_alt_050_00; + if (conf >= 10) return AltitudeConfidence_alt_020_00; + if (conf >= 5) return AltitudeConfidence_alt_010_00; + if (conf >= 1) return AltitudeConfidence_alt_005_00; + + return AltitudeConfidence_alt_001_00; +} + +static HeadingConfidence_t getHeadingConfidence(uint32_t conf) { + if (conf > 125) return 126; + if (conf == 125) return 125; + if (conf == 0) return 127; + if (conf < 1) return 1; + + return conf; +} + +static SpeedConfidence_t getSpeedConfidence(uint32_t conf) { + if (conf > 125) return 126; + if (conf == 125) return 125; + if (conf == 0) return 127; + if (conf < 1) return 1; + + return conf; +} + + +int mk_cam(uint8_t *cam, uint32_t *cam_len) { + int rv = 0; + + CAM_t *cam_tx = calloc(1, sizeof(CAM_t)); + + cam_tx->header.protocolVersion = protocolVersion_currentVersion; + cam_tx->header.messageID = ItsPduHeader__messageID_cam; + cam_tx->header.stationID = 0; + cam_tx->cam.camParameters.basicContainer.stationType = StationType_passengerCar; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable; + + cam_tx->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(struct LowFrequencyContainer)); + cam_tx->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole = VehicleRole_default; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf = calloc(1, sizeof(uint8_t) * 1); + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf[0] = 1 << 6; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.bits_unused = 5; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.size = 1; + *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf) = 0x00; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 0; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 0; + + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 1; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 1; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array = calloc(1, sizeof(struct PathPoint*)); + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0] = calloc(1, sizeof(struct PathPoint)); + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t)); + *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime) = PathDeltaTime_tenMilliSecondsInPast; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; + cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; + + struct timespec systemtime; + if (clock_gettime(CLOCK_REALTIME, &systemtime)) { + syslog_emerg("clock_gettime() failed"); + } + + long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + generationdeltatime = generationdeltatime - 1072915200000; // EPOCH -> 2004/01/01 00:00:000 + generationdeltatime = generationdeltatime % 64536; + cam_tx->cam.generationDeltaTime = generationdeltatime; + +// struct it2s_gps_data gps_data; +// it2s_gps_read(&gps_data); + // if (!isnan(gps_data->gps_altitude)) { + // cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data->gps_altitude) * 10); + // cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data->gps_epv); + // } else { + cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable; + // } + + // if (!isnan(gps_data->gps_latitude)) { + // cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data->gps_latitude) * 10000000); + // } else { + cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable; + // } + + // if (!isnan(gps_data->gps_longitude)) { + // cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data->gps_longitude) * 10000000); + // } else { + cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable; + // } + + cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; + + cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + +// if (!isnan(gps_data->gps_track)) { +// cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data->gps_track * 10)); +// cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data->gps_epd * 10)); +// } else { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable; + +// } +// +// if (!isnan(gps_data->gps_speed)) { +// cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data->gps_speed * 100)); // cm/s +// cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data->gps_eps * 100)); +// } else { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable; +// } + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256); + if (enc.encoded == -1) { + rv = 1; + goto cleanup; + } + *cam_len = (enc.encoded + 7) / 8; + +cleanup: + ASN_STRUCT_FREE(asn_DEF_CAM, cam_tx); + + return rv; +} diff --git a/src/cam.h b/src/cam.h new file mode 100644 index 0000000..76cfadd --- /dev/null +++ b/src/cam.h @@ -0,0 +1,8 @@ +#ifndef FACILITIES_CAM_H +#define FACILITIES_CAM_H + +#include + +int mk_cam(uint8_t *cam, uint32_t *cam_len); + +#endif diff --git a/src/facilities.c b/src/facilities.c index 34c1a8c..e5bc5e3 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -1,4 +1,5 @@ #include "facilities.h" +#include "cam.h" #include #include @@ -13,6 +14,7 @@ #include #include +#include #include #define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) @@ -25,7 +27,7 @@ #define syslog_debug(msg, ...) #endif -static int transport_indication(void *responder, uint8_t *msg, uint32_t msg_len, void* app_sock) { +static int transport_indication(facilities_t *facilities, uint8_t *msg, uint32_t msg_len) { int rv = 0, code = 0; FacilitiesDataIndication_t *fdi = NULL; @@ -36,11 +38,11 @@ static int transport_indication(void *responder, uint8_t *msg, uint32_t msg_len, syslog_err("[facilities] [in] invalid BTPDataIndication received"); rv = 1; code = 1; - zmq_send(responder, &code, 1, 0); + zmq_send(facilities->responder, &code, 1, 0); goto cleanup; } - zmq_send(responder, &code, 1, 0); + zmq_send(facilities->responder, &code, 1, 0); syslog_debug("[facilities] [in] received BTPDataIndication (%ldB)", bdi->data.size); // Parse message @@ -84,8 +86,8 @@ static int transport_indication(void *responder, uint8_t *msg, uint32_t msg_len, buffer[0] = 4; // Facilities asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, buffer+1, 511); syslog_debug("[facilities] [out] sending FacilitiesDataIndication to [app] (%ldB)", enc.encoded); - zmq_send(app_sock, buffer, enc.encoded, 0); - zmq_recv(app_sock, &code, 1, 0); + zmq_send(facilities->app_socket, buffer, enc.encoded, 0); + zmq_recv(facilities->app_socket, &code, 1, 0); cleanup: ASN_STRUCT_FREE(asn_DEF_BTPDataIndication, bdi); @@ -95,28 +97,79 @@ cleanup: return rv; } +static void* handler(void *fc) { + int rv = 0; + int code = 0; + facilities_t *facilities = (facilities_t*) fc; + + BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); + + bdr->btpType = BTPType_btpB; + + bdr->destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) {bdr->destinationAddress.buf[i] = 0xff;} + bdr->destinationAddress.size = 6; + + bdr->packetTransportType = PacketTransportType_shb; + + bdr->destinationPort = Port_cam; + + bdr->trafficClass = 2; + + bdr->data.buf = malloc(256); + + uint8_t bdr_oer[256]; + bdr_oer[0] = 4; // Facilities + while (!facilities->exit) { + sleep(1); + rv = mk_cam(bdr->data.buf, (uint32_t *) &bdr->data.size); + if (rv) { + syslog_err("[facilities] make cam failed (%d)", rv); + continue; + } + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 255); + if (enc.encoded == -1) { + syslog_err("[facilities] encoding BTPDataRequest for cam failed"); + continue; + } + syslog_debug("[facilities] [out] sending BTPDataRequest to [transport] (%ldB)", enc.encoded); + zmq_send(facilities->transport_socket, bdr_oer, enc.encoded, 0); + zmq_recv(facilities->transport_socket, &code, 1, 0); + } + + ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr); + + return NULL; +} + int main() { syslog_info("[facilities] starting"); facilities_t facilities; void *context = zmq_ctx_new(); - void *responder = zmq_socket(context, ZMQ_REP); - int rc = zmq_bind(responder, "ipc:///tmp/itss/facilities"); + facilities.responder = zmq_socket(context, ZMQ_REP); + int rc = zmq_bind(facilities.responder, "ipc:///tmp/itss/facilities"); - void *app_sock = zmq_socket(context, ZMQ_REQ); - rc = zmq_connect(app_sock, "ipc:///tmp/itss/application"); + facilities.app_socket = zmq_socket(context, ZMQ_REQ); + rc = zmq_connect(facilities.app_socket, "ipc:///tmp/itss/application"); + + facilities.transport_socket = zmq_socket(context, ZMQ_REQ); + rc = zmq_connect(facilities.transport_socket, "ipc:///tmp/itss/transport"); + + pthread_create(&facilities.handler, NULL, handler, (void*) &facilities); uint8_t buffer[512]; uint8_t code; syslog_info("[facilities] listening"); while (1) { - zmq_recv(responder, buffer, 512, 0); + zmq_recv(facilities.responder, buffer, 512, 0); switch (buffer[0]) { case 3: - code = transport_indication(responder, buffer+1, 511, app_sock); + code = transport_indication(&facilities, buffer+1, 511); break; default: syslog_debug("[facilities] unrecognized service"); diff --git a/src/facilities.h b/src/facilities.h index dea23ac..2ed25e0 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -2,9 +2,17 @@ #define FACILITIES_H #include +#include typedef struct facilities { - pthread_t todefine; + pthread_t handler; + + // ZMQ + void* responder; + void* transport_socket; + void* app_socket; + + bool exit; } facilities_t; #endif