diff --git a/CHANGELOGS-DEV.md b/CHANGELOGS-DEV.md index f4717e08c..75562b1c5 100644 --- a/CHANGELOGS-DEV.md +++ b/CHANGELOGS-DEV.md @@ -5,7 +5,19 @@ dragonpilot 0.7.10.1 * HYUNDAI_GENESIS 加入 Cruise 按紐 和 lkMode 支援。(感謝 @donfyffe 建議) * HYUNDAI_GENESIS added Cruise button event and lkMode feature. (Thanks to @donfyffe) * 支援台灣版 2018 Huyndai IONIQ + smart MDPS (dp_hkg_smart_mdps) (感謝 @andy741217 提供) -* Support 2018 Taiwan Hyundai IONIQ + smart MDPS (dp_hkg_smart_mdps) (Thanks to @andy741217) +* Support 2018 Taiwan Hyundai IONIQ + smart MDPS (dp_hkg_smart_mdps) (Thanks to @andy741217) +* 使用 openpilot v0.8 的模型。(感謝 @eisenheim) +* Use openpilot v0.8 model. (Thanks to @eisenheim) +* 加入 0.8 測試版的部分優化。 +* Added optimizations from pre-0.8. +* 加入 dp_honda_eps_mod 設定來使用更高的扭力 (需 eps mod)。(感謝 @Wuxl_369 提供) +* Added dp_honda_eps_mod setting to enable higher torque (eps mod required). (Thanks to @Wuxl_369) +* 修正 VW 對白/灰熊的支援 (感謝 @lirudy 提供) +* Fixed issue with white/grey panda support for VW (Thanks to @lirudy) +* GENESIS_G70 優化 (感謝 @sebastian4k 提供) +* GENESIS_G70 Optimisation (Thanks to @sebastian4k) +* HYUNDAI_GENESIS 優化 (感謝 @donfyffe 提供) +* HYUNDAI_GENESIS Optimisation (Thanks to @donfyffe) dragonpilot 0.7.10.0 ======================== diff --git a/CHANGELOGS-REL.md b/CHANGELOGS-REL.md index 3accef9a3..1fdc54269 100644 --- a/CHANGELOGS-REL.md +++ b/CHANGELOGS-REL.md @@ -23,6 +23,18 @@ dragonpilot 0.7.10.0 * HYUNDAI_GENESIS added Cruise button event and lkMode feature. (Thanks to @donfyffe) * 支援台灣版 2018 Huyndai IONIQ + smart MDPS (dp_hkg_smart_mdps) (感謝 @andy741217 提供) * Support 2018 Taiwan Hyundai IONIQ + smart MDPS (dp_hkg_smart_mdps) (Thanks to @andy741217) +* 使用 openpilot v0.8 的模型。(感謝 @eisenheim) +* Use openpilot v0.8 model. (Thanks to @eisenheim) +* 加入 0.8 測試版的部分優化。 +* Added optimizations from pre-0.8. +* 加入 dp_honda_eps_mod 設定來使用更高的扭力 (需 eps mod)。(感謝 @Wuxl_369 提供) +* Added dp_honda_eps_mod setting to enable higher torque (eps mod required). (Thanks to @Wuxl_369) +* 修正 VW 對白/灰熊的支援 (感謝 @lirudy 提供) +* Fixed issue with white/grey panda support for VW (Thanks to @lirudy) +* GENESIS_G70 優化 (感謝 @sebastian4k 提供) +* GENESIS_G70 Optimisation (Thanks to @sebastian4k) +* HYUNDAI_GENESIS 優化 (感謝 @donfyffe 提供) +* HYUNDAI_GENESIS Optimisation (Thanks to @donfyffe) dragonpilot 0.7.8 ======================== diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 7ae18c060..76e5e1cb3 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,7 +1,19 @@ -2020-11-12 (0.7.10.0) +2020-11-18 (0.7.10.0) ======================== * 支援台灣版 2018 Huyndai IONIQ + smart MDPS (dp_hkg_smart_mdps) (感謝 @andy741217 提供) * Support 2018 Taiwan Hyundai IONIQ + smart MDPS (dp_hkg_smart_mdps) (Thanks to @andy741217) +* 使用 openpilot v0.8 的模型。(感謝 @eisenheim) +* Use openpilot v0.8 model. (Thanks to @eisenheim) +* 加入 0.8 測試版的部分優化。 +* Added optimizations from pre-0.8. +* 加入 dp_honda_eps_mod 設定來使用更高的扭力 (需 eps mod)。(感謝 @Wuxl_369 提供) +* Added dp_honda_eps_mod setting to enable higher torque (eps mod required). (Thanks to @Wuxl_369) +* 修正 VW 對白/灰熊的支援 (感謝 @lirudy 提供) +* Fixed issue with white/grey panda support for VW (Thanks to @lirudy) +* GENESIS_G70 優化 (感謝 @sebastian4k 提供) +* GENESIS_G70 Optimisation (Thanks to @sebastian4k) +* HYUNDAI_GENESIS 優化 (感謝 @donfyffe 提供) +* HYUNDAI_GENESIS Optimisation (Thanks to @donfyffe) 2020-11-05 (0.7.10.0) ======================== diff --git a/cereal/log.capnp b/cereal/log.capnp index c4a2c30e0..66a0d926c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -701,7 +701,9 @@ struct ModelDataV2 { orientationRate @7 :XYZTData; laneLines @8 :List(XYZTData); laneLineProbs @9 :List(Float32); + laneLineStds @13 :List(Float32); roadEdges @10 :List(XYZTData); + roadEdgeStds @14 :List(Float32); leads @11 :List(LeadDataV2); meta @12 :MetaData; diff --git a/common/dp_conf.py b/common/dp_conf.py index 48f74f210..0c69c7985 100644 --- a/common/dp_conf.py +++ b/common/dp_conf.py @@ -108,6 +108,8 @@ confs = [ {'name': 'dp_toyota_lowest_cruise_override_speed', 'default': 32, 'type': 'Float32', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_toyota_lowest_cruise_override_speed', 'vals': [True]}], 'min': 0, 'max': 255., 'conf_type': ['param', 'struct']}, # hyundai {'name': 'dp_hkg_smart_mdps', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, + # honda + {'name': 'dp_honda_eps_mod', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, #misc {'name': 'dp_ip_addr', 'default': '', 'type': 'Text', 'conf_type': ['struct']}, {'name': 'dp_full_speed_fan', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, diff --git a/common/transformations/camera.py b/common/transformations/camera.py index b406c33fd..afb714718 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,29 +1,67 @@ import numpy as np -import common.transformations.orientation as orient -FULL_FRAME_SIZE = (1164, 874) -W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] -eon_focal_length = FOCAL = 910.0 +import common.transformations.orientation as orient +from common.hardware import TICI + +## -- hardcoded hardware params -- +eon_f_focal_length = 910.0 +eon_d_focal_length = 860.0 +leon_d_focal_length = 650.0 +tici_f_focal_length = 2648.0 +tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame + +eon_f_frame_size = (1164, 874) +eon_d_frame_size = (1152, 864) +leon_d_frame_size = (816, 612) +tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208) # aka 'K' aka camera_frame_from_view_frame -eon_intrinsics = np.array([ - [FOCAL, 0., W/2.], - [ 0., FOCAL, H/2.], - [ 0., 0., 1.]]) - +eon_fcam_intrinsics = np.array([ + [eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2], + [0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) +eon_intrinsics = eon_fcam_intrinsics # xx leon_dcam_intrinsics = np.array([ - [650, 0, 816//2], - [ 0, 650, 612//2], - [ 0, 0, 1]]) + [leon_d_focal_length, 0.0, float(leon_d_frame_size[0])/2], + [0.0, leon_d_focal_length, float(leon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) eon_dcam_intrinsics = np.array([ - [860, 0, 1152//2], - [ 0, 860, 864//2], - [ 0, 0, 1]]) + [eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2], + [0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_fcam_intrinsics = np.array([ + [tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2], + [0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_dcam_intrinsics = np.array([ + [tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2], + [0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) + +tici_ecam_intrinsics = tici_dcam_intrinsics # aka 'K_inv' aka view_frame_from_camera_frame -eon_intrinsics_inv = np.linalg.inv(eon_intrinsics) +eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics) +eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx + +tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) +tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) + + +if not TICI: + FULL_FRAME_SIZE = eon_f_frame_size + FOCAL = eon_f_focal_length + fcam_intrinsics = eon_fcam_intrinsics +else: + FULL_FRAME_SIZE = tici_f_frame_size + FOCAL = tici_f_focal_length + fcam_intrinsics = tici_fcam_intrinsics + +W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] # device/mesh : x->forward, y-> right, z->down @@ -69,9 +107,9 @@ def vp_from_ke(m): return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0]) -def vp_from_rpy(rpy): +def vp_from_rpy(rpy, intrinsics=fcam_intrinsics): e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22) - ke = np.dot(eon_intrinsics, e) + ke = np.dot(intrinsics, e) return vp_from_ke(ke) @@ -81,7 +119,7 @@ def roll_from_ke(m): -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) -def normalize(img_pts, intrinsics=eon_intrinsics): +def normalize(img_pts, intrinsics=fcam_intrinsics): # normalizes image coordinates # accepts single pt or array of pts intrinsics_inv = np.linalg.inv(intrinsics) @@ -94,7 +132,7 @@ def normalize(img_pts, intrinsics=eon_intrinsics): return img_pts_normalized[:, :2].reshape(input_shape) -def denormalize(img_pts, intrinsics=eon_intrinsics): +def denormalize(img_pts, intrinsics=fcam_intrinsics, width=W, height=H): # denormalizes image coordinates # accepts single pt or array of pts img_pts = np.array(img_pts) @@ -102,9 +140,9 @@ def denormalize(img_pts, intrinsics=eon_intrinsics): img_pts = np.atleast_2d(img_pts) img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1)))) img_pts_denormalized = img_pts.dot(intrinsics.T) - img_pts_denormalized[img_pts_denormalized[:, 0] > W] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan - img_pts_denormalized[img_pts_denormalized[:, 1] > H] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan return img_pts_denormalized[:, :2].reshape(input_shape) @@ -137,18 +175,10 @@ def img_from_device(pt_device): return pt_img.reshape(input_shape)[:, :2] -def get_camera_frame_from_calib_frame(camera_frame_from_road_frame): +def get_camera_frame_from_calib_frame(camera_frame_from_road_frame, intrinsics=fcam_intrinsics): camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)] - calib_frame_from_ground = np.dot(eon_intrinsics, - get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)] + calib_frame_from_ground = np.dot(intrinsics, + get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)] ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground) camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame) return camera_frame_from_calib_frame - - -def pretransform_from_calib(calib): - roll, pitch, yaw, height = calib - view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height) - camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame) - camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame) - return np.linalg.inv(camera_frame_from_calib_frame) diff --git a/common/transformations/model.py b/common/transformations/model.py index a3b46858b..af816b30a 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,34 +1,33 @@ import numpy as np -from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length, +from common.transformations.camera import (FULL_FRAME_SIZE, + FOCAL, get_view_frame_from_road_frame, get_view_frame_from_calib_frame, vp_from_ke) # segnet - SEGNET_SIZE = (512, 384) -segnet_frame_from_camera_frame = np.array([ - [float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0], 0., ], - [ 0., float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]]) - +def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE): + return np.array([[float(segnet_size[0]) / full_frame_size[0], 0.0], + [0.0, float(segnet_size[1]) / full_frame_size[1]]]) +segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx # model - MODEL_INPUT_SIZE = (320, 160) MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2) -MODEL_CX = MODEL_INPUT_SIZE[0]/2. +MODEL_CX = MODEL_INPUT_SIZE[0] / 2. MODEL_CY = 21. -model_zoom = 1.25 +model_fl = 728.0 model_height = 1.22 # canonical model transform -model_intrinsics = np.array( - [[ eon_focal_length / model_zoom, 0. , MODEL_CX], - [ 0. , eon_focal_length / model_zoom, MODEL_CY], - [ 0. , 0. , 1.]]) +model_intrinsics = np.array([ + [model_fl, 0.0, MODEL_CX], + [0.0, model_fl, MODEL_CY], + [0.0, 0.0, 1.0]]) # MED model @@ -36,64 +35,76 @@ MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) MEDMODEL_CY = 47.6 -medmodel_zoom = 1. -medmodel_intrinsics = np.array( - [[ eon_focal_length / medmodel_zoom, 0. , 0.5 * MEDMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / medmodel_zoom, MEDMODEL_CY], - [ 0. , 0. , 1.]]) +medmodel_fl = 910.0 +medmodel_intrinsics = np.array([ + [medmodel_fl, 0.0, 0.5 * MEDMODEL_INPUT_SIZE[0]], + [0.0, medmodel_fl, MEDMODEL_CY], + [0.0, 0.0, 1.0]]) + # CAL model CALMODEL_INPUT_SIZE = (512, 256) CALMODEL_YUV_SIZE = (CALMODEL_INPUT_SIZE[0], CALMODEL_INPUT_SIZE[1] * 3 // 2) CALMODEL_CY = 47.6 -calmodel_zoom = 1.5 -calmodel_intrinsics = np.array( - [[ eon_focal_length / calmodel_zoom, 0. , 0.5 * CALMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / calmodel_zoom, CALMODEL_CY], - [ 0. , 0. , 1.]]) +calmodel_fl = 606.7 +calmodel_intrinsics = np.array([ + [calmodel_fl, 0.0, 0.5 * CALMODEL_INPUT_SIZE[0]], + [0.0, calmodel_fl, CALMODEL_CY], + [0.0, 0.0, 1.0]]) # BIG model - BIGMODEL_INPUT_SIZE = (1024, 512) BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) -bigmodel_zoom = 1. -bigmodel_intrinsics = np.array( - [[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / bigmodel_zoom, 256+MEDMODEL_CY], - [ 0. , 0. , 1.]]) +bigmodel_fl = 910.0 +bigmodel_intrinsics = np.array([ + [bigmodel_fl, 0.0, 0.5 * BIGMODEL_INPUT_SIZE[0]], + [0.0, bigmodel_fl, 256 + MEDMODEL_CY], + [0.0, 0.0, 1.0]]) + + +# SBIG model (big model with the size of small model) +SBIGMODEL_INPUT_SIZE = (512, 256) +SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2) + +sbigmodel_fl = 455.0 +sbigmodel_intrinsics = np.array([ + [sbigmodel_fl, 0.0, 0.5 * SBIGMODEL_INPUT_SIZE[0]], + [0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)], + [0.0, 0.0, 1.0]]) model_frame_from_road_frame = np.dot(model_intrinsics, - get_view_frame_from_road_frame(0, 0, 0, model_height)) + get_view_frame_from_road_frame(0, 0, 0, model_height)) bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics, - get_view_frame_from_road_frame(0, 0, 0, model_height)) + get_view_frame_from_road_frame(0, 0, 0, model_height)) medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics, - get_view_frame_from_road_frame(0, 0, 0, model_height)) + get_view_frame_from_road_frame(0, 0, 0, model_height)) medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics, - get_view_frame_from_calib_frame(0, 0, 0, 0)) + get_view_frame_from_calib_frame(0, 0, 0, 0)) model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) + # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 0], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, 0], + [0, 0, 1], ])) camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, height - model_height], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, height - model_height], + [0, 0, 1], ])) road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high) @@ -104,13 +115,14 @@ def get_model_height_transform(camera_frame_from_road_frame, height): # camera_frame_from_model_frame aka 'warp matrix' # was: calibration.h/CalibrationTransform -def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height): +def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height, camera_fl=FOCAL): vp = vp_from_ke(camera_frame_from_road_frame) + model_zoom = camera_fl / model_fl model_camera_from_model_frame = np.array([ - [model_zoom, 0., vp[0] - MODEL_CX * model_zoom], - [ 0., model_zoom, vp[1] - MODEL_CY * model_zoom], - [ 0., 0., 1.], + [model_zoom, 0.0, vp[0] - MODEL_CX * model_zoom], + [0.0, model_zoom, vp[1] - MODEL_CY * model_zoom], + [0.0, 0.0, 1.0], ]) # This function is super slow, so skip it if height is very close to canonical diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 39dfb770c..52128a89b 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h index 793dc9615..a4554d92a 100644 --- a/panda/board/safety/safety_defaults.h +++ b/panda/board/safety/safety_defaults.h @@ -24,9 +24,31 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { } static int default_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { - UNUSED(bus_num); UNUSED(to_fwd); + #ifdef vw + // Volkswagen community port: Advanced Virtual Relay Technology! + // Make Panda fully transparent from bus 0->2 and bus 2->0 if not otherwise + // instructed by EON/OP, returning the car to stock behavior under NOOUTPUT. + // Don't do this for BP/C2, where we have Advanced Actual Relay Technology. + int bus_fwd = -1; + + if(!board_has_relay()) { + switch (bus_num) { + case 0: + bus_fwd = 2; + break; + case 2: + bus_fwd = 0; + break; + default: + bus_fwd = -1; + break; + } + } + #else + UNUSED(bus_num); return -1; + #endif } const safety_hooks nooutput_hooks = { diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 47120bcde..2bd9043a5 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -283,21 +283,14 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { } } -void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { const CameraBuf *b = &c->buf; uint32_t lum_binning[256] = {0}; for (int y = y_start; y < y_end; y += y_skip) { for (int x = x_start; x < x_end; x += x_skip) { - if (!front) { - uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; - lum_binning[lum]++; - } else { - // TODO: should get rid of RGB here - const uint8_t *pix = &pix_ptr[y * b->rgb_stride + x * 3]; - unsigned int lum = (unsigned int)(pix[0] + pix[1] + pix[2]); - lum_binning[std::min(lum / 3, 255u)]++; - } + uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; + lum_binning[lum]++; } } @@ -403,7 +396,7 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i y_end = 1148; skip = 4; #endif - set_exposure_target(c, (const uint8_t *)b->cur_rgb_buf->addr, 1, x_start, x_end, 2, y_start, y_end, skip); + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_start, x_end, 2, y_start, y_end, skip); } MessageBuilder msg; diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 0170db27e..d395241e9 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -129,7 +129,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt); void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr); -void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, bool front, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); +void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, const char *tname, CameraState *cs, int priority, process_thread_cb callback); void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index d90d900cf..6efcb2b5f 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -333,7 +333,8 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { s->rear.device = s->device; s->front.device = s->device; - s->sm = new SubMaster({"driverState", "sensorEvents"}); + s->sm_front = new SubMaster({"driverState"}); + s->sm_rear = new SubMaster({"sensorEvents"}); s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"}); int err; @@ -1399,7 +1400,7 @@ static void camera_open(CameraState *s, bool rear) { err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); LOG("ois init settings: %d", err); } else { - // leeco actuator + // leeco actuator (DW9800W H-Bridge Driver IC) // from sniff s->infinity_dac = 364; @@ -1407,6 +1408,7 @@ static void camera_open(CameraState *s, bool rear) { { .reg_write_type = MSM_ACTUATOR_WRITE_DAC, .hw_mask = 0, + // MSB here at address 3 .reg_addr = 3, .hw_shift = 0, .data_type = 9, @@ -1417,11 +1419,14 @@ static void camera_open(CameraState *s, bool rear) { }; struct reg_settings_t actuator_init_settings[] = { - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, - { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, - { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // PD = power down + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // 0 = power up + { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // RING = SAC mode + { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // 0x40 = SAC3 mode { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, + // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter) + // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms + // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms }; struct region_params_t region_params[] = { @@ -2042,7 +2047,7 @@ static void* ops_thread(void* arg) { } void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { - common_camera_process_front(s->sm, s->pm, c, cnt); + common_camera_process_front(s->sm_front, s->pm, c, cnt); } // called by processing_thread @@ -2051,11 +2056,11 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { // cache rgb roi and write to cl // gz compensation - s->sm->update(0); - if (s->sm->updated("sensorEvents")) { + s->sm_rear->update(0); + if (s->sm_rear->updated("sensorEvents")) { float vals[3] = {0.0}; bool got_accel = false; - auto sensor_events = (*(s->sm))["sensorEvents"].getSensorEvents(); + auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents(); for (auto sensor_event : sensor_events) { if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { auto v = sensor_event.getAcceleration().getV(); @@ -2154,7 +2159,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { const int exposure_height = 314; const int skip = 1; if (cnt % 3 == 0) { - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, 0, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); + set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); } } @@ -2287,6 +2292,7 @@ void cameras_close(MultiCameraState *s) { clReleaseProgram(s->prg_rgb_laplacian); clReleaseKernel(s->krnl_rgb_laplacian); - delete s->sm; + delete s->sm_front; + delete s->sm_rear; delete s->pm; } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 49afb0f66..63a428120 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -142,7 +142,8 @@ typedef struct MultiCameraState { CameraState rear; CameraState front; - SubMaster *sm; + SubMaster *sm_front; + SubMaster *sm_rear; PubMaster *pm; } MultiCameraState; diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 8d9ab6f16..24cf49ab5 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -189,16 +189,17 @@ class CarController(): if CS.CP.carFingerprint in HONDA_BOSCH: pass # TODO: implement else: - apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) - pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) - can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, - pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) - self.apply_brake_last = apply_brake + if not dragonconf.dpAtl: + apply_gas = clip(actuators.gas, 0., 1.) + apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) + pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) + can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, + pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) + self.apply_brake_last = apply_brake - if CS.CP.enableGasInterceptor: - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_command(self.packer, apply_gas, idx)) + if CS.CP.enableGasInterceptor: + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(create_gas_command(self.packer, apply_gas, idx)) return can_sends diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 8bf9c548b..be1a2c177 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -11,6 +11,7 @@ from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_ti from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING from selfdrive.car.interfaces import CarInterfaceBase from common.dp_common import common_interface_atl, common_interface_get_params_lqr +from common.params import Params A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -425,6 +426,23 @@ class CarInterface(CarInterfaceBase): raise ValueError("unsupported car %s" % candidate) # dp + if Params().get('dp_honda_eps_mod') == b'1': + if candidate == CAR.CIVIC: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2566, 8000], [0, 2566, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.32], [0.1]] #2.5x tuned by @CFranHonda + elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL): + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.32], [0.1]] #2.5 default mod #TMG put your values here + elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] + elif candidate == CAR.CRV_5G: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] #tuned by Titanminer (8000) + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] + elif candidate == CAR.CRV_HYBRID: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0x0, 0xB5, 0x161, 0x2D6, 0x4C0, 0x70D, 0xC42, 0x1058, 0x2C00], [0x0, 0x160, 0x1F0, 0x2E0, 0x378, 0x4A0, 0x5F0, 0x804, 0xF00]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] #still needs to finish tuning for the new car + ret.lateralTuning.pid.kf = 0.00004 + ret = common_interface_get_params_lqr(ret) # min speed to enable ACC. if car can do stop and go, then set enabling speed diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e807e3e6b..3b8e854e5 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -136,9 +136,7 @@ class CarState(CarStateBase): self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist'] - lkas_state = cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"] - if lkas_state != 7 and self.car_fingerprint not in [CAR.SONATA,CAR.PALISADE, CAR.SANTA_FE, CAR.KONA_EV, CAR.KONA]: - self.lkMode = bool(lkas_state) + # self.lkMode = bool(cp_cam.vl["LKAS11"]["CF_Lkas_LdwsSysState"]) return ret @staticmethod diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 120ac8b62..5f4636bb5 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -78,27 +78,34 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.HYUNDAI_GENESIS: - ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG ret.wheelbase = 3.01 - ret.steerRatio = 16.5 + ret.steerRatio = 15 # dp - indi value from donfyffe ret.lateralTuning.init('indi') ret.lateralTuning.indi.innerLoopGain = 3.1 ret.lateralTuning.indi.outerLoopGain = 2.1 ret.lateralTuning.indi.timeConstant = 1.4 - ret.lateralTuning.indi.actuatorEffectiveness = 1.3 + ret.lateralTuning.indi.actuatorEffectiveness = 1.4 + # ret.lateralTuning.pid.kf = 0.00005 # ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] # ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - ret.minSteerSpeed = 60 * CV.KPH_TO_MS + ret.minSteerSpeed = 32 * CV.MPH_TO_MS + ret.minEnableSpeed = 10 * CV.MPH_TO_MS elif candidate == CAR.GENESIS_G70: - ret.lateralTuning.pid.kf = 0.00005 - ret.mass = 1640. + STD_CARGO_KG - ret.wheelbase = 2.84 - ret.steerRatio = 16.5 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] + ret.lateralTuning.init('indi') # TODO: BPs for city speeds - this tuning is great on the highway but a bit lazy in town + ret.lateralTuning.indi.innerLoopGain = 2.4 # higher values steer more + ret.lateralTuning.indi.outerLoopGain = 3.0 # higher values steer more + ret.lateralTuning.indi.timeConstant = 1.0 # lower values steer more + ret.lateralTuning.indi.actuatorEffectiveness = 2.0 # lower values steer more + ret.steerActuatorDelay = 0.4 # 0.08 stock + ret.steerLimitTimer = 0.4 # down from 0.4 + tire_stiffness_factor = 1.0 + ret.steerRateCost = 1.0 + ret.mass = 1825. + STD_CARGO_KG + ret.wheelbase = 2.906 + ret.steerRatio = 14.4 elif candidate == CAR.GENESIS_G80: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 2060. + STD_CARGO_KG diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 682f0b700..09a71930a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -8,12 +8,10 @@ from common.params import Params # Steer torque limits class SteerLimitParams: def __init__(self, CP): - if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ]: - self.STEER_MAX = 384 - elif Params().get('dp_hkg_smart_mdps') == b'1': + if Params().get('dp_hkg_smart_mdps') == b'1' or CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70]: self.STEER_MAX = 384 else: - self.STEER_MAX = 255 + self.STEER_MAX = 409 self.STEER_DELTA_UP = 3 self.STEER_DELTA_DOWN = 7 self.STEER_DRIVER_ALLOWANCE = 50 @@ -103,7 +101,7 @@ FINGERPRINTS = { 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 }], CAR.GENESIS_G70: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832:8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173:8, 1184: 8, 1186: 2, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1419:8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832:8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173:8, 1184: 8, 1186: 2, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1419:8, 1425: 2, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], CAR.GENESIS_G80: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8 @@ -154,7 +152,7 @@ ECU_FINGERPRINT = { } # Don't use these fingerprints for fingerprinting, they are still used for ECU detection -IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA] +IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.KONA] FW_VERSIONS = { CAR.SONATA: { @@ -242,11 +240,19 @@ FW_VERSIONS = { }, CAR.GENESIS_G70: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', ], - (Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81640L0051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', + b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9220 4I2VL108', + ], (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ], - (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99 \xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', + b'\xf1\x87VCJLP18488832DN3\x88uXf\x88vgf\x97eVe\x98fhw\x88eWevUEUgu\xff\xfbW\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', + ], }, CAR.KONA: { (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 \xf1\xa01.00', ], diff --git a/selfdrive/common/mat.h b/selfdrive/common/mat.h index 1c20eae17..626f3404f 100644 --- a/selfdrive/common/mat.h +++ b/selfdrive/common/mat.h @@ -1,5 +1,4 @@ -#ifndef COMMON_MAT_H -#define COMMON_MAT_H +#pragma once typedef struct vec3 { float v[3]; @@ -17,7 +16,7 @@ typedef struct mat4 { float v[4*4]; } mat4; -static inline mat3 matmul3(const mat3 a, const mat3 b) { +static inline mat3 matmul3(const mat3 &a, const mat3 &b) { mat3 ret = {{0.0}}; for (int r=0; r<3; r++) { for (int c=0; c<3; c++) { @@ -31,7 +30,7 @@ static inline mat3 matmul3(const mat3 a, const mat3 b) { return ret; } -static inline vec3 matvecmul3(const mat3 a, const vec3 b) { +static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) { vec3 ret = {{0.0}}; for (int r=0; r<3; r++) { for (int c=0; c<3; c++) { @@ -41,7 +40,7 @@ static inline vec3 matvecmul3(const mat3 a, const vec3 b) { return ret; } -static inline mat4 matmul(const mat4 a, const mat4 b) { +static inline mat4 matmul(const mat4 &a, const mat4 &b) { mat4 ret = {{0.0}}; for (int r=0; r<4; r++) { for (int c=0; c<4; c++) { @@ -55,7 +54,7 @@ static inline mat4 matmul(const mat4 a, const mat4 b) { return ret; } -static inline vec4 matvecmul(const mat4 a, const vec4 b) { +static inline vec4 matvecmul(const mat4 &a, const vec4 &b) { vec4 ret = {{0.0}}; for (int r=0; r<4; r++) { for (int c=0; c<4; c++) { @@ -67,7 +66,7 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) { // scales the input and output space of a transformation matrix // that assumes pixel-center origin. -static inline mat3 transform_scale_buffer(const mat3 in, float s) { +static inline mat3 transform_scale_buffer(const mat3 &in, float s) { // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s mat3 transform_out = {{ @@ -84,5 +83,3 @@ static inline mat3 transform_scale_buffer(const mat3 in, float s) { return matmul3(transform_in, matmul3(in, transform_out)); } - -#endif diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 77f4a1b7a..abd47a783 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,6 +1,9 @@ #pragma once #define MODEL_PATH_DISTANCE 192 +#define TRAJECTORY_SIZE 33 +#define MIN_DRAW_DISTANCE 10.0 +#define MAX_DRAW_DISTANCE 100.0 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 #define DESIRE_PRED_SIZE 32 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 351909748..2d76a2cd0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -264,7 +264,6 @@ class Controls: if self.dp_lead_count >= 300 and abs(self.sm['plan'].vTargetFuture) >= 0.1: self.events.add(EventName.leadCarMoving) - self.dp_lead_count = 0 if CS.vEgo > 0. or CS.gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]: self.dp_lead_count = 0 diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index f25043068..5a18ab3bc 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -7,9 +7,9 @@ from common.dp_time import LAST_MODIFIED_LANE_PLANNER CAMERA_OFFSET = 0.06 # m from center car to camera -def compute_path_pinv(l=50): +def compute_path_pinv(length=50): deg = 3 - x = np.arange(l*1.0) + x = np.arange(length*1.0) X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T pinv = np.linalg.pinv(X) return pinv @@ -23,31 +23,7 @@ def eval_poly(poly, x): return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 -def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): - # This will improve behaviour when lanes suddenly widen - # these numbers were tested on 2000segments and found to work well - lane_width = min(4.0, lane_width) - width_poly = l_poly - r_poly - prob_mods = [] - for t_check in [0.0, 1.5, 3.0]: - width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) - prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) - mod = min(prob_mods) - l_prob = mod * l_prob - r_prob = mod * r_prob - - path_from_left_lane = l_poly.copy() - path_from_left_lane[3] -= lane_width / 2.0 - path_from_right_lane = r_poly.copy() - path_from_right_lane[3] += lane_width / 2.0 - - lr_prob = l_prob + r_prob - l_prob * r_prob - - d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly - - -class LanePlanner(): +class LanePlanner: def __init__(self): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] @@ -61,6 +37,9 @@ class LanePlanner(): self.l_prob = 0. self.r_prob = 0. + self.l_std = 0. + self.r_std = 0. + self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. @@ -77,7 +56,9 @@ class LanePlanner(): def parse_model(self, md): if len(md.leftLane.poly): self.l_poly = np.array(md.leftLane.poly) + self.l_std = float(md.leftLane.std) self.r_poly = np.array(md.rightLane.poly) + self.r_std = float(md.rightLane.std) self.p_poly = np.array(md.path.poly) else: self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line @@ -87,8 +68,8 @@ class LanePlanner(): self.r_prob = md.rightLane.prob # right line prob if len(md.meta.desireState): - self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] - self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] + self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense @@ -101,16 +82,42 @@ class LanePlanner(): self.r_poly[3] += offset self.p_poly[3] += offset + # Reduce reliance on lanelines that are too far apart or + # will be in a few seconds + l_prob, r_prob = self.l_prob, self.r_prob + width_poly = self.l_poly - self.r_poly + prob_mods = [] + for t_check in [0.0, 1.5, 3.0]: + width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + # dp note + # [3.5, 4.5] = from 3m to 4m (lane width) + # [1.0, 0.0] = "use lane line" to "not use lane line at all" + prob_mods.append(interp(width_at_t, [3.5, 4.5], [1.0, 0.0])) + mod = min(prob_mods) + l_prob *= mod + r_prob *= mod + + # Reduce reliance on uncertain lanelines + l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0]) + r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0]) + l_prob *= l_std_mod + r_prob *= r_std_mod + # Find current lanewidth - self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty) + self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty) current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) speed_lane_width = interp(v_ego, [0., 14., 20.], [2.5, 3., 3.5]) # German Standards self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width - self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego) + clipped_lane_width = min(4.0, self.lane_width) + path_from_left_lane = self.l_poly.copy() + path_from_left_lane[3] -= clipped_lane_width / 2.0 + path_from_right_lane = self.r_poly.copy() + path_from_right_lane[3] += clipped_lane_width / 2.0 - def update(self, v_ego, md): - self.parse_model(md) - self.update_d_poly(v_ego) + lr_prob = l_prob + r_prob - l_prob * r_prob + + d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) + self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy() diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index dee366922..17ac87271 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -43,6 +43,7 @@ DESIRES = { def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay + states[0].y = states[0].x * math.sin(states[0].psi / 2) return states diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 18f279bf7..4409033c5 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -6,10 +6,10 @@ libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'z TEST_THNEED = False common_src = [ - "models/commonmodel.c", + "models/commonmodel.cc", "runners/snpemodel.cc", "transforms/loadyuv.c", - "transforms/transform.c" + "transforms/transform.cc" ] if arch == "aarch64": @@ -23,12 +23,12 @@ elif arch == "larch64": else: libs += ['pthread'] - # for tensorflow support - common_src += ['runners/tfmodel.cc'] + # for onnx support + common_src += ['runners/onnxmodel.cc'] - # tell runners to use tensorflow - lenv['CFLAGS'].append("-DUSE_TF_MODEL") - lenv['CXXFLAGS'].append("-DUSE_TF_MODEL") + # tell runners to use onnx + lenv['CFLAGS'].append("-DUSE_ONNX_MODEL") + lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL") if arch == "Darwin": # fix OpenCL diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index c8c637b3f..7250fd38d 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -38,16 +38,15 @@ void* live_thread(void *arg) { -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; + Eigen::Matrix fcam_intrinsics; #ifndef QCOM2 - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 910.0, 0.0, 582.0, 0.0, 910.0, 437.0, 0.0, 0.0, 1.0; float db_s = 0.5; // debayering does a 2x downscale #else - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 2648.0, 0.0, 1928.0/2, 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0; @@ -69,7 +68,7 @@ void* live_thread(void *arg) { extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; } - auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + auto camera_frame_from_road_frame = fcam_intrinsics * extrinsic_matrix_eigen; Eigen::Matrix camera_frame_from_ground; camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); @@ -112,7 +111,7 @@ int main(int argc, char **argv) { assert(err == 0); // messaging - PubMaster pm({"model", "cameraOdometry"}); + PubMaster pm({"modelV2", "model", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); #if defined(QCOM) || defined(QCOM2) @@ -174,7 +173,7 @@ int main(int argc, char **argv) { if (sm.update(0) > 0){ // TODO: path planner timeout? - desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1; + desire = ((int)sm["pathPlan"].getPathPlan().getDesire()); frame_id = sm["frame"].getFrame().getFrameId(); } @@ -201,6 +200,7 @@ int main(int argc, char **argv) { float frame_drop_perc = frames_dropped / MODEL_FREQ; model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); + model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof); LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc); diff --git a/selfdrive/modeld/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.cc similarity index 83% rename from selfdrive/modeld/models/commonmodel.c rename to selfdrive/modeld/models/commonmodel.cc index c156ad39d..62b2a1271 100644 --- a/selfdrive/modeld/models/commonmodel.c +++ b/selfdrive/modeld/models/commonmodel.cc @@ -59,6 +59,29 @@ void frame_free(ModelFrame* frame) { clReleaseMemObject(frame->transformed_y_cl); } +void softmax(const float* input, float* output, size_t len) { + float max_val = -FLT_MAX; + for(int i = 0; i < len; i++) { + const float v = input[i]; + if( v > max_val ) { + max_val = v; + } + } + + float denominator = 0; + for(int i = 0; i < len; i++) { + float const v = input[i]; + float const v_exp = expf(v - max_val); + denominator += v_exp; + output[i] = v_exp; + } + + const float inv_denominator = 1. / denominator; + for(int i = 0; i < len; i++) { + output[i] *= inv_denominator; + } + +} float sigmoid(float input) { return 1 / (1 + expf(-input)); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 7aed3d799..06d46b8f2 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -8,6 +8,7 @@ #include #endif +#include #include "common/mat.h" #include "transforms/transform.h" #include "transforms/loadyuv.h" @@ -16,6 +17,7 @@ extern "C" { #endif +void softmax(const float* input, float* output, size_t len); float softplus(float input); float sigmoid(float input); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9303245d7..301e7fafb 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -9,14 +9,17 @@ #include "common/params.h" #include "driving.h" -#define PATH_IDX 0 -#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1 -#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION -#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 -#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 -#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define MIN_VALID_LEN 10.0 +#define TRAJECTORY_SIZE 33 +#define TRAJECTORY_TIME 10.0 +#define TRAJECTORY_DISTANCE 192.0 +#define PLAN_IDX 0 +#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE) +#define LL_PROB_IDX LL_IDX + 4*2*2*33 +#define RE_IDX LL_PROB_IDX + 4 +#define LEAD_IDX RE_IDX + 2*2*2*33 +#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE) +#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3 #define META_IDX DESIRE_STATE_IDX + DESIRE_LEN #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE @@ -29,6 +32,8 @@ // #define DUMP_YUV Eigen::Matrix vander; +float X_IDXS[TRAJECTORY_SIZE]; +float T_IDXS[TRAJECTORY_SIZE]; void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); @@ -63,9 +68,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #endif // Build Vandermonde matrix - for(int i = 0; i < MODEL_PATH_DISTANCE; i++) { + for(int i = 0; i < TRAJECTORY_SIZE; i++) { for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { - vander(i, j) = pow(i, POLYFIT_DEGREE-j-1); + X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2)); + T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2)); + vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); } } } @@ -76,7 +83,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_LEN; i++) { + for (int i = 1; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge if (desire_in[i] - s->prev_desire[i] > .99) { @@ -107,13 +114,12 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, // net outputs ModelDataRaw net_outputs; - net_outputs.path = &s->output[PATH_IDX]; - net_outputs.left_lane = &s->output[LL_IDX]; - net_outputs.right_lane = &s->output[RL_IDX]; + net_outputs.plan = &s->output[PLAN_IDX]; + net_outputs.lane_lines = &s->output[LL_IDX]; + net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX]; + net_outputs.road_edges = &s->output[RE_IDX]; net_outputs.lead = &s->output[LEAD_IDX]; - net_outputs.long_x = &s->output[LONG_X_IDX]; - net_outputs.long_v = &s->output[LONG_V_IDX]; - net_outputs.long_a = &s->output[LONG_A_IDX]; + net_outputs.lead_prob = &s->output[LEAD_PROB_IDX]; net_outputs.meta = &s->output[DESIRE_STATE_IDX]; net_outputs.pose = &s->output[POSE_IDX]; return net_outputs; @@ -151,35 +157,40 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { out[3] = y0; } -void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) { - float points_arr[MODEL_PATH_DISTANCE]; - float stds_arr[MODEL_PATH_DISTANCE]; +void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) { + float points_arr[TRAJECTORY_SIZE]; + float stds_arr[TRAJECTORY_SIZE]; float poly_arr[POLYFIT_DEGREE]; float std; - float prob; - float valid_len; - // clamp to 5 and MODEL_PATH_DISTANCE - valid_len = fmin(MODEL_PATH_DISTANCE, fmax(5, data[MODEL_PATH_DISTANCE*2])); - for (int i=0; i stds(&stds_arr[0], ARRAYSIZE(stds_arr)); - path.setStds(stds); + kj::ArrayPtr poly(&poly_arr[0], ARRAYSIZE(poly_arr)); + path.setPoly(poly); + path.setProb(1.0); + path.setStd(std); + path.setValidLen(valid_len); +} - kj::ArrayPtr points(&points_arr[0], ARRAYSIZE(points_arr)); - path.setPoints(points); +void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) { + float points_arr[TRAJECTORY_SIZE]; + float stds_arr[TRAJECTORY_SIZE]; + float poly_arr[POLYFIT_DEGREE]; + float std; + + for (int i=0; i poly(&poly_arr[0], ARRAYSIZE(poly_arr)); path.setPoly(poly); @@ -188,48 +199,189 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo path.setValidLen(valid_len); } -void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) { - const double x_scale = 10.0; - const double y_scale = 10.0; +void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) { + lead.setProb(prob); + lead.setT(t); + float xyva_arr[LEAD_MHP_VALS]; + float xyva_stds_arr[LEAD_MHP_VALS]; + for (int i=0; i xyva(xyva_arr, LEAD_MHP_VALS); + kj::ArrayPtr xyva_stds(xyva_stds_arr, LEAD_MHP_VALS); + lead.setXyva(xyva); + lead.setXyvaStd(xyva_stds); +} - lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset])); - lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]); - lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS])); - lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]); - lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1])); - lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]); - lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2])); - lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]); - lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3])); +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) { + lead.setProb(prob); + lead.setDist(data[0]); + lead.setStd(exp(data[LEAD_MHP_VALS])); + // TODO make all msgs same format + lead.setRelY(-data[1]); + lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1])); + lead.setRelVel(data[2]); + lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2])); + lead.setRelA(data[3]); + lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3])); } void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { - kj::ArrayPtr desire_state(&meta_data[0], DESIRE_LEN); + float desire_state_softmax[DESIRE_LEN]; + float desire_pred_softmax[4*DESIRE_LEN]; + softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); + for (int i=0; i<4; i++) { + softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], + &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); + } + kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); meta.setDesireState(desire_state); - meta.setEngagedProb(meta_data[DESIRE_LEN]); - meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]); - meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]); - meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]); - kj::ArrayPtr desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE); + meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); + meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); + meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); + meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); + kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); meta.setDesirePrediction(desire_pred); } -void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) { - // just doing 10 vals, 1 every sec for now - float dist_arr[TIME_DISTANCE/10]; - float speed_arr[TIME_DISTANCE/10]; - float accel_arr[TIME_DISTANCE/10]; - for (int i=0; i dist(&dist_arr[0], ARRAYSIZE(dist_arr)); - longi.setDistances(dist); - kj::ArrayPtr speed(&speed_arr[0], ARRAYSIZE(speed_arr)); - longi.setSpeeds(speed); - kj::ArrayPtr accel(&accel_arr[0], ARRAYSIZE(accel_arr)); - longi.setAccelerations(accel); + kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); + meta.setDesireState(desire_state); + meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); + meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); + meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); + meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); + kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); + meta.setDesirePrediction(desire_pred); +} + +void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, + int columns, int column_offset, float * plan_t_arr) { + float x_arr[TRAJECTORY_SIZE]; + float y_arr[TRAJECTORY_SIZE]; + float z_arr[TRAJECTORY_SIZE]; + //float x_std_arr[TRAJECTORY_SIZE]; + //float y_std_arr[TRAJECTORY_SIZE]; + //float z_std_arr[TRAJECTORY_SIZE]; + float t_arr[TRAJECTORY_SIZE]; + for (int i=0; i= 0) { + t_arr[i] = T_IDXS[i]; + x_arr[i] = data[i*columns + 0 + column_offset]; + //x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset]; + } else { + t_arr[i] = plan_t_arr[i]; + x_arr[i] = X_IDXS[i]; + //x_std_arr[i] = NAN; + } + y_arr[i] = data[i*columns + 1 + column_offset]; + //y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset]; + z_arr[i] = data[i*columns + 2 + column_offset]; + //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; + } + kj::ArrayPtr x(x_arr, TRAJECTORY_SIZE); + kj::ArrayPtr y(y_arr, TRAJECTORY_SIZE); + kj::ArrayPtr z(z_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); + //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); + kj::ArrayPtr t(t_arr, TRAJECTORY_SIZE); + xyzt.setX(x); + xyzt.setY(y); + xyzt.setZ(z); + //xyzt.setXStd(x_std); + //xyzt.setYStd(y_std); + //xyzt.setZStd(z_std); + xyzt.setT(t); +} + + +void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, + uint32_t vipc_dropped_frames, float frame_drop, + const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { + // make msg + MessageBuilder msg; + auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2(); + uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; + framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + + // plan + int plan_mhp_max_idx = 0; + for (int i=1; i + net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { + plan_mhp_max_idx = i; + } + } + + float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)]; + float plan_t_arr[TRAJECTORY_SIZE]; + for (int i=0; i lane_line_probs(lane_line_probs_arr, 4); + framed.setLaneLineProbs(lane_line_probs); + framed.setLaneLineStds(lane_line_stds_arr); + + // road edges + auto road_edges = framed.initRoadEdges(2); + float road_edge_stds_arr[2]; + for (int i = 0; i < 2; i++) { + fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr); + road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]); + } + framed.setRoadEdgeStds(road_edge_stds_arr); + + // meta + auto meta = framed.initMeta(); + fill_meta_v2(meta, net_outputs.meta); + + // leads + auto leads = framed.initLeads(LEAD_MHP_SELECTION); + int mdn_max_idx = 0; + float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0}; + for (int t_offset=0; t_offset + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) { + mdn_max_idx = i; + fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], + sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]); + } + } + } + pm.send("modelV2", msg); } void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, @@ -243,29 +395,66 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); - fill_path(framed.initPath(), net_outputs.path, false, 0); - fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8); - fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8); - fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); + // Find the distribution that corresponds to the most probable plan + int plan_mhp_max_idx = 0; + for (int i=1; i + net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) { + plan_mhp_max_idx = i; + } + } + + // x pos at 10s is a good valid_len + float valid_len = 0; + float valid_len_candidate; + for (int i=1; i= valid_len){ + valid_len = valid_len_candidate; + } + } + // clamp to 10 and MODEL_PATH_DISTANCE + valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); + int valid_len_idx = 0; + for (int i=1; i= X_IDXS[valid_len_idx]){ + valid_len_idx = i; + } + } + + auto lpath = framed.initPath(); + fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx); + + auto left_lane = framed.initLeftLane(); + int ll_idx = 1; + fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, + sigmoid(net_outputs.lane_lines_prob[ll_idx])); + auto right_lane = framed.initRightLane(); + ll_idx = 2; + fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx, + sigmoid(net_outputs.lane_lines_prob[ll_idx])); // 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]) { + for (int i=1; i + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { mdn_max_idx = i; } } - fill_lead(framed.initLead(), net_outputs.lead, mdn_max_idx, t_offset); + fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[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]) { + for (int i=1; i + net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) { mdn_max_idx = i; } } - fill_lead(framed.initLeadFuture(), net_outputs.lead, mdn_max_idx, t_offset); + fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset])); + fill_meta(framed.initMeta(), net_outputs.meta); pm.send("model", msg); @@ -280,10 +469,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, for (int i =0; i < 3; i++) { trans_arr[i] = net_outputs.pose[i]; - trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6; + trans_std_arr[i] = exp(net_outputs.pose[6 + i]); - rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0; - rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0; + rot_arr[i] = net_outputs.pose[3 + i]; + rot_std_arr[i] = exp(net_outputs.pose[9 + i]); } MessageBuilder msg; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 170982db1..613ba139c 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -17,33 +17,40 @@ #include #include "messaging.hpp" +#define MODEL_NAME "supercombo_dlc" + #define MODEL_WIDTH 512 #define MODEL_HEIGHT 256 #define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2 -#define MODEL_NAME "supercombo_dlc" - #define DESIRE_LEN 8 #define TRAFFIC_CONVENTION_LEN 2 -#define LEAD_MDN_N 5 // probs for 5 groups -#define MDN_VALS 4 // output xyva for each lead group -#define SELECTION 3 //output 3 group (lead now, in 2s and 6s) -#define MDN_GROUP_SIZE 11 -#define TIME_DISTANCE 100 + +#define PLAN_MHP_N 5 +#define PLAN_MHP_COLUMNS 30 +#define PLAN_MHP_VALS 30*33 +#define PLAN_MHP_SELECTION 1 +#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION) + +#define LEAD_MHP_N 5 +#define LEAD_MHP_VALS 4 +#define LEAD_MHP_SELECTION 3 +#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION) + #define POSE_SIZE 12 #define MODEL_FREQ 20 #define MAX_FRAME_DROP 0.05 struct ModelDataRaw { - float *path; - float *left_lane; - float *right_lane; + float *plan; + float *lane_lines; + float *lane_lines_prob; + float *road_edges; float *lead; - float *long_x; - float *long_v; - float *long_a; + float *lead_prob; float *desire_state; float *meta; + float *desire_pred; float *pose; }; @@ -72,6 +79,8 @@ 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, uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); +void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof); #endif diff --git a/selfdrive/modeld/transforms/transform.c b/selfdrive/modeld/transforms/transform.cc similarity index 96% rename from selfdrive/modeld/transforms/transform.c rename to selfdrive/modeld/transforms/transform.cc index 0b4150ddb..53e7fc488 100644 --- a/selfdrive/modeld/transforms/transform.c +++ b/selfdrive/modeld/transforms/transform.cc @@ -99,14 +99,14 @@ void transform_queue(Transform* s, err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl); assert(err == 0); - const size_t work_size_y[2] = {out_y_width, out_y_height}; + const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height}; err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, (const size_t*)&work_size_y, NULL, 0, 0, NULL); assert(err == 0); - const size_t work_size_uv[2] = {out_uv_width, out_uv_height}; + const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height}; err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width); assert(err == 0); diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 61437ff58..32e85c231 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -4,6 +4,7 @@ #include #include #include "common/util.h" +#include #define NANOVG_GLES3_IMPLEMENTATION #include "nanovg_gl.h" @@ -38,25 +39,21 @@ const mat3 intrinsic_matrix = (mat3){{ // Projects a point in car to space to the corresponding point in full frame // image space. -vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) { - const UIScene *scene = &s->scene; - +bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y) { + const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}}; // We'll call the car space point p. // First project into normalized image coordinates with the extrinsics matrix. - const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective); + const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective); // The last entry is zero because of how we store E (to use matvecmul). const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); // Project. - const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}}; - return p_image; -} + *out_x = KEp.v[0] / KEp.v[2]; + *out_y = KEp.v[1] / KEp.v[2]; -// Calculate an interpolation between two numbers at a specific increment -static float lerp(float v0, float v1, float t) { - return (1 - t) * v0 + t * v1; + return *out_x >= 0 && *out_x <= s->fb_w && *out_y >= 0 && *out_y <= s->fb_h; } static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){ @@ -68,12 +65,8 @@ static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, f static void draw_chevron(UIState *s, float x_in, float y_in, float sz, NVGcolor fillColor, NVGcolor glowColor) { - const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - - float x = p_full_frame.v[0]; - float y = p_full_frame.v[1]; - if (x < 0 || y < 0.){ + float x, y; + if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &x, &y)) { return; } @@ -135,110 +128,52 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); } -static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { - if (pvd->cnt == 0) return; +static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) { + if (cnt == 0) return; nvgBeginPath(s->vg); - nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y); - for (int i=1; icnt; i++) { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); + nvgMoveTo(s->vg, v[0].x, v[0].y); + for (int i = 1; i < cnt; i++) { + nvgLineTo(s->vg, v[i].x, v[i].y); } nvgClosePath(s->vg); - nvgFillColor(s->vg, color); + if (color) { + nvgFillColor(s->vg, *color); + } else if (paint) { + nvgFillPaint(s->vg, *paint); + } nvgFill(s->vg); } -static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) { +static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) { const UIScene *scene = &s->scene; - const float *points = scene->path_points; - const float *mpc_x_coords = &scene->mpc_x[0]; - const float *mpc_y_coords = &scene->mpc_y[0]; - - float off = is_mpc?0.3:0.5; - float lead_d = scene->lead_data[0].getDRel()*2.; - float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20. - :(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.; - path_height = fmin(path_height, scene->model.getPath().getValidLen()); - pvd->cnt = 0; - // left side up - for (int i=0; i<=path_height; i++) { - float px, py, mpx; - if (is_mpc) { - mpx = i==0?0.0:mpc_x_coords[i]; - px = lerp(mpx+1.0, mpx, i/100.0); - py = mpc_y_coords[i] - off; - } else { - px = lerp(i+1.0, i, i/100.0); - py = points[i] - off; - } - - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { - continue; - } - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; - } - - // right side down - for (int i=path_height; i>=0; i--) { - float px, py, mpx; - if (is_mpc) { - mpx = i==0?0.0:mpc_x_coords[i]; - px = lerp(mpx+1.0, mpx, i/100.0); - py = mpc_y_coords[i] + off; - } else { - px = lerp(i+1.0, i, i/100.0); - py = points[i] + off; - } - - vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) { - continue; - } - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; - } -} - -static void update_all_track_data(UIState *s) { - const UIScene *scene = &s->scene; - // Draw vision path - update_track_data(s, false, &s->track_vertices[0]); - - if (scene->controls_state.getEnabled()) { - // Draw MPC path when engaged - update_track_data(s, true, &s->track_vertices[1]); - } -} - -static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { - if (pvd->cnt == 0) return; - - nvgBeginPath(s->vg); - nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y); - for (int i=1; icnt; i++) { - nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y); - } - nvgClosePath(s->vg); - - NVGpaint track_bg; - if (is_mpc) { - // Draw colored MPC track - const NVGcolor clr = bg_colors[s->status]; - track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2)); + const float off = 0.5; + int max_idx; + float lead_d; + if(s->sm->updated("radarState")) { + lead_d = scene->lead_data[0].getDRel()*2.; } else { - // Draw white vision track - track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4, - COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + lead_d = MAX_DRAW_DISTANCE; } - nvgFillPaint(s->vg, track_bg); - nvgFill(s->vg); + float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE; + path_length = fmin(path_length, scene->max_distance); + + + vertex_data *v = &pvd->v[0]; + for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y); + max_idx = i; + } + for (int i = max_idx; i >= 0; i--) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y); + } + pvd->cnt = v - pvd->v; +} + +static void ui_draw_track(UIState *s, track_vertices_data *pvd) { + NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg); } static void draw_frame(UIState *s) { @@ -272,74 +207,51 @@ static void draw_frame(UIState *s) { glBindVertexArray(0); } -static inline bool valid_frame_pt(UIState *s, float x, float y) { - return x >= 0 && x <= s->stream.bufs_info.width && y >= 0 && y <= s->stream.bufs_info.height; -} - -static void update_lane_line_data(UIState *s, const float *points, float off, model_path_vertices_data *pvd, float valid_len) { - pvd->cnt = 0; - int rcount = fmin(MODEL_PATH_MAX_VERTICES_CNT / 2, valid_len); - for (int i = 0; i < rcount; i++) { - float px = (float)i; - float py = points[i] - off; - const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) - continue; - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; +static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) { + // TODO check that this doesn't overflow max vertex buffer + int max_idx; + vertex_data *v = &pvd->v[0]; + for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y); + max_idx = i; } - for (int i = rcount - 1; i > 0; i--) { - float px = (float)i; - float py = points[i] + off; - const vec4 p_car_space = (vec4){{px, py, 0., 1.}}; - const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); - if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1])) - continue; - pvd->v[pvd->cnt].x = p_full_frame.v[0]; - pvd->v[pvd->cnt].y = p_full_frame.v[1]; - pvd->cnt += 1; + for (int i = max_idx - 1; i > 0; i--) { + v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y); } + pvd->cnt = v - pvd->v; } -static void update_all_lane_lines_data(UIState *s, const cereal::ModelData::PathData::Reader &path, const float *points, model_path_vertices_data *pstart) { - update_lane_line_data(s, points, 0.025*path.getProb(), pstart, path.getValidLen()); - update_lane_line_data(s, points, fmin(path.getStd(), 0.7), pstart + 1, path.getValidLen()); -} -static void ui_draw_lane(UIState *s, model_path_vertices_data *pstart, NVGcolor color) { - ui_draw_lane_line(s, pstart, color); - color.a /= 25; - ui_draw_lane_line(s, pstart + 1, color); -} - -static void ui_draw_vision_lanes(UIState *s) { +static void ui_draw_vision_lane_lines(UIState *s) { const UIScene *scene = &s->scene; - model_path_vertices_data *pvd = &s->model_path_vertices[0]; - if(s->sm->updated("model")) { - update_all_lane_lines_data(s, scene->model.getLeftLane(), scene->left_lane_points, pvd); - update_all_lane_lines_data(s, scene->model.getRightLane(), scene->right_lane_points, pvd + MODEL_LANE_PATH_CNT); + // paint lanelines + if (scene->dpUiLane) { + line_vertices_data *pvd_ll = &s->lane_line_vertices[0]; + for (int ll_idx = 0; ll_idx < 4; ll_idx++) { + if(s->sm->updated("modelV2")) { + update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance); + } + NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]); + ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr); } - if (s->scene.dpUiLane) { - // Draw left lane edge - ui_draw_lane(s, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getLeftLane().getProb())); - - // Draw right lane edge - ui_draw_lane(s, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getRightLane().getProb())); + // paint road edges + line_vertices_data *pvd_re = &s->road_edge_vertices[0]; + for (int re_idx = 0; re_idx < 2; re_idx++) { + if(s->sm->updated("modelV2")) { + update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance); + } + NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0)); + ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr); } - if(s->sm->updated("radarState")) { - update_all_track_data(s); } - // Draw vision path - if (s->scene.dpUiPath) { - ui_draw_track(s, false, &s->track_vertices[0]); - if (scene->controls_state.getEnabled()) { - // Draw MPC path when engaged - ui_draw_track(s, true, &s->track_vertices[1]); + // paint path + if (scene->dpUiPath) { + if(s->sm->updated("modelV2")) { + update_track_data(s, scene->model.getPosition(), &s->track_vertices); } + ui_draw_track(s, &s->track_vertices); } } @@ -361,7 +273,7 @@ static void ui_draw_world(UIState *s) { nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); // Draw lane edges and vision/mpc tracks - ui_draw_vision_lanes(s); + ui_draw_vision_lane_lines(s); // Draw lead indicators if openpilot is handling longitudinal if (s->scene.dpUiLead) { @@ -440,7 +352,7 @@ static void ui_draw_vision_speed(UIState *s) { nvgLineTo(s->vg, viz_speed_x - viz_speed_w/2, viz_rect.y + header_h/4 + header_h/4); nvgLineTo(s->vg, viz_speed_x, viz_rect.y + header_h/2 + header_h/4); nvgClosePath(s->vg); - nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60)); + nvgFillColor(s->vg, nvgRGBA(0,255,0,s->scene.blinker_blinkingrate>=60?190:30)); nvgFill(s->vg); } if(s->scene.rightBlinker) { @@ -449,7 +361,7 @@ static void ui_draw_vision_speed(UIState *s) { nvgLineTo(s->vg, viz_speed_x+viz_speed_w + viz_speed_w/2, viz_rect.y + header_h/4 + header_h/4); nvgLineTo(s->vg, viz_speed_x+viz_speed_w, viz_rect.y + header_h/2 + header_h/4); nvgClosePath(s->vg); - nvgFillColor(s->vg, nvgRGBA(23,134,68,s->scene.blinker_blinkingrate>=50?210:60)); + nvgFillColor(s->vg, nvgRGBA(0,255,0,s->scene.blinker_blinkingrate>=60?190:30)); nvgFill(s->vg); } if(s->scene.leftBlinker || s->scene.rightBlinker) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4044e0abb..3a87c188a 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -24,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"model", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", "dragonConf", "carState"}); @@ -107,13 +108,6 @@ destroy: s->vision_connected = false; } -static inline void fill_path_points(const cereal::ModelData::PathData::Reader &path, float *points) { - const capnp::List::Reader &poly = path.getPoly(); - for (int i = 0; i < path.getValidLen(); i++) { - points[i] = poly[0] * (i * i * i) + poly[1] * (i * i) + poly[2] * i + poly[3]; - } -} - void update_sockets(UIState *s) { UIScene &scene = s->scene; @@ -183,11 +177,24 @@ void update_sockets(UIState *s) { scene.extrinsic_matrix.v[i] = extrinsicl[i]; } } - if (sm.updated("model")) { - scene.model = sm["model"].getModel(); - fill_path_points(scene.model.getPath(), scene.path_points); - fill_path_points(scene.model.getLeftLane(), scene.left_lane_points); - fill_path_points(scene.model.getRightLane(), scene.right_lane_points); + if (sm.updated("modelV2")) { + scene.model = sm["modelV2"].getModelV2(); + scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + for (int ll_idx = 0; ll_idx < 4; ll_idx++) { + if (scene.model.getLaneLineProbs().size() > ll_idx) { + scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx]; + } else { + scene.lane_line_probs[ll_idx] = 0.0; + } + } + + for (int re_idx = 0; re_idx < 2; re_idx++) { + if (scene.model.getRoadEdgeStds().size() > re_idx) { + scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx]; + } else { + scene.road_edge_stds[re_idx] = 1.0; + } + } } if (sm.updated("uiLayoutState")) { auto data = sm["uiLayoutState"].getUiLayoutState(); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index d68bdea60..e790b05b9 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -56,9 +56,8 @@ const Rect home_btn = {60, 1080 - 180 - 40, 180, 180}; const int UI_FREQ = 20; // Hz -const int MODEL_PATH_MAX_VERTICES_CNT = 98; -const int MODEL_LANE_PATH_CNT = 2; -const int TRACK_POINTS_MAX_CNT = 50 * 2; +const int MODEL_PATH_MAX_VERTICES_CNT = TRAJECTORY_SIZE*2; +const int TRACK_POINTS_MAX_CNT = TRAJECTORY_SIZE*4; const int SET_SPEED_NA = 255; @@ -96,10 +95,14 @@ static std::map bg_colors = { {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, }; -typedef struct UIScene { +typedef struct { + float x[TRAJECTORY_SIZE]; + float y[TRAJECTORY_SIZE]; + float z[TRAJECTORY_SIZE]; +} line; - float mpc_x[50]; - float mpc_y[50]; + +typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; @@ -125,10 +128,17 @@ typedef struct UIScene { cereal::ControlsState::Reader controls_state; cereal::DriverState::Reader driver_state; cereal::DMonitoringState::Reader dmonitoring_state; - cereal::ModelData::Reader model; - float left_lane_points[MODEL_PATH_DISTANCE]; - float path_points[MODEL_PATH_DISTANCE]; - float right_lane_points[MODEL_PATH_DISTANCE]; + cereal::ModelDataV2::Reader model; + line path; + line outer_left_lane_line; + line left_lane_line; + line right_lane_line; + line outer_right_lane_line; + line left_road_edge; + line right_road_edge; + float max_distance; + float lane_line_probs[4]; + float road_edge_stds[2]; // dp bool dpDashcam; @@ -180,7 +190,7 @@ typedef struct { typedef struct { vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; int cnt; -} model_path_vertices_data; +} line_vertices_data; typedef struct { vertex_data v[TRACK_POINTS_MAX_CNT]; @@ -246,8 +256,9 @@ typedef struct UIState { bool alert_blinked; float alert_blinking_alpha; - track_vertices_data track_vertices[2]; - model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2]; + track_vertices_data track_vertices; + line_vertices_data lane_line_vertices[4]; + line_vertices_data road_edge_vertices[2]; Rect video_rect; } UIState;