mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 05:22:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

torch model

Browse Source
This commit is contained in:
Rick Lan
2020-11-10 09:32:17 +10:00
parent d6b3cb2e32
commit 8a8dfafa56
33 changed files with 786 additions and 492 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS-DEV.md
+13 -1
View File
@@ -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
========================
CHANGELOGS-REL.md
+12
View File
@@ -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
========================
CHANGELOGS.md
+13 -1
View File
@@ -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)
========================
cereal/log.capnp
+2
View File
@@ -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;
common/dp_conf.py
+2
View File
@@ -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']},
common/transformations/camera.py
+63 -33
View File
@@ -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)
common/transformations/model.py
+57 -45
View File
@@ -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
models/supercombo.dlc
BIN
View File
Binary file not shown.
panda/board/safety/safety_defaults.h
+23 -1
View File
@@ -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 = {
selfdrive/camerad/cameras/camera_common.cc
+4 -11
View File
@@ -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;
selfdrive/camerad/cameras/camera_common.h
+1 -1
View File
@@ -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);
selfdrive/camerad/cameras/camera_qcom.cc
+18 -12
View File
@@ -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;
}
selfdrive/camerad/cameras/camera_qcom.h
+2 -1
View File
@@ -142,7 +142,8 @@ typedef struct MultiCameraState {
CameraState rear;
CameraState front;
SubMaster *sm;
SubMaster *sm_front;
SubMaster *sm_rear;
PubMaster *pm;
} MultiCameraState;
selfdrive/car/honda/carcontroller.py
+11 -10
View File
@@ -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
selfdrive/car/honda/interface.py
+18
View File
@@ -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
selfdrive/car/hyundai/carstate.py
+1 -3
View File
@@ -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
selfdrive/car/hyundai/interface.py
+17 -10
View File
@@ -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
selfdrive/car/hyundai/values.py
+16 -10
View File
@@ -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', ],
selfdrive/common/mat.h
+6 -9
View File
@@ -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
selfdrive/common/modeldata.h
+3
View File
@@ -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
selfdrive/controls/controlsd.py
-1
View File
@@ -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
selfdrive/controls/lib/lane_planner.py
+41 -34
View File
@@ -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()
selfdrive/controls/lib/pathplanner.py
+1
View File
@@ -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
selfdrive/modeld/SConscript
+7 -7
View File
@@ -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
selfdrive/modeld/modeld.cc
+7 -7
View File
@@ -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<float, 3, 3> fcam_intrinsics;
#ifndef QCOM2
Eigen::Matrix<float, 3, 3> 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<float, 3, 3> 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<float, 3, 3> 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);
selfdrive/modeld/models/commonmodel.c → selfdrive/modeld/models/commonmodel.cc
+23
View File
@@ -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));
selfdrive/modeld/models/commonmodel.h
+2
View File
@@ -8,6 +8,7 @@
#include <CL/cl.h>
#endif
#include <float.h>
#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);
selfdrive/modeld/models/driving.cc
+274 -85
View File
@@ -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<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> 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<MODEL_PATH_DISTANCE; i++) {
points_arr[i] = data[i] + offset;
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[30*i + 16];
stds_arr[i] = exp(data[30*(33 + i) + 16]);
}
if (has_prob) {
prob = sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
} else {
prob = 1.0;
}
std = softplus(data[MODEL_PATH_DISTANCE]) + 1e-6;
poly_fit(points_arr, stds_arr, poly_arr, valid_len);
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
if (std::getenv("DEBUG")){
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
path.setStds(stds);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setProb(1.0);
path.setStd(std);
path.setValidLen(valid_len);
}
kj::ArrayPtr<const float> 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<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[2*33*ll_idx + 2*i];
stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
}
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
kj::ArrayPtr<const float> 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<LEAD_MHP_VALS; i++) {
xyva_arr[i] = data[LEAD_MHP_VALS + i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
}
kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
kj::ArrayPtr<const float> 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<const float> 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<const float> 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<const float> 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<const float> 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<TIME_DISTANCE/10; i++) {
dist_arr[i] = long_x_data[i*10];
speed_arr[i] = long_v_data[i*10];
accel_arr[i] = long_a_data[i*10];
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
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<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
longi.setDistances(dist);
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
longi.setSpeeds(speed);
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
longi.setAccelerations(accel);
kj::ArrayPtr<const float> 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<const float> 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<TRAJECTORY_SIZE; i++) {
// column_offset == -1 means this data is X indexed not T indexed
if (column_offset >= 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<const float> x(x_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> 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<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
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<TRAJECTORY_SIZE; i++) {
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
}
auto position = framed.initPosition();
fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
auto velocity = framed.initVelocity();
fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
auto orientation = framed.initOrientation();
fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
auto orientation_rate = framed.initOrientationRate();
fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
// lane lines
auto lane_lines = framed.initLaneLines(4);
float lane_line_probs_arr[4];
float lane_line_stds_arr[4];
for (int i = 0; i < 4; i++) {
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
kj::ArrayPtr<const float> 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<LEAD_MHP_SELECTION; t_offset++) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
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<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
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<TRAJECTORY_SIZE; i++) {
valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
if (valid_len_candidate >= 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<TRAJECTORY_SIZE; i++) {
if (valid_len >= 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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
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;
selfdrive/modeld/models/driving.h
+22 -13
View File
@@ -17,33 +17,40 @@
#include <memory>
#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
selfdrive/modeld/transforms/transform.c → selfdrive/modeld/transforms/transform.cc
+2 -2
View File
@@ -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);
selfdrive/ui/paint.cc
+81 -169
View File
@@ -4,6 +4,7 @@
#include <cmath>
#include <iostream>
#include "common/util.h"
#include <algorithm>
#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; i<pvd->cnt; 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; i<pvd->cnt; 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<float>(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) {
selfdrive/ui/ui.cc
+20 -13
View File
@@ -1,4 +1,5 @@
#include <stdio.h>
#include <cmath>
#include <stdlib.h>
#include <stdbool.h>
#include <signal.h>
@@ -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<float>::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();
selfdrive/ui/ui.hpp
+24 -13
View File
@@ -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<UIStatus, NVGcolor> 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;