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

VCM: simple speed accel or deccel

This commit is contained in:
emanuel 2022-10-24 19:07:09 +01:00
parent 4904c8d3b2
commit 095c01bb43
1 changed files with 34 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/vcm.c
View File

@ -76,7 +76,39 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
const ssize_t buf_len = 512;
uint8_t buf1[buf_len], buf2[buf_len];
// Break a little bit
// Change speed
// Just check who is closer to mid trajectory point, if ego is closer, accelerate
// TODO a real trajectory analysis
STDeltaTrajectory_t* stdt = &request->desiredTrajectories.list.array[0]->trajectory;
itss_st_t* trj = calloc(stdt->list.count/2 + 1, sizeof(itss_st_t));
trj[0].latitude = vcm->vcm.currentPosition.latitude;
trj[0].longitude = vcm->vcm.currentPosition.longitude;
asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trj[0].timestamp);
int h = 0;
for (h = 1; h < stdt->list.count/2; ++h) {
trj[h].latitude = trj[h-1].latitude + stdt->list.array[h-1]->deltaLatitude;
trj[h].longitude = trj[h-1].longitude + stdt->list.array[h-1]->deltaLongitude;
trj[h].timestamp = trj[h-1].timestamp + stdt->list.array[h-1]->deltaTime;
}
if (h) --h;
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
itss_space_unlock();
double dreq = itss_haversine(
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
vcm->vcm.currentPosition.latitude / 1.0e7, vcm->vcm.currentPosition.longitude / 1.0e7
);
double dego = itss_haversine(
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
lat / 1.0e7, lon / 1.0e7
);
free(trj);
ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
mreq->present = ManagementRequest_PR_attributes;
mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
@ -85,7 +117,7 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
mgss->rate = 5; /* km/h/s */
mgss->temporary = true; /* go back to original speed after a while */
mgss->type.present = ManagementSpeedSetType_PR_diff; /* differential change set */
mgss->type.choice.diff = -10; /* % */
mgss->type.choice.diff = (dreq > dego) ? 10 : -10; /* % */
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, mreq, buf1, buf_len);
if (enc.encoded == -1) {
log_error("[vc] failed to encode MReq.speedSet (%s)", enc.failed_type->name);
@ -96,7 +128,6 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
}
ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
// Respond
VCM_t* vcm_rep = NULL;
TransportRequest_t* tr = NULL;
@ -113,12 +144,6 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
neighbour->proposed = true;
neighbour->t_proposal = now;
int32_t lat, lon;
itss_space_lock();
itss_space_get();
lat = epv.space.latitude;
lon = epv.space.longitude;
itss_space_unlock();
vcm_rep = calloc(1, sizeof(VCM_t));