Revert modeld vipc conflate behavior and alert on dropped frame percent (#1877)
* Revert "Fix modeld dropping frames if processing takes over 50 ms"
This reverts commit a8e7a187ca
.
* track frame drop
* fix unused
* alert on frame drop perc
* reduce thresholds
* posenet is invalid on non-consecutive frames
* Refactor filter time constant
* Use vipc_dropped_frames for posenet valid check
* Actually set frame drop percentage
* Add explicit cast just to be sure
* Prevent frames dropped to go up to quickly on startup
* bump cereal
* reduce soft disable threshold to 1%
Co-authored-by: Willem Melching <willem.melching@gmail.com>
pull/1879/head
parent
d9adbae9a8
commit
58c3b5ba84
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit 80bbbd4bf70698a3e7b7eaa1e463753c3db2ab5f
|
||||
Subproject commit f725dcd617b0ff577ad5caee8b9b3c861d6be4a8
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu", mt2-mt1, mt1-last, extra.frame_id, frame_id);
|
||||
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, frame_drop %.3f%", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc);
|
||||
last = mt1;
|
||||
last_vipc_frame_id = extra.frame_id;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -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<cereal::Event>();
|
||||
|
@ -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<cereal::Event>();
|
||||
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);
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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),
|
||||
|
|
Loading…
Reference in New Issue