diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 10cc2fe56e..3356656180 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -55,7 +55,7 @@ mat3 update_calibration(Eigen::Matrix &extrinsics, bool wide_camera void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) { // messaging PubMaster pm({"modelV2", "cameraOdometry"}); - SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration"}); + SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState"}); // setup filter to track dropped frames FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); @@ -111,6 +111,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl // TODO: path planner timeout? sm.update(0); int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); + bool is_rhd = ((bool)sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); // timeout? frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); if (sm.updated("liveCalibration")) { auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); @@ -146,7 +147,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } double mt1 = millis_since_boot(); - ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, prepare_only); + ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only); double mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 59c0b249d9..9bf7e62184 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -49,14 +49,12 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #endif #ifdef TRAFFIC_CONVENTION - const int idx = Params().getBool("IsRHD") ? 1 : 0; - s->traffic_convention[idx] = 1.0; s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif } ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only) { + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { #ifdef DESIRE if (desire_in != NULL) { for (int i = 1; i < DESIRE_LEN; i++) { @@ -72,6 +70,10 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, } #endif + int rhd_idx = is_rhd; + s->traffic_convention[rhd_idx] = 1.0; + s->traffic_convention[1-rhd_idx] = 0.0; + // if getInputBuf is not NULL, net_input_buf will be auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, buf->stride, buf->uv_offset, transform, static_cast(s->m->getInputBuf())); s->m->addImage(net_input_buf, s->frame->buf_size); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 97e65fbc0e..d551bdf488 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -268,7 +268,7 @@ struct ModelState { void model_init(ModelState* s, cl_device_id device_id, cl_context context); ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only); + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only); void model_free(ModelState* s); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, const ModelOutput &net_outputs, uint64_t timestamp_eof,