diff --git a/cereal b/cereal index f35491f0c5..80bbbd4bf7 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit f35491f0c5bf21d7d8c0cae9b5761fdf64a8d350 +Subproject commit 80bbbd4bf70698a3e7b7eaa1e463753c3db2ab5f diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f64acae101..349a6cb840 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -227,6 +227,8 @@ class Controls: self.events.add(EventName.relayMalfunction) if self.sm['plan'].fcw: self.events.add(EventName.fcw) + if self.sm['model'].frameAge > 1: + self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 6f184bbb36..f576cf4d2b 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -204,8 +204,6 @@ def wrong_car_mode_alert(CP, sm, metric): EVENTS = { # ********** events with no alerts ********** - EventName.modeldLagging: {}, - # ********** events only containing alerts displayed in all states ********** EventName.debugAlert: { @@ -651,6 +649,11 @@ EVENTS = { ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"), }, + EventName.modeldLagging: { + ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"), + ET.NO_ENTRY : NoEntryAlert("Driving model lagging"), + }, + EventName.lowMemory: { ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), ET.PERMANENT: Alert( diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 5cf8ae0881..e77a9cacfa 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -217,8 +217,8 @@ 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); + model_publish(pm, extra.frame_id, frame_id, sm.allAliveAndValid(), model_buf, extra.timestamp_eof); + posenet_publish(pm, extra.frame_id, frame_id, sm.allAliveAndValid(), 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); last = mt1; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 7156b09733..8b17dd74f7 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -245,15 +245,18 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float longi.setAccelerations(accel); } -void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { // make msg capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(nanos_since_boot()); + uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + auto framed = event.initModel(); framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); framed.setTimestampEof(timestamp_eof); auto lpath = framed.initPath(); @@ -290,13 +293,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_id < vipc_frame_id + MAX_FRAME_AGE); + event.setValid((frame_age < MAX_FRAME_AGE) && sm_alive_valid); 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) { +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(nanos_since_boot()); @@ -324,9 +327,12 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, kj::ArrayPtr rot_std_vs(&rot_std_arr[0], 3); posenetd.setRotStd(rot_std_vs); + posenetd.setTimestampEof(timestamp_eof); posenetd.setFrameId(vipc_frame_id); - event.setValid(frame_id < vipc_frame_id + MAX_FRAME_AGE); + + uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + event.setValid((frame_age < MAX_FRAME_AGE) && sm_alive_valid); pm.send("cameraOdometry", msg); } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 29d7a68f6a..dab8fc0faa 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -73,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 vipc_frame_id, uint32_t frame_id, - const ModelDataRaw &data, uint64_t timestamp_eof); -void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid, const ModelDataRaw &data, uint64_t timestamp_eof); +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid, + const ModelDataRaw &data, uint64_t timestamp_eof); #endif