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

Make model packets invalid when frames are too old (#1759)

Browse Source 
* make model packets invalid when frames are too old

* add frame numbers to cloudlog
old-commit-hash: 88f92663780665b27fafdb5ab86ad501cad4cbcf
This commit is contained in:
Willem Melching
2020-06-22 14:27:58 -07:00
committed by GitHub
parent 6dc1c6e58e
commit b772d12f87
3 changed files with 15 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.cc
+5 -4
View File
@@ -82,7 +82,7 @@ int main(int argc, char **argv) {
// messaging
PubMaster pm({"model", "cameraOdometry"});
SubMaster sm({"pathPlan"});
SubMaster sm({"pathPlan", "frame"});
#ifdef QCOM
cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT;
@@ -202,6 +202,7 @@ int main(int argc, char **argv) {
}
mat3 model_transform = matmul3(yuv_transform, transform);
uint32_t frame_id = sm["frame"].getFrame().getFrameId();
mt1 = millis_since_boot();
@@ -213,10 +214,10 @@ int main(int argc, char **argv) {
model_transform, NULL, vec_desire);
mt2 = millis_since_boot();
model_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof);
model_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu", mt2-mt1, mt1-last, extra.frame_id, frame_id);
last = mt1;
}
selfdrive/modeld/models/driving.cc
+6 -4
View File
@@ -245,7 +245,7 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float
longi.setAccelerations(accel);
}
void model_publish(PubMaster &pm, uint32_t frame_id,
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
// make msg
capnp::MallocMessageBuilder msg;
@@ -253,7 +253,7 @@ void model_publish(PubMaster &pm, uint32_t frame_id,
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setFrameId(vipc_frame_id);
framed.setTimestampEof(timestamp_eof);
auto lpath = framed.initPath();
@@ -290,11 +290,12 @@ void model_publish(PubMaster &pm, uint32_t frame_id,
auto meta = framed.initMeta();
fill_meta(meta, net_outputs.meta);
event.setValid(frame_id - vipc_frame_id < MAX_FRAME_AGE);
pm.send("model", msg);
}
void posenet_publish(PubMaster &pm, uint32_t frame_id,
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
@@ -324,7 +325,8 @@ void posenet_publish(PubMaster &pm, uint32_t frame_id,
posenetd.setRotStd(rot_std_vs);
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(frame_id);
posenetd.setFrameId(vipc_frame_id);
event.setValid(frame_id - vipc_frame_id < MAX_FRAME_AGE);
pm.send("cameraOdometry", msg);
}
selfdrive/modeld/models/driving.h
+4 -2
View File
@@ -35,6 +35,8 @@
#define TIME_DISTANCE 100
#define POSE_SIZE 12
#define MAX_FRAME_AGE 5
struct ModelDataRaw {
float *path;
float *left_lane;
@@ -71,8 +73,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out);
void model_publish(PubMaster &pm, uint32_t frame_id,
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
const ModelDataRaw &data, uint64_t timestamp_eof);
void posenet_publish(PubMaster &pm, uint32_t frame_id,
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
const ModelDataRaw &data, uint64_t timestamp_eof);
#endif