mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-22 16:53:49 +08:00
dragonpilot 0.7.10.1
======================== * HYUNDAI_GENESIS uses INDI controller. (Thanks to @donfyffe) * HYUNDAI_GENESIS added Cruise button event and lkMode feature. (Thanks to @donfyffe) * Support 2018 Taiwan Hyundai IONIQ + smart MDPS (dp_hkg_smart_mdps) (Thanks to @andy741217) * Use openpilot v0.8 model. (Thanks to @eisenheim) * Added optimizations from pre-0.8. * Added dp_honda_eps_mod setting to enable higher torque (eps mod required). (Thanks to @Wuxl_369) * Fixed issue with white/grey panda support for VW (Thanks to @lirudy) * GENESIS_G70 Optimisation (Thanks to @sebastian4k) * HYUNDAI_GENESIS Optimisation (Thanks to @donfyffe) * Added Dynamic Gas Lite. (Thanks to @toyboxZ) * Added Honda inspire, accord, crv SnG optimisation from afa fork. (Thanks to @menwenliang) * Added dp_toyota_lowest_cruise_override_vego. (Thanks to @toyboxZ)
This commit is contained in:
Rick Lan
@@ -1,3 +1,30 @@
|
||||
dragonpilot 0.7.10.1
|
||||
========================
|
||||
* HYUNDAI_GENESIS 使用 INDI 控制器。(感謝 @donfyffe 提供)
|
||||
* HYUNDAI_GENESIS uses INDI controller. (Thanks to @donfyffe)
|
||||
* 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)
|
||||
* 使用 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)
|
||||
* 加入 Dynamic gas Lite。(感謝 @toyboxZ 提供)
|
||||
* Added Dynamic Gas Lite. (Thanks to @toyboxZ)
|
||||
* 加入來自 afa 的 Honda inspire, accord, crv SnG 優化。(感謝 @menwenliang 提供)
|
||||
* Added Honda inspire, accord, crv SnG optimisation from afa fork. (Thanks to @menwenliang)
|
||||
* 加入 dp_toyota_lowest_cruise_override_vego。(感謝 @toyboxZ 提供)
|
||||
* Added dp_toyota_lowest_cruise_override_vego. (Thanks to @toyboxZ)
|
||||
|
||||
dragonpilot 0.7.10.0
|
||||
========================
|
||||
* 基於最新 openpilot 0.7.10 devel.
|
||||
@@ -17,6 +44,8 @@ dragonpilot 0.7.10.0
|
||||
* Based on latest openpilot 0.7.10 devel.
|
||||
* 修正 EON 接 PC/USB 充電器時仍會自動關機的錯誤。(感謝 @小愛 回報)
|
||||
* Fixed auto shutdown issue when EON connect to PC/USB Charger. (Thanks to @LOVEChen)
|
||||
* HYUNDAI_GENESIS 使用 INDI 控制器。(感謝 @donfyffe 提供)
|
||||
* HYUNDAI_GENESIS uses INDI controller. (Thanks to @donfyffe)
|
||||
|
||||
dragonpilot 0.7.8.3
|
||||
========================
|
||||
|
||||
@@ -17,6 +17,30 @@ dragonpilot 0.7.10.0
|
||||
* Based on latest openpilot 0.7.10 devel.
|
||||
* 修正 EON 接 PC/USB 充電器時仍會自動關機的錯誤。(感謝 @小愛 回報)
|
||||
* Fixed auto shutdown issue when EON connect to PC/USB Charger. (Thanks to @LOVEChen)
|
||||
* HYUNDAI_GENESIS 使用 INDI 控制器。(感謝 @donfyffe 提供)
|
||||
* HYUNDAI_GENESIS uses INDI controller. (Thanks to @donfyffe)
|
||||
* 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)
|
||||
* 使用 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)
|
||||
* 加入 Dynamic gas Lite。(感謝 @toyboxZ 提供)
|
||||
* Added Dynamic Gas Lite. (Thanks to @toyboxZ)
|
||||
* 加入來自 afa 的 Honda inspire, accord, crv SnG 優化。(感謝 @menwenliang 提供)
|
||||
* Added Honda inspire, accord, crv SnG optimisation from afa fork. (Thanks to @menwenliang)
|
||||
* 加入 dp_toyota_lowest_cruise_override_vego。(感謝 @toyboxZ 提供)
|
||||
* Added dp_toyota_lowest_cruise_override_vego. (Thanks to @toyboxZ)
|
||||
|
||||
dragonpilot 0.7.8
|
||||
========================
|
||||
|
||||
@@ -1,3 +1,47 @@
|
||||
2020-11-28 (0.7.10.0)
|
||||
========================
|
||||
* 加入來自 afa 的 Honda inspire, accord, crv SnG 優化。(感謝 @menwenliang 提供)
|
||||
* Added Honda inspire, accord, crv SnG optimisation from afa fork. (Thanks to @menwenliang)
|
||||
* 加入 dp_toyota_lowest_cruise_override_vego。(感謝 @toyboxZ 提供)
|
||||
* Added dp_toyota_lowest_cruise_override_vego. (Thanks to @toyboxZ)
|
||||
|
||||
2020-11-20 (0.7.10.0)
|
||||
========================
|
||||
* 加入 Dynamic gas Lite。(感謝 @toyboxZ 提供)
|
||||
* Added Dynamic Gas Lite. (Thanks to @toyboxZ)
|
||||
|
||||
2020-11-19 (0.7.10.0)
|
||||
========================
|
||||
* 更新所有 honda/hyunda/toyota 指紋。
|
||||
* Updated all honda/hyunda/toyota fingerprints.
|
||||
|
||||
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)
|
||||
========================
|
||||
* HYUNDAI_GENESIS 加入 Cruise 按紐 和 lkMode 支援。(感謝 @donfyffe 建議)
|
||||
* HYUNDAI_GENESIS added Cruise button event and lkMode feature. (Thanks to @donfyffe)
|
||||
|
||||
2020-11-04 (0.7.10.0)
|
||||
========================
|
||||
* HYUNDAI_GENESIS 使用 INDI 控制器。(感謝 @donfyffe 提供)
|
||||
* HYUNDAI_GENESIS uses INDI controller. (Thanks to @donfyffe)
|
||||
|
||||
2020-10-30 (0.7.10.0)
|
||||
========================
|
||||
* 基於最新 openpilot 0.7.10 devel.
|
||||
|
||||
+10
-8
@@ -102,6 +102,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
modeldLagging @89;
|
||||
deviceFalling @90;
|
||||
fanMalfunction @91;
|
||||
cameraMalfunction @92;
|
||||
|
||||
gasUnavailableDEPRECATED @3;
|
||||
dataNeededDEPRECATED @16;
|
||||
@@ -115,18 +116,19 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
canErrorPersistentDEPRECATED @83;
|
||||
focusRecoverActiveDEPRECATED @86;
|
||||
neosUpdateRequiredDEPRECATED @88;
|
||||
modelLagWarningDEPRECATED @93;
|
||||
|
||||
#dp
|
||||
preLaneChangeLeftALC @92;
|
||||
preLaneChangeRightALC @93;
|
||||
manualSteeringRequired @94;
|
||||
manualSteeringRequiredBlinkersOn @95;
|
||||
leadCarMoving @96;
|
||||
preLaneChangeLeftALC @94;
|
||||
preLaneChangeRightALC @95;
|
||||
manualSteeringRequired @96;
|
||||
manualSteeringRequiredBlinkersOn @97;
|
||||
leadCarMoving @98;
|
||||
|
||||
# timebomb assist
|
||||
timebombWarn @97;
|
||||
timebombBypassing @98;
|
||||
timebombBypassed @99;
|
||||
timebombWarn @99;
|
||||
timebombBypassing @100;
|
||||
timebombBypassed @101;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
+26
-13
@@ -628,6 +628,8 @@ struct ModelData {
|
||||
frameAge @12 :UInt32;
|
||||
frameDropPerc @13 :Float32;
|
||||
timestampEof @9 :UInt64;
|
||||
modelExecutionTime @14 :Float32;
|
||||
rawPred @15 :Data;
|
||||
|
||||
path @1 :PathData;
|
||||
leftLane @2 :PathData;
|
||||
@@ -694,6 +696,8 @@ struct ModelDataV2 {
|
||||
frameAge @1 :UInt32;
|
||||
frameDropPerc @2 :Float32;
|
||||
timestampEof @3 :UInt64;
|
||||
modelExecutionTime @15 :Float32;
|
||||
rawPred @16 :Data;
|
||||
|
||||
position @4 :XYZTData;
|
||||
orientation @5 :XYZTData;
|
||||
@@ -701,7 +705,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;
|
||||
@@ -754,6 +760,8 @@ struct EncodeIndex {
|
||||
segmentId @4 :UInt32;
|
||||
# index into camera file in segment in encode order
|
||||
segmentIdEncode @5 :UInt32;
|
||||
timestampSof @6 :UInt64;
|
||||
timestampEof @7 :UInt64;
|
||||
|
||||
enum Type {
|
||||
bigBoxLossless @0; # rcamera.mkv
|
||||
@@ -1946,6 +1954,9 @@ struct OrbKeyFrame {
|
||||
|
||||
struct DriverState {
|
||||
frameId @0 :UInt32;
|
||||
modelExecutionTime @14 :Float32;
|
||||
rawPred @15 :Data;
|
||||
|
||||
descriptorDEPRECATED @1 :List(Float32);
|
||||
stdDEPRECATED @2 :Float32;
|
||||
faceOrientation @3 :List(Float32);
|
||||
@@ -2122,17 +2133,18 @@ struct Event {
|
||||
thumbnail @66: Thumbnail;
|
||||
carEvents @68: List(Car.CarEvent);
|
||||
carParams @69: Car.CarParams;
|
||||
frontFrame @70: FrameData;
|
||||
frontFrame @70: FrameData; # driver facing camera
|
||||
dMonitoringState @71: DMonitoringState;
|
||||
liveLocationKalman @72 :LiveLocationKalman;
|
||||
sentinel @73 :Sentinel;
|
||||
wideFrame @74: FrameData;
|
||||
modelV2 @75 :ModelDataV2;
|
||||
dragonConf @76 :DragonConf;
|
||||
frontEncodeIdx @76 :EncodeIndex; # driver facing camera
|
||||
wideEncodeIdx @77 :EncodeIndex;
|
||||
dragonConf @78 :DragonConf;
|
||||
}
|
||||
}
|
||||
|
||||
# dp
|
||||
struct DragonConf {
|
||||
dpThermalStarted @0 :Bool;
|
||||
dpThermalOverheat @1 :Bool;
|
||||
@@ -2198,14 +2210,15 @@ struct DragonConf {
|
||||
dpToyotaLdw @61 :Bool;
|
||||
dpToyotaSng @62 :Bool;
|
||||
dpToyotaLowestCruiseOverride @63 :Bool;
|
||||
dpToyotaLowestCruiseOverrideAt @64 :Float32;
|
||||
dpToyotaLowestCruiseOverrideSpeed @65 :Float32;
|
||||
dpIpAddr @66 :Text;
|
||||
dpCameraOffset @67 :Int8;
|
||||
dpLocale @68 :Text;
|
||||
dpChargingCtrl @69 :Bool;
|
||||
dpChargingAt @70 :UInt8;
|
||||
dpDischargingAt @71 :UInt8;
|
||||
dpIsUpdating @72 :Bool;
|
||||
dpTimebombAssist @73 :Bool;
|
||||
dpToyotaLowestCruiseOverrideVego @64 :Bool;
|
||||
dpToyotaLowestCruiseOverrideAt @65 :Float32;
|
||||
dpToyotaLowestCruiseOverrideSpeed @66 :Float32;
|
||||
dpIpAddr @67 :Text;
|
||||
dpCameraOffset @68 :Int8;
|
||||
dpLocale @69 :Text;
|
||||
dpChargingCtrl @70 :Bool;
|
||||
dpChargingAt @71 :UInt8;
|
||||
dpDischargingAt @72 :UInt8;
|
||||
dpIsUpdating @73 :Bool;
|
||||
dpTimebombAssist @74 :Bool;
|
||||
}
|
||||
@@ -58,6 +58,7 @@ confs = [
|
||||
{'name': 'dp_dynamic_follow', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 4, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_dynamic_follow_multiplier', 'default': 1., 'type': 'Float32', 'min': 0.85, 'max': 1.2, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param']},
|
||||
{'name': 'dp_dynamic_follow_min_tr', 'default': 0.9, 'type': 'Float32', 'min': 0.85, 'max': 1.6, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param']},
|
||||
{'name': 'dp_dynamic_gas', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param']},
|
||||
{'name': 'dp_accel_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 3, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
# safety
|
||||
{'name': 'dp_driver_monitor', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
@@ -104,8 +105,13 @@ confs = [
|
||||
{'name': 'dp_toyota_sng', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_toyota_zss', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}], 'conf_type': ['param']},
|
||||
{'name': 'dp_toyota_lowest_cruise_override', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_toyota_lowest_cruise_override_vego', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_toyota_lowest_cruise_override_at', 'default': 44, 'type': 'Float32', 'depends': [{'name': 'dp_car_detected', 'vals': ['toyota']}, {'name': 'dp_toyota_lowest_cruise_override', 'vals': [True]}], 'min': 0, 'max': 255., 'conf_type': ['param', 'struct']},
|
||||
{'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']},
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
+27
-1
@@ -8,6 +8,11 @@ source "$BASEDIR/launch_env.sh"
|
||||
|
||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
|
||||
function tici_init {
|
||||
sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor'
|
||||
sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor'
|
||||
}
|
||||
|
||||
function two_init {
|
||||
# Restrict Android and other system processes to the first two cores
|
||||
echo 0-1 > /dev/cpuset/background/cpus
|
||||
@@ -19,15 +24,32 @@ function two_init {
|
||||
# openpilot gets all the cores
|
||||
echo 0-3 > /dev/cpuset/app/cpus
|
||||
|
||||
# set up governors
|
||||
# +50mW offroad, +500mW onroad for 30% more RAM bandwidth
|
||||
echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor
|
||||
echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq
|
||||
echo "performance" > /sys/class/devfreq/soc:qcom,m4m/governor
|
||||
|
||||
# unclear if these help, but they don't seem to hurt
|
||||
echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor
|
||||
echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu2/governor
|
||||
|
||||
# GPU
|
||||
echo "performance" > /sys/class/devfreq/b00000.qcom,kgsl-3d0/governor
|
||||
|
||||
# /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave"
|
||||
# it seems to gain nothing but a wasted 500mW
|
||||
|
||||
# Collect RIL and other possibly long-running I/O interrupts onto CPU 1
|
||||
echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio)
|
||||
echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage)
|
||||
echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci)
|
||||
echo 1 > /proc/irq/6/smp_affinity_list # MDSS
|
||||
|
||||
# USB traffic needs realtime handling on cpu 3
|
||||
[ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list # USB for LeEco
|
||||
[ -d "/proc/irq/736" ] && echo 3 > /proc/irq/736/smp_affinity_list # USB for OP3T
|
||||
|
||||
|
||||
# Check for NEOS update
|
||||
if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
|
||||
if [ -f "$DIR/scripts/continue.sh" ]; then
|
||||
@@ -109,6 +131,10 @@ function launch {
|
||||
two_init
|
||||
fi
|
||||
|
||||
if [ -f /TICI ]; then
|
||||
tici_init
|
||||
fi
|
||||
|
||||
# handle pythonpath
|
||||
ln -sfn $(pwd) /data/pythonpath
|
||||
export PYTHONPATH="$PWD"
|
||||
|
||||
Binary file not shown.
@@ -126,7 +126,7 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
||||
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
|
||||
break;
|
||||
case 0x117: // ACC_10 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
|
||||
crc ^= (uint8_t[]){0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16,0x16}[counter];
|
||||
break;
|
||||
case 0x120: // TSK_06 Drivetrain Coordinator
|
||||
crc ^= (uint8_t[]){0xC4,0xE2,0x4F,0xE4,0xF8,0x2F,0x56,0x81,0x9F,0xE5,0x83,0x44,0x05,0x3F,0x97,0xDF}[counter];
|
||||
|
||||
@@ -418,13 +418,15 @@ BO_ 892 CRUISE_PARAMS: 8 PCM
|
||||
BO_ 927 RADAR_HUD: 8 RADAR
|
||||
SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY
|
||||
SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ZEROS_BOH3 : 31|32@0+ (1,0) [0|4294967295] "" XXX
|
||||
SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX
|
||||
SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BOH : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BOH_2 : 30|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
|
||||
@@ -405,13 +405,15 @@ BO_ 432 STANDSTILL: 7 VSA
|
||||
BO_ 927 RADAR_HUD: 8 RADAR
|
||||
SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY
|
||||
SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ZEROS_BOH3 : 31|32@0+ (1,0) [0|4294967295] "" XXX
|
||||
SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX
|
||||
SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BOH : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BOH_2 : 30|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
|
||||
@@ -0,0 +1,404 @@
|
||||
CM_ "AUTOGENERATED FILE, DO NOT EDIT"
|
||||
|
||||
|
||||
CM_ "Imported file _comma.dbc starts here"
|
||||
BO_ 359 STEERING_IPAS_COMMA: 8 IPAS
|
||||
SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX
|
||||
SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
CM BO_ STEERING_IPAS_COMMA "Copy of msg 614 so we can do angle control while the Park Assist ECU is connected (Panda spoofs 614 with 359 on connector J70). Note that addresses 0x266 and 0x167 are checksum-invariant";
|
||||
|
||||
BO_ 512 GAS_COMMAND: 6 EON
|
||||
SG_ GAS_COMMAND : 7|16@0+ (0.159375,-75.555) [0|1] "" INTERCEPTOR
|
||||
SG_ GAS_COMMAND2 : 23|16@0+ (0.159375,-151.111) [0|1] "" INTERCEPTOR
|
||||
SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
|
||||
SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" INTERCEPTOR
|
||||
SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" INTERCEPTOR
|
||||
|
||||
BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
|
||||
SG_ INTERCEPTOR_GAS : 7|16@0+ (0.159375,-75.555) [0|1] "" EON
|
||||
SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.159375,-151.111) [0|1] "" EON
|
||||
SG_ STATE : 39|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ COUNTER_PEDAL : 35|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ CHECKSUM_PEDAL : 47|8@0+ (1,0) [0|255] "" EON
|
||||
|
||||
VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
|
||||
|
||||
|
||||
CM_ "Imported file _toyota_2017.dbc starts here"
|
||||
VERSION ""
|
||||
|
||||
|
||||
NS_ :
|
||||
NS_DESC_
|
||||
CM_
|
||||
BA_DEF_
|
||||
BA_
|
||||
VAL_
|
||||
CAT_DEF_
|
||||
CAT_
|
||||
FILTER
|
||||
BA_DEF_DEF_
|
||||
EV_DATA_
|
||||
ENVVAR_DATA_
|
||||
SGTYPE_
|
||||
SGTYPE_VAL_
|
||||
BA_DEF_SGTYPE_
|
||||
BA_SGTYPE_
|
||||
SIG_TYPE_REF_
|
||||
VAL_TABLE_
|
||||
SIG_GROUP_
|
||||
SIG_VALTYPE_
|
||||
SIGTYPE_VALTYPE_
|
||||
BO_TX_BU_
|
||||
BA_DEF_REL_
|
||||
BA_REL_
|
||||
BA_DEF_DEF_REL_
|
||||
BU_SG_REL_
|
||||
BU_EV_REL_
|
||||
BU_BO_REL_
|
||||
SG_MUL_VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
BU_: XXX DSU HCU EPS IPAS CGW
|
||||
|
||||
BO_ 36 KINEMATICS: 8 XXX
|
||||
SG_ ACCEL_Y : 33|10@0+ (0.03589,-18.375) [0|65535] "m/s^2" XXX
|
||||
SG_ YAW_RATE : 1|10@0+ (0.244,-125) [0|65535] "deg/sec" XXX
|
||||
SG_ STEERING_TORQUE : 17|10@0+ (1,-512) [0|65535] "" XXX
|
||||
|
||||
BO_ 37 STEER_ANGLE_SENSOR: 8 XXX
|
||||
SG_ STEER_ANGLE : 3|12@0- (1.5,0) [-500|500] "deg" XXX
|
||||
SG_ STEER_FRACTION : 39|4@0- (0.1,0) [-0.7|0.7] "deg" XXX
|
||||
SG_ STEER_RATE : 35|12@0- (1,0) [-2000|2000] "deg/s" XXX
|
||||
|
||||
BO_ 166 BRAKE: 8 XXX
|
||||
SG_ BRAKE_AMOUNT : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BRAKE_PEDAL : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 170 WHEEL_SPEEDS: 8 XXX
|
||||
SG_ WHEEL_SPEED_FR : 7|16@0+ (0.01,-67.67) [0|250] "kph" XXX
|
||||
SG_ WHEEL_SPEED_FL : 23|16@0+ (0.01,-67.67) [0|250] "kph" XXX
|
||||
SG_ WHEEL_SPEED_RR : 39|16@0+ (0.01,-67.67) [0|250] "kph" XXX
|
||||
SG_ WHEEL_SPEED_RL : 55|16@0+ (0.01,-67.67) [0|250] "kph" XXX
|
||||
|
||||
BO_ 180 SPEED: 8 XXX
|
||||
SG_ ENCODER : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED : 47|16@0+ (0.01,0) [0|250] "kph" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 353 DSU_SPEED: 7 XXX
|
||||
SG_ FORWARD_SPEED : 15|16@0- (0.00390625,-30) [0|255] "kph" XXX
|
||||
|
||||
BO_ 452 ENGINE_RPM: 8 CGW
|
||||
SG_ RPM : 7|16@0- (0.78125,0) [0|0] "rpm" SCS
|
||||
|
||||
BO_ 466 PCM_CRUISE: 8 XXX
|
||||
SG_ GAS_RELEASED : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ STANDSTILL_ON : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACCEL_NET : 23|16@0- (0.001,0) [-20|20] "m/s2" XXX
|
||||
SG_ CRUISE_STATE : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CANCEL_REQ : 49|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 467 PCM_CRUISE_2: 8 XXX
|
||||
SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "kph" XXX
|
||||
SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "kph" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 552 ACCELEROMETER: 8 XXX
|
||||
SG_ ACCEL_Z : 22|15@0- (1,0) [0|32767] "" XXX
|
||||
SG_ ACCEL_X : 6|15@0- (0.001,0) [-20|20] "m/s2" XXX
|
||||
|
||||
BO_ 560 BRAKE_MODULE2: 7 XXX
|
||||
SG_ BRAKE_PRESSED : 26|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 614 STEERING_IPAS: 8 IPAS
|
||||
SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX
|
||||
SG_ SET_ME_X10 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ DIRECTION_CMD : 38|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X40 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00_1 : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 643 PRE_COLLISION: 7 DSU
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ FORCE : 23|16@0- (2,0) [0|255] "N" XXX
|
||||
SG_ SET_ME_X002 : 33|8@0+ (1,0) [0|3] "" XXX
|
||||
SG_ BRAKE_STATUS : 39|3@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STATE : 36|3@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X003 : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ PRECOLLISION_ACTIVE : 41|1@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 740 STEERING_LKA: 5 XXX
|
||||
SG_ LKA_STATE : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 6|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ SET_ME_1 : 7|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX
|
||||
SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 742 LEAD_INFO: 8 DSU
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" HCU
|
||||
SG_ LEAD_REL_SPEED : 23|12@0- (0.025,0) [-100|100] "m/s" HCU
|
||||
SG_ LEAD_LONG_DIST : 7|13@0+ (0.05,0) [0|300] "m" HCU
|
||||
|
||||
BO_ 835 ACC_CONTROL: 8 DSU
|
||||
SG_ ACCEL_CMD : 7|16@0- (0.001,0) [-20|20] "m/s2" HCU
|
||||
SG_ SET_ME_X01 : 23|2@0+ (1,0) [0|3] "" HCU
|
||||
SG_ DISTANCE : 20|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ MINI_CAR : 21|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X3 : 19|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU
|
||||
SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU
|
||||
SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ ACCEL_CMD_ALT : 47|8@0- (0.05,0) [0|0] "m/s^2" XXX
|
||||
|
||||
BO_ 836 PRE_COLLISION_2: 8 DSU
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 869 DSU_CRUISE : 7 DSU
|
||||
SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SET_BTN : 2|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ CANCEL_BTN : 1|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ MAIN_ON : 0|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SET_SPEED : 15|8@0+ (1,0) [0|0] "km/h" XXX
|
||||
SG_ CRUISE_REQUEST : 31|8@0+ (100,-12800) [0|0] "N" XXX
|
||||
SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|0] "m" XXX
|
||||
|
||||
BO_ 921 PCM_CRUISE_SM: 8 XXX
|
||||
SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 951 ESP_CONTROL: 8 ESP
|
||||
SG_ TC_DISABLED : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ VSC_DISABLED : 12|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_LIGHTS_ACC : 18|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1041 ACC_HUD: 8 DSU
|
||||
SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1042 LKAS_HUD: 8 XXX
|
||||
SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ RIGHT_LINE : 3|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LEFT_LINE : 5|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X01 : 7|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X01_2 : 11|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LDA_ALERT : 9|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ TWO_BEEPS : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ADJUSTING_CAMERA : 13|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LDA_MALFUNCTION : 15|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ REPEATED_BEEPS : 32|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X0C : 23|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X2C : 47|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X38 : 55|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X02 : 63|8@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1043 TIME : 8 CGW
|
||||
SG_ YEAR : 7|8@0+ (1,0) [0|0] "year" XXX
|
||||
SG_ MONTH : 15|8@0+ (1,0) [0|0] "month" XXX
|
||||
SG_ DAY : 23|8@0+ (1,0) [0|0] "day" XXX
|
||||
SG_ HOUR : 31|8@0+ (1,0) [0|0] "hour" XXX
|
||||
SG_ MINUTE : 39|8@0+ (1,0) [0|0] "minute" XXX
|
||||
SG_ GMT_DIFF : 55|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ GMTDIFF_HOURS : 54|4@0+ (1,0) [0|0] "hours" XXX
|
||||
SG_ GMTDIFF_MINUTES : 50|6@0+ (1,0) [0|0] "minutes" XXX
|
||||
SG_ SUMMER : 60|1@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1408 VIN_PART_1: 8 CGW
|
||||
SG_ VIN_1 : 7|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_2 : 15|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_3 : 23|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_4 : 31|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_5 : 39|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_6 : 47|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_7 : 55|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_8 : 63|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1409 VIN_PART_2: 8 CGW
|
||||
SG_ VIN_9 : 7|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_10 : 15|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_11 : 23|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_12 : 31|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_13 : 39|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_14 : 47|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_15 : 55|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ VIN_16 : 63|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1410 VIN_PART_3: 8 CGW
|
||||
SG_ VIN_17 : 7|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1553 UI_SETTING: 8 XXX
|
||||
SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1556 STEERING_LEVERS: 8 XXX
|
||||
SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1568 SEATS_DOORS: 8 XXX
|
||||
SG_ SEATBELT_DRIVER_UNLATCHED : 62|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_FL : 45|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_RL : 42|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_RR : 43|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_FR : 44|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1570 LIGHT_STALK: 8 SCM
|
||||
SG_ AUTO_HIGH_BEAM : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1161 RSA1: 8 FCM
|
||||
SG_ TSGN1 : 7|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNGRY1 : 12|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNHLT1 : 9|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPDVAL1 : 23|8@0+ (1,0) [0|0] "kph" XXX
|
||||
SG_ SPLSGN1 : 31|4@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPLSGN2 : 27|4@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGN2 : 39|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNGRY2 : 44|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNHLT2 : 41|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPDVAL2 : 55|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ BZRRQ_P : 63|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ BZRRQ_A : 61|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SYNCID1 : 59|4@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1162 RSA2: 8 FCM
|
||||
SG_ TSGN3 : 7|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNGRY3 : 12|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNHLT3 : 9|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPLSGN3 : 31|4@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPLSGN4 : 27|4@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGN4 : 39|8@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNGRY4 : 44|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSGNHLT4 : 41|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ DPSGNREQ : 54|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SGNNUMP : 53|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SGNNUMA : 50|3@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SPDUNT : 63|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSRWMSG : 61|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ SYNCID2 : 59|4@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1163 RSA3: 8 FCM
|
||||
SG_ TSREQPD : 7|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ TSRMSW : 6|1@0+ (1,0) [0|0] "" XXX
|
||||
SG_ OTSGNNTM : 5|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ NTLVLSPD : 3|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ OVSPNTM : 1|2@0+ (1,0) [0|0] "" XXX
|
||||
SG_ OVSPVALL : 11|4@0+ (1,-5) [0|0] "" XXX
|
||||
SG_ OVSPVALM : 19|4@0+ (1,-5) [0|0] "" XXX
|
||||
SG_ OVSPVALH : 27|4@0+ (1,-5) [0|0] "" XXX
|
||||
SG_ TSRSPU : 33|2@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
CM_ SG_ 36 ACCEL_Y "unit is tbd";
|
||||
CM_ SG_ 36 YAW_RATE "verify";
|
||||
CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
|
||||
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
|
||||
CM_ SG_ 37 STEER_RATE "factor is tbd";
|
||||
CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors";
|
||||
CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect";
|
||||
CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph";
|
||||
CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?";
|
||||
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
|
||||
CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
|
||||
CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
|
||||
CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
|
||||
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
|
||||
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
|
||||
CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was "SET_ME_1" and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG.";
|
||||
CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
|
||||
CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
|
||||
CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
|
||||
CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
|
||||
CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
|
||||
CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
|
||||
CM_ SG_ 1161 SYNCID1 "counter from 1 to f at 1 Hz";
|
||||
CM_ SG_ 1161 SPDVAL2 "conditional speed value 70"
|
||||
CM_ SG_ 1162 SGNNUMP "1 if SPDVAL1 is set, otherwise 0";
|
||||
CM_ SG_ 1162 SYNCID2 "counter from 1 to f at 1 Hz";
|
||||
CM_ SG_ 1163 TSREQPD "always 1";
|
||||
CM_ SG_ 1163 TSRMSW "always 1";
|
||||
CM_ SG_ 1163 OTSGNNTM "always 3";
|
||||
CM_ SG_ 1163 NTLVLSPD "always 3";
|
||||
CM_ SG_ 1163 OVSPNTM "always 3";
|
||||
CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds";
|
||||
CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds";
|
||||
CM_ SG_ 1163 TSRSPU "always 1";
|
||||
|
||||
VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off";
|
||||
VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok";
|
||||
VAL_ 614 STATE 3 "enabled" 1 "disabled";
|
||||
VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
|
||||
VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
|
||||
VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
|
||||
VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
|
||||
VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
|
||||
VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
|
||||
VAL_ 1042 LEFT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
|
||||
VAL_ 1553 UNITS 1 "km" 2 "miles";
|
||||
VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left";
|
||||
VAL_ 1161 TSGN1 1 "speed sign" 0 "none";
|
||||
VAL_ 1161 TSGN2 1 "speed sign" 0 "none";
|
||||
VAL_ 1161 SPLSGN2 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 129 "no entry";
|
||||
VAL_ 1162 SPLSGN3 15 "conditional blank" 4 "wet road" 5 "rain" 0 "none";
|
||||
|
||||
|
||||
CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180";
|
||||
|
||||
CM_ "lexus_nx300_2018_pt.dbc starts here"
|
||||
|
||||
|
||||
|
||||
BO_ 550 BRAKE_MODULE: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_POSITION : 16|9@0+ (1,0) [0|511] "" XXX
|
||||
SG_ BRAKE_PRESSED : 37|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 705 GAS_PEDAL: 8 XXX
|
||||
SG_ GAS_RELEASED : 3|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ GAS_PEDAL : 55|8@0+ (0.005,0) [0|1] "" XXX
|
||||
|
||||
BO_ 608 STEER_TORQUE_SENSOR: 8 XXX
|
||||
SG_ STEER_TORQUE_EPS : 47|16@0- (0.73,0) [-20000|20000] "" XXX
|
||||
SG_ STEER_TORQUE_DRIVER : 15|16@0- (1,0) [-32768|32767] "" XXX
|
||||
SG_ STEER_OVERRIDE : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_ANGLE : 31|16@0- (0.0573,0) [-500|500] "" XXX
|
||||
|
||||
BO_ 610 EPS_STATUS: 5 EPS
|
||||
SG_ IPAS_STATE : 3|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ LKA_STATE : 31|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 956 GEAR_PACKET: 8 XXX
|
||||
SG_ SPORT_ON : 2|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ GEAR : 13|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force";
|
||||
CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8";
|
||||
CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
|
||||
CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others";
|
||||
VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled";
|
||||
VAL_ 610 LKA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby";
|
||||
VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
|
||||
VAL_ 956 SPORT_ON 0 "off" 1 "on";
|
||||
VAL_ 956 ECON_ON 0 "off" 1 "on";
|
||||
@@ -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 = {
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
// Safety-relevant steering constants for Volkswagen
|
||||
const int VOLKSWAGEN_MAX_STEER = 300; // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
|
||||
const int VOLKSWAGEN_MAX_RT_DELTA = 75; // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
|
||||
const int VOLKSWAGEN_MAX_RT_DELTA = 188; // 10 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 125 ; 125 * 1.5 for safety pad = 187.5
|
||||
const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int VOLKSWAGEN_MAX_RATE_UP = 4; // 2.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_MAX_RATE_UP = 10; // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_MAX_RATE_DOWN = 10; // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
|
||||
const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
|
||||
|
||||
@@ -164,9 +164,8 @@ bool CameraBuf::acquire() {
|
||||
}
|
||||
assert(clSetKernelArg(krnl_debayer, 2, sizeof(float), &digital_gain) == 0);
|
||||
const size_t debayer_work_size = rgb_height; // doesn't divide evenly, is this okay?
|
||||
const size_t debayer_local_work_size = 128;
|
||||
assert(clEnqueueNDRangeKernel(q, krnl_debayer, 1, NULL,
|
||||
&debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event) == 0);
|
||||
&debayer_work_size, NULL, 0, 0, &debayer_event) == 0);
|
||||
#endif
|
||||
} else {
|
||||
assert(cur_rgb_buf->len >= frame_size);
|
||||
@@ -283,21 +282,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 +395,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;
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <memory>
|
||||
@@ -34,6 +35,10 @@
|
||||
#define LOG_CAMERA_ID_QCAMERA 3
|
||||
#define LOG_CAMERA_ID_MAX 4
|
||||
|
||||
const bool env_send_front = getenv("SEND_FRONT") != NULL;
|
||||
const bool env_send_rear = getenv("SEND_REAR") != NULL;
|
||||
const bool env_send_wide = getenv("SEND_WIDE") != NULL;
|
||||
|
||||
typedef struct CameraInfo {
|
||||
const char* name;
|
||||
int frame_width, frame_height;
|
||||
@@ -58,6 +63,7 @@ typedef struct LogCameraInfo {
|
||||
|
||||
typedef struct FrameMetadata {
|
||||
uint32_t frame_id;
|
||||
uint64_t timestamp_sof; // only set on tici
|
||||
uint64_t timestamp_eof;
|
||||
unsigned int frame_length;
|
||||
unsigned int integ_lines;
|
||||
@@ -128,8 +134,9 @@ public:
|
||||
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 fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride);
|
||||
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);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -142,7 +142,8 @@ typedef struct MultiCameraState {
|
||||
CameraState rear;
|
||||
CameraState front;
|
||||
|
||||
SubMaster *sm;
|
||||
SubMaster *sm_front;
|
||||
SubMaster *sm_rear;
|
||||
PubMaster *pm;
|
||||
|
||||
} MultiCameraState;
|
||||
|
||||
@@ -205,6 +205,7 @@ void* visionserver_client_thread(void* arg) {
|
||||
stream_i == VISION_STREAM_YUV_WIDE) {
|
||||
CameraBuf *b = get_camerabuf_by_type(s, (VisionStreamType)stream_i);
|
||||
rep.d.stream_acq.extra.frame_id = b->yuv_metas[idx].frame_id;
|
||||
rep.d.stream_acq.extra.timestamp_sof = b->yuv_metas[idx].timestamp_sof;
|
||||
rep.d.stream_acq.extra.timestamp_eof = b->yuv_metas[idx].timestamp_eof;
|
||||
}
|
||||
vipc_send(fd, &rep);
|
||||
|
||||
@@ -28,7 +28,7 @@ SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
|
||||
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||
uds.SESSION_TYPE.DEFAULT])
|
||||
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
|
||||
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
|
||||
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
|
||||
|
||||
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
|
||||
@@ -36,20 +36,20 @@ EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTRO
|
||||
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
|
||||
|
||||
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
|
||||
|
||||
HYUNDAI_VERSION_REQUEST_SHORT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(0xf1a0) # 4 Byte version number
|
||||
p16(0xf1a0) # 4 Byte version number
|
||||
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(0xf100) # Long description
|
||||
p16(0xf100) # Long description
|
||||
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
|
||||
p16(0xf100) + \
|
||||
p16(0xf1a0)
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
|
||||
p16(0xf100) + \
|
||||
p16(0xf1a0)
|
||||
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
|
||||
|
||||
|
||||
@@ -127,8 +127,8 @@ def match_fw_to_car(fw_versions):
|
||||
if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None:
|
||||
continue
|
||||
|
||||
# TODO: COROLLA_TSS2 engine can show on two different addresses
|
||||
if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None:
|
||||
# TODO: on some toyota, the engine can show on two different addresses
|
||||
if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None:
|
||||
continue
|
||||
|
||||
# ignore non essential ecus
|
||||
|
||||
@@ -77,13 +77,13 @@ HUDData = namedtuple("HUDData",
|
||||
|
||||
class CarControllerParams():
|
||||
def __init__(self, CP):
|
||||
self.BRAKE_MAX = 1024//4
|
||||
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
|
||||
# mirror of list (assuming first item is zero) for interp of signed request values
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
|
||||
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
|
||||
self.BRAKE_MAX = 1024//4
|
||||
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
|
||||
# mirror of list (assuming first item is zero) for interp of signed request values
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
|
||||
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
@@ -100,6 +100,23 @@ class CarController():
|
||||
# dp
|
||||
self.last_blinker_on = False
|
||||
self.blinker_end_frame = 0.
|
||||
self.prev_lead_distance = 0.0
|
||||
self.stopped_lead_distance = 0.0
|
||||
self.lead_distance_counter = 1
|
||||
self.lead_distance_counter_prev = 1
|
||||
self.rough_lead_speed = 0.0
|
||||
|
||||
def rough_speed(self, lead_distance):
|
||||
if self.prev_lead_distance != lead_distance:
|
||||
self.lead_distance_counter_prev = self.lead_distance_counter
|
||||
self.rough_lead_speed += 0.3334 * (
|
||||
(lead_distance - self.prev_lead_distance) / self.lead_distance_counter_prev - self.rough_lead_speed)
|
||||
self.lead_distance_counter = 0.0
|
||||
elif self.lead_distance_counter >= self.lead_distance_counter_prev:
|
||||
self.rough_lead_speed = (self.lead_distance_counter * self.rough_lead_speed) / (self.lead_distance_counter + 1.0)
|
||||
self.lead_distance_counter += 1.0
|
||||
self.prev_lead_distance = lead_distance
|
||||
return self.rough_lead_speed
|
||||
|
||||
def update(self, enabled, CS, frame, actuators,
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel,
|
||||
@@ -165,7 +182,7 @@ class CarController():
|
||||
lkas_active, CS.CP.carFingerprint, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
if not dragonconf.dpAtl and (frame % 10) == 0:
|
||||
idx = (frame//10) % 4
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
|
||||
|
||||
@@ -179,8 +196,23 @@ class CarController():
|
||||
elif not dragonconf.dpAllowGas and pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack))
|
||||
|
||||
if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
|
||||
rough_lead_speed = self.rough_speed(CS.lead_distance)
|
||||
if CS.lead_distance > (self.stopped_lead_distance + 15.0) or rough_lead_speed > 0.1:
|
||||
self.stopped_lead_distance = 0.0
|
||||
can_sends.append(
|
||||
hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint,
|
||||
CS.CP.isPandaBlack))
|
||||
elif CS.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
|
||||
if CS.hud_lead == 1:
|
||||
can_sends.append(
|
||||
hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint,
|
||||
CS.CP.isPandaBlack))
|
||||
else:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint,CS.CP.isPandaBlack))
|
||||
else:
|
||||
self.stopped_lead_distance = CS.lead_distance
|
||||
self.prev_lead_distance = CS.lead_distance
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
@@ -189,16 +221,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
|
||||
|
||||
@@ -22,41 +22,42 @@ def calc_cruise_offset(offset, speed):
|
||||
def get_can_signals(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
signals = [
|
||||
("XMISSION_SPEED", "ENGINE_DATA", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("STEER_ANGLE", "STEERING_SENSORS", 0),
|
||||
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
|
||||
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
|
||||
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
|
||||
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("GEAR", "GEARBOX", 0),
|
||||
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1),
|
||||
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0),
|
||||
("BRAKE_PRESSED", "POWERTRAIN_DATA", 0),
|
||||
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
|
||||
("ESP_DISABLED", "VSA_STATUS", 1),
|
||||
("USER_BRAKE", "VSA_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
|
||||
("STEER_STATUS", "STEER_STATUS", 5),
|
||||
("GEAR_SHIFTER", "GEARBOX", 0),
|
||||
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_SETTING", "SCM_BUTTONS", 0),
|
||||
("ACC_STATUS", "POWERTRAIN_DATA", 0),
|
||||
("XMISSION_SPEED", "ENGINE_DATA", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("STEER_ANGLE", "STEERING_SENSORS", 0),
|
||||
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
|
||||
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
|
||||
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
|
||||
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
|
||||
("GEAR", "GEARBOX", 0),
|
||||
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1),
|
||||
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0),
|
||||
("BRAKE_PRESSED", "POWERTRAIN_DATA", 0),
|
||||
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
|
||||
("ESP_DISABLED", "VSA_STATUS", 1),
|
||||
("USER_BRAKE", "VSA_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
|
||||
("HUD_LEAD", "ACC_HUD", 0),
|
||||
("STEER_STATUS", "STEER_STATUS", 5),
|
||||
("GEAR_SHIFTER", "GEARBOX", 0),
|
||||
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_SETTING", "SCM_BUTTONS", 0),
|
||||
("ACC_STATUS", "POWERTRAIN_DATA", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("ENGINE_DATA", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING_SENSORS", 100),
|
||||
("SEATBELT_STATUS", 10),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("VSA_STATUS", 50),
|
||||
("ENGINE_DATA", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING_SENSORS", 100),
|
||||
("SEATBELT_STATUS", 10),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("VSA_STATUS", 50),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
@@ -106,8 +107,13 @@ def get_can_signals(CP):
|
||||
checks += [("CRUISE_PARAMS", 10)]
|
||||
else:
|
||||
checks += [("CRUISE_PARAMS", 50)]
|
||||
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
|
||||
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT):
|
||||
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1),
|
||||
("LEAD_DISTANCE", "RADAR_HUD", 0)]
|
||||
checks += [("RADAR_HUD", 50)]
|
||||
elif CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.ACURA_RDX_3G):
|
||||
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
|
||||
checks += [("RADAR_HUD", 50)]
|
||||
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
|
||||
elif CP.carFingerprint in [CAR.HRV, CAR.JADE]:
|
||||
@@ -177,6 +183,7 @@ class CarState(CarStateBase):
|
||||
|
||||
#dp
|
||||
self.lkMode = True
|
||||
self.lead_distance = 0.
|
||||
|
||||
def update(self, cp, cp_cam, cp_body):
|
||||
ret = car.CarState.new_message()
|
||||
@@ -191,7 +198,11 @@ class CarState(CarStateBase):
|
||||
|
||||
# ******************* parse out can *******************
|
||||
# TODO: find wheels moving bit in dbc
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.INSIGHT):
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
||||
self.lead_distance = cp.vl["RADAR_HUD"]['LEAD_DISTANCE']
|
||||
elif self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.ACURA_RDX_3G):
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
@@ -315,6 +326,9 @@ class CarState(CarStateBase):
|
||||
ret.cruiseState.available = bool(main_on)
|
||||
ret.cruiseState.nonAdaptive = self.cruise_mode != 0
|
||||
|
||||
# afa feature
|
||||
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
|
||||
|
||||
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
|
||||
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
|
||||
if ret.brake > 0.05:
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -255,7 +256,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
|
||||
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
|
||||
@@ -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
|
||||
|
||||
@@ -970,11 +970,23 @@ FW_VERSIONS = {
|
||||
],
|
||||
},
|
||||
CAR.HRV: {
|
||||
(Ecu.gateway, 0x18daeff1, None): [b'38897-T7A-A010\x00\x00'],
|
||||
(Ecu.eps, 0x18da30f1, None): [b'39990-THX-A020\x00\x00'],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T7A-A240\x00\x00'],
|
||||
(Ecu.srs, 0x18da53f1, None): [b'77959-T7A-A230\x00\x00'],
|
||||
(Ecu.combinationMeter, 0x18da60f1, None): [b'78109-THX-A210\x00\x00'],
|
||||
(Ecu.gateway, 0x18daeff1, None): [
|
||||
b'38897-T7A-A010\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-THX-A020\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36161-T7A-A140\x00\x00',
|
||||
b'36161-T7A-A240\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-T7A-A230\x00\x00',
|
||||
],
|
||||
(Ecu.combinationMeter, 0x18da60f1, None): [
|
||||
b'78109-THX-A110\x00\x00',
|
||||
b'78109-THX-A210\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@ from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create
|
||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
from common.dp_common import common_controller_ctrl
|
||||
from common.params import Params
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
@@ -38,17 +39,18 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.p = SteerLimitParams(CP)
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.steer_rate_limited = False
|
||||
self.last_resume_frame = 0
|
||||
|
||||
self.p = SteerLimitParams(CP)
|
||||
|
||||
# dp
|
||||
self.last_blinker_on = False
|
||||
self.blinker_end_frame = 0.
|
||||
self.dp_hkg_smart_mdps = Params().get('dp_hkg_smart_mdps') == b'1'
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart, dragonconf):
|
||||
@@ -61,7 +63,7 @@ class CarController():
|
||||
lkas_active = enabled and abs(CS.out.steeringAngle) < 90.
|
||||
|
||||
# fix for Genesis hard fault at low speed
|
||||
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
|
||||
if not self.dp_hkg_smart_mdps and CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
|
||||
lkas_active = False
|
||||
|
||||
if not lkas_active:
|
||||
@@ -100,7 +102,7 @@ class CarController():
|
||||
self.last_resume_frame = frame
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ]:
|
||||
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV]:
|
||||
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
|
||||
|
||||
return can_sends
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
import copy
|
||||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_HYBRID
|
||||
from selfdrive.car.hyundai.values import CAR, DBC, STEER_THRESHOLD, FEATURES, EV_HYBRID
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
@@ -37,7 +37,7 @@ class CarState(CarStateBase):
|
||||
ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0
|
||||
|
||||
# cruise state
|
||||
ret.cruiseState.available = True
|
||||
ret.cruiseState.available = cp.vl["SCC11"]["MainMode_ACC"] != 0 if self.CP.carFingerprint in [CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.IONIQ] else True
|
||||
ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
|
||||
|
||||
|
||||
@@ -15,11 +15,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
|
||||
values["CF_Lkas_LdwsRHWarning"] = right_lane_depart
|
||||
values["CR_Lkas_StrToqReq"] = apply_steer
|
||||
values["CF_Lkas_ActToi"] = steer_req
|
||||
values["CF_Lkas_ToiFlt"] = 0
|
||||
values["CF_Lkas_MsgCount"] = frame % 0x10
|
||||
values["CF_Lkas_Chksum"] = 0
|
||||
|
||||
if car_fingerprint in [CAR.SONATA, CAR.PALISADE]:
|
||||
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV]:
|
||||
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
|
||||
values["CF_Lkas_LdwsOpt_USM"] = 2
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@ from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from common.dp_common import common_interface_atl, common_interface_get_params_lqr
|
||||
from common.params import Params
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@@ -60,13 +61,6 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerRatio = 13.75 * 1.15
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.KIA_SORENTO:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1985. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.mass = 1275. + STD_CARGO_KG
|
||||
@@ -77,48 +71,17 @@ 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.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
|
||||
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]]
|
||||
elif candidate == CAR.GENESIS_G80:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 2060. + STD_CARGO_KG
|
||||
ret.wheelbase = 3.01
|
||||
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]]
|
||||
elif candidate == CAR.GENESIS_G90:
|
||||
ret.mass = 2200
|
||||
ret.wheelbase = 3.15
|
||||
ret.steerRatio = 12.069
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
|
||||
elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.steerRatio = 13.75
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.KIA_STINGER:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1825. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
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.4
|
||||
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
|
||||
ret.minEnableSpeed = 10 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.KONA:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1275. + STD_CARGO_KG
|
||||
@@ -144,14 +107,6 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.KIA_FORTE:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.steerRatio = 13.75
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.VELOSTER:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
@@ -159,18 +114,93 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerRatio = 13.75 * 1.15
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
|
||||
# Kia
|
||||
elif candidate == CAR.KIA_SORENTO:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1985. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.KIA_NIRO_EV:
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.mass = 1737. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 13.73 # Spec
|
||||
tire_stiffness_factor = 0.385
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.steerRatio = 13.75
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
elif candidate == CAR.KIA_STINGER:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 1825. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
|
||||
elif candidate == CAR.KIA_FORTE:
|
||||
ret.lateralTuning.pid.kf = 0.00005
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.steerRatio = 13.75
|
||||
tire_stiffness_factor = 0.5
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
|
||||
|
||||
# dp
|
||||
ret = common_interface_get_params_lqr(ret)
|
||||
# Genesis
|
||||
elif candidate == CAR.GENESIS_G70:
|
||||
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
|
||||
ret.wheelbase = 3.01
|
||||
ret.steerRatio = 16.5
|
||||
ret.lateralTuning.init('indi')
|
||||
ret.lateralTuning.indi.innerLoopGain = 3.5
|
||||
ret.lateralTuning.indi.outerLoopGain = 2.0
|
||||
ret.lateralTuning.indi.timeConstant = 1.4
|
||||
ret.lateralTuning.indi.actuatorEffectiveness = 2.3
|
||||
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
|
||||
elif candidate == CAR.GENESIS_G90:
|
||||
ret.mass = 2200
|
||||
ret.wheelbase = 3.15
|
||||
ret.steerRatio = 12.069
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
|
||||
|
||||
# these cars require a special panda safety mode due to missing counters and checksums in the messages
|
||||
if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019,
|
||||
CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70]:
|
||||
if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019,
|
||||
CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70]:
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
|
||||
# dp
|
||||
if Params().get('dp_hkg_smart_mdps') == b'1':
|
||||
ret.minSteerSpeed = 0.
|
||||
ret = common_interface_get_params_lqr(ret)
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||
@@ -191,19 +221,24 @@ class CarInterface(CarInterfaceBase):
|
||||
ret = self.CS.update(self.cp, self.cp_cam)
|
||||
# dp
|
||||
self.dragonconf = dragonconf
|
||||
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
|
||||
if ret.vEgo >= self.CP.minSteerSpeed:
|
||||
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
|
||||
events = self.create_common_events(ret)
|
||||
#TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
|
||||
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
self.low_speed_alert = True
|
||||
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
|
||||
self.low_speed_alert = False
|
||||
if self.low_speed_alert:
|
||||
events.add(car.CarEvent.EventName.belowSteerSpeed)
|
||||
if dragonconf.dpAtl:
|
||||
if ret.vEgo < self.CP.minSteerSpeed:
|
||||
events.add(car.CarEvent.EventName.belowSteerSpeed)
|
||||
else:
|
||||
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
|
||||
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
|
||||
self.low_speed_alert = True
|
||||
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
|
||||
self.low_speed_alert = False
|
||||
if self.low_speed_alert:
|
||||
events.add(car.CarEvent.EventName.belowSteerSpeed)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
|
||||
@@ -3,11 +3,12 @@
|
||||
from cereal import car
|
||||
from selfdrive.car import dbc_dict
|
||||
Ecu = car.CarParams.Ecu
|
||||
from common.params import Params
|
||||
|
||||
# Steer torque limits
|
||||
class SteerLimitParams:
|
||||
def __init__(self, CP):
|
||||
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE]:
|
||||
if Params().get('dp_hkg_smart_mdps') == b'1' or CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70]:
|
||||
self.STEER_MAX = 384
|
||||
else:
|
||||
self.STEER_MAX = 255
|
||||
@@ -19,19 +20,12 @@ class SteerLimitParams:
|
||||
|
||||
|
||||
class CAR:
|
||||
# Hyundai
|
||||
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
|
||||
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
|
||||
GENESIS_G70 = "GENESIS G70 2018"
|
||||
GENESIS_G80 = "GENESIS G80 2017"
|
||||
GENESIS_G90 = "GENESIS G90 2017"
|
||||
HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
|
||||
IONIQ = "HYUNDAI IONIQ ELECTRIC PREMIUM SE 2020"
|
||||
IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
|
||||
KIA_FORTE = "KIA FORTE E 2018"
|
||||
KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
|
||||
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
KONA = "HYUNDAI KONA 2020"
|
||||
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
|
||||
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
|
||||
@@ -40,6 +34,19 @@ class CAR:
|
||||
PALISADE = "HYUNDAI PALISADE 2020"
|
||||
VELOSTER = "HYUNDAI VELOSTER 2019"
|
||||
|
||||
# Kia
|
||||
KIA_FORTE = "KIA FORTE E 2018"
|
||||
KIA_NIRO_EV = "KIA NIRO EV 2020"
|
||||
KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
|
||||
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
|
||||
# Genesis
|
||||
GENESIS_G70 = "GENESIS G70 2018"
|
||||
GENESIS_G80 = "GENESIS G80 2017"
|
||||
GENESIS_G90 = "GENESIS G90 2017"
|
||||
|
||||
|
||||
class Buttons:
|
||||
NONE = 0
|
||||
@@ -91,7 +98,7 @@ FINGERPRINTS = {
|
||||
{66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8},
|
||||
],
|
||||
CAR.KIA_OPTIMA: [{
|
||||
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8, 1371: 8, 1397: 8, 1961: 8
|
||||
}],
|
||||
CAR.KIA_SORENTO: [{
|
||||
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 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, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
|
||||
@@ -100,7 +107,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
|
||||
@@ -127,7 +134,10 @@ FINGERPRINTS = {
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 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, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1193: 8, 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, 1378: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8
|
||||
}],
|
||||
CAR.KONA_EV: [{
|
||||
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
|
||||
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8
|
||||
}],
|
||||
CAR.KIA_NIRO_EV: [{
|
||||
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
|
||||
}],
|
||||
CAR.KIA_FORTE: [{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 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, 909: 8, 916: 8, 1040: 8, 1042: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1427: 6, 1456: 4, 1470: 8
|
||||
@@ -151,30 +161,37 @@ 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: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ',
|
||||
b'\xf1\x00DN8_ SCC FHCUP 1.00 1.00 99110-L0000 ',
|
||||
b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ',
|
||||
],
|
||||
(Ecu.esp, 0x7d1, None): [
|
||||
b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02',
|
||||
b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
|
||||
b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'HM6M2_0a0_BD0',
|
||||
b'\xf1\x87391162M003\xf1\xa0000F',
|
||||
b'\xf1\x87391162M003\xf1\xa00240',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03',
|
||||
b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01',
|
||||
b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422',
|
||||
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.00 99211-L0000 190716',
|
||||
b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.01 99211-L0000 191016',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
|
||||
],
|
||||
},
|
||||
@@ -235,11 +252,13 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7e0, None): [b'\x01TJS-JNU06F200H0A', ],
|
||||
(Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ],
|
||||
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', ],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80',
|
||||
b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
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.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ],
|
||||
@@ -253,6 +272,30 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
|
||||
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ],
|
||||
},
|
||||
CAR.KONA_EV: {
|
||||
(Ecu.esp, 0x7D1, None): [b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', ],
|
||||
(Ecu.fwdCamera, 0x7C4, None): [b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', ],
|
||||
(Ecu.eps, 0x7D4, None): [b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ],
|
||||
(Ecu.fwdRadar, 0x7D0, None): [b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 \xf1\xa01.01', ],
|
||||
},
|
||||
CAR.KIA_NIRO_EV: {
|
||||
(Ecu.fwdRadar, 0x7D0, None): [
|
||||
b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 \xf1\xa01.03',
|
||||
b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 \xf1\xa01.00',
|
||||
],
|
||||
(Ecu.esp, 0x7D1, None): [
|
||||
b'\xf1\xa01.06',
|
||||
b'\xf1\xa01.07',
|
||||
],
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105',
|
||||
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7C4, None): [
|
||||
b'\xf1\x00DEE MFC AT USA LHD 1.00 1.03 95740-Q4000 180821',
|
||||
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211',
|
||||
],
|
||||
},
|
||||
CAR.KIA_OPTIMA: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 '],
|
||||
(Ecu.esp, 0x7d1, None): [b'\xf1\x00JF ESC \v 11 \x18\x030 58920-D5180',],
|
||||
@@ -272,15 +315,15 @@ FEATURES = {
|
||||
# which message has the gear
|
||||
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
|
||||
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]),
|
||||
"use_elect_gears": set([CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]),
|
||||
"use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]),
|
||||
|
||||
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
|
||||
"use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]),
|
||||
"use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]),
|
||||
|
||||
"use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA]),
|
||||
}
|
||||
|
||||
EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV])
|
||||
EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
|
||||
|
||||
DBC = {
|
||||
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
|
||||
@@ -292,6 +335,7 @@ DBC = {
|
||||
CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
|
||||
|
||||
@@ -11,6 +11,12 @@ from common.params import Params
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController, CarState):
|
||||
super().__init__(CP, CarController, CarState)
|
||||
|
||||
# dp
|
||||
self.dp_cruise_speed = 0.
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
return float(accel) / 3.0
|
||||
@@ -248,7 +254,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007
|
||||
|
||||
elif candidate == CAR.LEXUS_NXH:
|
||||
elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.66
|
||||
@@ -258,6 +264,16 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.PRIUS_TSS2:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 73
|
||||
ret.wheelbase = 2.70002 #from toyota online sepc.
|
||||
ret.steerRatio = 13.4 # True steerRation from older prius
|
||||
tire_stiffness_factor = 0.6371 # hand-tune
|
||||
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.35], [0.15]]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
elif candidate == CAR.LEXUS_ISH:
|
||||
stop_and_go = True # set to true because it's a hybrid
|
||||
ret.safetyParam = 130
|
||||
@@ -357,8 +373,16 @@ class CarInterface(CarInterfaceBase):
|
||||
# dp
|
||||
self.dragonconf = dragonconf
|
||||
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
|
||||
if dragonconf.dpToyotaLowestCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaLowestCruiseOverrideAt * CV.KPH_TO_MS:
|
||||
ret.cruiseState.speed = dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS
|
||||
if ret.cruiseState.enabled and dragonconf.dpToyotaLowestCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaLowestCruiseOverrideAt * CV.KPH_TO_MS:
|
||||
if dragonconf.dpToyotaLowestCruiseOverrideVego:
|
||||
if self.dp_cruise_speed == 0.:
|
||||
ret.cruiseState.speed = self.dp_cruise_speed = max( dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS,ret.vEgo)
|
||||
else:
|
||||
ret.cruiseState.speed = self.dp_cruise_speed
|
||||
else:
|
||||
ret.cruiseState.speed = dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS
|
||||
else:
|
||||
self.dp_cruise_speed = 0.
|
||||
|
||||
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
|
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
|
||||
|
||||
@@ -13,6 +13,7 @@ class SteerLimitParams:
|
||||
|
||||
class CAR:
|
||||
PRIUS = "TOYOTA PRIUS 2017"
|
||||
PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
|
||||
RAV4H = "TOYOTA RAV4 HYBRID 2017"
|
||||
RAV4 = "TOYOTA RAV4 2017"
|
||||
COROLLA = "TOYOTA COROLLA 2017"
|
||||
@@ -39,6 +40,7 @@ class CAR:
|
||||
LEXUS_CTH = "LEXUS CT 200H 2018"
|
||||
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
|
||||
LEXUS_NXH = "LEXUS NX300H 2018"
|
||||
LEXUS_NX = "LEXUS NX300 2018"
|
||||
LEXUS_ISH = "LEXUS IS300h 2017"
|
||||
LEXUS_GSH = "LEXUS GS450h 2017"
|
||||
LEXUS_NXT = "LEXUS NX200T 2015"
|
||||
@@ -46,24 +48,24 @@ class CAR:
|
||||
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = [
|
||||
(0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
|
||||
(0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
(0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
(0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
(0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
(0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
(0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
|
||||
(0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'),
|
||||
(0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
]
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
@@ -325,6 +327,12 @@ FINGERPRINTS = {
|
||||
CAR.LEXUS_NXH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 822: 8, 824: 8, 835: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 889: 8, 891: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 987: 8, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_NX: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 658: 8, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 810: 2, 812: 3, 818: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1006: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1195: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.PRIUS_TSS2: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 800: 8, 810: 2, 814: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.LEXUS_ISH: [{
|
||||
# IS300H 2017
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767: 4, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
@@ -339,7 +347,7 @@ FINGERPRINTS = {
|
||||
}
|
||||
|
||||
# Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection
|
||||
IGNORED_FINGERPRINTS = [CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2]
|
||||
IGNORED_FINGERPRINTS = [CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, CAR.LEXUS_NX]
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.AVALON: {
|
||||
@@ -370,6 +378,7 @@ FW_VERSIONS = {
|
||||
b'\x018966306Q3100\x00\x00\x00\x00',
|
||||
b'\x018966306Q4000\x00\x00\x00\x00',
|
||||
b'\x018966306Q4100\x00\x00\x00\x00',
|
||||
b'\x018966306Q4200\x00\x00\x00\x00',
|
||||
b'\x018966333P3100\x00\x00\x00\x00',
|
||||
b'\x018966333P3200\x00\x00\x00\x00',
|
||||
b'\x018966333P4200\x00\x00\x00\x00',
|
||||
@@ -379,6 +388,7 @@ FW_VERSIONS = {
|
||||
b'\x018966333P4700\x00\x00\x00\x00',
|
||||
b'\x018966333Q6000\x00\x00\x00\x00',
|
||||
b'\x018966333Q6200\x00\x00\x00\x00',
|
||||
b'\x018966333Q6300\x00\x00\x00\x00',
|
||||
b'\x018966333W6000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
@@ -387,6 +397,7 @@ FW_VERSIONS = {
|
||||
b'8821F0603300 ',
|
||||
b'8821F0607200 ',
|
||||
b'8821F0608000 ',
|
||||
b'8821F0608200 ',
|
||||
b'8821F0609100 ',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
@@ -400,6 +411,7 @@ FW_VERSIONS = {
|
||||
b'8965B33540\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B33542\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B33580\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B33581\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B33621\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791
|
||||
@@ -408,6 +420,7 @@ FW_VERSIONS = {
|
||||
b'8821F0603300 ',
|
||||
b'8821F0607200 ',
|
||||
b'8821F0608000 ',
|
||||
b'8821F0608200 ',
|
||||
b'8821F0609100 ',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
@@ -633,8 +646,10 @@ FW_VERSIONS = {
|
||||
b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
|
||||
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
|
||||
b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
|
||||
b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'\x018965B1255000\x00\x00\x00\x00',
|
||||
b'8965B12361\x00\x00\x00\x00\x00\x00',
|
||||
b'\x018965B12350\x00\x00\x00\x00\x00\x00',
|
||||
b'\x018965B12470\x00\x00\x00\x00\x00\x00',
|
||||
@@ -762,6 +777,7 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x0230E40000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0230EA2000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0230EA2100\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'8821F4702100\x00\x00\x00\x00',
|
||||
@@ -813,23 +829,33 @@ FW_VERSIONS = {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x018966353M7100\x00\x00\x00\x00',
|
||||
b'\x018966353Q2300\x00\x00\x00\x00',
|
||||
b'\x018966353R1100\x00\x00\x00\x00',
|
||||
b'\x018966353R8100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x02353P7000\x00\x00\x00\x00\x00\x00\x00\x00530J5000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F152653330\x00\x00\x00\x00\x00\x00',
|
||||
b'F152653301\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
b'881515306400\x00\x00\x00\x00',
|
||||
b'881515306500\x00\x00\x00\x00',
|
||||
b'881515306200\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B53271\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B53280\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'8821F4702300\x00\x00\x00\x00',
|
||||
b'8821F4702100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
b'8646F5301300\x00\x00\x00\x00',
|
||||
b'8646F5301400\x00\x00\x00\x00',
|
||||
b'8646F5301200\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.PRIUS: {
|
||||
@@ -1005,9 +1031,11 @@ FW_VERSIONS = {
|
||||
b'\x01896634A19100\x00\x00\x00\x00',
|
||||
b'\x01896634A20000\x00\x00\x00\x00',
|
||||
b'\x01896634A22000\x00\x00\x00\x00',
|
||||
b'\x01896634A45000\x00\x00\x00\x00',
|
||||
b'\x01896634A46000\x00\x00\x00\x00',
|
||||
b'\x01F152642551\x00\x00\x00\x00\x00\x00',
|
||||
b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00',
|
||||
b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -1025,6 +1053,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B42170\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B42171\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B42180\x00\x00\x00\x00\x00\x00',
|
||||
b'8965B42181\x00\x00\x00\x00\x00\x00',
|
||||
b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00',
|
||||
b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
|
||||
@@ -1045,6 +1074,7 @@ FW_VERSIONS = {
|
||||
},
|
||||
CAR.RAV4H_TSS2: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896634A15000\x00\x00\x00\x00',
|
||||
b'\x018966342M5000\x00\x00\x00\x00',
|
||||
b'\x018966342X6000\x00\x00\x00\x00',
|
||||
b'\x018966342W8000\x00\x00\x00\x00',
|
||||
@@ -1060,7 +1090,9 @@ FW_VERSIONS = {
|
||||
b'F152642531\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642532\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642521\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642540\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642541\x00\x00\x00\x00\x00\x00',
|
||||
b'F152642542\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B42170\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1151,9 +1183,30 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
|
||||
b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
|
||||
b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.LEXUS_NX: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896637851000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F152678140\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.dsu, 0x791, None): [
|
||||
b'881517803100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B78060\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'8821F4702300\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
b'8646F7801100\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.LEXUS_NXH: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1302,6 +1355,23 @@ FW_VERSIONS = {
|
||||
b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.PRIUS_TSS2: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F152647520\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B47070\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F3301400\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
b'\x028646F4707000\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 100
|
||||
@@ -1334,11 +1404,13 @@ DBC = {
|
||||
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.LEXUS_ISH: dbc_dict('lexus_is300h_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_GSH: dbc_dict('lexus_is300h_2017_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_NXT: dbc_dict('lexus_nxt_2015_pt_generated', 'toyota_adas'),
|
||||
}
|
||||
|
||||
NO_DSU_CAR = set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_NXT])
|
||||
TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])
|
||||
NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_NXT]) # no resume button press required
|
||||
TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2])
|
||||
NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_NXT, CAR.PRIUS_TSS2]) # no resume button press required
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -57,6 +57,7 @@ typedef struct VisionStreamBufs {
|
||||
typedef struct VIPCBufExtra {
|
||||
// only for yuv
|
||||
uint32_t frame_id;
|
||||
uint64_t timestamp_sof;
|
||||
uint64_t timestamp_eof;
|
||||
} VIPCBufExtra;
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
import os
|
||||
from cereal import car, log
|
||||
from common.hardware import HARDWARE
|
||||
from common.numpy_fast import clip
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
|
||||
from common.profiler import Profiler
|
||||
from common.params import Params, put_nonblocking
|
||||
@@ -39,6 +39,10 @@ LaneChangeState = log.PathPlan.LaneChangeState
|
||||
LaneChangeDirection = log.PathPlan.LaneChangeDirection
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
LEAD_AWAY_STATE_OFF = 0
|
||||
LEAD_AWAY_STATE_ON = 1
|
||||
LEAD_AWAY_STATE_ALERTED = 2
|
||||
|
||||
|
||||
class Controls:
|
||||
def __init__(self, sm=None, pm=None, can_sock=None):
|
||||
@@ -54,7 +58,7 @@ class Controls:
|
||||
|
||||
self.sm = sm
|
||||
if self.sm is None:
|
||||
socks = ['thermal', 'health', 'model', 'liveCalibration',
|
||||
socks = ['thermal', 'health', 'model', 'liveCalibration', 'radarState', 'frontFrame',
|
||||
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'dragonConf']
|
||||
ignore_alive = None if params.get('dp_driver_monitor') == b'1' else ['dMonitoringState']
|
||||
self.sm = messaging.SubMaster(socks, ignore_alive=ignore_alive)
|
||||
@@ -108,12 +112,12 @@ class Controls:
|
||||
|
||||
if params.get('dp_lqr') == b'1':
|
||||
self.LaC = LatControlLQR(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'indi':
|
||||
self.LaC = LatControlINDI(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP)
|
||||
|
||||
self.state = State.disabled
|
||||
self.enabled = False
|
||||
@@ -160,6 +164,13 @@ class Controls:
|
||||
self.sm['dragonConf'].dpAtl = False
|
||||
self.sm['dragonConf'].dpCameraOffset = 6
|
||||
|
||||
self.dp_lead_away_alert = params.get('dp_driver_monitor') == b'0' and params.get('dp_steering_monitor') == b'0'
|
||||
self.dp_lead_away_min_speed = 40 # kph
|
||||
self.dp_lead_away_alert_lead_count = 0
|
||||
self.dp_lead_away_alert_nolead_count = 0
|
||||
|
||||
self.dp_lead_away_state = LEAD_AWAY_STATE_OFF
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute carEvents from carState"""
|
||||
|
||||
@@ -247,14 +258,39 @@ class Controls:
|
||||
self.events.add(EventName.relayMalfunction)
|
||||
if self.sm['plan'].fcw:
|
||||
self.events.add(EventName.fcw)
|
||||
if self.sm['model'].frameDropPerc > 1 and (not SIMULATION):
|
||||
self.events.add(EventName.modeldLagging)
|
||||
if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION:
|
||||
self.events.add(EventName.cameraMalfunction)
|
||||
|
||||
if self.sm['model'].frameDropPerc > 20 and not SIMULATION:
|
||||
self.events.add(EventName.modeldLagging)
|
||||
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
if not self.sm['dragonConf'].dpAtl and CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
|
||||
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
|
||||
self.events.add(EventName.noTarget)
|
||||
|
||||
if self.dp_lead_away_alert:
|
||||
current_speed = CS.vEgo * 3.6
|
||||
|
||||
if CS.brakePressed or current_speed < self.dp_lead_away_min_speed or self.dp_lead_away_state == LEAD_AWAY_STATE_ALERTED:
|
||||
self.dp_lead_away_alert_lead_count = 0
|
||||
self.dp_lead_away_alert_nolead_count = 0
|
||||
self.dp_lead_away_state = LEAD_AWAY_STATE_OFF
|
||||
|
||||
if current_speed >= self.dp_lead_away_min_speed:
|
||||
nolead_count = interp(current_speed, [self.dp_lead_away_min_speed, 100], [500, 250])
|
||||
# when car had lead for 5 more secs and lead move away for 3 secs
|
||||
if self.dp_lead_away_state == LEAD_AWAY_STATE_OFF and self.sm['plan'].hasLead:
|
||||
self.dp_lead_away_alert_lead_count += 1
|
||||
elif self.dp_lead_away_state == LEAD_AWAY_STATE_ON and not self.sm['plan'].hasLead:
|
||||
self.dp_lead_away_alert_nolead_count += 1
|
||||
|
||||
if self.dp_lead_away_state == LEAD_AWAY_STATE_OFF and self.dp_lead_away_alert_lead_count >= 300:
|
||||
self.dp_lead_away_state = LEAD_AWAY_STATE_ON
|
||||
elif self.dp_lead_away_state == LEAD_AWAY_STATE_ON and self.dp_lead_away_alert_nolead_count >= nolead_count:
|
||||
self.events.add(EventName.leadCarMoving)
|
||||
self.dp_lead_away_state = LEAD_AWAY_STATE_ALERTED
|
||||
|
||||
# dp lead car moving alert
|
||||
if self.sm['dragonConf'].dpLeadCarAlert:
|
||||
if not self.CP.radarOffCan and self.sm['plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs(self.sm['plan'].vTarget) >= 0:
|
||||
@@ -264,7 +300,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
|
||||
@@ -401,7 +436,7 @@ class Controls:
|
||||
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
|
||||
|
||||
# Gas/Brake PID loop
|
||||
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
|
||||
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP, self.sm)
|
||||
# Steering PID loop and lateral MPC
|
||||
actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)
|
||||
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
import numpy as np
|
||||
from common.numpy_fast import clip, interp
|
||||
|
||||
# dp
|
||||
DP_OFF = 0
|
||||
DP_ECO = 1
|
||||
DP_NORMAL = 2
|
||||
DP_SPORT = 3
|
||||
|
||||
class DynamicGas:
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.candidate = self.CP.carFingerprint
|
||||
self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
|
||||
self.mpc_TR = 1.8
|
||||
self.blinker_status = False
|
||||
self.gas_pressed = False
|
||||
self.dp_profile = DP_OFF
|
||||
|
||||
def update(self, CS, sm):
|
||||
v_ego = CS.vEgo
|
||||
self.handle_passable(CS, sm)
|
||||
|
||||
current_dp_profile = sm['dragonConf'].dpAccelProfile
|
||||
if self.dp_profile != current_dp_profile:
|
||||
self.set_profile()
|
||||
self.dp_profile = current_dp_profile
|
||||
|
||||
if self.dp_profile == DP_OFF:
|
||||
return float(interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV))
|
||||
|
||||
|
||||
gas = interp(v_ego, self.gasMaxBP, self.gasMaxV)
|
||||
if self.lead_data['status']: # if lead
|
||||
x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481] # relative velocity mod
|
||||
y = [0.9901, 0.905, 0.8045, 0.625, 0.431, 0.2083, .0667, 0]
|
||||
gas_mod = -(gas * interp(self.lead_data['v_rel'], x, y))
|
||||
|
||||
x = [0.44704, 1.1176, 1.34112] # lead accel mod
|
||||
y = [1.0, 0.75, 0.625] # maximum we can reduce gas_mod is 40 percent (never increases mod)
|
||||
gas_mod *= interp(self.lead_data['a_lead'], x, y)
|
||||
|
||||
# as lead gets further from car, lessen gas mod/reduction
|
||||
x = [(i+v_ego) for i in self.x_lead_mod_x ]
|
||||
gas_mod *= interp(self.lead_data['x_lead'],x , self.x_lead_mod_y)
|
||||
gas = gas + gas_mod
|
||||
|
||||
if (self.blinker_status and self.lead_data['v_rel'] >= 0 ):
|
||||
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
|
||||
y = [1.0, 1.115, 1.225]
|
||||
gas *= interp(v_ego, x, y)
|
||||
|
||||
return float(clip(gas, 0.0, 1.0))
|
||||
|
||||
def set_profile(self):
|
||||
self.x_lead_mod_y = [1.0, 0.75, 0.5, 0.25, 0.0] # as lead gets further from car, lessen gas mod/reduction
|
||||
x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326, 40]
|
||||
if self.dp_profile == DP_ECO:
|
||||
#km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144]
|
||||
y = [0.45, 0.42, 0.38, 0.33, 0.3, 0.32, 0.31, 0.30, 0.30, 0.28, 0.24, 0.21, 0.20, 0.20, 0.19, 0.19, 0.17, 0.15]
|
||||
self.x_lead_mod_x = [8.1, 12.15, 25.24, 35 , 50 ]
|
||||
elif self.dp_profile == DP_SPORT:
|
||||
#km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144]
|
||||
y = [0.65, 0.67, 0.63, 0.50, 0.53, 0.53, 0.5229, 0.51784, 0.50765, 0.48, 0.496, 0.509, 0.525, 0.538, 0.45, 0.421, 0.42,0.35]
|
||||
self.x_lead_mod_x = [4.1, 6.15, 8.24, 10 , 15 ]
|
||||
else:
|
||||
#km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144]
|
||||
y = [0.55, 0.57, 0.43, 0.35, 0.33, 0.33, 0.3529, 0.36784, 0.37765, 0.38, 0.396, 0.309, 0.325, 0.358, 0.32, 0.301, 0.29,0.25]
|
||||
self.x_lead_mod_x = [7.1, 10.15, 12.24, 15 , 20 ]
|
||||
|
||||
y = [interp(i, [0.2, (0.2 + 0.45) / 2, 0.45], [1.075 * i, i * 1.05, i]) for i in y]
|
||||
self.gasMaxBP, self.gasMaxV = x, y
|
||||
|
||||
def handle_passable(self, CS, sm):
|
||||
self.blinker_status = CS.leftBlinker or CS.rightBlinker
|
||||
self.gas_pressed = CS.gasPressed
|
||||
lead_one = sm['radarState'].leadOne
|
||||
self.lead_data['v_rel'] = lead_one.vRel
|
||||
self.lead_data['a_lead'] = lead_one.aLeadK
|
||||
self.lead_data['x_lead'] = lead_one.dRel
|
||||
self.lead_data['status'] = sm['plan'].hasLead # this fixes radarstate always reporting a lead, thanks to arne
|
||||
@@ -485,6 +485,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Fan Malfunction"), _("Contact Support")),
|
||||
},
|
||||
|
||||
EventName.cameraMalfunction: {
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Camera Malfunction"), _("Contact Support")),
|
||||
},
|
||||
|
||||
# ********** events that affect controls state transitions **********
|
||||
|
||||
EventName.pcmEnable: {
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
from cereal import log
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.controls.lib.pid import PIController
|
||||
from common.params import Params
|
||||
from selfdrive.controls.lib.dynamic_gas import DynamicGas
|
||||
|
||||
LongCtrlState = log.ControlsState.LongControlState
|
||||
|
||||
@@ -62,18 +64,27 @@ class LongControl():
|
||||
convert=compute_gb)
|
||||
self.v_pid = 0.0
|
||||
self.last_output_gb = 0.0
|
||||
#dynamic_gas
|
||||
params = Params()
|
||||
self.dp_dynamic_gas = (params.get('dp_dynamic_gas') == b'1')
|
||||
if self.dp_dynamic_gas:
|
||||
self.dynamic_gas = DynamicGas(CP)
|
||||
|
||||
def reset(self, v_pid):
|
||||
"""Reset PID controller and change setpoint"""
|
||||
self.pid.reset()
|
||||
self.v_pid = v_pid
|
||||
|
||||
def update(self, active, CS, v_target, v_target_future, a_target, CP):
|
||||
def update(self, active, CS, v_target, v_target_future, a_target, CP, sm):
|
||||
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
|
||||
# Actuation limits
|
||||
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
|
||||
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
|
||||
|
||||
#dynamic_gas
|
||||
if self.dp_dynamic_gas:
|
||||
gas_max = self.dynamic_gas.update(CS, sm)
|
||||
|
||||
# Update state machine
|
||||
output_gb = self.last_output_gb
|
||||
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -18,7 +18,6 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||
MAX_SPEED = 255.0
|
||||
|
||||
LON_MPC_STEP = 0.2 # first step is 0.2s
|
||||
MAX_SPEED_ERROR = 2.0
|
||||
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
|
||||
|
||||
# lookup tables VS speed to determine min and max accels in cruise
|
||||
|
||||
@@ -62,4 +62,8 @@ class Dashcam():
|
||||
pass
|
||||
|
||||
def get_used_spaces(self):
|
||||
return sum(os.path.getsize(DASHCAM_VIDEOS_PATH + f) for f in os.listdir(DASHCAM_VIDEOS_PATH) if os.path.isfile(DASHCAM_VIDEOS_PATH + f))
|
||||
try:
|
||||
val = sum(os.path.getsize(DASHCAM_VIDEOS_PATH + f) for f in os.listdir(DASHCAM_VIDEOS_PATH) if os.path.isfile(DASHCAM_VIDEOS_PATH + f))
|
||||
except (IndexError, FileNotFoundError, OSError):
|
||||
val = 0
|
||||
return val
|
||||
|
||||
@@ -196,14 +196,14 @@ class Localizer():
|
||||
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
|
||||
orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
|
||||
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
|
||||
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
|
||||
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
|
||||
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
|
||||
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
|
||||
self.reset_kalman(init_orient=initial_pose_ecef_quat)
|
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
|
||||
elif gps_est_error > 50:
|
||||
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
|
||||
self.reset_kalman()
|
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
|
||||
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
|
||||
@@ -267,12 +267,14 @@ class Localizer():
|
||||
self.calib_from_device = self.device_from_calib.T
|
||||
self.calibrated = log.calStatus == 1
|
||||
|
||||
def reset_kalman(self, current_time=None, init_orient=None):
|
||||
def reset_kalman(self, current_time=None, init_orient=None, init_pos=None):
|
||||
self.filter_time = current_time
|
||||
init_x = LiveKalman.initial_x.copy()
|
||||
# too nonlinear to init on completely wrong
|
||||
if init_orient is not None:
|
||||
init_x[3:7] = init_orient
|
||||
if init_pos is not None:
|
||||
init_x[:3] = init_pos
|
||||
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
|
||||
|
||||
self.observation_buffer = []
|
||||
|
||||
@@ -67,7 +67,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
|
||||
.stream_type = VISION_STREAM_YUV,
|
||||
.filename = "fcamera.hevc",
|
||||
.frame_packet_name = "frame",
|
||||
.encode_idx_name = "encodeIdx",
|
||||
.fps = MAIN_FPS,
|
||||
.bitrate = MAIN_BITRATE,
|
||||
.is_h265 = true,
|
||||
@@ -78,7 +77,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
|
||||
.stream_type = VISION_STREAM_YUV_FRONT,
|
||||
.filename = "dcamera.hevc",
|
||||
.frame_packet_name = "frontFrame",
|
||||
.encode_idx_name = "frontEncodeIdx",
|
||||
.fps = MAIN_FPS, // on EONs, more compressed this way
|
||||
.bitrate = DCAM_BITRATE,
|
||||
.is_h265 = true,
|
||||
@@ -89,7 +87,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
|
||||
.stream_type = VISION_STREAM_YUV_WIDE,
|
||||
.filename = "ecamera.hevc",
|
||||
.frame_packet_name = "wideFrame",
|
||||
.encode_idx_name = "wideEncodeIdx",
|
||||
.fps = MAIN_FPS,
|
||||
.bitrate = MAIN_BITRATE,
|
||||
.is_h265 = true,
|
||||
@@ -235,9 +232,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
|
||||
s.num_encoder += 1;
|
||||
pthread_mutex_unlock(&s.rotate_lock);
|
||||
|
||||
PubSocket *idx_sock = PubSocket::create(s.ctx, cameras_logged[cam_idx].encode_idx_name);
|
||||
assert(idx_sock != NULL);
|
||||
|
||||
LoggerHandle *lh = NULL;
|
||||
|
||||
while (!do_exit) {
|
||||
@@ -377,8 +371,12 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
|
||||
|
||||
// publish encode index
|
||||
MessageBuilder msg;
|
||||
auto eidx = msg.initEvent().initEncodeIdx();
|
||||
// this is really ugly
|
||||
auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() :
|
||||
(cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx());
|
||||
eidx.setFrameId(extra.frame_id);
|
||||
eidx.setTimestampSof(extra.timestamp_sof);
|
||||
eidx.setTimestampEof(extra.timestamp_eof);
|
||||
#ifdef QCOM2
|
||||
eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C);
|
||||
#else
|
||||
@@ -390,10 +388,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
|
||||
eidx.setSegmentId(out_id);
|
||||
|
||||
auto bytes = msg.toBytes();
|
||||
|
||||
if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) {
|
||||
printf("err sending encodeIdx pkt: %s\n", strerror(errno));
|
||||
}
|
||||
if (lh) {
|
||||
lh_log(lh, bytes.begin(), bytes.size(), false);
|
||||
}
|
||||
@@ -453,8 +447,6 @@ void encoder_thread(RotateState *rotate_state, bool raw_clips, int cam_idx) {
|
||||
visionstream_destroy(&stream);
|
||||
}
|
||||
|
||||
delete idx_sock;
|
||||
|
||||
if (encoder_inited) {
|
||||
LOG("encoder destroy");
|
||||
encoder_close(&encoder);
|
||||
@@ -606,6 +598,10 @@ static void bootlog() {
|
||||
int main(int argc, char** argv) {
|
||||
int err;
|
||||
|
||||
#ifdef QCOM
|
||||
setpriority(PRIO_PROCESS, 0, -12);
|
||||
#endif
|
||||
|
||||
if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) {
|
||||
bootlog();
|
||||
return 0;
|
||||
@@ -620,8 +616,6 @@ int main(int argc, char** argv) {
|
||||
record_front = Params().read_db_bool("RecordFront");
|
||||
#endif
|
||||
|
||||
setpriority(PRIO_PROCESS, 0, -12);
|
||||
|
||||
clear_locks();
|
||||
|
||||
signal(SIGINT, (sighandler_t)set_do_exit);
|
||||
@@ -751,7 +745,7 @@ int main(int argc, char** argv) {
|
||||
|
||||
if (s.logger.part > -1) {
|
||||
new_segment = true;
|
||||
if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE) {
|
||||
if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && s.num_encoder > 0) {
|
||||
for (int cid=0;cid<=MAX_CAM_IDX;cid++) {
|
||||
// this *should* be redundant on tici since all camera frames are synced
|
||||
new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) &&
|
||||
|
||||
+12
-3
@@ -127,7 +127,8 @@ if not prebuilt:
|
||||
print("....%d" % i)
|
||||
time.sleep(1)
|
||||
subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
|
||||
shutil.rmtree("/tmp/scons_cache")
|
||||
shutil.rmtree("/tmp/scons_cache", ignore_errors=True)
|
||||
shutil.rmtree("/data/scons_cache", ignore_errors=True)
|
||||
else:
|
||||
print("scons build failed after retry")
|
||||
sys.exit(1)
|
||||
@@ -604,9 +605,17 @@ def main():
|
||||
|
||||
# dp
|
||||
del managed_processes['tombstoned']
|
||||
if params.get("dp_logger") == b'0' or \
|
||||
steering_monitor = params.get("dp_steering_monitor") == b'1'
|
||||
if not steering_monitor and params.get("dp_driver_monitor") == b'0':
|
||||
del managed_processes['loggerd']
|
||||
del managed_processes['logmessaged']
|
||||
del managed_processes['proclogd']
|
||||
del managed_processes['logcatd']
|
||||
del managed_processes['dmonitoringd']
|
||||
del managed_processes['dmonitoringmodeld']
|
||||
elif params.get("dp_logger") == b'0' or \
|
||||
params.get("dp_atl") == b'1' or \
|
||||
params.get("dp_steering_monitor") == b'0':
|
||||
not steering_monitor:
|
||||
del managed_processes['loggerd']
|
||||
del managed_processes['logmessaged']
|
||||
del managed_processes['proclogd']
|
||||
|
||||
@@ -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":
|
||||
@@ -19,16 +19,20 @@ if arch == "aarch64":
|
||||
lenv['CFLAGS'].append("-DUSE_THNEED")
|
||||
lenv['CXXFLAGS'].append("-DUSE_THNEED")
|
||||
elif arch == "larch64":
|
||||
libs += ['gsl', 'CB', 'pthread']
|
||||
libs += ['gsl', 'CB', 'pthread', 'dl']
|
||||
if not TEST_THNEED:
|
||||
common_src += ["thneed/thneed.cc"]
|
||||
lenv['CFLAGS'].append("-DUSE_THNEED")
|
||||
lenv['CXXFLAGS'].append("-DUSE_THNEED")
|
||||
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
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
#include <unistd.h>
|
||||
#include <signal.h>
|
||||
#include <cassert>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include "common/visionbuf.h"
|
||||
#include "common/visionipc.h"
|
||||
@@ -23,7 +24,7 @@ static void set_do_exit(int sig) {
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
int err;
|
||||
set_realtime_priority(51);
|
||||
setpriority(PRIO_PROCESS, 0, -15);
|
||||
|
||||
#ifdef QCOM2
|
||||
set_core_affinity(5);
|
||||
@@ -65,7 +66,7 @@ int main(int argc, char **argv) {
|
||||
double t2 = millis_since_boot();
|
||||
|
||||
// send dm packet
|
||||
dmonitoring_publish(pm, extra.frame_id, res);
|
||||
dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0);
|
||||
|
||||
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
|
||||
last = t1;
|
||||
|
||||
+17
-12
@@ -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)
|
||||
@@ -148,7 +147,7 @@ int main(int argc, char **argv) {
|
||||
|
||||
// setup filter to track dropped frames
|
||||
const float dt = 1. / MODEL_FREQ;
|
||||
const float ts = 5.0; // 5 s filter time constant
|
||||
const float ts = 10.0; // filter time constant (s)
|
||||
const float frame_filter_k = (dt / ts) / (1. + dt / ts);
|
||||
float frames_dropped = 0;
|
||||
|
||||
@@ -158,6 +157,7 @@ int main(int argc, char **argv) {
|
||||
uint32_t frame_id = 0, last_vipc_frame_id = 0;
|
||||
double last = 0;
|
||||
int desire = -1;
|
||||
uint32_t run_count = 0;
|
||||
while (!do_exit) {
|
||||
VIPCBuf *buf;
|
||||
VIPCBufExtra extra;
|
||||
@@ -174,12 +174,14 @@ 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();
|
||||
}
|
||||
|
||||
double mt1 = 0, mt2 = 0;
|
||||
if (run_model_this_iter) {
|
||||
run_count++;
|
||||
|
||||
float vec_desire[DESIRE_LEN] = {0};
|
||||
if (desire >= 0 && desire < DESIRE_LEN) {
|
||||
vec_desire[desire] = 1.0;
|
||||
@@ -194,16 +196,19 @@ int main(int argc, char **argv) {
|
||||
model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height,
|
||||
model_transform, NULL, vec_desire);
|
||||
mt2 = millis_since_boot();
|
||||
float model_execution_time = (mt2 - mt1) / 1000.0;
|
||||
|
||||
// tracked dropped frames
|
||||
uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
|
||||
frames_dropped = (1. - frame_filter_k) * frames_dropped + frame_filter_k * (float)std::min(vipc_dropped_frames, 10U);
|
||||
float frame_drop_perc = frames_dropped / MODEL_FREQ;
|
||||
if (run_count < 10) frames_dropped = 0; // let frame drops warm up
|
||||
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
|
||||
|
||||
model_publish(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);
|
||||
model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
|
||||
model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time);
|
||||
posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, 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);
|
||||
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_ratio);
|
||||
last = mt1;
|
||||
last_vipc_frame_id = extra.frame_id;
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -114,24 +114,39 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
|
||||
resized_width, resized_height,
|
||||
mode);
|
||||
|
||||
// prerotate to be cache aware
|
||||
uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2);
|
||||
uint8_t *resized_y_buf_rot = resized_buf_rot;
|
||||
uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height);
|
||||
uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2));
|
||||
|
||||
libyuv::I420Rotate(resized_y_buf, resized_width,
|
||||
resized_u_buf, resized_width/2,
|
||||
resized_v_buf, resized_width/2,
|
||||
resized_y_buf_rot, resized_height,
|
||||
resized_u_buf_rot, resized_height/2,
|
||||
resized_v_buf_rot, resized_height/2,
|
||||
// negative height causes a vertical flip to match previous
|
||||
resized_width, -resized_height, libyuv::kRotate90);
|
||||
|
||||
int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
|
||||
float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
|
||||
// one shot conversion, O(n) anyway
|
||||
// yuvframe2tensor, normalize
|
||||
for (int r = 0; r < MODEL_HEIGHT/2; r++) {
|
||||
for (int c = 0; c < MODEL_WIDTH/2; c++) {
|
||||
for (int c = 0; c < MODEL_WIDTH/2; c++) {
|
||||
for (int r = 0; r < MODEL_HEIGHT/2; r++) {
|
||||
// Y_ul
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]);
|
||||
// Y_ur
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]);
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]);
|
||||
// Y_dl
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]);
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]);
|
||||
// Y_ur
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]);
|
||||
// Y_dr
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]);
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]);
|
||||
// U
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]);
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]);
|
||||
// V
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]);
|
||||
net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -140,6 +155,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
|
||||
//fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file);
|
||||
//fclose(dump_yuv_file);
|
||||
|
||||
// *** testing ***
|
||||
// idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320)
|
||||
// imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420))
|
||||
|
||||
//FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb");
|
||||
//fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
|
||||
//fclose(dump_yuv_file2);
|
||||
@@ -165,11 +184,12 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
|
||||
return ret;
|
||||
}
|
||||
|
||||
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res){
|
||||
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){
|
||||
// make msg
|
||||
MessageBuilder msg;
|
||||
auto framed = msg.initEvent().initDriverState();
|
||||
framed.setFrameId(frame_id);
|
||||
framed.setModelExecutionTime(execution_time);
|
||||
|
||||
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
|
||||
kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));
|
||||
|
||||
@@ -29,6 +29,7 @@ typedef struct DMonitoringModelState {
|
||||
bool is_rhd;
|
||||
float output[OUTPUT_SIZE];
|
||||
std::vector<uint8_t> resized_buf;
|
||||
std::vector<uint8_t> resized_buf_rot;
|
||||
std::vector<uint8_t> cropped_buf;
|
||||
std::vector<uint8_t> premirror_cropped_buf;
|
||||
std::vector<float> net_input_buf;
|
||||
@@ -36,7 +37,7 @@ typedef struct DMonitoringModelState {
|
||||
|
||||
void dmonitoring_init(DMonitoringModelState* s);
|
||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
|
||||
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res);
|
||||
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time);
|
||||
void dmonitoring_free(DMonitoringModelState* s);
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
@@ -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,91 +199,275 @@ 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,
|
||||
float model_execution_time) {
|
||||
// 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);
|
||||
framed.setModelExecutionTime(model_execution_time);
|
||||
|
||||
// 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,
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
|
||||
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||
uint32_t vipc_dropped_frames, float frame_drop,
|
||||
const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
|
||||
float model_execution_time) {
|
||||
|
||||
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||
MessageBuilder msg;
|
||||
auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel();
|
||||
framed.setFrameId(vipc_frame_id);
|
||||
framed.setFrameAge(frame_age);
|
||||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
framed.setModelExecutionTime(model_execution_time);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void posenet_publish(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) {
|
||||
uint32_t vipc_dropped_frames, float frame_drop,
|
||||
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
|
||||
float trans_arr[3];
|
||||
float trans_std_arr[3];
|
||||
float rot_arr[3];
|
||||
@@ -280,10 +475,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;
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
@@ -71,7 +78,12 @@ void model_free(ModelState* s);
|
||||
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);
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
|
||||
uint64_t timestamp_eof, float model_execution_time);
|
||||
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, float model_execution_time);
|
||||
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);
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
|
||||
uint64_t timestamp_eof);
|
||||
#endif
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#pragma clang diagnostic ignored "-Wexceptions"
|
||||
|
||||
#include <cassert>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "common/util.h"
|
||||
#include "snpemodel.h"
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
#include "thneed.h"
|
||||
#include <cassert>
|
||||
#include <sys/mman.h>
|
||||
#include <dlfcn.h>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include "thneed.h"
|
||||
|
||||
Thneed *g_thneed = NULL;
|
||||
int g_fd = -1;
|
||||
@@ -31,12 +32,20 @@ extern "C" {
|
||||
int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL;
|
||||
#undef ioctl
|
||||
int ioctl(int filedes, unsigned long request, void *argp) {
|
||||
request &= 0xFFFFFFFF; // needed on QCOM2
|
||||
if (my_ioctl == NULL) my_ioctl = reinterpret_cast<decltype(my_ioctl)>(dlsym(RTLD_NEXT, "ioctl"));
|
||||
Thneed *thneed = g_thneed;
|
||||
|
||||
// save the fd
|
||||
if (request == IOCTL_KGSL_GPUOBJ_ALLOC) g_fd = filedes;
|
||||
|
||||
if (request == IOCTL_KGSL_DRAWCTXT_CREATE) {
|
||||
struct kgsl_drawctxt_create *create = (struct kgsl_drawctxt_create *)argp;
|
||||
create->flags &= ~KGSL_CONTEXT_PRIORITY_MASK;
|
||||
create->flags |= 1 << KGSL_CONTEXT_PRIORITY_SHIFT; // priority from 1-15, 1 is max priority
|
||||
printf("creating context with flags 0x%x\n", create->flags);
|
||||
}
|
||||
|
||||
if (thneed != NULL) {
|
||||
if (request == IOCTL_KGSL_GPU_COMMAND) {
|
||||
struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp;
|
||||
@@ -440,7 +449,14 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c
|
||||
#endif
|
||||
|
||||
void *dlsym(void *handle, const char *symbol) {
|
||||
// TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so
|
||||
#if defined QCOM
|
||||
void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4);
|
||||
#elif defined QCOM2
|
||||
void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138);
|
||||
#else
|
||||
#error "Unsupported platform for thneed"
|
||||
#endif
|
||||
if (memcmp("REAL_", symbol, 5) == 0) {
|
||||
return my_dlsym(handle, symbol+5);
|
||||
} else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) {
|
||||
|
||||
@@ -1,8 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef __user
|
||||
#define __user __attribute__(())
|
||||
#endif
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include "include/msm_kgsl.h"
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <CL/cl.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
@@ -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);
|
||||
@@ -1,16 +1,19 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from common.realtime import set_core_affinity, set_realtime_priority
|
||||
|
||||
|
||||
# RT shield - ensure CPU 3 always remains available for RT processes
|
||||
# runs as SCHED_FIFO with minimum priority to ensure kthreads don't
|
||||
# get scheduled onto CPU 3, but it's always preemptible by realtime
|
||||
# openpilot processes
|
||||
|
||||
def main():
|
||||
set_core_affinity(3)
|
||||
set_core_affinity(int(os.getenv("CORE", "3")))
|
||||
set_realtime_priority(1)
|
||||
|
||||
while True:
|
||||
time.sleep(0.000001)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -410,7 +410,6 @@ def thermald_thread():
|
||||
if started_ts is None:
|
||||
started_ts = sec_since_boot()
|
||||
started_seen = True
|
||||
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
|
||||
else:
|
||||
if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
|
||||
cloudlog.event("Startup blocked", startup_conditions=startup_conditions)
|
||||
@@ -420,7 +419,6 @@ def thermald_thread():
|
||||
started_ts = None
|
||||
if off_ts is None:
|
||||
off_ts = sec_since_boot()
|
||||
os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')
|
||||
|
||||
# Offroad power monitoring
|
||||
pm.calculate(health)
|
||||
|
||||
+81
-169
@@ -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 = 0;
|
||||
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) {
|
||||
|
||||
+20
-13
@@ -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();
|
||||
|
||||
+24
-13
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user