diff --git a/CHANGELOGS-DEV.md b/CHANGELOGS-DEV.md index 61c6ce921..8c3452afa 100644 --- a/CHANGELOGS-DEV.md +++ b/CHANGELOGS-DEV.md @@ -1,3 +1,14 @@ +dragonpilot 0.7.7.1 +======================== +* 加入 C2 風扇靜音模式。(感謝 @dingliangxue) +* Added C2 quiet fan mode. (Thanks to @dingliangxue) +* 加入「輔助換道最低啟動速度」、「自動換道最低啟動速度」設定。 +* Added "Assisted Lane Change Min Engage Speed" and "Auto Lane Change Min Engage Speed" settings. +* 加入回調校介面。(感謝 @Kent) +* Re-added Dev UI. (Thanks to @Kent) +* 加入 "dp_lqr" 設定來強制使用 RAV4 的 lqr 調校。(感謝 @eisenheim) +* Added "dp_lqr" setting to force enable lqr tuning from RAV4. (Thanks to eisenheim) + dragonpilot 0.7.7.0 ======================== * 基於最新 openpilot 0.7.7 devel. diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 7f52be36e..1671ae7f7 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,3 +1,29 @@ +2020-07-28 (0.7.7.0) +======================== +* 修正 steer ratio learner 關閉。(感謝 @Mojo 回報, @ShaneSmiskol 提供代碼) +* Fixed steer ratio learner toggle. (Thanks to @Mojo, @ShaneSmiskol) +* 加入 "dp_lqr" 設定來強制使用 RAV4 的 lqr 調校。(感謝 @eisenheim) +* Added "dp_lqr" setting to force enable lqr tuning from RAV4. (Thanks to eisenheim) + +2020-07-28 (0.7.7.0) +======================== +* 修正無法上傳記錄的問題。(感謝 @Mojo) +* Fixed unable to upload log issue. (Thanks to @Mojo) +* 修正無法關閉警示音的問題。(感謝 @Mojo) +* Fixed unable to disable audio alert (-100%) issue. ($Thanks to @Mojo) + +2020-07-27 (0.7.7.0) +======================== +* 加入回調校介面。(感謝 @Kent) +* Re-added Dev UI. (Thanks to @Kent) + +2020-07-27 (0.7.7.0) +======================== +* 加入 C2 風扇靜音模式。(感謝 @dingliangxue) +* Added C2 quiet fan mode. (Thanks to @dingliangxue) +* 加入「輔助換道最低啟動速度」、「自動換道最低啟動速度」設定。 +* Added "Assisted Lane Change Min Engage Speed" and "Auto Lane Change Min Engage Speed" settings. + 2020-07-23 (0.7.7.0) ======================== * 修正 appd。(感謝 @cgw1968) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index ce9038a85..2679ffe0e 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/common/dp_common.py b/common/dp_common.py index bc87031a5..54e2e4d52 100644 --- a/common/dp_common.py +++ b/common/dp_common.py @@ -26,3 +26,17 @@ def common_interface_atl(ret, atl): if ret.seatbeltUnlatched or ret.doorOpen: enable_acc = False return enable_acc + +def common_interface_get_params_lqr(ret): + if params.get('dp_lqr') == b'1': + ret.lateralTuning.init('lqr') + ret.lateralTuning.lqr.scale = 1500.0 + ret.lateralTuning.lqr.ki = 0.05 + + ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] + ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] + ret.lateralTuning.lqr.c = [1., 0.] + ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] + ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] + ret.lateralTuning.lqr.dcGain = 0.002237852961363602 + return ret \ No newline at end of file diff --git a/common/dp_conf.py b/common/dp_conf.py index e3b3dc0fc..8916d8b2a 100644 --- a/common/dp_conf.py +++ b/common/dp_conf.py @@ -89,6 +89,7 @@ confs = [ #misc {'name': 'dp_ip_addr', 'default': '', 'type': 'Text', 'conf_type': ['struct']}, {'name': 'dp_full_speed_fan', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, + {'name': 'dp_uno_fan_mode', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, {'name': 'dp_last_modified', 'default': str(floor(time.time())), 'type': 'Text', 'conf_type': ['param']}, {'name': 'dp_camera_offset', 'default': 6, 'type': 'Int8', 'min': -255, 'max': 255, 'conf_type': ['param', 'struct']}, @@ -101,6 +102,7 @@ confs = [ {'name': 'dp_is_updating', 'default': False, 'type': 'Bool', 'set_param_only': True, 'conf_type': ['param', 'struct']}, {'name': 'dp_sr_learner', 'default': True, 'type': 'Bool', 'conf_type': ['param']}, + {'name': 'dp_lqr', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, # including thermal data {'name': 'dp_thermal_started', 'default': False, 'type': 'Bool', 'conf_type': ['struct']}, diff --git a/panda/board/board.h b/panda/board/board.h index ba5920bc5..84fca5469 100644 --- a/panda/board/board.h +++ b/panda/board/board.h @@ -18,18 +18,12 @@ #include "boards/pedal.h" #endif -//#define DP_USE_DOS 1 - void detect_board_type(void) { #ifdef PANDA // SPI lines floating: white (TODO: is this reliable? Not really, we have to enable ESP/GPS to be able to detect this on the UART) set_gpio_output(GPIOC, 14, 1); set_gpio_output(GPIOC, 5, 1); - #ifdef DP_USE_DOS - if(!detect_with_pull(GPIOB, 1, PULL_UP)){ - #else - if (false) { - #endif + if(!detect_with_pull(GPIOB, 1, PULL_UP) && detect_with_pull(GPIOB, 15, PULL_UP)){ hw_type = HW_TYPE_DOS; current_board = &board_dos; } else if((detect_with_pull(GPIOA, 4, PULL_DOWN)) || (detect_with_pull(GPIOA, 5, PULL_DOWN)) || (detect_with_pull(GPIOA, 6, PULL_DOWN)) || (detect_with_pull(GPIOA, 7, PULL_DOWN))){ diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 67d72e0df..42c450a58 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -3,7 +3,7 @@ from cereal import car from selfdrive.car.chrysler.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 +from common.dp_common import common_interface_atl, common_interface_get_params_lqr class CarInterface(CarInterfaceBase): @staticmethod @@ -40,6 +40,9 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 12.7 ret.steerActuatorDelay = 0.2 # in seconds + # dp + ret = common_interface_get_params_lqr(ret) + ret.centerToFront = ret.wheelbase * 0.44 ret.minSteerSpeed = 3.8 # m/s diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 138023df9..19e0b007a 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.car.ford.values import MAX_ANGLE, Ecu, ECU_FINGERPRINT, 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 +from common.dp_common import common_interface_atl, common_interface_get_params_lqr class CarInterface(CarInterfaceBase): @@ -32,6 +32,9 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 + # dp + 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) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 51ff17642..208266a49 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -5,7 +5,7 @@ from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \ AccState, 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 +from common.dp_common import common_interface_atl, common_interface_get_params_lqr ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -93,6 +93,9 @@ class CarInterface(CarInterfaceBase): ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.49 + # dp + 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) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ab8e4dfcb..5eea9d821 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -10,7 +10,7 @@ from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, Ecu, ECU from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint 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 +from common.dp_common import common_interface_atl, common_interface_get_params_lqr A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -394,6 +394,9 @@ class CarInterface(CarInterfaceBase): else: raise ValueError("unsupported car %s" % candidate) + # dp + ret = common_interface_get_params_lqr(ret) + # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 161f52967..982257c1c 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV 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 +from common.dp_common import common_interface_atl, common_interface_get_params_lqr class CarInterface(CarInterfaceBase): @@ -145,6 +145,9 @@ 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]] + # dp + ret = common_interface_get_params_lqr(ret) + # 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]: ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index dce15fcd5..b1488d9d9 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, FINGERPRINTS, ECU_FINGERPRINT, Ecu from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl +from common.dp_common import common_interface_atl, common_interface_get_params_lqr ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -47,6 +47,9 @@ class CarInterface(CarInterfaceBase): # No steer below disable speed ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS + # dp + ret = common_interface_get_params_lqr(ret) + ret.centerToFront = ret.wheelbase * 0.41 # TODO: get actual value, for now starting with reasonable value for diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 39c42e786..b8af252d3 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -5,7 +5,6 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl # mocked car interface to work with chffrplus TS = 0.01 # 100Hz diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 1f68f8e3f..f5d2389dc 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -3,7 +3,7 @@ from cereal import car from selfdrive.car.nissan.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl +from common.dp_common import common_interface_atl, common_interface_get_params_lqr class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): @@ -46,6 +46,9 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 17 + # dp + ret = common_interface_get_params_lqr(ret) + ret.steerControlType = car.CarParams.SteerControlType.angle ret.radarOffCan = True diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 54bd7270f..84380a98d 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,7 +3,7 @@ from cereal import car from selfdrive.car.subaru.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl +from common.dp_common import common_interface_atl, common_interface_get_params_lqr class CarInterface(CarInterfaceBase): @@ -59,6 +59,9 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] + # dp + 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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a4d5882a7..64cd38a86 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -5,8 +5,7 @@ from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FIN from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl -from common.params import Params +from common.dp_common import common_interface_atl, common_interface_get_params_lqr EventName = car.CarEvent.EventName @@ -288,6 +287,9 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00006 + # dp + ret = common_interface_get_params_lqr(ret) + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index ea38c5b31..979f5dbbe 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -3,7 +3,7 @@ from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, NWL, TRANS, GEAR from common.params import put_nonblocking from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -from common.dp_common import common_interface_atl +from common.dp_common import common_interface_atl, common_interface_get_params_lqr EventName = car.CarEvent.EventName @@ -88,6 +88,9 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.6 tire_stiffness_factor = 1.0 + # dp + 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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7530531c7..063423953 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -103,7 +103,9 @@ class Controls: self.LoC = LongControl(self.CP, self.CI.compute_gb) self.VM = VehicleModel(self.CP) - if self.CP.lateralTuning.which() == 'pid': + 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) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index dc3d1f4b2..390d639ff 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -16,6 +16,7 @@ import numpy as np from numpy.linalg import solve from typing import Tuple from cereal import car +from common.params import Params class VehicleModel: @@ -34,13 +35,17 @@ class VehicleModel: self.cF_orig = CP.tireStiffnessFront self.cR_orig = CP.tireStiffnessRear + # dp + self.sR_orig = CP.steerRatio + self.dp_sr_learner = Params().get('dp_sr_learner') == b'1' + self.update_params(1.0, CP.steerRatio) def update_params(self, stiffness_factor: float, steer_ratio: float) -> None: """Update the vehicle model with a new stiffness factor and steer ratio""" self.cF = stiffness_factor * self.cF_orig self.cR = stiffness_factor * self.cR_orig - self.sR = steer_ratio + self.sR = steer_ratio if self.dp_sr_learner else self.sR_orig def steady_state_sol(self, sa: float, u: float) -> np.ndarray: """Returns the steady state solution. diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 863b571a5..d1fa88dee 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -33,12 +33,12 @@ else: error_tags = {'dirty': dirty, 'username': uniqueID, 'dongle_id': dongle_id, 'branch': branch, 'remote': origin} - client = Client('https://fa39b8804ae94ea6bbb22279d68b3dc7:5ac1b337f7be42308cabbb534b342669@sentry.io/1428745', - install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags) - - # client = Client('https://980a0cba712a4c3593c33c78a12446e1:fecab286bcaf4dba8b04f7cff0188e2d@sentry.io/1488600', + # client = Client('https://fa39b8804ae94ea6bbb22279d68b3dc7:5ac1b337f7be42308cabbb534b342669@sentry.io/1428745', # install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags) + client = Client('https://980a0cba712a4c3593c33c78a12446e1:fecab286bcaf4dba8b04f7cff0188e2d@sentry.io/1488600', + install_sys_hook=False, transport=HTTPTransport, release=version, tags=error_tags) + def capture_exception(*args, **kwargs): exc_info = sys.exc_info() if not exc_info[0] is capnp.lib.capnp.KjException: diff --git a/selfdrive/dragonpilot/systemd.py b/selfdrive/dragonpilot/systemd.py index d7beb9f00..ecc2c9c4d 100644 --- a/selfdrive/dragonpilot/systemd.py +++ b/selfdrive/dragonpilot/systemd.py @@ -157,6 +157,9 @@ def confd_thread(): we can have some logic here =================================================== ''' + if msg.dragonConf.dpAssistedLcMinMph > msg.dragonConf.dpAutoLcMinMph: + put_nonblocking('dp_auto_lc_min_mph', str(msg.dragonConf.dpAssistedLcMinMph)) + msg.dragonConf.dpAutoLcMinMph = msg.dragonConf.dpAssistedLcMinMph if msg.dragonConf.dpAtl: msg.dragonConf.dpAllowGas = True msg.dragonConf.dpDynamicFollow = 0 diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc index 9dc51e49b..a9d261c20 100644 --- a/selfdrive/locationd/params_learner.cc +++ b/selfdrive/locationd/params_learner.cc @@ -44,23 +44,20 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params, alpha4 = 1.0 * learning_rate; } -bool ParamsLearner::update(double psi, double u, double sa, bool dp_sr_leaner) { +bool ParamsLearner::update(double psi, double u, double sa) { if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) { double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); double new_ao = ao - alpha1 * ao_diff; double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); double new_slow_ao = slow_ao - alpha2 * slow_ao_diff; - + double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2))); double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3))); ao = new_ao; slow_ao = new_slow_ao; x = new_x; - if (dp_sr_leaner) { - double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2))); - sR = new_sR; - } + sR = new_sR; } #ifdef DEBUG @@ -95,7 +92,7 @@ extern "C" { bool params_learner_update(void * params_learner, double psi, double u, double sa) { ParamsLearner * p = (ParamsLearner*) params_learner; - return p->update(psi, u, sa, true); + return p->update(psi, u, sa); } double params_learner_get_ao(void * params_learner){ diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h index 399da31fc..4d97551b3 100644 --- a/selfdrive/locationd/params_learner.h +++ b/selfdrive/locationd/params_learner.h @@ -31,5 +31,5 @@ public: double steer_ratio, double learning_rate); - bool update(double psi, double u, double sa, bool dp_sr_leaner); + bool update(double psi, double u, double sa); }; diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index d7fbee553..aee957bc8 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -78,13 +78,6 @@ int main(int argc, char *argv[]) { ParamsLearner learner(car_params, ao, x, sR, 1.0); - // dp - sr learner - bool enable_sr_learner = true; - std::vector result = read_db_bytes("dp_sr_learner"); - if (result.size() > 0 && result[0] == '0') { - enable_sr_learner = false; - } - // Main loop int save_counter = 0; while (true){ @@ -95,7 +88,7 @@ int main(int argc, char *argv[]) { save_counter++; double yaw_rate = -localizer.x[0]; - bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle, enable_sr_learner); + bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index eef1babca..40487c39e 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -199,7 +199,7 @@ class Uploader(): return self.last_resp - def upload(self, key, fn, atl = False): + def upload(self, key, fn): try: sz = os.path.getsize(fn) except OSError: @@ -210,9 +210,7 @@ class Uploader(): cloudlog.info("checking %r with size %r", key, sz) - if atl: - setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) - elif sz == 0: + if sz == 0: try: # tag files of 0 size as uploaded setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) @@ -236,7 +234,7 @@ class Uploader(): return success -def uploader_fn(exit_event, sm=None): +def uploader_fn(exit_event): cloudlog.info("uploader_fn") params = Params() @@ -249,9 +247,7 @@ def uploader_fn(exit_event, sm=None): uploader = Uploader(dongle_id, ROOT) # dp - if sm is None: - sm = messaging.SubMaster(['dragonConf']) - atl = False + sm = messaging.SubMaster(['dragonConf']) backoff = 0.1 while True: @@ -263,7 +259,6 @@ def uploader_fn(exit_event, sm=None): if sm.updated['dragonConf']: on_wifi = True if sm['dragonConf'].dpUploadOnMobile else on_wifi on_hotspot = False if sm['dragonConf'].dpUploadOnHotspot else on_hotspot - atl = sm['dragonConf'].dpAtl should_upload = on_wifi and not on_hotspot @@ -280,7 +275,7 @@ def uploader_fn(exit_event, sm=None): cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) cloudlog.info("to upload %r", d) - success = uploader.upload(key, fn, atl) + success = uploader.upload(key, fn) if success: backoff = 0.1 else: @@ -289,8 +284,8 @@ def uploader_fn(exit_event, sm=None): backoff = min(backoff*2, 120) cloudlog.info("upload done, success=%r", success) -def main(sm=None): - uploader_fn(threading.Event(), sm) +def main(): + uploader_fn(threading.Event()) if __name__ == "__main__": main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 97417e04c..25c624bee 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -146,10 +146,17 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): - new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80])) + dp_uno_fan_mode = params.get('dp_uno_fan_mode') == b'1' + if dp_uno_fan_mode: + new_speed = int(interp(max_cpu_temp, [65.0, 80.0, 90.0], [0, 20, 60])) + else: + new_speed = int(interp(max_cpu_temp, [40.0, 80.0], [0, 80])) if not ignition: - new_speed = min(30, new_speed) + if dp_uno_fan_mode: + new_speed = min(10, new_speed) + else: + new_speed = min(30, new_speed) return new_speed diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c6ad81fef..aa21af713 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -758,7 +758,7 @@ static void ui_draw_infobar(UIState *s) { char battery[5]; snprintf(battery, sizeof(battery), "%02d%%", scene->thermal.getBatteryPercent()); - if (scene->dpUiDev) { + if (false) { char rel_steer[9]; snprintf(rel_steer, sizeof(rel_steer), "%s%05.1f°", scene->controls_state.getAngleSteers() < 0? "-" : "+", fabs(scene->angleSteers)); @@ -837,6 +837,215 @@ static void ui_draw_blindspots(UIState *s) { nvgFill(s->vg); } } + +//BB START: functions added for the display of various items +static int bb_ui_draw_measure(UIState *s, const char* bb_value, const char* bb_uom, const char* bb_label, + int bb_x, int bb_y, int bb_uom_dx, + NVGcolor bb_valueColor, NVGcolor bb_labelColor, NVGcolor bb_uomColor, + int bb_valueFontSize, int bb_labelFontSize, int bb_uomFontSize ) { + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + int dx = 0; + if (strlen(bb_uom) > 0) { + dx = (int)(bb_uomFontSize*2.5/2); + } + //print value + nvgFontFaceId(s->vg, s->font_sans_bold); + nvgFontSize(s->vg, bb_valueFontSize*2.5); + nvgFillColor(s->vg, bb_valueColor); + nvgText(s->vg, bb_x-dx/2, bb_y+ (int)(bb_valueFontSize*2.5)+5, bb_value, NULL); + //print label + nvgFontFaceId(s->vg, s->font_sans_regular); + nvgFontSize(s->vg, bb_labelFontSize*2.5); + nvgFillColor(s->vg, bb_labelColor); + nvgText(s->vg, bb_x, bb_y + (int)(bb_valueFontSize*2.5)+5 + (int)(bb_labelFontSize*2.5)+5, bb_label, NULL); + //print uom + if (strlen(bb_uom) > 0) { + nvgSave(s->vg); + int rx =bb_x + bb_uom_dx + bb_valueFontSize -3; + int ry = bb_y + (int)(bb_valueFontSize*2.5/2)+25; + nvgTranslate(s->vg,rx,ry); + nvgRotate(s->vg, -1.5708); //-90deg in radians + nvgFontFaceId(s->vg, s->font_sans_regular); + nvgFontSize(s->vg, (int)(bb_uomFontSize*2.5)); + nvgFillColor(s->vg, bb_uomColor); + nvgText(s->vg, 0, 0, bb_uom, NULL); + nvgRestore(s->vg); + } + return (int)((bb_valueFontSize + bb_labelFontSize)*2.5) + 5; +} + +static void bb_ui_draw_measures_left(UIState *s, int bb_x, int bb_y, int bb_w ) { + const UIScene *scene = &s->scene; + int bb_rx = bb_x + (int)(bb_w/2); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = COLOR_WHITE_ALPHA(200); + NVGcolor uom_color = COLOR_WHITE_ALPHA(200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + float d_rel = scene->lead_data[0].getDRel(); + float v_rel = scene->lead_data[0].getVRel(); + + //add visual radar relative distance + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = COLOR_WHITE_ALPHA(200); + if (scene->lead_data[0].getStatus()) { + //show RED if less than 5 meters + //show orange if less than 15 meters + if((int)(d_rel) < 15) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(d_rel) < 5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative distance is always in meters + snprintf(val_str, sizeof(val_str), "%d", (int)d_rel); + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + snprintf(uom_str, sizeof(uom_str), "m "); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, + (s->scene.dpLocale == "zh-TW"? "真實車距" : s->scene.dpLocale == "zh-CN"? "真实车距" : "REL DIST"), + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add visual radar relative speed + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = COLOR_WHITE_ALPHA(200); + if (scene->lead_data[0].getStatus()) { + //show Orange if negative speed (approaching) + //show Orange if negative speed faster than 5mph (approaching fast) + if((int)(v_rel) < 0) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if((int)(v_rel) < -5) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // lead car relative speed is always in meters + if (s->is_metric) { + snprintf(val_str, sizeof(val_str), "%d", (int)(v_rel * 3.6 + 0.5)); + } else { + snprintf(val_str, sizeof(val_str), "%d", (int)(v_rel * 2.2374144 + 0.5)); + } + } else { + snprintf(val_str, sizeof(val_str), "-"); + } + if (s->is_metric) { + snprintf(uom_str, sizeof(uom_str), "km/h");; + } else { + snprintf(uom_str, sizeof(uom_str), "mph"); + } + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, + (s->scene.dpLocale == "zh-TW"? "相對速度" : s->scene.dpLocale == "zh-CN"? "相对速度" : "REAL SPEED"), + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, COLOR_WHITE_ALPHA(80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + +static void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) { + const UIScene *scene = &s->scene; + int bb_rx = bb_x + (int)(bb_w/2); + int bb_ry = bb_y; + int bb_h = 5; + NVGcolor lab_color = COLOR_WHITE_ALPHA(200); + NVGcolor uom_color = COLOR_WHITE_ALPHA(200); + int value_fontSize=30; + int label_fontSize=15; + int uom_fontSize = 15; + int bb_uom_dx = (int)(bb_w /2 - uom_fontSize*2.5) ; + + //add steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = COLOR_WHITE_ALPHA(200); + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(scene->angleSteers) < -6) || ((int)(scene->angleSteers) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(scene->angleSteers) < -12) || ((int)(scene->angleSteers) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.1f°",(scene->angleSteers)); + + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, + (s->scene.dpLocale == "zh-TW"? "實際轉角" : s->scene.dpLocale == "zh-CN"? "实际转角" : "REAL STEER"), + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //add desired steering angle + if (true) { + char val_str[16]; + char uom_str[6]; + NVGcolor val_color = COLOR_WHITE_ALPHA(200); + //show Orange if more than 6 degrees + //show red if more than 12 degrees + if(((int)(scene->angleSteersDes) < -6) || ((int)(scene->angleSteersDes) > 6)) { + val_color = nvgRGBA(255, 188, 3, 200); + } + if(((int)(scene->angleSteersDes) < -12) || ((int)(scene->angleSteersDes) > 12)) { + val_color = nvgRGBA(255, 0, 0, 200); + } + // steering is in degrees + snprintf(val_str, sizeof(val_str), "%.1f°",(scene->angleSteersDes)); + + snprintf(uom_str, sizeof(uom_str), ""); + bb_h +=bb_ui_draw_measure(s, val_str, uom_str, + (s->scene.dpLocale == "zh-TW"? "預測轉角" : s->scene.dpLocale == "zh-CN"? "预测转角" : "DESIR STEER"), + bb_rx, bb_ry, bb_uom_dx, + val_color, lab_color, uom_color, + value_fontSize, label_fontSize, uom_fontSize ); + bb_ry = bb_y + bb_h; + } + + //finally draw the frame + bb_h += 20; + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, bb_x, bb_y, bb_w, bb_h, 20); + nvgStrokeColor(s->vg, COLOR_WHITE_ALPHA(80)); + nvgStrokeWidth(s->vg, 6); + nvgStroke(s->vg); +} + +static void ui_draw_bbui(UIState *s) { + const UIScene *scene = &s->scene; + const int bb_dml_w = 180; + const int bb_dml_x = (scene->ui_viz_rx + (bdr_s * 2)); + const int bb_dml_y = (box_y + (bdr_s * 1.5)) + 220; + + const int bb_dmr_w = 180; + const int bb_dmr_x = scene->ui_viz_rx + scene->ui_viz_rw - bb_dmr_w - (bdr_s * 2); + const int bb_dmr_y = (box_y + (bdr_s * 1.5)) + 220; + + bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w); + bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w); +} ////////////////////////////////////////////////////// DP END ////////////////////////////////////////////////////// static void ui_draw_vision_footer(UIState *s) { @@ -851,7 +1060,10 @@ static void ui_draw_vision_footer(UIState *s) { if ((int)s->scene.dpAccelProfile > 0) { ui_draw_ap_button(s); } - if (s->scene.dpUiDev || s->scene.dpDashcam || s->scene.dpAppWaze) { + if (s->scene.dpUiDev) { + ui_draw_bbui(s); + } + if (s->scene.dpDashcam || s->scene.dpAppWaze) { ui_draw_infobar(s); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 44abdda4b..feb13460f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -948,7 +948,7 @@ int main(int argc, char* argv[]) { float min = MIN_VOLUME + s->scene.controls_state.getVEgo() / 5; if (s->scene.dpUiVolumeBoost > 0 || s->scene.dpUiVolumeBoost < 0) { - min = fmax(MIN_VOLUME, min * (1 + s->scene.dpUiVolumeBoost * 0.01)); + min = min * (1 + s->scene.dpUiVolumeBoost * 0.01); } s->sound.setVolume(fmin(MAX_VOLUME, min)); // up one notch every 5 m/s