TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Path History reference implementation

This commit is contained in:
emanuel 2022-10-31 09:56:28 +00:00
parent 07dcd7ef97
commit 2078d067a3
4 changed files with 224 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
src/cam.c
View File

@ -1,7 +1,6 @@
#include "cam.h"
#include "facilities.h"
#include <camv2/INTEGER.h>
#include <itss-transport/TransportRequest.h>
#include <itss-facilities/FacilitiesIndication.h>
#include <itss-management/ManagementRequest.h>
@ -24,20 +23,15 @@
#include <it2s-obd.h>
#define LEAP_SECONDS 5
#define EARTH_RADIUS 6369000
#define EARTH_RADIUS 6369000.0
#define RAD_PER_DEG M_PI/180.0
double haversine(double lat1, double lon1, double lat2, double lon2) {
double d_lat = (lat1-lat2) * RAD_PER_DEG;
double d_lon = (lon1-lon2) * RAD_PER_DEG;
double a = pow(sin(d_lat/2.0), 2) + cos(lat1 * RAD_PER_DEG) * cos(lat2 * RAD_PER_DEG) * pow(sin(d_lon/2.0), 2);
double c = 2 * atan2(sqrt(a), sqrt(1-a));
return EARTH_RADIUS * c;
}
// Using Vanetza's Path History constraints
#define PATH_HISTORY_ALLOWABLE_ERROR 0.47
#define PATH_HISTORY_CHORD_LENGTH_THRESHOLD 22.5
#define PATH_HISTORY_SMALL_DELTA_PHI 1.0
#define PATH_HISTORY_DISTANCE 200.0
#define PATH_HISTORY_MAX_ESTIMATED_RADIUS EARTH_RADIUS
const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000},
@ -83,6 +77,86 @@ static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_
}
/*
* Implementation of Path History as per the report,
* "Vehicle Safety Communications – Applications (VSC-A) Final Report: Appendix Volume 1"
* "Appendix B-2 - Method One (Section 3.2)"
*/
static void path_history_update(pos_point_t* pn) {
lightship_t* ls = &facilities.lightship;
++ls->concise_points_len;
if (ls->concise_points_len > 3) {
ls->concise_points_len = 3;
}
for (int i = ls->concise_points_len - (1 + ls->concise_keep_start); i > 0; --i) {
ls->concise_points[i] = ls->concise_points[i-1];
}
ls->concise_points[0] = *pn;
if (ls->concise_points_len == 3) {
double actual_error;
double actual_chord_length = itss_haversine(
ls->concise_points[2].lat / 1.0e7,
ls->concise_points[2].lon / 1.0e7,
ls->concise_points[0].lat / 1.0e7,
ls->concise_points[0].lon / 1.0e7
);
if (actual_chord_length > PATH_HISTORY_CHORD_LENGTH_THRESHOLD) {
actual_error = PATH_HISTORY_ALLOWABLE_ERROR + 1.0;
} else {
double dtheta = ((int)ls->concise_points[0].heading - ls->concise_points[2].heading) / 10.0;
double estimated_r;
if (dtheta < PATH_HISTORY_SMALL_DELTA_PHI) {
actual_error = 0;
estimated_r = PATH_HISTORY_MAX_ESTIMATED_RADIUS;
} else {
estimated_r = actual_chord_length/(2*sin(RAD_PER_DEG * dtheta/2));
double d = estimated_r * cos(RAD_PER_DEG * dtheta/2);
actual_error = estimated_r - d;
}
}
if (actual_error > PATH_HISTORY_ALLOWABLE_ERROR) { /* add to path history */
if (ls->path_history_len != PATH_HISTORY_MAX_LEN) {
++ls->path_history_len;
} else { /* Path History is full */
free(ls->path_history[PATH_HISTORY_MAX_LEN - 1]);
}
for (int i = ls->path_history_len-1; i > 0; --i) {
ls->path_history[i] = ls->path_history[i-1];
}
ls->path_history[0] = malloc(sizeof(pos_point_t));
*ls->path_history[0] = ls->concise_points[1];
ls->concise_keep_start = false;
} else {
ls->concise_keep_start = true;
}
}
if (ls->path_history_len >= 2) {
double distance = 0.0;
for (int i = 0; i < ls->path_history_len - 1; ++i) {
distance += itss_haversine(
ls->path_history[i]->lat / 1.0e7,
ls->path_history[i]->lon / 1.0e7,
ls->path_history[i+1]->lat / 1.0e7,
ls->path_history[i+1]->lon / 1.0e7
);
if (distance > PATH_HISTORY_DISTANCE) {
for (int j = i; j < ls->path_history_len - 1; ++j) {
free(ls->path_history[j+1]);
}
ls->path_history_len = i + 1;
break;
}
}
}
}
static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
@ -188,33 +262,36 @@ static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) {
bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
/*
// Save current values
if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) {
free(lightship->pos_history[POS_HISTORY_MAX_LEN-1]);
--lightship->pos_history_len;
if (lightship->path_history_len == POS_HISTORY_MAX_LEN) {
free(lightship->path_history[POS_HISTORY_MAX_LEN-1]);
--lightship->path_history_len;
}
*/
for (int i = lightship->pos_history_len-1; i >= 0; --i) {
lightship->pos_history[i+1] = lightship->pos_history[i];
/*
for (int i = lightship->path_history_len-1; i >= 0; --i) {
lightship->path_history[i+1] = lightship->path_history[i];
}
*/
lightship->pos_history[0] = malloc(sizeof(pos_vector_t));
lightship->pos_history[0]->heading = bvc_hf->heading.headingValue;
lightship->pos_history[0]->lat = bc->referencePosition.latitude;
lightship->pos_history[0]->lon = bc->referencePosition.longitude;
lightship->pos_history[0]->alt = bc->referencePosition.altitude.altitudeValue;
lightship->pos_history[0]->speed = bvc_hf->speed.speedValue;
pos_point_t pn;
pn.heading = bvc_hf->heading.headingValue;
pn.lat = bc->referencePosition.latitude;
pn.lon = bc->referencePosition.longitude;
pn.alt = bc->referencePosition.altitude.altitudeValue;
pn.speed = bvc_hf->speed.speedValue;
pn.ts = now;
path_history_update(&pn);
lightship->last_cam = now;
lightship->pos_history[0]->ts = now;
++lightship->pos_history_len;
lightship->t_last_cam = now;
// Acceleration
if (lightship->pos_history_len > 1) {
double delta_angle = (lightship->pos_history[0]->heading - lightship->pos_history[1]->heading) / 10.0; /* 1º */
double delta_speed = (lightship->pos_history[0]->speed - (lightship->pos_history[1]->speed * cos(delta_angle * M_PI/180))) / 10.0; /* 0.1 m/s */
int16_t long_a = (int16_t) (delta_speed / (double) ((lightship->pos_history[0]->ts - lightship->pos_history[1]->ts) / 1000)); /* 0.1 m/s^2 */
if (lightship->last_pos.ts != 0) {
double delta_angle = ((int)pn.heading - lightship->last_pos.heading) / 10.0; /* 1º */
double delta_speed = (pn.speed - (lightship->last_pos.speed * cos(delta_angle * RAD_PER_DEG))) / 10.0; /* 0.1 m/s */
int16_t long_a = (int16_t) (delta_speed / (double) ((pn.ts - lightship->last_pos.ts) / 1000.0)); /* 0.1 m/s^2 */
if (long_a > 160) long_a = 160;
else if (long_a < -160) long_a = -160;
@ -227,7 +304,7 @@ static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) {
}
// Low frequency container
if (now > lightship->last_cam_lfc + 500) {
if (now > lightship->t_last_cam_lfc + 500) {
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(LowFrequencyContainer_t));
cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
@ -250,39 +327,65 @@ static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) {
PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;
ph->list.array = malloc((lightship->pos_history_len-1) * sizeof(void*));
ph->list.count = lightship->pos_history_len-1;
ph->list.size = (lightship->pos_history_len-1) * sizeof(void*);
if (lightship->path_history_len != 0) {
ph->list.array = malloc((lightship->path_history_len) * sizeof(void*));
ph->list.count = lightship->path_history_len;
ph->list.size = (lightship->path_history_len) * sizeof(void*);
for (int i = 0; i < lightship->pos_history_len-1; ++i) {
ph->list.array[0] = calloc(1,sizeof(PathPoint_t));
if (lightship->path_history[0]->alt != AltitudeValue_unavailable && pn.alt != AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = pn.alt - lightship->path_history[0]->alt;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
}
if (lightship->path_history[0]->lat != Latitude_unavailable && pn.lat != Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = pn.lat - lightship->path_history[0]->lat;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
}
if (lightship->path_history[0]->lon != Longitude_unavailable && pn.lon != Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = pn.lon - lightship->path_history[0]->lon;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
}
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
*ph->list.array[0]->pathDeltaTime = (pn.ts - lightship->path_history[0]->ts)/10;
for (int i = 1; i < lightship->path_history_len; ++i) {
ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
if (lightship->pos_history[i+1]->alt != AltitudeValue_unavailable && lightship->pos_history[i]->alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = lightship->pos_history[i+1]->alt - lightship->pos_history[i]->alt;
if (lightship->path_history[i]->alt != AltitudeValue_unavailable && lightship->path_history[i-1]->alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = lightship->path_history[i]->alt - lightship->path_history[i-1]->alt;
} else {
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
}
if (lightship->pos_history[i+1]->lat != Latitude_unavailable && lightship->pos_history[i]->lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = lightship->pos_history[i+1]->lat - lightship->pos_history[i]->lat;
if (lightship->path_history[i]->lat != Latitude_unavailable && lightship->path_history[i-1]->lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = lightship->path_history[i]->lat - lightship->path_history[i-1]->lat;
} else {
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
}
if (lightship->pos_history[i+1]->lon != Longitude_unavailable && lightship->pos_history[i]->lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = lightship->pos_history[i+1]->lon - lightship->pos_history[i]->lon;
if (lightship->path_history[i]->lon != Longitude_unavailable && lightship->path_history[i-1]->lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = lightship->path_history[i]->lon - lightship->path_history[i-1]->lon;
} else {
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
}
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
*ph->list.array[i]->pathDeltaTime = (lightship->pos_history[i]->ts - lightship->pos_history[i+1]->ts)/10;
*ph->list.array[i]->pathDeltaTime = (lightship->path_history[i-1]->ts - lightship->path_history[i]->ts)/10;
}
}
lightship->last_cam_lfc = now;
lightship->t_last_cam_lfc = now;
}
lightship->last_pos = pn;
} else {
cam->cam.generationDeltaTime = now % 65536;
@ -358,8 +461,6 @@ int lightship_init() {
lightship->protected_zones.pz = calloc(256, sizeof(void*));
pthread_mutex_init(&lightship->lock, NULL);
lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*));
int shm_fd;
shm_fd = shm_open("it2s-obd", O_RDONLY, 0666);
if (shm_fd == -1) {
@ -397,19 +498,19 @@ int lightship_check() {
itss_space_get();
// Check heading delta > 4º
int diff = epv.space.heading - lightship->pos_history[0]->heading;
int diff = epv.space.heading - lightship->last_pos.heading;
if (abs(diff) > 40) rv = 1;
if (!rv) {
// Check speed delta > 0.5 m/s
diff = epv.space.speed - lightship->pos_history[0]->speed;
diff = epv.space.speed - lightship->last_pos.speed;
if (abs(diff) > 50) rv = 1;
if (!rv) {
// Check position delta > 4 m
// TODO make an *accurate* distance calculator using GPS coords
int32_t avg_speed = (epv.space.speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */
int32_t avg_speed = (epv.space.speed + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->t_last_cam) / 1000; /* ms to s */
if (avg_speed * delta_time > 4) rv = 1;
}
}
@ -683,7 +784,7 @@ static int check_pz() {
pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
double d = haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);
double d = itss_haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);
int pz_radius = 50;
if (lightship->protected_zones.pz[i]->protectedZoneRadius) {

18
src/cam.h
View File

@ -44,14 +44,14 @@ enum CAM_CHECK_R {
CAM_BAD_PERMISSIONS
};
typedef struct pos_vector {
typedef struct pos_point {
uint64_t ts;
uint16_t heading;
int32_t lon;
int32_t lat;
int32_t alt;
uint16_t speed;
} pos_vector_t;
} pos_point_t;
typedef struct lightship {
bool active;
@ -60,14 +60,20 @@ typedef struct lightship {
uint8_t type;
uint64_t last_cam;
pos_point_t last_pos;
uint64_t t_last_cam;
uint64_t next_cam_max;
uint64_t next_cam_min;
uint64_t last_cam_lfc;
uint64_t t_last_cam_lfc;
pos_point_t concise_points[3];
uint8_t concise_points_len;
bool concise_keep_start;
pos_point_t* path_history[PATH_HISTORY_MAX_LEN];
uint16_t path_history_len;
pos_vector_t** pos_history;
uint16_t pos_history_len;
bool is_vehicle_near;
uint64_t last_vehicle;

13
src/facilities.c
View File

@ -564,13 +564,13 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
facilities.id.change.stage = ID_CHANGE_COMMIT;
// Reset lightship
for (int i = 0; i < facilities.lightship.pos_history_len; ++i) {
free(facilities.lightship.pos_history[i]);
for (int i = 0; i < facilities.lightship.path_history_len; ++i) {
free(facilities.lightship.path_history[i]);
}
facilities.lightship.pos_history_len = 0;
facilities.lightship.path_history_len = 0;
facilities.lightship.last_cam = 0;
facilities.lightship.last_cam_lfc = 0;
facilities.lightship.t_last_cam = 0;
facilities.lightship.t_last_cam_lfc = 0;
facilities.lightship.next_cam_max = 0;
facilities.lightship.next_cam_min = 0;
@ -808,6 +808,7 @@ int main() {
signal(SIGINT, sigh);
signal(SIGKILL, sigh);
facilities.tx_queue = itss_queue_new();
lightship_init();
den_init();
@ -819,7 +820,7 @@ int main() {
time_t t;
srand((unsigned) time(&t));
if (facilities_config(&facilities)) {
if (facilities_config()) {
goto cleanup;
}

58
src/requests.c
View File

@ -14,6 +14,8 @@
#include <it2s-tender/time.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
int facilities_request_result_accepted(void* responder) {
int rv = 0;
@ -151,7 +153,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_
// Set only one trace
if (facilities.station_type != 15) {
pthread_mutex_lock(&facilities.lightship.lock);
if (facilities.lightship.pos_history_len > 0) {
if (facilities.lightship.path_history_len > 0) {
if (!((DENM_t*)its_msg)->denm.location) {
((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t));
@ -162,42 +164,67 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_
((DENM_t*)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t));
PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0];
pos_vector_t** pos_history = facilities.lightship.pos_history;
uint16_t pos_history_len = facilities.lightship.pos_history_len;
pos_point_t** path_history = facilities.lightship.path_history;
uint16_t path_history_len = facilities.lightship.path_history_len;
ph->list.array = malloc((pos_history_len-1) * sizeof(void*));
ph->list.count = pos_history_len-1;
ph->list.size = (pos_history_len-1) * sizeof(void*);
ph->list.array = malloc((path_history_len) * sizeof(void*));
ph->list.count = path_history_len;
ph->list.size = (path_history_len) * sizeof(void*);
for (int i = 0; i < pos_history_len-1; ++i) {
ph->list.array[0] = calloc(1,sizeof(PathPoint_t));
itss_space_lock();
if (path_history[0]->alt != AltitudeValue_unavailable && epv.space.altitude != AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = epv.space.altitude - path_history[0]->alt;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
}
if (path_history[0]->lat != Latitude_unavailable && epv.space.latitude != Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = epv.space.latitude - path_history[0]->lat;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
}
if (path_history[0]->lon != Longitude_unavailable && epv.space.longitude != Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = epv.space.longitude - path_history[0]->lon;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
}
itss_space_unlock();
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
*ph->list.array[0]->pathDeltaTime = (itss_time_get() - path_history[0]->ts)/10;
for (int i = 1; i < path_history_len; ++i) {
ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
if (pos_history[i+1]->alt != AltitudeValue_unavailable && pos_history[i]->alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = pos_history[i+1]->alt - pos_history[i]->alt;
if (path_history[i]->alt != AltitudeValue_unavailable && path_history[i-1]->alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = path_history[i]->alt - path_history[i-1]->alt;
} else {
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
}
if (pos_history[i+1]->lat != Latitude_unavailable && pos_history[i]->lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = pos_history[i+1]->lat - pos_history[i]->lat;
if (path_history[i]->lat != Latitude_unavailable && path_history[i-1]->lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = path_history[i]->lat - path_history[i-1]->lat;
} else {
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
}
if (pos_history[i+1]->lon != Longitude_unavailable && pos_history[i]->lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = pos_history[i+1]->lon - pos_history[i]->lon;
if (path_history[i]->lon != Longitude_unavailable && path_history[i-1]->lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = path_history[i]->lon - path_history[i-1]->lon;
} else {
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
}
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
*ph->list.array[i]->pathDeltaTime = (pos_history[i]->ts - pos_history[i+1]->ts)/10;
*ph->list.array[i]->pathDeltaTime = (path_history[i-1]->ts - path_history[i]->ts)/10;
}
}
pthread_mutex_unlock(&facilities.lightship.lock);
}
// get, set retransmission, duration
if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) {
transmission_interval = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.transmissionInterval );
@ -557,4 +584,3 @@ cleanup:
return rv;
}