diff --git a/cereal b/cereal index 46728e7e3b..7dcbf6c41f 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 46728e7e3b47cf73586e33540dc0b65317e308ce +Subproject commit 7dcbf6c41fb30b85dbb597fe771dc9f5e7f29161 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 3b14199867..1940667d80 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -175,7 +175,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo float valid_len; // clamp to 5 and 192 - valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2])); + valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2])); for (int i=0; i(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initModel(); - framed.setFrameId(frame_id); - framed.setTimestampEof(timestamp_eof); - - auto lpath = framed.initPath(); - fill_path(lpath, net_outputs.path, false, 0); - auto left_lane = framed.initLeftLane(); - fill_path(left_lane, net_outputs.left_lane, true, 1.8); - auto right_lane = framed.initRightLane(); - fill_path(right_lane, net_outputs.right_lane, true, -1.8); - auto longi = framed.initLongitudinal(); - fill_longi(longi, net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); - - - // Find the distribution that corresponds to the current lead - int mdn_max_idx = 0; - int t_offset = 0; - for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { - mdn_max_idx = i; - } + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initModel(); + framed.setFrameId(frame_id); + framed.setTimestampEof(timestamp_eof); + + auto lpath = framed.initPath(); + fill_path(lpath, net_outputs.path, false, 0); + auto left_lane = framed.initLeftLane(); + fill_path(left_lane, net_outputs.left_lane, true, 1.8); + auto right_lane = framed.initRightLane(); + fill_path(right_lane, net_outputs.right_lane, true, -1.8); + auto longi = framed.initLongitudinal(); + fill_longi(longi, net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); + + + // Find the distribution that corresponds to the current lead + int mdn_max_idx = 0; + int t_offset = 0; + for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { + mdn_max_idx = i; } - auto lead = framed.initLead(); - fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset); - // Find the distribution that corresponds to the lead in 2s - mdn_max_idx = 0; - t_offset = 1; - for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { - mdn_max_idx = i; - } + } + auto lead = framed.initLead(); + fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset); + // Find the distribution that corresponds to the lead in 2s + mdn_max_idx = 0; + t_offset = 1; + for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { + mdn_max_idx = i; } - auto lead_future = framed.initLeadFuture(); - fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset); + } + auto lead_future = framed.initLeadFuture(); + fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset); - auto meta = framed.initMeta(); - fill_meta(meta, net_outputs.meta); + auto meta = framed.initMeta(); + fill_meta(meta, net_outputs.meta); - pm.send("model", msg); - } + pm.send("model", msg); +} void posenet_publish(PubMaster &pm, uint32_t frame_id, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {