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

Alert when modeld is lagging by more than a frame (#1823)

Browse Source 
* alert when modeld is lagging by more than 1 frame

* log frameAge in modelData

* set posenet valid

* compute frame_age once
old-commit-hash: 03e824a4b558daa15191408e88b73fee7ab1792f
This commit is contained in:
Adeeb Shihadeh
2020-07-09 17:02:21 -07:00
committed by GitHub
parent bda90d828e
commit a1dbb38a0c
6 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: f35491f0c5...80bbbd4bf7
selfdrive/controls/controlsd.py
+2
View File
@@ -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 \
selfdrive/controls/lib/events.py
+5 -2
View File
@@ -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(
selfdrive/modeld/modeld.cc
+2 -2
View File
@@ -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;
selfdrive/modeld/models/driving.cc
+11 -5
View File
@@ -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<cereal::Event>();
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<cereal::Event>();
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<const float> 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);
}
selfdrive/modeld/models/driving.h
+3 -3
View File
@@ -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