224 lines
11 KiB
C
224 lines
11 KiB
C
#include "cam.h"
|
|
#include "facilities.h"
|
|
|
|
#include <itss-transport/BTPDataRequest.h>
|
|
#include <camv2/CAM.h>
|
|
#include <it2s-gps.h>
|
|
|
|
#include <stdint.h>
|
|
#include <time.h>
|
|
#include <zmq.h>
|
|
#include <unistd.h>
|
|
#include <syslog.h>
|
|
#include <math.h>
|
|
|
|
#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;
|
|
}
|
|
|
|
|
|
static int mk_cam(uint8_t *cam, uint32_t *cam_len) {
|
|
int rv = 0;
|
|
|
|
struct timespec st;
|
|
if (clock_gettime(CLOCK_REALTIME, &st)) {
|
|
syslog_emerg("clock_gettime() failed");
|
|
}
|
|
|
|
CAM_t *cam_tx = calloc(1, sizeof(CAM_t));
|
|
|
|
cam_tx->header.protocolVersion = 2;
|
|
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;
|
|
}
|
|
|
|
|
|
void *ca_service(void *fc) {
|
|
|
|
int rv = 0;
|
|
uint8_t code = 0;
|
|
facilities_t *facilities = (facilities_t*) fc;
|
|
|
|
void* h = zmq_socket(facilities->ctx, ZMQ_REQ);
|
|
zmq_connect(h, TRANSPORT_ADDRESS);
|
|
|
|
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
|
|
|
|
bdr->btpType = BTPType_btpB;
|
|
|
|
bdr->gnDestinationAddress.buf = malloc(6);
|
|
for (int i = 0; i < 6; ++i) {bdr->gnDestinationAddress.buf[i] = 0xff;}
|
|
bdr->gnDestinationAddress.size = 6;
|
|
|
|
bdr->gnPacketTransportType = PacketTransportType_shb;
|
|
|
|
bdr->destinationPort = Port_cam;
|
|
|
|
bdr->gnTrafficClass = 2;
|
|
|
|
bdr->data.buf = malloc(256);
|
|
|
|
uint8_t bdr_oer[256];
|
|
bdr_oer[0] = 4; // Facilities
|
|
while (!facilities->exit) {
|
|
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;
|
|
}
|
|
|
|
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
|
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
|
|
|
usleep(1000*1000);
|
|
}
|
|
|
|
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);
|
|
|
|
return NULL;
|
|
}
|