diff --git a/cereal b/cereal index 80bbbd4bf7..f725dcd617 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 80bbbd4bf70698a3e7b7eaa1e463753c3db2ab5f +Subproject commit f725dcd617b0ff577ad5caee8b9b3c861d6be4a8 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 349a6cb840..4c11c5e05a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -227,7 +227,7 @@ class Controls: self.events.add(EventName.relayMalfunction) if self.sm['plan'].fcw: self.events.add(EventName.fcw) - if self.sm['model'].frameAge > 1: + if self.sm['model'].frameDropPerc > 1: self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 369c446141..68b370124c 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -163,7 +163,7 @@ int main(int argc, char **argv) { VisionStream stream; while (!do_exit) { VisionStreamBufs buf_info; - err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); + err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); if (err) { LOGW("visionstream connect failed"); usleep(100000); @@ -171,10 +171,17 @@ int main(int argc, char **argv) { } LOGW("connected with buffer size: %d", buf_info.buf_len); + // setup filter to track dropped frames + const float dt = 1. / MODEL_FREQ; + const float ts = 5.0; // 5 s filter time constant + const float frame_filter_k = (dt / ts) / (1. + dt / ts); + float frames_dropped = 0; + // one frame in memory cl_mem yuv_cl; VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl); + uint32_t last_vipc_frame_id = 0; double last = 0; int desire = -1; while (!do_exit) { @@ -217,11 +224,17 @@ int main(int argc, char **argv) { model_transform, NULL, vec_desire); mt2 = millis_since_boot(); - 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); + // tracked dropped frames + uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1; + frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U); + float frame_drop_perc = frames_dropped / MODEL_FREQ; + + model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); + posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); - LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu", mt2-mt1, mt1-last, extra.frame_id, frame_id); + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f%", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc); last = mt1; + last_vipc_frame_id = extra.frame_id; } } diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9ac7604195..af481712b1 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -246,7 +246,7 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float } void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { // make msg capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); @@ -257,6 +257,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, auto framed = event.initModel(); framed.setFrameId(vipc_frame_id); framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); auto lpath = framed.initPath(); @@ -293,13 +294,13 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, auto meta = framed.initMeta(); fill_meta(meta, net_outputs.meta); - event.setValid(frame_age < MAX_FRAME_AGE); + event.setValid(frame_drop < MAX_FRAME_DROP); pm.send("model", msg); } void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(nanos_since_boot()); @@ -331,8 +332,7 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, posenetd.setTimestampEof(timestamp_eof); posenetd.setFrameId(vipc_frame_id); - uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - event.setValid(frame_age < MAX_FRAME_AGE); + event.setValid(vipc_dropped_frames < 1); pm.send("cameraOdometry", msg); } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 97941db198..83d967d23a 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -35,7 +35,8 @@ #define TIME_DISTANCE 100 #define POSE_SIZE 12 -#define MAX_FRAME_AGE 5 +#define MODEL_FREQ 20 +#define MAX_FRAME_DROP 0.05 struct ModelDataRaw { float *path; @@ -74,7 +75,7 @@ void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - const ModelDataRaw &data, uint64_t timestamp_eof); + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); #endif diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py index 0ed651ad4b..eba08fd872 100755 --- a/selfdrive/test/test_cpu_usage.py +++ b/selfdrive/test/test_cpu_usage.py @@ -17,7 +17,7 @@ def print_cpu_usage(first_proc, last_proc): procs = [ ("selfdrive.controls.controlsd", 59.46), ("selfdrive.locationd.locationd", 34.38), - ("./loggerd", 28.49), + ("./loggerd", 33.90), ("selfdrive.controls.plannerd", 19.77), ("./_modeld", 12.74), ("selfdrive.locationd.paramsd", 11.53),