openpilot v0.5.3 release
Vehicle Researcher
parent
8f6e36f426
commit
285c52eb69
88
README.md
88
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 | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| GM<sup>3</sup>| Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph |
|
||||
| GM<sup>3</sup>| 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 | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph<sup>1</sup>| 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 | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
| Toyota | C-HR 2018<sup>4</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Corolla 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| -------------------| ----------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| GM<sup>3</sup> | Volt 2017 | Driver Confidence II | Yes | Yes | 0mph | 7mph |
|
||||
| GM<sup>3</sup> | 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 | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph<sup>1</sup>| 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 | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai<sup>6</sup>| Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup>| All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
| Toyota | C-HR 2018<sup>4</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Corolla 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Highlander 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2018| All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
|
||||
<sup>1</sup>[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)***
|
||||
<sup>2</sup>When 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)
|
||||
<sup>3</sup>[GM installation guide](https://www.zoneos.com/volt.htm)
|
||||
<sup>4</sup>It 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/).
|
||||
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
|
||||
<sup>6</sup>Giraffe 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?
|
||||
------
|
||||
|
|
|
|||
|
|
@ -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!
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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("<Q", st)[0]
|
||||
x2_int = le
|
||||
x2_int = le >> (64 - 8 * size)
|
||||
ss = s[1]
|
||||
data_bit_pos = ss
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
#include <cassert>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 ###
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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"]
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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),
|
||||
}
|
||||
|
||||
|
|
@ -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'
|
||||
|
|
|
|||
|
|
@ -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'),
|
||||
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'),
|
||||
|
||||
(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'),
|
||||
(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'),
|
||||
|
||||
(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'),
|
||||
]
|
||||
(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]
|
||||
|
|
|
|||
|
|
@ -1,15 +1,20 @@
|
|||
#include "selfdrive/common/params.h"
|
||||
|
||||
#include "selfdrive/common/util.h"
|
||||
|
||||
#ifndef _GNU_SOURCE
|
||||
#define _GNU_SOURCE
|
||||
#endif // _GNU_SOURCE
|
||||
|
||||
#include <sys/file.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <dirent.h>
|
||||
#include <sys/file.h>
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#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<std::string, std::string> *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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,4 +32,10 @@ void read_db_value_blocking(const char* params_path, const char* key,
|
|||
} // extern "C"
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
#include <map>
|
||||
#include <string>
|
||||
int read_db_all(const char* params_path, std::map<std::string, std::string> *params);
|
||||
#endif
|
||||
|
||||
#endif // _SELFDRIVE_COMMON_PARAMS_H_
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.5.2-release"
|
||||
#define COMMA_VERSION "0.5.3-release"
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -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')
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue