mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-04 21:12:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

add long_x

Browse Source 
old-commit-hash: d02f55ae0b0ee39ed758c5b8a19411fddc4905a4
This commit is contained in:
Harald Schafer
2020-03-26 17:40:12 -07:00
parent a0a2f24fcc
commit 5f17520b8e
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/models/driving.cc
+6 -2
View File
@@ -205,14 +205,18 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
meta.setDesirePrediction(desire_pred);
}
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) {
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) {
// just doing 10 vals, 1 every sec for now
float dist_arr[TIME_DISTANCE/10];
float speed_arr[TIME_DISTANCE/10];
float accel_arr[TIME_DISTANCE/10];
for (int i=0; i<TIME_DISTANCE/10; i++) {
dist_arr[i] = long_x_data[i*10];
speed_arr[i] = long_v_data[i*10];
accel_arr[i] = long_a_data[i*10];
}
kj::ArrayPtr<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
longi.setDistances(dist);
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
longi.setSpeeds(speed);
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
@@ -237,7 +241,7 @@ void model_publish(PubSocket *sock, uint32_t frame_id,
auto right_lane = framed.initRightLane();
fill_path(right_lane, net_outputs.right_lane, true, -1.8);
auto longi = framed.initLongitudinal();
fill_longi(longi, net_outputs.long_v, net_outputs.long_a);
fill_longi(longi, net_outputs.long_x, net_outputs.long_v, net_outputs.long_a);
// Find the distribution that corresponds to the current lead