diff --git a/README.md b/README.md index c4106ac75..dec4d00ae 100644 --- a/README.md +++ b/README.md @@ -41,53 +41,57 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur Supported Cars ------ -| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- | -| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | -| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | -| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | -| GM3| Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph | -| GM3| Volt 2018 | Driver Confidence II | Yes | Yes | 0mph | 7mph | -| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | -| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph1| 12mph | -| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph1| 12mph | -| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph1| 0mph | -| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 0mph | -| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | -| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | -| Lexus | RX Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | -| Lexus | RX Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | -| Toyota | C-HR 20184 | All | Yes | Stock | 0mph | 0mph | -| Toyota | Corolla 2017 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Corolla 2018 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius Prime 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Prius Prime 2018 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 2017 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 2018 | All | Yes | Yes2| 20mph | 0mph | -| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | - +| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | +| -------------------| ----------------------| ---------------------| --------| ---------------| -----------------| ---------------| +| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | +| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | +| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | +| GM3 | Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph | +| GM3 | Volt 2018 | Driver Confidence II | Yes | Yes | 0mph | 7mph | +| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | +| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph | +| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph1| 12mph | +| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph1| 12mph | +| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph | +| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph1| 0mph | +| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph1| 0mph | +| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph1| 0mph | +| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | +| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | +| Hyundai6| Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | +| Lexus | RX Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | +| Lexus | RX Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Camry 20184| All | Yes | Stock | 0mph5 | 0mph | +| Toyota | C-HR 20184 | All | Yes | Stock | 0mph | 0mph | +| Toyota | Corolla 2017 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Corolla 2018 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Highlander 2017 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Highlander Hybrid 2018| All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius Prime 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Prius Prime 2018 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 2017 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 2018 | All | Yes | Yes2| 20mph | 0mph | +| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes2| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | 1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)*** 2When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota) 3[GM installation guide](https://www.zoneos.com/volt.htm) 4It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/). 528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. +6Giraffe is under development: architecture similar to Toyota giraffe, with an extra 120Ohm resistor on bus 3. Community Maintained Cars ------ @@ -108,6 +112,8 @@ In Progress Cars - All LSS-P Lexus with Steering Assist or Lane Keep Assist. - 'All-Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the GS, GSH, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option. - Even though the LX have TSS-P, it does not have Steering Assist and is not supported. +- All Hyundai with SmartSense. +- All Kia with ACC and LKAS. How can I add support for my car? ------ diff --git a/RELEASES.md b/RELEASES.md index 1f822b6f6..eae938037 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Version 0.5.3 (2018-09-03) +======================== + * Hyundai Santa Fe support! + * Honda Pilot 2019 support thanks to energee! + * Toyota Hyghlander support thanks to daehahn! + * Improve steering tuning for Honda Odyssey + Version 0.5.2 (2018-08-16) ======================== * New calibration: more accurate, a lot faster, open source! diff --git a/cereal/log.capnp b/cereal/log.capnp index 06392b88a..4f4e224f2 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -42,6 +42,7 @@ struct InitData { dirty @9 :Bool; passive @12 :Bool; + params @17 :Map(Text, Text); enum DeviceType { unknown @0; @@ -186,6 +187,10 @@ struct SensorEventData { iOS @1; fiber @2; velodyne @3; # Velodyne IMU + # c3 sensors below + bno055 @4; + lsm6ds3 @5; + bmp280 @6; } } @@ -262,6 +267,8 @@ struct ThermalData { freeSpace @7 :Float32; batteryPercent @8 :Int16; batteryStatus @9 :Text; + batteryCurrent @15 :Int32; + batteryVoltage @16 :Int32; usbOnline @12 :Bool; fanSpeed @10 :UInt16; @@ -327,6 +334,7 @@ struct Live20Data { aLeadK @9 :Float32; fcw @10 :Bool; status @11 :Bool; + aLeadTau @12 :Float32; } } diff --git a/common/dbc.py b/common/dbc.py index 7135b07cf..2a5e4f868 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -199,6 +199,7 @@ class dbc(object): st = x[2].rjust(8, '\x00') le, be = None, None + size = msg[0][1] for s in msg[1]: if arr is not None and s[0] not in arr: @@ -215,7 +216,7 @@ class dbc(object): else: if le is None: le = struct.unpack("> (64 - 8 * size) ss = s[1] data_bit_pos = ss diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 669f0fdd8..e037555b7 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -39,6 +39,7 @@ #define SAFETY_HONDA_BOSCH 4 #define SAFETY_FORD 5 #define SAFETY_CADILLAC 6 +#define SAFETY_HYUNDAI 7 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 @@ -113,6 +114,9 @@ void *safety_setter_thread(void *s) { case (int)cereal::CarParams::SafetyModels::CADILLAC: safety_setting = SAFETY_CADILLAC; break; + case (int)cereal::CarParams::SafetyModels::HYUNDAI: + safety_setting = SAFETY_HYUNDAI; + break; default: LOGE("unknown safety model: %d", safety_model); } @@ -584,7 +588,7 @@ void *pigeon_thread(void *crap) { //printf("got %d\n", len); alen += len; } - if (alen > 0) { + if (alen > 0) { if (dat[0] == (char)0x00){ LOGW("received invalid ublox message, resetting pigeon"); pigeon_init(); diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc index b1fd8e5d5..986de6b11 100644 --- a/selfdrive/can/packer.cc +++ b/selfdrive/can/packer.cc @@ -1,5 +1,4 @@ #include - #include #include #include diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 895538393..cc669e60c 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -47,11 +47,16 @@ class CANPacker(object): if __name__ == "__main__": ## little endian test - cp = CANPacker("hyundai_2015_ccan") + cp = CANPacker("hyundai_santa_fe_2019_ccan") s = cp.pack_bytes(0x340, { "CR_Lkas_StrToqReq": -0.06, - "CF_Lkas_FcwBasReq": 1, - "CF_Lkas_Chksum": 3, + #"CF_Lkas_FcwBasReq": 1, + "CF_Lkas_MsgCount": 7, + "CF_Lkas_HbaSysState": 0, + #"CF_Lkas_Chksum": 3, + }) + s = cp.pack_bytes(0x340, { + "CF_Lkas_MsgCount": 1, }) # big endian test #cp = CANPacker("honda_civic_touring_2016_can_generated") diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 9246ae5b7..4a82d9a02 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -259,6 +259,7 @@ class CANParser { memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); // Assumes all signals in the message are of the same type (little or big endian) + // TODO: allow signals within the same message to have different endianess auto& sig = message_states[cmsg.getAddress()].parse_sigs[0]; if (sig.is_little_endian) { p = read_u64_le(dat); diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 80e79f034..0473c8874 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,4 +1,26 @@ # functions common among cars +from common.numpy_fast import clip + def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} + + +def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): + + # limits due to driver torque + driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-LIMITS.STEER_MAX, driver_min_torque), 0) + apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), + apply_torque_last + LIMITS.STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP, + min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) + + return int(round(apply_torque)) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index afe5a9e4c..dbc392cb2 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -97,6 +97,7 @@ def get_car(logcan, sendcan=None, passive=True): return None, None interface_cls = interfaces[candidate] + if interface_cls is None: cloudlog.warning("car matched %s, but interface wasn't available or failed to import" % candidate) return None, None diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index d3c8ee196..3625a6b1d 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,7 +1,8 @@ -from common.numpy_fast import clip, interp +from common.numpy_fast import interp from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan from selfdrive.car.gm.values import CAR, DBC from selfdrive.can.packer import CANPacker @@ -88,27 +89,13 @@ class CarController(object): final_steer = actuators.steer if enabled else 0. apply_steer = final_steer * P.STEER_MAX - # limits due to driver torque - driver_max_torque = P.STEER_MAX + (P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER - driver_min_torque = -P.STEER_MAX + (-P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER - max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0) - min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0) - apply_steer = clip(apply_steer, min_steer_allowed, max_steer_allowed) - - # slow rate if steer torque increases in magnitude - if self.apply_steer_last > 0: - apply_steer = clip(apply_steer, max(self.apply_steer_last - P.STEER_DELTA_DOWN, -P.STEER_DELTA_UP), - self.apply_steer_last + P.STEER_DELTA_UP) - else: - apply_steer = clip(apply_steer, self.apply_steer_last - P.STEER_DELTA_UP, - min(self.apply_steer_last + P.STEER_DELTA_DOWN, P.STEER_DELTA_UP)) + apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3. if not lkas_enabled: apply_steer = 0 - apply_steer = int(round(apply_steer)) self.apply_steer_last = apply_steer idx = (frame / P.STEER_STEP) % 4 @@ -116,7 +103,7 @@ class CarController(object): can_sends.append(gmcan.create_steering_control(self.packer_pt, canbus.powertrain, apply_steer, idx, lkas_enabled)) if self.car_fingerprint == CAR.CADILLAC_CT6: - can_sends += gmcan.create_steering_control_ct6(self.packer_pt, + can_sends += gmcan.create_steering_control_ct6(self.packer_pt, canbus, apply_steer, CS.v_ego, idx, lkas_enabled) ### GAS/BRAKE ### diff --git a/selfdrive/car/hyundai/__init__.py b/selfdrive/car/hyundai/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py new file mode 100644 index 000000000..b5b2f82cc --- /dev/null +++ b/selfdrive/car/hyundai/carcontroller.py @@ -0,0 +1,76 @@ +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \ + create_1191, create_1156, \ + create_clu11 +from selfdrive.car.hyundai.values import Buttons +from selfdrive.can.packer import CANPacker + + +# Steer torque limits + +class SteerLimitParams: + STEER_MAX = 250 # 409 is the max + STEER_DELTA_UP = 3 + STEER_DELTA_DOWN = 7 + STEER_DRIVER_ALLOWANCE = 50 + STEER_DRIVER_MULTIPLIER = 2 + STEER_DRIVER_FACTOR = 1 + +class CarController(object): + def __init__(self, dbc_name, car_fingerprint, enable_camera): + self.apply_steer_last = 0 + self.car_fingerprint = car_fingerprint + self.lkas11_cnt = 0 + self.cnt = 0 + self.last_resume_cnt = 0 + self.enable_camera = enable_camera + # True when giraffe switch 2 is low and we need to replace all the camera messages + # otherwise we forward the camera msgs and we just replace the lkas cmd signals + self.camera_disconnected = False + + self.packer = CANPacker(dbc_name) + + def update(self, sendcan, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): + + if not self.enable_camera: + return + + ### Steering Torque + apply_steer = actuators.steer * SteerLimitParams.STEER_MAX + + apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) + + if not enabled: + apply_steer = 0 + + steer_req = 1 if enabled else 0 + + self.apply_steer_last = apply_steer + + can_sends = [] + + self.lkas11_cnt = self.cnt % 0x10 + self.clu11_cnt = self.cnt % 0x10 + + if self.camera_disconnected: + if (self.cnt % 10) == 0: + can_sends.append(create_lkas12()) + if (self.cnt % 50) == 0: + can_sends.append(create_1191()) + if (self.cnt % 7) == 0: + can_sends.append(create_1156()) + + can_sends.append(create_lkas11(self.packer, apply_steer, steer_req, self.lkas11_cnt, + enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected))) + + if pcm_cancel_cmd: + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) + elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: + self.last_resume_cnt = self.cnt + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) + + ### Send messages to canbus + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) + + self.cnt += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py new file mode 100644 index 000000000..d67c26ed8 --- /dev/null +++ b/selfdrive/car/hyundai/carstate.py @@ -0,0 +1,220 @@ +from selfdrive.car.hyundai.values import DBC +from selfdrive.can.parser import CANParser +from selfdrive.config import Conversions as CV +from common.kalman.simple_kalman import KF1D +import numpy as np + + +def get_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("WHL_SPD_FL", "WHL_SPD11", 0), + ("WHL_SPD_FR", "WHL_SPD11", 0), + ("WHL_SPD_RL", "WHL_SPD11", 0), + ("WHL_SPD_RR", "WHL_SPD11", 0), + + ("YAW_RATE", "ESP12", 0), + + ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), + ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), + ("CF_Gway_TSigLHSw", "CGW1", 0), + ("CF_Gway_TurnSigLh", "CGW1", 0), + ("CF_Gway_TSigRHSw", "CGW1", 0), + ("CF_Gway_TurnSigRh", "CGW1", 0), + + ("BRAKE_ACT", "EMS12", 0), + ("PV_AV_CAN", "EMS12", 0), + ("TPS", "EMS12", 0), + + ("CYL_PRES", "ESP12", 0), + + ("CF_Clu_CruiseSwState", "CLU11", 0), + ("CF_Clu_CruiseSwMain" , "CLU11", 0), + ("CF_Clu_SldMainSW", "CLU11", 0), + ("CF_Clu_ParityBit1", "CLU11", 0), + ("CF_Clu_VanzDecimal" , "CLU11", 0), + ("CF_Clu_Vanz", "CLU11", 0), + ("CF_Clu_SPEED_UNIT", "CLU11", 0), + ("CF_Clu_DetentOut", "CLU11", 0), + ("CF_Clu_RheostatLevel", "CLU11", 0), + ("CF_Clu_CluInfo", "CLU11", 0), + ("CF_Clu_AmpInfo", "CLU11", 0), + ("CF_Clu_AliveCnt1", "CLU11", 0), + + ("CF_Lvr_Gear","LVR12",0), + + ("ACCEnable", "TCS13", 0), + ("ACC_REQ", "TCS13", 0), + ("DriverBraking", "TCS13", 0), + ("DriverOverride", "TCS13", 0), + + ("ESC_Off_Step", "TCS15", 0), + + ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) + + ("CR_Mdps_DrvTq", "MDPS11", 0), + + ("CR_Mdps_StrColTq", "MDPS12", 0), + ("CF_Mdps_ToiActive", "MDPS12", 0), + ("CF_Mdps_ToiUnavail", "MDPS12", 0), + ("CF_Mdps_FailStat", "MDPS12", 0), + ("CR_Mdps_OutTq", "MDPS12", 0), + + ("VSetDis", "SCC11", 0), + ("SCCInfoDisplay", "SCC11", 0), + ("ACCMode", "SCC12", 1), + + ("SAS_Angle", "SAS11", 0), + ("SAS_Speed", "SAS11", 0), + ] + + checks = [ + # address, frequency + ("MDPS12", 50), + ("MDPS11", 100), + ("TCS15", 10), + ("TCS13", 50), + ("CLU11", 50), + ("ESP12", 100), + ("EMS12", 100), + ("CGW1", 10), + ("CGW4", 5), + ("WHL_SPD11", 50), + ("SCC11", 50), + ("SCC12", 50), + ("SAS11", 100) + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + +def get_camera_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("CF_Lkas_LdwsSysState", "LKAS11", 0), + ("CF_Lkas_SysWarning", "LKAS11", 0), + ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), + ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), + ("CF_Lkas_HbaLamp", "LKAS11", 0), + ("CF_Lkas_FcwBasReq", "LKAS11", 0), + ("CF_Lkas_ToiFlt", "LKAS11", 0), + ("CF_Lkas_HbaSysState", "LKAS11", 0), + ("CF_Lkas_FcwOpt", "LKAS11", 0), + ("CF_Lkas_HbaOpt", "LKAS11", 0), + ("CF_Lkas_FcwSysState", "LKAS11", 0), + ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), + ("CF_Lkas_FusionState", "LKAS11", 0), + ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), + ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0) + ] + + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) + +class CarState(object): + def __init__(self, CP): + + self.CP = CP + + # initialize can parser + self.car_fingerprint = CP.carFingerprint + + # vEgo kalman filter + dt = 0.01 + # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) + # R = 1e3 + self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), + A=np.matrix([[1.0, dt], [0.0, 1.0]]), + C=np.matrix([1.0, 0.0]), + K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego = 0.0 + self.left_blinker_on = 0 + self.left_blinker_flash = 0 + self.right_blinker_on = 0 + self.right_blinker_flash = 0 + + def update(self, cp, cp_cam): + # copy can_valid + self.can_valid = cp.can_valid + + # update prevs, update must run once per Loop + self.prev_left_blinker_on = self.left_blinker_on + self.prev_right_blinker_on = self.right_blinker_on + + self.door_all_closed = True + self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] + + self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] + self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step'] + + self.park_brake = False + self.main_on = True + self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 + self.pcm_acc_status = int(self.acc_active) + + # calc best v_ego estimate, by averaging two opposite corners + self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS + self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS + self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS + self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS + self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. + + self.low_speed_lockout = self.v_wheel < 1.0 + + # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default + if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_x = np.matrix([[self.v_wheel], [0.0]]) + + self.v_ego_raw = self.v_wheel + v_ego_x = self.v_ego_kf.update(self.v_wheel) + self.v_ego = float(v_ego_x[0]) + self.a_ego = float(v_ego_x[1]) + is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) + speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS + self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv + self.standstill = not self.v_wheel > 0.1 + + self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] + self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] + self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] + self.main_on = True + self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] + self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh'] + self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] + self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh'] + self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > 100. + self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE + self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] + self.brake_error = 0 + self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] + self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq'] + self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. + + self.user_brake = 0 + + self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] + self.brake_lights = bool(self.brake_pressed) + if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1): + self.pedal_gas = 0 + else: + self.pedal_gas = cp.vl["EMS12"]['TPS'] + self.car_gas = cp.vl["EMS12"]['TPS'] + + # Gear Selecton - This should be compatible with all Kia/Hyundai with Auto's + gear = cp.vl["LVR12"]["CF_Lvr_Gear"] + if gear == 5: + self.gear_shifter = "drive" + elif gear == 6: + self.gear_shifter = "neutral" + elif gear == 0: + self.gear_shifter = "park" + elif gear == 7: + self.gear_shifter = "reverse" + else: + self.gear_shifter = "unknown" + + # save the entire LKAS11 and CLU11 + self.lkas11 = cp_cam.vl["LKAS11"] + self.clu11 = cp.vl["CLU11"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py new file mode 100644 index 000000000..f9f8c3cbd --- /dev/null +++ b/selfdrive/car/hyundai/hyundaican.py @@ -0,0 +1,67 @@ +import crcmod + +hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) + +def make_can_msg(addr, dat, alt): + return [addr, 0, dat, alt] + +def create_lkas11(packer, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False): + values = { + "CF_Lkas_Icon": 3 if enabled else 0, + "CF_Lkas_LdwsSysState": lkas11["CF_Lkas_LdwsSysState"] if keep_stock else 1, + "CF_Lkas_SysWarning": hud_alert, + "CF_Lkas_LdwsLHWarning": lkas11["CF_Lkas_LdwsLHWarning"] if keep_stock else 0, + "CF_Lkas_LdwsRHWarning": lkas11["CF_Lkas_LdwsRHWarning"] if keep_stock else 0, + "CF_Lkas_HbaLamp": lkas11["CF_Lkas_HbaLamp"] if keep_stock else 0, + "CF_Lkas_FcwBasReq": lkas11["CF_Lkas_FcwBasReq"] if keep_stock else 0, + "CR_Lkas_StrToqReq": apply_steer, + "CF_Lkas_ActToi": steer_req, + "CF_Lkas_ToiFlt": lkas11["CF_Lkas_ToiFlt"] if keep_stock else 0, + "CF_Lkas_HbaSysState": lkas11["CF_Lkas_HbaSysState"] if keep_stock else 1, + "CF_Lkas_FcwOpt": lkas11["CF_Lkas_FcwOpt"] if keep_stock else 0, + "CF_Lkas_HbaOpt": lkas11["CF_Lkas_HbaOpt"] if keep_stock else 3, + "CF_Lkas_MsgCount": cnt, + "CF_Lkas_FcwSysState": lkas11["CF_Lkas_FcwSysState"] if keep_stock else 0, + "CF_Lkas_FcwCollisionWarning": lkas11["CF_Lkas_FcwCollisionWarning"] if keep_stock else 0, + "CF_Lkas_FusionState": lkas11["CF_Lkas_FusionState"] if keep_stock else 0, + "CF_Lkas_Chksum": 0, + "CF_Lkas_FcwOpt_USM": 2 if enabled else 1, + "CF_Lkas_LdwsOpt_USM": lkas11["CF_Lkas_LdwsOpt_USM"] if keep_stock else 3, + } + + dat = packer.make_can_msg("LKAS11", 0, values)[2] + dat = dat[:6] + dat[7] + checksum = hyundai_checksum(dat) + + values["CF_Lkas_Chksum"] = checksum + + return packer.make_can_msg("LKAS11", 0, values) + +def create_lkas12(): + return make_can_msg(1342, "\x00\x00\x00\x00\x60\x05", 0) + + +def create_1191(): + return make_can_msg(1191, "\x01\x00", 0) + + +def create_1156(): + return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) + +def create_clu11(packer, clu11, button): + values = { + "CF_Clu_CruiseSwState": button, + "CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"], + "CF_Clu_SldMainSW": clu11["CF_Clu_SldMainSW"], + "CF_Clu_ParityBit1": clu11["CF_Clu_ParityBit1"], + "CF_Clu_VanzDecimal": clu11["CF_Clu_VanzDecimal"], + "CF_Clu_Vanz": clu11["CF_Clu_Vanz"], + "CF_Clu_SPEED_UNIT": clu11["CF_Clu_SPEED_UNIT"], + "CF_Clu_DetentOut": clu11["CF_Clu_DetentOut"], + "CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"], + "CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"], + "CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"], + "CF_Clu_AliveCnt1": 0, + } + + return packer.make_can_msg("CLU11", 0, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py new file mode 100644 index 000000000..ff4ec24b6 --- /dev/null +++ b/selfdrive/car/hyundai/interface.py @@ -0,0 +1,259 @@ +#!/usr/bin/env python +from cereal import car, log +from common.realtime import sec_since_boot +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser +from selfdrive.car.hyundai.values import CAMERA_MSGS, get_hud_alerts + +try: + from selfdrive.car.hyundai.carcontroller import CarController +except ImportError: + CarController = None + + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + self.CP = CP + self.VM = VehicleModel(CP) + self.idx = 0 + self.lanes = 0 + self.lkas_request = 0 + + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.can_invalid_count = 0 + self.cruise_enabled_prev = False + + # *** init the major players *** + self.CS = CarState(CP) + self.cp = get_can_parser(CP) + self.cp_cam = get_camera_parser(CP) + + # sending if read only is False + if sendcan is not None: + self.sendcan = sendcan + self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera) + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 3.0 + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + + @staticmethod + def get_params(candidate, fingerprint): + + # kg of standard extra cargo to count for drive, gas, etc... + std_cargo = 136 + + ret = car.CarParams.new_message() + + ret.carName = "hyundai" + ret.carFingerprint = candidate + ret.radarOffCan = True + + ret.safetyModel = car.CarParams.SafetyModels.hyundai + + ret.enableCruise = True # stock acc + + # FIXME: hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + mass_civic = 2923 * CV.LB_TO_KG + std_cargo + wheelbase_civic = 2.70 + centerToFront_civic = wheelbase_civic * 0.4 + centerToRear_civic = wheelbase_civic - centerToFront_civic + rotationalInertia_civic = 2500 + tireStiffnessFront_civic = 192150 + tireStiffnessRear_civic = 202500 + + ret.steerActuatorDelay = 0.1 # Default delay, Prius has larger delay + + #borrowing a lot from corolla, given similar car size + ret.steerKf = 0.00005 # full torque for 20 deg at 80mph means 0.00007818594 + ret.steerRateCost = 0.5 + ret.mass = 3982 * CV.LB_TO_KG + std_cargo + ret.wheelbase = 2.766 + ret.steerRatio = 13.8 * 1.15 # 15% higher at the center seems reasonable + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + ret.steerKpV, ret.steerKiV = [[0.37], [0.1]] + ret.longitudinalKpBP = [0.] + ret.longitudinalKpV = [0.] + ret.longitudinalKiBP = [0.] + ret.longitudinalKiV = [0.] + tire_stiffness_factor = 1. + + ret.centerToFront = ret.wheelbase * 0.4 + + # 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. + ret.minEnableSpeed = -1. + + centerToRear = ret.wheelbase - ret.centerToFront + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = rotationalInertia_civic * \ + ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ + ret.mass / mass_civic * \ + (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) + ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ + ret.mass / mass_civic * \ + (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + + + # no rear steering, at least on the listed cars above + ret.steerRatioRear = 0. + ret.steerControlType = car.CarParams.SteerControlType.torque + + # steer, gas, brake limitations VS speed + ret.steerMaxBP = [0.] + ret.steerMaxV = [1.0] + ret.gasMaxBP = [0.] + ret.gasMaxV = [1.] + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [1.] + ret.longPidDeadzoneBP = [0.] + ret.longPidDeadzoneV = [0.] + + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) + + ret.steerLimitAlert = False + ret.stoppingControl = False + ret.startAccel = 0.0 + + return ret + + # returns a car.CarState + def update(self, c): + # ******************* do can recv ******************* + canMonoTimes = [] + self.cp.update(int(sec_since_boot() * 1e9), False) + self.cp_cam.update(int(sec_since_boot() * 1e9), False) + self.CS.update(self.cp, self.cp_cam) + # create message + ret = car.CarState.new_message() + # speeds + ret.vEgo = self.CS.v_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.aEgo = self.CS.a_ego + ret.yawRate = self.CS.yaw_rate + ret.standstill = self.CS.standstill + ret.wheelSpeeds.fl = self.CS.v_wheel_fl + ret.wheelSpeeds.fr = self.CS.v_wheel_fr + ret.wheelSpeeds.rl = self.CS.v_wheel_rl + ret.wheelSpeeds.rr = self.CS.v_wheel_rr + + # gear shifter + ret.gearShifter = self.CS.gear_shifter + + # gas pedal + ret.gas = self.CS.car_gas + ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading + + # brake pedal + ret.brake = self.CS.user_brake + ret.brakePressed = self.CS.brake_pressed != 0 + ret.brakeLights = self.CS.brake_lights + + # steering wheel + ret.steeringAngle = self.CS.angle_steers + ret.steeringRate = self.CS.angle_steers_rate # it's unsigned + + ret.steeringTorque = self.CS.steer_torque_driver + ret.steeringPressed = self.CS.steer_override + + # cruise state + ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 + if self.CS.pcm_acc_status != 0: + ret.cruiseState.speed = self.CS.cruise_set_speed + else: + ret.cruiseState.speed = 0 + ret.cruiseState.available = bool(self.CS.main_on) + ret.cruiseState.standstill = False + + # TODO: button presses + buttonEvents = [] + + if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'leftBlinker' + be.pressed = self.CS.left_blinker_on != 0 + buttonEvents.append(be) + + if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'rightBlinker' + be.pressed = self.CS.right_blinker_on != 0 + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + ret.leftBlinker = bool(self.CS.left_blinker_on) + ret.rightBlinker = bool(self.CS.right_blinker_on) + + ret.doorOpen = not self.CS.door_all_closed + ret.seatbeltUnlatched = not self.CS.seatbelt + + #ret.genericToggle = self.CS.generic_toggle + + # events + events = [] + if not self.CS.can_valid: + self.can_invalid_count += 1 + if self.can_invalid_count >= 5: + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + else: + self.can_invalid_count = 0 + if not ret.gearShifter == 'drive': + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if ret.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if ret.seatbeltUnlatched: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if self.CS.esp_disabled: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.CS.main_on: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gearShifter == 'reverse': + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.CS.steer_error: + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + + # enable request in prius is simple, as we activate when Toyota is active (rising edge) + if ret.cruiseState.enabled and not self.cruise_enabled_prev: + events.append(create_event('pcmEnable', [ET.ENABLE])) + elif not ret.cruiseState.enabled: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + + # disable on pedals rising edge or when brake is pressed and speed isn't zero + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + ret.events = events + ret.canMonoTimes = canMonoTimes + + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled + + return ret.as_reader() + + def apply(self, c, perception_state=log.Live20Data.new_message()): + + hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert) + + self.CC.update(self.sendcan, c.enabled, self.CS, c.actuators, + c.cruiseControl.cancel, hud_alert) + + return False diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py new file mode 100644 index 000000000..96159fd87 --- /dev/null +++ b/selfdrive/car/hyundai/radar_interface.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +from cereal import car +import time + + +class RadarInterface(object): + def __init__(self, CP): + # radar + self.pts = {} + self.delay = 0.1 + + def update(self): + + ret = car.RadarState.new_message() + time.sleep(0.05) # radard runs on RI updates + + return ret + +if __name__ == "__main__": + RI = RadarInterface(None) + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py new file mode 100644 index 000000000..9ca9ef08d --- /dev/null +++ b/selfdrive/car/hyundai/values.py @@ -0,0 +1,29 @@ +from selfdrive.car import dbc_dict + +def get_hud_alerts(visual_alert, audble_alert): + if visual_alert == "steerRequired": + return 4 if audble_alert != "none" else 5 + else: + return 0 + +class CAR: + SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" + +class Buttons: + NONE = 0 + RES_ACCEL = 1 + SET_DECEL = 2 + CANCEL = 4 + +FINGERPRINTS = { + CAR.SANTA_FE: [{ + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 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, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 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, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 + }], +} + +CAMERA_MSGS = [832, 1156, 1191, 1342] # msgs sent by the camera + +DBC = { + CAR.SANTA_FE: dbc_dict('hyundai_santa_fe_2019_ccan', None), +} + diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 6a1585978..a14cfb53a 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -120,7 +120,7 @@ class CarInterface(object): ret.steerKf = 0.00006 elif candidate in [CAR.CAMRY, CAR.CAMRYH]: - ret.safetyParam = 100 + ret.safetyParam = 100 ret.wheelbase = 2.82448 ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 @@ -128,6 +128,15 @@ class CarInterface(object): ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 + elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: + ret.safetyParam = 100 + ret.wheelbase = 2.78 + ret.steerRatio = 16.0 + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 4607 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid limited + ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] + ret.steerKf = 0.00006 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 @@ -136,9 +145,11 @@ class CarInterface(object): # 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. - if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]: # rav4 hybrid can do stop and go + # hybrid models can't do stop and go even though the stock ACC can't + if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR, + CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.HIGHLANDERH]: ret.minEnableSpeed = -1. - elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go + elif candidate in [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER]: # TODO: hack ICE to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront @@ -233,16 +244,14 @@ class CarInterface(object): ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. - if self.CP.carFingerprint == CAR.RAV4H: - # ignore standstill in hybrid rav4, since pcm allows to restart without + if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH]: + # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command ret.cruiseState.standstill = False else: ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 - # TODO: button presses buttonEvents = [] - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = 'leftBlinker' diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2e0cf3e6f..0fb407abe 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -10,6 +10,8 @@ class CAR: CHRH = "TOYOTA C-HR HYBRID 2018" CAMRY = "TOYOTA CAMRY 2018" CAMRYH = "TOYOTA CAMRY HYBRID 2018" + HIGHLANDER = "TOYOTA HIGHLANDER 2017" + HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" class ECU: @@ -19,49 +21,49 @@ class ECU: # addr: (ecu, cars, bus, 1/freq*100, vl) -STATIC_MSGS = [(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 2, '\x00\x00\x00\x46'), - (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 3, '\xf4\x01\x90\x83\x00\x37'), - - (0x292, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), - (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), - (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), - (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), - - (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), - (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), - - (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), - - (0x32E, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - (0x365, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - (0x365, ECU.DSU, (CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), - (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), - - (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 40, '\x06\x00'), - - (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), - (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), - (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), - (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), - (0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4), 1, 100, '\x20\x20\xAD'), - (0x466, ECU.CAM, (CAR.COROLLA), 1, 100, '\x24\x20\xB1'), - (0x396, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), - (0x43A, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), - (0x43B, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x497, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x4CC, ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), - (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), - (0x470, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), - ] +STATIC_MSGS = [ + (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), + (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), + (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 40, '\x06\x00'), + (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), + (0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x20\x20\xAD'), + (0x466, ECU.CAM, (CAR.COROLLA), 1, 100, '\x24\x20\xB1'), + (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), + (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + + (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 3, '\xf4\x01\x90\x83\x00\x37'), + (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 3, '\x03\x00\x20\x00\x00\x52'), + (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 2, '\x00\x00\x00\x46'), + (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), + (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), + (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x365, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), + (0x365, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), + (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + (0x470, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), + (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x00\x00\x01\x79'), + (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + + (0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), + (0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x396, ECU.APGS, (CAR.PRIUS), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), + (0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), + (0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x497, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), +] ECU_FINGERPRINT = { ECU.CAM: 0x2e4, # steer torque cmd @@ -81,7 +83,7 @@ FINGERPRINTS = { }], CAR.RAV4H: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 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, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 - }], + }], CAR.PRIUS: [{ 36: 8, 37: 8, 166: 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: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 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, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, @@ -119,6 +121,16 @@ FINGERPRINTS = { { 36: 8, 37: 8, 166: 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: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 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, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 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, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572:8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.HIGHLANDER: [{ + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }, + # 2017 Highlander Limited + { + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], + CAR.HIGHLANDERH: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 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, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 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, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], } STEER_THRESHOLD = 100 @@ -133,6 +145,8 @@ DBC = { CAR.CHRH: dbc_dict('toyota_chr_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), CAR.CAMRY: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'), CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), + CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'), } NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH] diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 5b0d216bd..02d91efce 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -1,15 +1,20 @@ #include "selfdrive/common/params.h" -#include "selfdrive/common/util.h" - #ifndef _GNU_SOURCE #define _GNU_SOURCE #endif // _GNU_SOURCE -#include -#include -#include #include +#include +#include +#include +#include + +#include +#include + +#include "selfdrive/common/util.h" +#include "selfdrive/common/utilpp.h" namespace { @@ -152,3 +157,40 @@ void read_db_value_blocking(const char* params_path, const char* key, } } } + +int read_db_all(const char* params_path, std::map *params) { + int err = 0; + + if (params_path == NULL) { + params_path = default_params_path; + } + + std::string lock_path = util::string_format("%s/.lock", params_path); + + int lock_fd = open(lock_path.c_str(), 0); + if (lock_fd < 0) return -1; + + err = flock(lock_fd, LOCK_EX); + if (err < 0) return err; + + std::string key_path = util::string_format("%s/d", params_path); + DIR *d = opendir(key_path.c_str()); + if (!d) { + close(lock_fd); + return -1; + } + + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (!isalnum(de->d_name[0])) continue; + std::string key = std::string(de->d_name); + std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str())); + + (*params)[key] = value; + } + + closedir(d); + + close(lock_fd); + return 0; +} diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index bd4d86dcf..299dcccd0 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -32,4 +32,10 @@ void read_db_value_blocking(const char* params_path, const char* key, } // extern "C" #endif +#ifdef __cplusplus +#include +#include +int read_db_all(const char* params_path, std::map *params); +#endif + #endif // _SELFDRIVE_COMMON_PARAMS_H_ diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 702230838..1193b7af4 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.5.2-release" +#define COMMA_VERSION "0.5.3-release" diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 451330fe0..32d427f84 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -17,12 +17,12 @@ from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED +from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU _DT = 0.01 # 100Hz _DT_MPC = 0.2 # 5Hz MAX_SPEED_ERROR = 2.0 AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted -_LEAD_ACCEL_TAU = 1.5 GPS_PLANNER_ADDR = "192.168.5.1" @@ -163,7 +163,7 @@ class LongitudinalMpc(object): dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l) dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost - dat.liveLongitudinalMpc.aLeadTau = self.l + dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau dat.liveLongitudinalMpc.qpIterations = qp_iterations dat.liveLongitudinalMpc.mpcId = self.mpc_id dat.liveLongitudinalMpc.calculationTime = calculation_time @@ -178,7 +178,7 @@ class LongitudinalMpc(object): self.cur_state = ffi.new("state_t *") self.cur_state[0].v_ego = 0 self.cur_state[0].a_ego = 0 - self.l = _LEAD_ACCEL_TAU + self.a_lead_tau = _LEAD_ACCEL_TAU def set_cur_state(self, v, a): self.cur_state[0].v_ego = v @@ -197,16 +197,10 @@ class LongitudinalMpc(object): v_lead = 0.0 a_lead = 0.0 - # Learn if constant acceleration - if abs(a_lead) < 0.5: - self.l = _LEAD_ACCEL_TAU - else: - self.l *= 0.9 - - l = max(self.l, -a_lead / (v_lead + 0.01)) + self.a_lead_tau = max(lead.aLeadTau, -a_lead / (v_lead + 0.01)) self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: - self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, l) + self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) self.new_lead = True self.prev_lead_status = True @@ -220,11 +214,11 @@ class LongitudinalMpc(object): self.cur_state[0].x_l = 50.0 self.cur_state[0].v_l = CS.vEgo + 10.0 self.cur_state[0].a_l = 0.0 - l = _LEAD_ACCEL_TAU + self.a_lead_tau = _LEAD_ACCEL_TAU # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau) duration = int((sec_since_boot() - t) * 1e9) self.send_mpc_solution(n_its, duration) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index b5ee51e48..da7d3f71b 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -6,6 +6,7 @@ import numpy as np from common.numpy_fast import clip, interp from common.kalman.simple_kalman import KF1D +_LEAD_ACCEL_TAU = 1.5 NO_FUSION_SCORE = 100 # bad default fusion score # radar tracks @@ -32,6 +33,8 @@ _VLEAD_C = [[1.0, 0.0]] #_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]]) _VLEAD_K = [[ 0.1988689 ], [ 0.28555364]] +RDR_TO_LDR = 2.7 + class Track(object): def __init__(self): @@ -60,6 +63,7 @@ class Track(object): if not self.initted: self.initted = True + self.aLeadTau = _LEAD_ACCEL_TAU self.cnt = 1 self.vision_cnt = 0 self.vision = False @@ -92,6 +96,12 @@ class Track(object): self.vision_score = NO_FUSION_SCORE + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau = _LEAD_ACCEL_TAU + else: + self.aLeadTau *= 0.9 + def update_vision_score(self, dist_to_vision, rel_speed_diff): # rel speed is very hard to estimate from vision if dist_to_vision < 4.0 and rel_speed_diff < 10.: @@ -110,8 +120,8 @@ class Track(object): # Weigh y higher since radar is inaccurate in this dimension return [self.dRel, self.yRel*2, self.vRel] -# ******************* Cluster ******************* +# ******************* Cluster ******************* if platform.machine() == 'aarch64': for x in sys.path: pp = os.path.join(x, "phonelibs/hierarchy/lib") @@ -122,6 +132,7 @@ if platform.machine() == 'aarch64': else: from scipy.cluster import _hierarchy + def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None): # supersimplified function to get fast clustering. Got it from scipy Z = np.asarray(Z, order='c') @@ -130,10 +141,10 @@ def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None): _hierarchy.cluster_dist(Z, T, float(t), int(n)) return T -RDR_TO_LDR = 2.7 def mean(l): - return sum(l)/len(l) + return sum(l) / len(l) + class Cluster(object): def __init__(self): @@ -180,6 +191,10 @@ class Cluster(object): def aLeadK(self): return mean([t.aLeadK for t in self.tracks]) + @property + def aLeadTau(self): + return mean([t.aLeadTau for t in self.tracks]) + @property def vision(self): return any([t.vision for t in self.tracks]) @@ -213,6 +228,7 @@ class Cluster(object): "aLeadK": float(self.aLeadK), "status": True, "fcw": self.is_potential_fcw(), + "aLeadTau": float(self.aLeadTau) } def __str__(self): diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 834f65e50..a42b8112a 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -94,9 +94,9 @@ class VehicleModel(object): if __name__ == '__main__': from selfdrive.car.honda.interface import CarInterface - # load car params - #CP = CarInterface.get_params("TOYOTA PRIUS 2017", {}) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) + #from selfdrive.car.hyundai.interface import CarInterface + #CP = CarInterface.get_params("HYUNDAI SANTA FE UNLIMITED 2019", {}) #print CP VM = VehicleModel(CP) #print VM.steady_state_sol(.1, 0.15) diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index cb58f18aa..5b8722558 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 26186f74f..ac49c7a2a 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -10,21 +10,7 @@ from common.basedir import BASEDIR sys.path.append(os.path.join(BASEDIR, "pyextra")) os.environ['BASEDIR'] = BASEDIR -if __name__ == "__main__": - if os.path.isfile("/init.qcom.rc") \ - and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 6): - - # update continue.sh before updating NEOS - if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")): - from shutil import copyfile - copyfile(os.path.join(BASEDIR, "scripts", "continue.sh"), "/data/data/com.termux/files/continue.sh") - - # run the updater - print("Starting NEOS updater") - subprocess.check_call(["git", "clean", "-xdf"], cwd=BASEDIR) - os.system(os.path.join(BASEDIR, "installer", "updater", "updater")) - raise Exception("NEOS outdated") - +def unblock_stdout(): # get a non-blocking stdout child_pid, child_pty = os.forkpty() if child_pid != 0: # parent @@ -54,6 +40,23 @@ if __name__ == "__main__": os._exit(os.wait()[1]) +if __name__ == "__main__": + if os.path.isfile("/init.qcom.rc") \ + and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 6): + + # update continue.sh before updating NEOS + if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")): + from shutil import copyfile + copyfile(os.path.join(BASEDIR, "scripts", "continue.sh"), "/data/data/com.termux/files/continue.sh") + + # run the updater + print("Starting NEOS updater") + subprocess.check_call(["git", "clean", "-xdf"], cwd=BASEDIR) + os.system(os.path.join(BASEDIR, "installer", "updater", "updater")) + raise Exception("NEOS outdated") + + unblock_stdout() + import glob import shutil import hashlib diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 0fa7dac68..077800d3b 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 42d0228e2..c5e8eab6c 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 6db8b61bc..749831be1 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -56,8 +56,16 @@ def set_eon_fan(val): if last_eon_fan_val is None or last_eon_fan_val != val: bus = SMBus(7, force=True) if LEON: - i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] - bus.write_i2c_block_data(0x3d, 0, [i]) + try: + i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] + bus.write_i2c_block_data(0x3d, 0, [i]) + except IOError: + # tusb320 + if val == 0: + bus.write_i2c_block_data(0x67, 0xa, [0]) + else: + bus.write_i2c_block_data(0x67, 0xa, [0x20]) + bus.write_i2c_block_data(0x67, 0x8, [(val-1)<<6]) else: bus.write_byte_data(0x21, 0x04, 0x2) bus.write_byte_data(0x21, 0x03, (val*2)+1) @@ -126,6 +134,9 @@ class LocationStarter(object): def thermald_thread(): setup_eon_fan() + # prevent LEECO from undervoltage + BATT_PERC_OFF = 10 if LEON else 3 + # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) @@ -160,6 +171,10 @@ def thermald_thread(): msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() + with open("/sys/class/power_supply/battery/current_now") as f: + msg.thermal.batteryCurrent = int(f.read()) + with open("/sys/class/power_supply/battery/voltage_now") as f: + msg.thermal.batteryVoltage = int(f.read()) with open("/sys/class/power_supply/usb/online") as f: msg.thermal.usbOnline = bool(int(f.read())) @@ -243,7 +258,7 @@ def thermald_thread(): # shutdown if the battery gets lower than 3%, it's discharging, we aren't running for # more than a minute but we were running - if msg.thermal.batteryPercent < 3 and msg.thermal.batteryStatus == "Discharging" and \ + if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \ started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 5f34afbcc..ca148f016 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ