|
|
@ -288,38 +288,32 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, |
|
|
|
s->limit_set_speed_timeout = UI_FREQ; |
|
|
|
s->limit_set_speed_timeout = UI_FREQ; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
static PathData read_path(cereal::ModelData::PathData::Reader pathp) { |
|
|
|
static void read_path(PathData& p, const cereal::ModelData::PathData::Reader &pathp) { |
|
|
|
PathData ret = {0}; |
|
|
|
p = {}; |
|
|
|
|
|
|
|
|
|
|
|
ret.prob = pathp.getProb(); |
|
|
|
p.prob = pathp.getProb(); |
|
|
|
ret.std = pathp.getStd(); |
|
|
|
p.std = pathp.getStd(); |
|
|
|
|
|
|
|
|
|
|
|
auto polyp = pathp.getPoly(); |
|
|
|
auto polyp = pathp.getPoly(); |
|
|
|
for (int i = 0; i < POLYFIT_DEGREE; i++) { |
|
|
|
for (int i = 0; i < POLYFIT_DEGREE; i++) { |
|
|
|
ret.poly[i] = polyp[i]; |
|
|
|
p.poly[i] = polyp[i]; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Compute points locations
|
|
|
|
// Compute points locations
|
|
|
|
for (int i = 0; i < MODEL_PATH_DISTANCE; i++) { |
|
|
|
for (int i = 0; i < MODEL_PATH_DISTANCE; i++) { |
|
|
|
ret.points[i] = ret.poly[0] * (i*i*i) + ret.poly[1] * (i*i)+ ret.poly[2] * i + ret.poly[3]; |
|
|
|
p.points[i] = p.poly[0] * (i*i*i) + p.poly[1] * (i*i)+ p.poly[2] * i + p.poly[3]; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
return ret; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
static ModelData read_model(cereal::ModelData::Reader model) { |
|
|
|
static void read_model(ModelData &d, const cereal::ModelData::Reader &model) { |
|
|
|
ModelData d = {0}; |
|
|
|
d = {}; |
|
|
|
|
|
|
|
read_path(d.path, model.getPath()); |
|
|
|
d.path = read_path(model.getPath()); |
|
|
|
read_path(d.left_lane, model.getLeftLane()); |
|
|
|
d.left_lane = read_path(model.getLeftLane()); |
|
|
|
read_path(d.right_lane, model.getRightLane()); |
|
|
|
d.right_lane = read_path(model.getRightLane()); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
auto leadd = model.getLead(); |
|
|
|
auto leadd = model.getLead(); |
|
|
|
d.lead = (LeadData){ |
|
|
|
d.lead = (LeadData){ |
|
|
|
.dist = leadd.getDist(), .prob = leadd.getProb(), .std = leadd.getStd(), |
|
|
|
.dist = leadd.getDist(), .prob = leadd.getProb(), .std = leadd.getStd(), |
|
|
|
}; |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
return d; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
static void update_status(UIState *s, int status) { |
|
|
|
static void update_status(UIState *s, int status) { |
|
|
@ -414,7 +408,7 @@ void handle_message(UIState *s, SubMaster &sm) { |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
if (sm.updated("model")) { |
|
|
|
if (sm.updated("model")) { |
|
|
|
scene.model = read_model(sm["model"].getModel()); |
|
|
|
read_model(scene.model, sm["model"].getModel()); |
|
|
|
s->model_changed = true; |
|
|
|
s->model_changed = true; |
|
|
|
} |
|
|
|
} |
|
|
|
// else if (which == cereal::Event::LIVE_MPC) {
|
|
|
|
// else if (which == cereal::Event::LIVE_MPC) {
|
|
|
|