diff --git a/src/vcm.c b/src/vcm.c index cfa4647..ca43a31 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -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));