CAM generation
This commit is contained in:
emanuel
parent
e54d67e4ac
commit
2ee1b68f4e
|
|
@ -1,4 +1,5 @@
|
|||
ADD_EXECUTABLE(it2s-itss-facilities
|
||||
cam.c
|
||||
facilities.c
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,162 @@
|
|||
#include "cam.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <camv2/CAM.h>
|
||||
//#include <it2s-gps.h>
|
||||
|
||||
#include <syslog.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;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#ifndef FACILITIES_CAM_H
|
||||
#define FACILITIES_CAM_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
int mk_cam(uint8_t *cam, uint32_t *cam_len);
|
||||
|
||||
#endif
|
||||
|
|
@ -1,4 +1,5 @@
|
|||
#include "facilities.h"
|
||||
#include "cam.h"
|
||||
|
||||
#include <itss-transport/BTPDataRequest.h>
|
||||
#include <itss-transport/BTPDataIndication.h>
|
||||
|
|
@ -13,6 +14,7 @@
|
|||
#include <it2s-btp.h>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <unistd.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#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");
|
||||
|
|
|
|||
|
|
@ -2,9 +2,17 @@
|
|||
#define FACILITIES_H
|
||||
|
||||
#include <pthread.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct facilities {
|
||||
pthread_t todefine;
|
||||
pthread_t handler;
|
||||
|
||||
// ZMQ
|
||||
void* responder;
|
||||
void* transport_socket;
|
||||
void* app_socket;
|
||||
|
||||
bool exit;
|
||||
} facilities_t;
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue