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

215
src/cam.c
View File

@ -1,7 +1,6 @@
#include "cam.h" #include "cam.h"
#include "facilities.h" #include "facilities.h"
#include <camv2/INTEGER.h>
#include <itss-transport/TransportRequest.h> #include <itss-transport/TransportRequest.h>
#include <itss-facilities/FacilitiesIndication.h> #include <itss-facilities/FacilitiesIndication.h>
#include <itss-management/ManagementRequest.h> #include <itss-management/ManagementRequest.h>
@ -24,20 +23,15 @@
#include <it2s-obd.h> #include <it2s-obd.h>
#define LEAP_SECONDS 5 #define EARTH_RADIUS 6369000.0
#define EARTH_RADIUS 6369000
#define RAD_PER_DEG M_PI/180.0 #define RAD_PER_DEG M_PI/180.0
double haversine(double lat1, double lon1, double lat2, double lon2) { // Using Vanetza's Path History constraints
double d_lat = (lat1-lat2) * RAD_PER_DEG; #define PATH_HISTORY_ALLOWABLE_ERROR 0.47
double d_lon = (lon1-lon2) * RAD_PER_DEG; #define PATH_HISTORY_CHORD_LENGTH_THRESHOLD 22.5
#define PATH_HISTORY_SMALL_DELTA_PHI 1.0
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); #define PATH_HISTORY_DISTANCE 200.0
double c = 2 * atan2(sqrt(a), sqrt(1-a)); #define PATH_HISTORY_MAX_ESTIMATED_RADIUS EARTH_RADIUS
return EARTH_RADIUS * c;
}
const cid_ssp_bm_t CID_SSP_BM_MAP[] = { const cid_ssp_bm_t CID_SSP_BM_MAP[] = {
{"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000}, {"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) { static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) {
int rv = 0; int rv = 0;
int shm_fd, shm_valid = 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.yawRateValue = YawRateValue_unavailable;
bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable; bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
/*
// Save current values // Save current values
if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) { if (lightship->path_history_len == POS_HISTORY_MAX_LEN) {
free(lightship->pos_history[POS_HISTORY_MAX_LEN-1]); free(lightship->path_history[POS_HISTORY_MAX_LEN-1]);
--lightship->pos_history_len; --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)); pos_point_t pn;
lightship->pos_history[0]->heading = bvc_hf->heading.headingValue; pn.heading = bvc_hf->heading.headingValue;
lightship->pos_history[0]->lat = bc->referencePosition.latitude; pn.lat = bc->referencePosition.latitude;
lightship->pos_history[0]->lon = bc->referencePosition.longitude; pn.lon = bc->referencePosition.longitude;
lightship->pos_history[0]->alt = bc->referencePosition.altitude.altitudeValue; pn.alt = bc->referencePosition.altitude.altitudeValue;
lightship->pos_history[0]->speed = bvc_hf->speed.speedValue; pn.speed = bvc_hf->speed.speedValue;
pn.ts = now;
path_history_update(&pn);
lightship->last_cam = now; lightship->t_last_cam = now;
lightship->pos_history[0]->ts = now;
++lightship->pos_history_len;
// Acceleration // Acceleration
if (lightship->pos_history_len > 1) { if (lightship->last_pos.ts != 0) {
double delta_angle = (lightship->pos_history[0]->heading - lightship->pos_history[1]->heading) / 10.0; /* 1º */ double delta_angle = ((int)pn.heading - lightship->last_pos.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 */ 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) ((lightship->pos_history[0]->ts - lightship->pos_history[1]->ts) / 1000)); /* 0.1 m/s^2 */ 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; if (long_a > 160) long_a = 160;
else 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 // 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 = calloc(1, sizeof(LowFrequencyContainer_t));
cam->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; 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; PathHistory_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;
ph->list.array = malloc((lightship->pos_history_len-1) * sizeof(void*)); if (lightship->path_history_len != 0) {
ph->list.count = lightship->pos_history_len-1; ph->list.array = malloc((lightship->path_history_len) * sizeof(void*));
ph->list.size = (lightship->pos_history_len-1) * 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));
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) { if (lightship->path_history[0]->alt != AltitudeValue_unavailable && pn.alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = lightship->pos_history[i+1]->alt - lightship->pos_history[i]->alt; ph->list.array[0]->pathPosition.deltaAltitude = pn.alt - lightship->path_history[0]->alt;
} else { } else {
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
} }
if (lightship->pos_history[i+1]->lat != Latitude_unavailable && lightship->pos_history[i]->lat != Latitude_unavailable) { if (lightship->path_history[0]->lat != Latitude_unavailable && pn.lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = lightship->pos_history[i+1]->lat - lightship->pos_history[i]->lat; ph->list.array[0]->pathPosition.deltaLatitude = pn.lat - lightship->path_history[0]->lat;
} else { } else {
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
} }
if (lightship->pos_history[i+1]->lon != Longitude_unavailable && lightship->pos_history[i]->lon != Longitude_unavailable) { if (lightship->path_history[0]->lon != Longitude_unavailable && pn.lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = lightship->pos_history[i+1]->lon - lightship->pos_history[i]->lon; ph->list.array[0]->pathPosition.deltaLongitude = pn.lon - lightship->path_history[0]->lon;
} else { } else {
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
} }
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); ph->list.array[0]->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[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->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->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->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->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 { } else {
cam->cam.generationDeltaTime = now % 65536; cam->cam.generationDeltaTime = now % 65536;
@ -358,8 +461,6 @@ int lightship_init() {
lightship->protected_zones.pz = calloc(256, sizeof(void*)); lightship->protected_zones.pz = calloc(256, sizeof(void*));
pthread_mutex_init(&lightship->lock, NULL); pthread_mutex_init(&lightship->lock, NULL);
lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*));
int shm_fd; int shm_fd;
shm_fd = shm_open("it2s-obd", O_RDONLY, 0666); shm_fd = shm_open("it2s-obd", O_RDONLY, 0666);
if (shm_fd == -1) { if (shm_fd == -1) {
@ -397,19 +498,19 @@ int lightship_check() {
itss_space_get(); itss_space_get();
// Check heading delta > 4º // 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 (abs(diff) > 40) rv = 1;
if (!rv) { if (!rv) {
// Check speed delta > 0.5 m/s // 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 (abs(diff) > 50) rv = 1;
if (!rv) { if (!rv) {
// Check position delta > 4 m // Check position delta > 4 m
// TODO make an *accurate* distance calculator using GPS coords // 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 */ int32_t avg_speed = (epv.space.speed + lightship->last_pos.speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */ uint64_t delta_time = (now - lightship->t_last_cam) / 1000; /* ms to s */
if (avg_speed * delta_time > 4) rv = 1; if (avg_speed * delta_time > 4) rv = 1;
} }
} }
@ -683,7 +784,7 @@ static int check_pz() {
pthread_mutex_lock(&lightship->lock); pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) { 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; int pz_radius = 50;
if (lightship->protected_zones.pz[i]->protectedZoneRadius) { if (lightship->protected_zones.pz[i]->protectedZoneRadius) {

18
src/cam.h
View File

@ -44,14 +44,14 @@ enum CAM_CHECK_R {
CAM_BAD_PERMISSIONS CAM_BAD_PERMISSIONS
}; };
typedef struct pos_vector { typedef struct pos_point {
uint64_t ts; uint64_t ts;
uint16_t heading; uint16_t heading;
int32_t lon; int32_t lon;
int32_t lat; int32_t lat;
int32_t alt; int32_t alt;
uint16_t speed; uint16_t speed;
} pos_vector_t; } pos_point_t;
typedef struct lightship { typedef struct lightship {
bool active; bool active;
@ -60,14 +60,20 @@ typedef struct lightship {
uint8_t type; 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_max;
uint64_t next_cam_min; 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; bool is_vehicle_near;
uint64_t last_vehicle; 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; facilities.id.change.stage = ID_CHANGE_COMMIT;
// Reset lightship // Reset lightship
for (int i = 0; i < facilities.lightship.pos_history_len; ++i) { for (int i = 0; i < facilities.lightship.path_history_len; ++i) {
free(facilities.lightship.pos_history[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.t_last_cam = 0;
facilities.lightship.last_cam_lfc = 0; facilities.lightship.t_last_cam_lfc = 0;
facilities.lightship.next_cam_max = 0; facilities.lightship.next_cam_max = 0;
facilities.lightship.next_cam_min = 0; facilities.lightship.next_cam_min = 0;
@ -808,6 +808,7 @@ int main() {
signal(SIGINT, sigh); signal(SIGINT, sigh);
signal(SIGKILL, sigh); signal(SIGKILL, sigh);
facilities.tx_queue = itss_queue_new(); facilities.tx_queue = itss_queue_new();
lightship_init(); lightship_init();
den_init(); den_init();
@ -819,7 +820,7 @@ int main() {
time_t t; time_t t;
srand((unsigned) time(&t)); srand((unsigned) time(&t));
if (facilities_config(&facilities)) { if (facilities_config()) {
goto cleanup; goto cleanup;
} }

60
src/requests.c
View File

@ -14,6 +14,8 @@
#include <it2s-tender/time.h> #include <it2s-tender/time.h>
#include <it2s-tender/recorder.h> #include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h> #include <it2s-tender/packet.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
int facilities_request_result_accepted(void* responder) { int facilities_request_result_accepted(void* responder) {
int rv = 0; int rv = 0;
@ -88,7 +90,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_
break; break;
case ItsMessageType_cpm: case ItsMessageType_cpm:
its_msg_def = &asn_DEF_CPM; its_msg_def = &asn_DEF_CPM;
its_msg = calloc(1, sizeof(CPM_t)); its_msg = calloc(1, sizeof(CPM_t));
bpr->destinationPort = Port_cpm; bpr->destinationPort = Port_cpm;
@ -151,7 +153,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_
// Set only one trace // Set only one trace
if (facilities.station_type != 15) { if (facilities.station_type != 15) {
pthread_mutex_lock(&facilities.lightship.lock); 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) { if (!((DENM_t*)its_msg)->denm.location) {
((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t)); ((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)); ((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]; PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0];
pos_vector_t** pos_history = facilities.lightship.pos_history; pos_point_t** path_history = facilities.lightship.path_history;
uint16_t pos_history_len = facilities.lightship.pos_history_len; uint16_t path_history_len = facilities.lightship.path_history_len;
ph->list.array = malloc((pos_history_len-1) * sizeof(void*)); ph->list.array = malloc((path_history_len) * sizeof(void*));
ph->list.count = pos_history_len-1; ph->list.count = path_history_len;
ph->list.size = (pos_history_len-1) * sizeof(void*); 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)); ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
if (pos_history[i+1]->alt != AltitudeValue_unavailable && pos_history[i]->alt != AltitudeValue_unavailable) { if (path_history[i]->alt != AltitudeValue_unavailable && path_history[i-1]->alt != AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = pos_history[i+1]->alt - pos_history[i]->alt; ph->list.array[i]->pathPosition.deltaAltitude = path_history[i]->alt - path_history[i-1]->alt;
} else { } else {
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
} }
if (pos_history[i+1]->lat != Latitude_unavailable && pos_history[i]->lat != Latitude_unavailable) { if (path_history[i]->lat != Latitude_unavailable && path_history[i-1]->lat != Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = pos_history[i+1]->lat - pos_history[i]->lat; ph->list.array[i]->pathPosition.deltaLatitude = path_history[i]->lat - path_history[i-1]->lat;
} else { } else {
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
} }
if (pos_history[i+1]->lon != Longitude_unavailable && pos_history[i]->lon != Longitude_unavailable) { if (path_history[i]->lon != Longitude_unavailable && path_history[i-1]->lon != Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = pos_history[i+1]->lon - pos_history[i]->lon; ph->list.array[i]->pathPosition.deltaLongitude = path_history[i]->lon - path_history[i-1]->lon;
} else { } else {
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
} }
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); 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); pthread_mutex_unlock(&facilities.lightship.lock);
} }
// get, set retransmission, duration // get, set retransmission, duration
if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) { if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) {
transmission_interval = *( (uint32_t*) ((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; return rv;
} }