mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-30 19:33:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'devel' of https://github.com/commaai/openpilot into devel-unsupported-cars

Browse Source 
# Conflicts:
#	opendbc/generator/toyota/lexus_is_hybrid_2017_pt.dbc
#	opendbc/lexus_is_hybrid_2017_pt_generated.dbc
#	selfdrive/car/toyota/carstate.py
#	selfdrive/car/toyota/interface.py
#	selfdrive/car/toyota/values.py
This commit is contained in:
dragonpilot
2019-09-11 11:16:41 +10:00
parent 8c442ec989 b0563a5968
commit c68503d8a1
199 changed files with 5774 additions and 2283 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/assets/sounds/disengaged.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/engaged.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/error.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/warning_1.wav
BIN
View File
Binary file not shown.
selfdrive/assets/sounds/warning_2.wav
BIN
View File
Binary file not shown.
selfdrive/athena/athenad.py
+2 -7
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python2.7
import json
import jwt
import os
import random
import re
@@ -13,7 +12,6 @@ import traceback
import zmq
import requests
import six.moves.queue
from datetime import datetime, timedelta
from functools import partial
from jsonrpc import JSONRPCResponseManager, dispatcher
from websocket import create_connection, WebSocketTimeoutException, ABNF
@@ -104,9 +102,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
params = Params()
dongle_id = params.get("DongleId")
private_key = open("/persist/comma/id_rsa").read()
identity_token = jwt.encode({'identity':dongle_id, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256')
identity_token = Api(dongle_id).get_token()
ws = create_connection(remote_ws_uri,
cookie="jwt=" + identity_token,
enable_multithread=True)
@@ -232,8 +228,7 @@ def main(gctx=None):
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
crash.install()
private_key = open("/persist/comma/id_rsa").read()
api = Api(dongle_id, private_key)
api = Api(dongle_id)
conn_retries = 0
while 1:
selfdrive/boardd/boardd.cc
+4
View File
@@ -43,6 +43,7 @@
#define SAFETY_TESLA 8
#define SAFETY_CHRYSLER 9
#define SAFETY_SUBARU 10
#define SAFETY_GM_PASSIVE 11
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@@ -135,6 +136,9 @@ void *safety_setter_thread(void *s) {
case cereal::CarParams::SafetyModel::GM:
safety_setting = SAFETY_GM;
break;
case cereal::CarParams::SafetyModel::GM_PASSIVE:
safety_setting = SAFETY_GM_PASSIVE;
break;
case cereal::CarParams::SafetyModel::HONDA_BOSCH:
safety_setting = SAFETY_HONDA_BOSCH;
break;
selfdrive/can/libdbc_py.py
+1 -1
View File
@@ -82,7 +82,7 @@ void* can_init(int bus, const char* dbc_name,
int can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
const DBC* dbc_lookup(const char* dbc_name);
selfdrive/can/parser.cc
+16 -12
View File
@@ -330,7 +330,7 @@ class CANParser {
}
}
void update_string(uint64_t sec, std::string data) {
void update_string(std::string data) {
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), data.data(), data.length());
@@ -339,10 +339,12 @@ class CANParser {
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto cans = event.getCan();
UpdateCans(sec, cans);
last_sec = event.getLogMonoTime();
UpdateValid(sec);
auto cans = event.getCan();
UpdateCans(last_sec, cans);
UpdateValid(last_sec);
}
int update(uint64_t sec, bool wait) {
@@ -381,17 +383,18 @@ class CANParser {
UpdateCans(sec, cans);
}
last_sec = sec;
UpdateValid(sec);
zmq_msg_close(&msg);
return result;
}
std::vector<SignalValue> query(uint64_t sec) {
std::vector<SignalValue> query_latest() {
std::vector<SignalValue> ret;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (sec != 0 && state.seen != sec) continue;
if (last_sec != 0 && state.seen != last_sec) continue;
for (int i=0; i<state.parse_sigs.size(); i++) {
const Signal &sig = state.parse_sigs[i];
@@ -408,6 +411,7 @@ class CANParser {
}
bool can_valid = false;
uint64_t last_sec = 0;
private:
const int bus;
@@ -451,31 +455,31 @@ int can_update(void* can, uint64_t sec, bool wait) {
return cp->update(sec, wait);
}
void can_update_string(void *can, uint64_t sec, const char* dat, int len) {
void can_update_string(void *can, const char* dat, int len) {
CANParser* cp = (CANParser*)can;
cp->update_string(sec, std::string(dat, len));
cp->update_string(std::string(dat, len));
}
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
CANParser* cp = (CANParser*)can;
if (out_can_valid) {
*out_can_valid = cp->can_valid;
}
const std::vector<SignalValue> values = cp->query(sec);
const std::vector<SignalValue> values = cp->query_latest();
if (out_values) {
std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values);
}
return values.size();
};
void can_query_vector(void* can, uint64_t sec, bool *out_can_valid, std::vector<SignalValue> &values) {
void can_query_latest_vector(void* can, bool *out_can_valid, std::vector<SignalValue> &values) {
CANParser* cp = (CANParser*)can;
if (out_can_valid) {
*out_can_valid = cp->can_valid;
}
values = cp->query(sec);
values = cp->query_latest();
};
}
selfdrive/can/parser_pyx.pxd
+5 -5
View File
@@ -67,9 +67,9 @@ ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
const char* tcp_addr,
int timeout)
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
ctypedef void (*can_update_string_func)(void* can, uint64_t sec, const char* dat, int len);
ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid, vector[SignalValue] &values)
ctypedef void (*can_update_string_func)(void* can, const char* dat, int len);
ctypedef size_t (*can_query_latest_func)(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_latest_vector_func)(void* can, bool *out_can_valid, vector[SignalValue] &values)
cdef class CANParser:
cdef:
@@ -79,7 +79,7 @@ cdef class CANParser:
can_init_with_vectors_func can_init_with_vectors
can_update_func can_update
can_update_string_func can_update_string
can_query_vector_func can_query_vector
can_query_latest_vector_func can_query_latest_vector
map[string, uint32_t] msg_name_to_address
map[uint32_t, string] address_to_msg_name
vector[SignalValue] can_values
@@ -91,4 +91,4 @@ cdef class CANParser:
bool can_valid
int can_invalid_cnt
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec)
cdef unordered_set[uint32_t] update_vl(self)
selfdrive/can/parser_pyx.pyx
+10 -10
View File
@@ -18,7 +18,7 @@ cdef class CANParser:
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
self.can_query_vector = <can_query_vector_func>dlsym(libdbc, 'can_query_vector')
self.can_query_latest_vector = <can_query_latest_vector_func>dlsym(libdbc, 'can_query_latest_vector')
if checks is None:
checks = []
@@ -72,14 +72,14 @@ cdef class CANParser:
message_options_v.push_back(mpo)
self.can = self.can_init_with_vectors(bus, dbc_name, message_options_v, signal_options_v, sendcan, tcp_addr, timeout)
self.update_vl(0)
self.update_vl()
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec):
cdef unordered_set[uint32_t] update_vl(self):
cdef string sig_name
cdef unordered_set[uint32_t] updated_val
cdef bool valid = False
self.can_query_vector(self.can, sec, &valid, self.can_values)
self.can_query_latest_vector(self.can, &valid, self.can_values)
# Update invalid flag
self.can_invalid_cnt += 1
@@ -100,20 +100,20 @@ cdef class CANParser:
return updated_val
def update_string(self, uint64_t sec, dat):
self.can_update_string(self.can, sec, dat, len(dat))
return self.update_vl(sec)
def update_string(self, dat):
self.can_update_string(self.can, dat, len(dat))
return self.update_vl()
def update_strings(self, uint64_t sec, strings):
def update_strings(self, strings):
updated_vals = set()
for s in strings:
updated_val = self.update_string(sec, s)
updated_val = self.update_string(s)
updated_vals.update(updated_val)
return updated_vals
def update(self, uint64_t sec, bool wait):
r = (self.can_update(self.can, sec, wait) >= 0)
updated_val = self.update_vl(sec)
updated_val = self.update_vl()
return r, updated_val
selfdrive/can/tests/parser_old.py
+2 -2
View File
@@ -71,12 +71,12 @@ class CANParser(object):
self.p_can_valid = ffi.new("bool*")
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
value_count = libdbc.can_query_latest(self.can, self.p_can_valid, 0, ffi.NULL)
self.can_values = ffi.new("SignalValue[%d]" % value_count)
self.update_vl(0)
def update_vl(self, sec):
can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
can_values_len = libdbc.can_query_latest(self.can, self.p_can_valid, len(self.can_values), self.can_values)
assert can_values_len <= len(self.can_values)
self.can_invalid_cnt += 1
selfdrive/can/tests/test_packer_toyota.py
+6 -4
View File
@@ -47,32 +47,34 @@ class TestPackerMethods(unittest.TestCase):
self.assertEqual(m_old, m)
steer = (random.randint(0, 2) % 2 == 0)
chime = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
m_old = create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
m = create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
m_old = create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
m = create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
self.assertEqual(m_old, m)
def test_performance(self):
n1 = sec_since_boot()
recursions = 100000
steer = (random.randint(0, 2) % 2 == 0)
chime = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
for _ in xrange(recursions):
create_ui_command(self.cp_old, steer, left_line, right_line, left_lane_depart, right_lane_depart)
create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_old = n2 - n1
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
n1 = sec_since_boot()
for _ in xrange(recursions):
create_ui_command(self.cp, steer, left_line, right_line, left_lane_depart, right_lane_depart)
create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_new = n2 - n1
# print('New API, elapsed time: {} secs'.format(elapsed_new))
selfdrive/can/tests/test_parser.py
+2 -3
View File
@@ -58,16 +58,15 @@ def run_route(route):
route_ok = True
t = 0
for msg in lr:
if msg.which() == 'can':
t += DT
t = msg.logMonoTime
msg_bytes = msg.as_builder().to_bytes()
can.send(msg_bytes)
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
updated_string = parser_string.update_string(t, msg_bytes)
updated_string = parser_string.update_string(msg_bytes)
if updated_old != updated_new:
route_ok = False
selfdrive/car/car_helpers.py
+12 -1
View File
@@ -1,4 +1,5 @@
import os
import zmq
from cereal import car
from common.params import Params
from common.vin import get_vin, VIN_UNKNOWN
@@ -8,6 +9,16 @@ from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
def get_one_can(logcan):
while True:
try:
can = messaging.recv_one(logcan)
if len(can.can) > 0:
return can
except zmq.error.Again:
continue
def get_startup_alert(car_recognized, controller_available):
alert = 'startup'
if not car_recognized:
@@ -86,7 +97,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
done = False
while not done:
a = messaging.recv_one(logcan)
a = get_one_can(logcan)
for can in a.can:
# need to independently try to fingerprint both bus 0 and 1 to work
selfdrive/car/chrysler/carcontroller.py
+1 -1
View File
@@ -31,7 +31,7 @@ class CarController(object):
self.packer = CANPacker(dbc_name)
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert):
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
selfdrive/car/chrysler/carstate.py
+8 -6
View File
@@ -1,20 +1,22 @@
from cereal import car
from selfdrive.can.parser import CANParser
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
from common.kalman.simple_kalman import KF1D
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(can_gear):
if can_gear == 0x1:
return "park"
return GearShifter.park
elif can_gear == 0x2:
return "reverse"
return GearShifter.reverse
elif can_gear == 0x3:
return "neutral"
return GearShifter.neutral
elif can_gear == 0x4:
return "drive"
return GearShifter.drive
elif can_gear == 0x5:
return "low"
return "unknown"
return GearShifter.low
return GearShifter.unknown
def get_can_parser(CP):
selfdrive/car/chrysler/interface.py
+9 -13
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@@ -8,13 +7,14 @@ from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera
from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
GearShifter = car.CarState.GearShifter
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
@@ -115,8 +115,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
@@ -160,8 +160,6 @@ class CarInterface(object):
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = self.CS.main_on
ret.cruiseState.speedOffset = 0.
# ignore standstill in hybrid rav4, since pcm allows to restart without
# receiving any special command
ret.cruiseState.standstill = False
# TODO: button presses
@@ -169,13 +167,13 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@@ -193,7 +191,7 @@ class CarInterface(object):
# events
events = []
if not (ret.gearShifter in ('drive', 'low')):
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
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]))
@@ -203,7 +201,7 @@ class CarInterface(object):
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':
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
@@ -236,8 +234,6 @@ class CarInterface(object):
if (self.CS.frame == -1):
return [] # if we haven't seen a frame 220, then do not update.
self.frame = self.CS.frame
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
can_sends = self.CC.update(c.enabled, self.CS, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert)
return can_sends
selfdrive/car/chrysler/radar_interface.py
+1 -3
View File
@@ -2,7 +2,6 @@
import os
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724
RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
@@ -55,8 +54,7 @@ class RadarInterface(object):
self.trigger_msg = LAST_MSG
def update(self, can_strings):
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
selfdrive/car/ford/carcontroller.py
+87
View File
@@ -0,0 +1,87 @@
from cereal import car
from selfdrive.car.ford.fordcan import make_can_msg, create_steer_command, create_lkas_ui, \
spam_cancel_button
from selfdrive.can.packer import CANPacker
MAX_STEER_DELTA = 1
TOGGLE_DEBUG = False
class CarController(object):
def __init__(self, dbc_name, enable_camera, vehicle_model):
self.packer = CANPacker(dbc_name)
self.enable_camera = enable_camera
self.enabled_last = False
self.main_on_last = False
self.vehicle_model = vehicle_model
self.generic_toggle_last = 0
self.steer_alert_last = False
self.lkas_action = 0
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
can_sends = []
steer_alert = visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired
apply_steer = actuators.steer
if self.enable_camera:
if pcm_cancel:
#print "CANCELING!!!!"
can_sends.append(spam_cancel_button(self.packer))
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last)
self.lkas_action &= 0xf
else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action))
self.generic_toggle_last = CS.generic_toggle
if (frame % 100) == 0:
can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False))
#can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False))
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
(self.steer_alert_last != steer_alert):
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
if (frame % 200) == 0:
can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False))
if (frame % 10) == 0:
can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False))
can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False))
can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False))
can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False))
can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False))
can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False))
can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False))
can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False))
can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False))
can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False))
static_msgs = range(1653, 1658)
for addr in static_msgs:
cnt = (frame % 10) + 1
can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False))
self.enabled_last = enabled
self.main_on_last = CS.main_on
self.steer_alert_last = steer_alert
return can_sends
selfdrive/car/ford/fordcan.py
+54
View File
@@ -0,0 +1,54 @@
from common.numpy_fast import clip
from selfdrive.car.ford.values import MAX_ANGLE
def make_can_msg(addr, dat, alt, cks=False):
return [addr, 0, dat, alt]
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action):
"""Creates a CAN message for the Ford Steer Command."""
#if enabled and lkas available:
if enabled and lkas_state in [2,3]: #and (frame % 500) >= 3:
action = lkas_action
else:
action = 0xf
angle_cmd = angle_steers/MAX_ANGLE
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
values = {
"Lkas_Action": action,
"Lkas_Alert": 0xf, # no alerts
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug?
#"Lane_Curvature": 0, # is it just for debug?
"Steer_Angle_Req": angle_cmd
}
return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values)
def create_lkas_ui(packer, main_on, enabled, steer_alert):
"""Creates a CAN message for the Ford Steer Ui."""
if not main_on:
lines = 0xf
elif enabled:
lines = 0x3
else:
lines = 0x6
values = {
"Set_Me_X80": 0x80,
"Set_Me_X45": 0x45,
"Set_Me_X30": 0x30,
"Lines_Hud": lines,
"Hands_Warning_W_Chime": steer_alert,
}
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values)
def spam_cancel_button(packer):
values = {
"Cancel": 1
}
return packer.make_can_msg("Steering_Buttons", 0, values)
selfdrive/car/ford/interface.py
+2 -2
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
@@ -48,6 +47,7 @@ class CarInterface(object):
ret.isPandaBlack = is_panda_black
ret.safetyModel = car.CarParams.SafetyModel.ford
ret.dashcamOnly = True
# pedal
ret.enableCruise = True
@@ -108,7 +108,7 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
self.CS.update(self.cp)
selfdrive/car/ford/radar_interface.py
+1 -3
View File
@@ -3,7 +3,6 @@ import os
import numpy as np
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
RADAR_MSGS = range(0x500, 0x540)
@@ -32,8 +31,7 @@ class RadarInterface(object):
self.updated_messages = set()
def update(self, can_strings):
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
selfdrive/car/gm/interface.py
+12 -11
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -9,6 +8,7 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
ButtonType = car.CarState.ButtonEvent.Type
class CanBus(object):
def __init__(self):
@@ -62,6 +62,7 @@ class CarInterface(object):
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
ret.openpilotLongitudinalControl = ret.enableCamera
tire_stiffness_factor = 0.444 # not optimized yet
ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive
if candidate == CAR.VOLT:
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -172,7 +173,7 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.pt_cp.update_strings(can_strings)
self.CS.update(self.pt_cp)
@@ -225,19 +226,19 @@ class CarInterface(object):
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
be.pressed = True
but = self.CS.cruise_buttons
@@ -246,13 +247,13 @@ class CarInterface(object):
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
if not (cruiseEnabled and self.CS.standstill):
be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
be.type = ButtonType.altButton3
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
@@ -302,10 +303,10 @@ class CarInterface(object):
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == "cancel" and b.pressed:
if b.type == ButtonType.cancel and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
selfdrive/car/gm/radar_interface.py
+1 -3
View File
@@ -6,7 +6,6 @@ from cereal import car
from selfdrive.can.parser import CANParser
from selfdrive.car.gm.interface import CanBus
from selfdrive.car.gm.values import DBC, CAR
from common.realtime import sec_since_boot
RADAR_HEADER_MSG = 1120
SLOT_1_MSG = RADAR_HEADER_MSG + 1
@@ -60,8 +59,7 @@ class RadarInterface(object):
time.sleep(0.05) # nothing to do
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
selfdrive/car/honda/carstate.py
+5 -2
View File
@@ -1,3 +1,4 @@
from cereal import car
from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from selfdrive.can.can_define import CANDefine
@@ -5,10 +6,12 @@ from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear, vals):
val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
'D': 'drive', 'S': 'sport', 'L': 'low'}
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}
try:
return val_to_capnp[vals[gear]]
except KeyError:
selfdrive/car/honda/hondacan.py
+1 -1
View File
@@ -47,7 +47,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
"FCW": fcw << 1,
"AEB_REQ_1": 0,
"AEB_REQ_2": 0,
"AEB": 0,
"AEB_STATUS": 0,
}
bus = get_pt_bus(car_fingerprint, is_panda_black)
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
selfdrive/car/honda/interface.py
+17 -15
View File
@@ -3,7 +3,7 @@ import os
import numpy as np
from cereal import car
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot, DT_CTRL
from common.realtime import DT_CTRL
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
@@ -15,6 +15,8 @@ from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
def compute_gb_honda(accel, speed):
creep_brake = 0.0
@@ -374,8 +376,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
@@ -438,19 +440,19 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
@@ -458,18 +460,18 @@ class CarInterface(object):
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = 'accelCruise'
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
be.type = ButtonType.altButton3
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.type = ButtonType.unknown
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
@@ -477,7 +479,7 @@ class CarInterface(object):
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = 'altButton1'
be.type = ButtonType.altButton1
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
@@ -493,7 +495,7 @@ class CarInterface(object):
events.append(create_event('steerTempUnavailable', [ET.WARNING]))
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not ret.gearShifter == 'drive':
if not ret.gearShifter == 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]))
@@ -503,7 +505,7 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on or self.CS.cruise_mode:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
@@ -538,7 +540,7 @@ class CarInterface(object):
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
self.last_enable_pressed = cur_time
enable_pressed = True
selfdrive/car/honda/radar_interface.py
+3 -4
View File
@@ -3,7 +3,6 @@ import os
import time
from cereal import car
from selfdrive.can.parser import CANParser
from common.realtime import sec_since_boot
def _create_nidec_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
@@ -38,11 +37,11 @@ class RadarInterface(object):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
time.sleep(0.05)
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05)
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
selfdrive/car/honda/values.py
+2 -2
View File
@@ -103,7 +103,7 @@ FINGERPRINTS = {
57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
}],
CAR.ODYSSEY_CHN: [{
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
}],
# 2017 Pilot Touring AND 2016 Pilot EX-L w/ Added Comma Pedal Support (512L & 513L)
CAR.PILOT: [{
@@ -146,7 +146,7 @@ DBC = {
CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can', 'acura_ilx_2016_nidec'),
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'),
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
selfdrive/car/hyundai/interface.py
+8 -8
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
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
@@ -8,6 +7,8 @@ from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
GearShifter = car.CarState.GearShifter
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
@@ -154,8 +155,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
@@ -212,13 +213,13 @@ class CarInterface(object):
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@@ -236,7 +237,7 @@ class CarInterface(object):
self.low_speed_alert = False
events = []
if not ret.gearShifter == 'drive':
if not ret.gearShifter == 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]))
@@ -246,12 +247,11 @@ class CarInterface(object):
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':
if ret.gearShifter == 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:
selfdrive/car/hyundai/radar_interface.py
+5 -3
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python
from cereal import car
import os
import time
from cereal import car
class RadarInterface(object):
def __init__(self, CP):
@@ -11,6 +11,8 @@ class RadarInterface(object):
def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05) # radard runs on RI updates
return ret
selfdrive/car/subaru/interface.py
+7 -6
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -8,6 +7,7 @@ from selfdrive.car.subaru.values import CAR
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
ButtonType = car.CarState.ButtonEvent.Type
class CarInterface(object):
def __init__(self, CP, CarController):
@@ -42,6 +42,7 @@ class CarInterface(object):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.radarOffCan = True
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = is_panda_black
@@ -96,8 +97,8 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
self.pt_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cam_cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
@@ -144,18 +145,18 @@ class CarInterface(object):
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
be = car.CarState.ButtonEvent.new_message()
be.type = 'accelCruise'
be.type = ButtonType.accelCruise
buttonEvents.append(be)
selfdrive/car/subaru/radar_interface.py
+5 -4
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python
from cereal import car
import os
import time
from cereal import car
class RadarInterface(object):
def __init__(self, CP):
@@ -10,8 +10,9 @@ class RadarInterface(object):
self.delay = 0.1
def update(self, can_strings):
ret = car.RadarData.new_message()
time.sleep(0.05) # radard runs on RI updates
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05) # radard runs on RI updates
return ret
selfdrive/car/toyota/carcontroller.py
+12 -4
View File
@@ -5,8 +5,8 @@ from selfdrive.car import create_gas_command
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
create_fcw_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
create_acc_cancel_command, create_fcw_command
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -209,7 +209,11 @@ class CarController(object):
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged
if ECU.DSU in self.fake_ecus:
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer))
elif ECU.DSU in self.fake_ecus:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
@@ -236,8 +240,12 @@ class CarController(object):
else:
send_ui = False
# disengage msg causes a bad fault sound so play a good sound instead
if pcm_cancel_cmd:
send_ui = True
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, left_line, right_line, left_lane_depart, right_lane_depart))
can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
selfdrive/car/toyota/carstate.py
+25 -41
View File
@@ -1,14 +1,17 @@
import numpy as np
from cereal import car
from common.kalman.simple_kalman import KF1D
from selfdrive.can.can_define import CANDefine
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear, vals):
val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
'D': 'drive', 'B': 'brake'}
val_to_capnp = {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'B': GearShifter.brake}
try:
return val_to_capnp[vals[gear]]
except KeyError:
@@ -41,10 +44,14 @@ def get_can_parser(CP):
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
("LKA_STATE", "EPS_STATUS", 0),
("IPAS_STATE", "EPS_STATUS", 1),
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
]
checks = [
("BRAKE_MODULE", 40),
("GAS_PEDAL", 33),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
@@ -52,35 +59,19 @@ def get_can_parser(CP):
("EPS_STATUS", 25),
]
if CP.carFingerprint == CAR.LEXUS_IS:
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
checks.append(("DSU_CRUISE", 5))
else:
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
checks.append(("PCM_CRUISE_2", 33))
if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
if CP.carFingerprint == CAR.LEXUS_ISH:
checks += [
("BRAKE_MODULE", 50),
("GAS_PEDAL", 50),
("PCM_CRUISE_ISH", 1),
]
signals += [
("MAIN_ON", "PCM_CRUISE_ISH", 0),
("SET_SPEED", "PCM_CRUISE_ISH", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK_ISH", 0),
]
else:
checks += [
("BRAKE_MODULE", 40),
("GAS_PEDAL", 33),
("PCM_CRUISE_2", 33),
]
signals += [
("MAIN_ON", "PCM_CRUISE_2", 0),
("SET_SPEED", "PCM_CRUISE_2", 0),
("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0),
("IPAS_STATE", "EPS_STATUS", 1),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
]
if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)]
@@ -176,8 +167,8 @@ class CarState(object):
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
if self.CP.carFingerprint == CAR.LEXUS_ISH:
self.main_on = cp.vl["PCM_CRUISE_ISH"]['MAIN_ON']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
@@ -186,10 +177,7 @@ class CarState(object):
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
if self.CP.carFingerprint == CAR.LEXUS_ISH:
self.ipas_active = False
else:
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
@@ -197,20 +185,16 @@ class CarState(object):
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
if self.CP.carFingerprint == CAR.LEXUS_ISH:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_ISH"]['SET_SPEED']
# ish does not have curise status value (always 0), so we use curise_active value instead
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
self.low_speed_lockout = False
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
elif self.CP.carFingerprint == CAR.LEXUS_ISH:
self.generic_toggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM'])
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
selfdrive/car/toyota/interface.py
+21 -19
View File
@@ -1,14 +1,16 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
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.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR, TSS2_CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
from selfdrive.swaglog import cloudlog
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
class CarInterface(object):
def __init__(self, CP, CarController):
self.CP = CP
@@ -207,15 +209,15 @@ class CarInterface(object):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kf = 0.00007818594
elif candidate == CAR.LEXUS_ISH:
stop_and_go = True
ret.safetyParam = 100
ret.wheelbase = 2.80 # in spec
ret.steerRatio = 13.3 # in spec
tire_stiffness_factor = 0.444 # from camry
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG # in spec, mean of is300 (1680 kg) / is300h (1720 kg) / is350 (1685 kg)
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.04]]
ret.lateralTuning.pid.kf = 0.00006 # from camry
elif candidate == CAR.LEXUS_IS:
stop_and_go = False
ret.safetyParam = 66
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
tire_stiffness_factor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kf = 0.00006
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@@ -246,8 +248,8 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM, candidate) or is_panda_black
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU, candidate) or (is_panda_black and candidate in TSS2_CAR)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
@@ -280,11 +282,11 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp.update_strings(can_strings)
# run the cam can update for 10s as we just need to know if the camera is alive
if self.frame < 1000:
self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp)
@@ -345,13 +347,13 @@ class CarInterface(object):
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.type = ButtonType.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.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
@@ -369,7 +371,7 @@ class CarInterface(object):
if self.cp_cam.can_valid:
self.forwarding_camera = True
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu:
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]))
@@ -379,7 +381,7 @@ class CarInterface(object):
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on and self.CP.enableDsu:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse' and self.CP.enableDsu:
if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu:
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]))
selfdrive/car/toyota/radar_interface.py
+1 -3
View File
@@ -3,7 +3,6 @@ import os
import time
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
def _create_radar_can_parser(car_fingerprint):
@@ -58,8 +57,7 @@ class RadarInterface(object):
time.sleep(0.05)
return car.RadarData.new_message()
tm = int(sec_since_boot() * 1e9)
vls = self.rcp.update_strings(tm, can_strings)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
selfdrive/car/toyota/toyotacan.py
+14 -2
View File
@@ -95,6 +95,18 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
return packer.make_can_msg("ACC_CONTROL", 0, values)
def create_acc_cancel_command(packer):
values = {
"GAS_RELEASED": 0,
"CRUISE_ACTIVE": 0,
"STANDSTILL_ON": 0,
"ACCEL_NET": 0,
"CRUISE_STATE": 0,
"CANCEL_REQ": 1,
}
return packer.make_can_msg("PCM_CRUISE", 0, values)
def create_fcw_command(packer, fcw):
values = {
"FCW": fcw,
@@ -105,7 +117,7 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values)
def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, right_lane_depart):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart):
values = {
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
@@ -117,7 +129,7 @@ def create_ui_command(packer, steer, left_line, right_line, left_lane_depart, ri
"SET_ME_X01": 1,
"SET_ME_X01_2": 1,
"REPEATED_BEEPS": 0,
"TWO_BEEPS": 0,
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
}
return packer.make_can_msg("LKAS_HUD", 0, values)
selfdrive/car/toyota/values.py
+28 -24
View File
@@ -17,7 +17,8 @@ class CAR:
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
SIENNA = "TOYOTA SIENNA XLE 2018"
LEXUS_ISH = "LEXUS IS HYBRID 2017"
LEXUS_IS = "LEXUS IS300 2018"
class ECU:
CAM = 0 # camera
@@ -28,25 +29,19 @@ class ECU:
# addr: (ecu, cars, bus, 1/freq*100, vl)
STATIC_MSGS = [
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
(0x130, ECU.CAM, (CAR.LEXUS_ISH), 1, 100, '\x00\x00\x03\xff\x00\x00\x3a'),
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.LEXUS_ISH), 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, CAR.AVALON, CAR.LEXUS_ISH), 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, CAR.AVALON, CAR.LEXUS_ISH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 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, CAR.AVALON), 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, CAR.AVALON), 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, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x245, ECU.CAM, (CAR.LEXUS_ISH), 1, 5, '\x00\x10\x01\x00'),
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
(0x248, ECU.CAM, (CAR.LEXUS_ISH), 1, 5, '\x00\x01\x00\x00\x00\x00\x00'),
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, '\x06\x00'),
(0x367, ECU.CAM, (CAR.LEXUS_ISH), 0, 40, '\x02\x00'),
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.LEXUS_ISH), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 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, CAR.AVALON), 1, 100, '\x24\x20\xB1'),
(0x466, ECU.CAM, (CAR.LEXUS_ISH), 1, 100, '\x41\x20\xce'),
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.LEXUS_ISH), 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, CAR.AVALON, CAR.LEXUS_ISH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 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, CAR.AVALON), 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, CAR.AVALON), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
(0x48b, ECU.CAM, (CAR.LEXUS_ISH), 0, 100, '\x66\x07\x0a\x0f\x01\x00\x00\x00'),
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_ISH), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1, 3, '\x03\x00\x20\x00\x00\x52'),
@@ -83,15 +78,13 @@ ECU_FINGERPRINT = {
}
def check_ecu_msgs(fingerprint, ecu):
# return True if fingerprint contains messages normally sent by a given ecu
return ECU_FINGERPRINT[ecu] in fingerprint
def check_ecu_msgs(fingerprint, ecu, car):
# return True if the reference car fingerprint doesn't contain the ecu fingerprint msg or
# fingerprint contains messages normally sent by a given ecu
return any(ECU_FINGERPRINT[ecu] not in finger for finger in FINGERPRINTS[car]) or ECU_FINGERPRINT[ecu] in fingerprint
FINGERPRINTS = {
CAR.LEXUS_ISH: [{
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.RAV4: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 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, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 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, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 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, 2015: 8, 2024: 8
}],
@@ -118,8 +111,9 @@ FINGERPRINTS = {
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
},
# RX450HL
# TODO: get proper fingerprint in stock mode
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 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: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 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: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# RX540H 2019 with color hud
{
@@ -137,8 +131,9 @@ FINGERPRINTS = {
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 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, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
#XSE and SE
# TODO: get proper fingerprint in stock mode
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 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, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 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, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.CAMRYH: [
#SE, LE and LE with Blindspot Monitor
@@ -198,6 +193,15 @@ FINGERPRINTS = {
CAR.SIENNA: [{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 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, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.LEXUS_IS: [
# IS300 2018
{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 400: 6, 426: 6, 452: 8, 464: 8, 466: 8, 467: 5, 544: 4, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1009: 8, 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, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1590: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1648: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# IS300H 2017
{
36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
}
STEER_THRESHOLD = 100
@@ -219,9 +223,9 @@ DBC = {
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_ISH: dbc_dict('lexus_is_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.LEXUS_ISH] # no resume button press required
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required
selfdrive/common/touch.c
+4 -4
View File
@@ -69,14 +69,15 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
return -1;
}
switch (event.type) {
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
} else if (event.code == ABS_MT_POSITION_Y) {
s->last_y = event.value;
} else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
up = true;
}
up = true;
break;
default:
break;
@@ -98,7 +99,7 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
return -1;
}
bool up = false;
switch (event.type) {
switch (event.type) {
case EV_ABS:
if (event.code == ABS_MT_POSITION_X) {
s->last_x = event.value;
@@ -117,4 +118,3 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
}
return up;
}
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.6.3-release"
#define COMMA_VERSION "0.6.4-release"
selfdrive/config.py
+2 -1
View File
@@ -17,7 +17,8 @@ class Conversions:
LB_TO_KG = 0.453592
RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
class UIParams:
lidar_x, lidar_y, lidar_zoom = 384, 960, 6
selfdrive/controls/controlsd.py
+42 -42
View File
@@ -6,12 +6,12 @@ from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler
from common.params import Params
from common.params import Params, put_nonblocking
import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_alert
from selfdrive.car.car_helpers import get_car, get_startup_alert, get_one_can
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import get_events, \
create_event, \
@@ -26,6 +26,7 @@ from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.controls.lib.gps_helpers import is_rhd_region
from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus
@@ -50,10 +51,6 @@ def events_to_bytes(events):
ret.append(e.to_bytes())
return ret
def wait_for_can(logcan):
print("Waiting for CAN messages...")
while len(messaging.recv_one(logcan).can) == 0:
pass
def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params):
@@ -85,6 +82,12 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", str(int(is_rhd)))
# Handle calibration
if sm.updated['liveCalibration']:
@@ -118,7 +121,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
# Driver monitoring
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@@ -283,9 +286,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
@@ -324,7 +326,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = driver_status.awareness < 0.
@@ -341,7 +343,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"alertType": AM.alert_type,
"alertSound": AM.audible_alert,
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
"driverMonitoringOn": bool(driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'],
@@ -364,7 +366,6 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(sm['plan'].jerkFactor),
"angleModelBias": 0.,
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
@@ -380,7 +381,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
dat.controlsState.lateralControlState.lqrState = lac_log
elif CP.lateralTuning.which() == 'indi':
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
pm.send('controlsState', dat)
# carState
cs_send = messaging.new_message()
@@ -388,7 +389,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
pm.send('carState', cs_send)
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
@@ -396,26 +397,26 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
ce_send = messaging.new_message()
ce_send.init('carEvents', len(events))
ce_send.carEvents = events
carevents.send(ce_send.to_bytes())
pm.send('carEvents', ce_send)
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message()
cp_send.init('carParams')
cp_send.carParams = CP
carparams.send(cp_send.to_bytes())
pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
pm.send('carControl', cc_send)
return CC, events_bytes
def controlsd_thread(gctx=None):
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
@@ -423,39 +424,35 @@ def controlsd_thread(gctx=None):
params = Params()
# Pub Sockets
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
logcan = messaging.sub_sock(service_list['can'].port)
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
'gpsLocation'], ignore_alive=['gpsLocation'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
wait_for_can(logcan)
print("Waiting for CAN messages...")
get_one_can(can_sock)
CI, CP = get_car(logcan, sendcan, is_panda_black)
logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
read_only = not car_recognized or not controller_available or CP.dashcamOnly
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
CP.safetyModel = CP.safetyModelPassive
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
@@ -478,6 +475,9 @@ def controlsd_thread(gctx=None):
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled
soft_disable_timer = 0
@@ -550,16 +550,16 @@ def controlsd_thread(gctx=None):
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def main(gctx=None):
controlsd_thread(gctx)
def main(sm=None, pm=None, logcan=None):
controlsd_thread(sm, pm, logcan)
if __name__ == "__main__":
selfdrive/controls/lib/drive_helpers.py
-20
View File
@@ -59,26 +59,6 @@ def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
min_offset = -5. # deg
max_offset = 5. # deg
alpha = 1. / 36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
min_learn_speed = 1.
# learn less at low speed or when turning
slow_factor = 1. / (1. + 0.02 * abs(angle_steers) * v_ego)
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) * slow_factor
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
angle_model_bias += c_poly[3] * alpha_v
angle_model_bias = clip(angle_model_bias, min_offset, max_offset)
return angle_model_bias
def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
selfdrive/controls/lib/driver_monitor.py
+93 -80
View File
@@ -1,14 +1,15 @@
import numpy as np
from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
from common.realtime import DT_CTRL, DT_DMON
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter
_AWARENESS_TIME = 90. # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME_TILL_TERMINAL = 20. # a first alert is issued 20s before expiration
_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5. # a second alert is issued 5s before start decelerating the car
_DISTRACTED_TIME = 10.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration
_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car
_DISTRACTED_TIME = 11.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
_FACE_THRESHOLD = 0.4
_EYE_THRESHOLD = 0.4
@@ -18,8 +19,15 @@ _METRIC_THRESHOLD = 0.4
_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch
_PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_VARIANCE_FILTER_TS = 20. # 0.008Hz
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
@@ -36,7 +44,6 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
# TODO: calibrate based on position
pitch_prnet = angles_desc[0]
yaw_prnet = angles_desc[1]
roll_prnet = angles_desc[2]
@@ -52,152 +59,158 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
return np.array([roll, pitch, yaw])
class _DriverPose():
class DriverPose():
def __init__(self):
self.yaw = 0.
self.pitch = 0.
self.roll = 0.
self.yaw_offset = 0.
self.pitch_offset = 0.
self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT)
class _DriverBlink():
class DriverBlink():
def __init__(self):
self.left_blink = 0.
self.right_blink = 0.
def _monitor_hysteresis(variance_level, monitor_valid_prev):
var_thr = 0.63 if monitor_valid_prev else 0.37
return variance_level < var_thr
class DriverStatus():
def __init__(self, monitor_on=False):
self.pose = _DriverPose()
self.blink = _DriverBlink()
self.monitor_on = monitor_on
self.monitor_param_on = monitor_on
self.monitor_valid = True # variance needs to be low
def __init__(self):
self.pose = DriverPose()
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
self.blink = DriverBlink()
self.awareness = 1.
self.awareness_active = 1.
self.awareness_passive = 1.
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON)
self.variance_high = False
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
self.ts_last_check = 0.
self.face_detected = False
self.terminal_alert_cnt = 0
self.step_change = 0.
self._set_timers(self.monitor_on)
self.active_monitoring_mode = True
self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
def _reset_filters(self):
self.driver_distraction_filter.x = 0.
self.variance_filter.x = 0.
self.monitor_valid = True
self.is_rhd_region = False
self.is_rhd_region_checked = False
self._set_timers(active_monitoring=True)
def _set_timers(self, active_monitoring):
if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
if active_monitoring:
self.step_change = DT_CTRL / _DISTRACTED_TIME
else:
self.step_change = 0.
return # no exploit after orange alert
elif self.awareness <= 0.:
return
if active_monitoring:
# when falling back from passive mode to active mode, reset awareness to avoid false alert
if self.step_change == DT_CTRL / _AWARENESS_TIME:
self.awareness = 1.
if not self.active_monitoring_mode:
self.awareness_passive = self.awareness
self.awareness = self.awareness_active
self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
self.step_change = DT_CTRL / _DISTRACTED_TIME
self.active_monitoring_mode = True
else:
if self.active_monitoring_mode:
self.awareness_active = self.awareness
self.awareness = self.awareness_passive
self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
self.step_change = DT_CTRL / _AWARENESS_TIME
self.active_monitoring_mode = False
def _is_driver_distracted(self, pose, blink):
# TODO: natural pose calib of each driver
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
if not self.pose_calibrated:
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
else:
pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
pitch_error *= _PITCH_WEIGHT
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
if pose_metric > _METRIC_THRESHOLD:
return DistractedType.BAD_POSE
return DistractedType.BAD_POSE
elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
return DistractedType.BAD_BLINK
else:
return DistractedType.NOT_DISTRACTED
def get_pose(self, driver_monitoring, params, cal_rpy):
def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged):
# 10 Hz
if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
return
self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD
self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
# first order filters
self.driver_distraction_filter.update(self.driver_distracted)
monitor_param_on_prev = self.monitor_param_on
# update offseter
# only update when driver is actively driving the car above a certain speed
if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and not op_engaged:
self.pose.pitch_offseter.push_and_update(self.pose.pitch)
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
# don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.ts_last_check > 1.:
self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
self.ts_last_check = ts
self.monitor_on = self.monitor_valid and self.monitor_param_on
if monitor_param_on_prev != self.monitor_param_on:
self._reset_filters()
self._set_timers(self.monitor_on and self.face_detected)
self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \
self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT
self._set_timers(self.face_detected)
def update(self, events, driver_engaged, ctrl_active, standstill):
if driver_engaged:
if (driver_engaged and self.awareness > 0) or not ctrl_active:
# reset only when on disengagement if red reached
self.awareness = 1.
self.awareness_active = 1.
self.awareness_passive = 1.
return events
driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
driver_attentive = self.driver_distraction_filter.x < 0.37
awareness_prev = self.awareness
if (driver_engaged and self.awareness > 0) or not ctrl_active:
# always reset if driver is in control (unless we are in red alert state) or op isn't active
self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)
if (driver_attentive and self.face_detected and self.awareness > 0):
# only restore awareness when paying attention and alert is not red
self.awareness = min(self.awareness + ((_RECOVERY_FACTOR_MAX-_RECOVERY_FACTOR_MIN)*(1.-self.awareness)+_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
if self.awareness == 1.:
self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
# don't display alert banner when awareness is recovering and has cleared orange
if self.awareness > self.threshold_prompt:
return events
# should always be counting if distracted unless at standstill and reaching orange
if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
if (not self.face_detected or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1)
alert = None
if self.awareness < 0.:
if self.awareness <= 0.:
# terminal red alert: disengagement required
alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
if awareness_prev >= 0.:
alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive'
if awareness_prev > 0.:
self.terminal_alert_cnt += 1
elif self.awareness <= self.threshold_prompt:
# prompt orange alert
alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
alert = 'promptDriverDistracted' if self.active_monitoring_mode else 'promptDriverUnresponsive'
elif self.awareness <= self.threshold_pre:
# pre green alert
alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
alert = 'preDriverDistracted' if self.active_monitoring_mode else 'preDriverUnresponsive'
if alert is not None:
events.append(create_event(alert, [ET.WARNING]))
return events
if __name__ == "__main__":
ds = DriverStatus(True)
ds.driver_distraction_filter.x = 0.
ds.driver_distracted = 1
for i in range(10):
ds.update([], False, True, False)
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
ds.update([], True, True, False)
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
selfdrive/controls/lib/fcw.py
+1 -3
View File
@@ -45,7 +45,6 @@ class FCWChecker(object):
def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
a_target = mpc_solution_a[1]
self.last_min_a = min(mpc_solution_a)
self.v_lead_max = max(self.v_lead_max, v_lead)
@@ -66,9 +65,8 @@ class FCWChecker(object):
future_fcw_allowed = all(c >= 10 for c in self.counters.values())
future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
current_fcw = a_target < -3.0 and active
if (future_fcw or current_fcw) and (self.last_fcw_time + 5.0 < cur_time):
if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
self.last_fcw_time = cur_time
self.last_fcw_a = self.last_min_a
return True
selfdrive/controls/lib/gps_helpers.py
Executable
+17
View File
@@ -0,0 +1,17 @@
_RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \
['IN1', 6.75, 28.10, 68.17, 97.4], \
['IN2', 28.09, 35.99, 72.18, 80.87], \
['IRL', 51.42, 55.38, -10.58, -5.99], \
['JP1', 32.66, 45.52, 137.27, 146.02], \
['JP2', 32.79, 37.60, 131.41, 137.28], \
['JP3', 24.04, 34.78, 122.93, 131.42], \
['NZ', -52.61, -29.24, 166, 178.84], \
['SF', -35.14, -22.13, 16.07, 33.21], \
['UK', 49.9, 60.84, -8.62, 1.77] ]
def is_rhd_region(latitude, longitude):
for region in _RHD_REGION_MAP:
if region[1] <= latitude <= region[2] and \
region[3] <= longitude <= region[4]:
return True
return False
selfdrive/controls/lib/lane_planner.py
+12 -5
View File
@@ -1,9 +1,19 @@
from common.numpy_fast import interp
import numpy as np
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
CAMERA_OFFSET = 0.06 # m from center car to camera
def compute_path_pinv(l=50):
deg = 3
x = np.arange(l*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
# This will improve behaviour when lanes suddenly widen
@@ -16,7 +26,7 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
path_from_right_lane = r_poly.copy()
path_from_right_lane[3] += lane_width / 2.0
lr_prob = l_prob + r_prob - l_prob * r_prob
lr_prob = l_prob * r_prob
d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
@@ -35,7 +45,6 @@ class LanePlanner(object):
self.l_prob = 0.
self.r_prob = 0.
self.lr_prob = 0.
self._path_pinv = compute_path_pinv()
self.x_points = np.arange(50)
@@ -57,8 +66,6 @@ class LanePlanner(object):
self.l_poly[3] += CAMERA_OFFSET
self.r_poly[3] += CAMERA_OFFSET
self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
# Find current lanewidth
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
selfdrive/controls/lib/latcontrol_helpers.py
-62
View File
@@ -1,62 +0,0 @@
import numpy as np
import math
from common.numpy_fast import interp
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_d_lookahead(v_ego, d_poly):
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly) > 0:
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts)) / pts_len
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math.radians(angle_steers - angle_offset)
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
return steer_des, curvature
def compute_path_pinv(l=50):
deg = 3
x = np.arange(l*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
selfdrive/controls/lib/latcontrol_lqr.py
+11 -6
View File
@@ -22,6 +22,7 @@ class LatControlLQR(object):
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.reset()
def reset(self):
@@ -31,6 +32,7 @@ class LatControlLQR(object):
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego)
torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
@@ -44,29 +46,32 @@ class LatControlLQR(object):
if v_ego < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
lqr_output = torque_scale * u_lqr / self.scale
self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0))):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
# Clip output
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
return self.output_steer, float(self.angle_steers_des), lqr_log
selfdrive/controls/lib/long_mpc.py
+5 -6
View File
@@ -12,8 +12,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False)
class LongitudinalMpc(object):
def __init__(self, mpc_id, live_longitudinal_mpc):
self.live_longitudinal_mpc = live_longitudinal_mpc
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.setup_mpc()
@@ -27,7 +26,7 @@ class LongitudinalMpc(object):
self.last_cloudlog_t = 0.0
def send_mpc_solution(self, qp_iterations, calculation_time):
def send_mpc_solution(self, pm, qp_iterations, calculation_time):
qp_iterations = max(0, qp_iterations)
dat = messaging.new_message()
dat.init('liveLongitudinalMpc')
@@ -41,7 +40,7 @@ class LongitudinalMpc(object):
dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.mpcId = self.mpc_id
dat.liveLongitudinalMpc.calculationTime = calculation_time
self.live_longitudinal_mpc.send(dat.to_bytes())
pm.send('liveLongitudinalMpc', dat)
def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
@@ -58,7 +57,7 @@ class LongitudinalMpc(object):
self.cur_state[0].v_ego = v
self.cur_state[0].a_ego = a
def update(self, CS, lead, v_cruise_setpoint):
def update(self, pm, CS, lead, v_cruise_setpoint):
v_ego = CS.vEgo
# Setup current mpc state
@@ -97,7 +96,7 @@ class LongitudinalMpc(object):
duration = int((sec_since_boot() - t) * 1e9)
if LOG_MPC:
self.send_mpc_solution(n_its, duration)
self.send_mpc_solution(pm, n_its, duration)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
selfdrive/controls/lib/pathplanner.py
+9 -14
View File
@@ -4,7 +4,6 @@ import numpy as np
# from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
@@ -26,9 +25,6 @@ class PathPlanner(object):
self.last_cloudlog_t = 0
self.plan = messaging.pub_sock(service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(service_list['liveMpc'].port)
self.setup_mpc(CP.steerRateCost)
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
@@ -49,13 +45,12 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def update(self, sm, CP, VM):
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset_average = sm['liveParameters'].angleOffsetAverage
angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
angle_offset = sm['liveParameters'].angleOffset
self.LP.update(v_ego, sm['model'])
@@ -73,7 +68,7 @@ class PathPlanner(object):
# self.path_offset_i = 0.0
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -85,19 +80,19 @@ class PathPlanner(object):
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers - angle_offset_bias) / VM.sR
self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
@@ -121,13 +116,13 @@ class PathPlanner(object):
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(self.path_offset_i)
plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
self.plan.send(plan_send.to_bytes())
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message()
@@ -137,4 +132,4 @@ class PathPlanner(object):
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
self.livempc.send(dat.to_bytes())
pm.send('liveMpc', dat)
selfdrive/controls/lib/planner.py
+7 -13
View File
@@ -9,7 +9,6 @@ from cereal import car
from common.realtime import sec_since_boot, DT_PLAN
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.fcw import FCWChecker
@@ -71,14 +70,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
class Planner(object):
def __init__(self, CP, fcw_enabled):
def __init__(self, CP):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
@@ -93,7 +89,6 @@ class Planner(object):
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.path_x = np.arange(192)
self.params = Params()
@@ -125,7 +120,7 @@ class Planner(object):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
def update(self, sm, CP, VM, PP):
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
@@ -199,8 +194,8 @@ class Planner(object):
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
@@ -251,10 +246,9 @@ class Planner(object):
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send out fcw
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
selfdrive/controls/lib/radar_helpers.py
+2 -1
View File
@@ -26,6 +26,7 @@ class Track(object):
def __init__(self):
self.ekf = None
self.cnt = 0
self.aLeadTau = _LEAD_ACCEL_TAU
def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
# relative values, copy
@@ -123,7 +124,7 @@ class Cluster(object):
@property
def measured(self):
return any([t.measured for t in self.tracks])
return any(t.measured for t in self.tracks)
def get_RadarState(self, model_prob=0.0):
return {
selfdrive/controls/plannerd.py
+11 -12
View File
@@ -11,27 +11,26 @@ from selfdrive.controls.lib.pathplanner import PathPlanner
import selfdrive.messaging as messaging
def plannerd_thread():
def plannerd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(2)
params = Params()
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP, fcw_enabled)
PL = Planner(CP)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
if sm is None:
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'])
if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
sm['liveParameters'].valid = True
sm['liveParameters'].sensorValid = True
@@ -42,13 +41,13 @@ def plannerd_thread():
sm.update()
if sm.updated['model']:
PP.update(sm, CP, VM)
PP.update(sm, pm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, CP, VM, PP)
PL.update(sm, pm, CP, VM, PP)
def main(gctx=None):
plannerd_thread()
def main(sm=None, pm=None):
plannerd_thread(sm, pm)
if __name__ == "__main__":
selfdrive/controls/radard.py
+13 -10
View File
@@ -173,7 +173,7 @@ class RadarD(object):
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
@@ -186,14 +186,17 @@ def radard_thread(gctx=None):
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
can_sock = messaging.sub_sock(service_list['can'].port)
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
if can_sock is None:
can_sock = messaging.sub_sock(service_list['can'].port)
RI = RadarInterface(CP)
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
radarState = messaging.pub_sock(service_list['radarState'].port)
liveTracks = messaging.pub_sock(service_list['liveTracks'].port)
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(rate, print_delay_threshold=None)
RD = RadarD(mocked)
@@ -212,7 +215,7 @@ def radard_thread(gctx=None):
dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
radarState.send(dat.to_bytes())
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
@@ -226,13 +229,13 @@ def radard_thread(gctx=None):
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
liveTracks.send(dat.to_bytes())
pm.send('liveTracks', dat)
rk.monitor_time()
def main(gctx=None):
radard_thread(gctx)
def main(sm=None, pm=None, can_sock=None):
radard_thread(sm, pm, can_sock)
if __name__ == "__main__":
selfdrive/controls/tests/test_following_distance.py
+7 -6
View File
@@ -15,9 +15,9 @@ def RW(v_ego, v_l):
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
class FakeSocket(object):
def send(self, data):
assert data
class FakePubMaster(object):
def send(self, s, data):
assert data
def run_following_distance_simulation(v_lead, t_end=200.0):
@@ -32,7 +32,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
v_cruise_setpoint = v_lead + 10.
mpc = LongitudinalMpc(1, FakeSocket())
pm = FakePubMaster()
mpc = LongitudinalMpc(1)
first = True
while t < t_end:
@@ -61,8 +62,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS.carState, lead, v_cruise_setpoint)
mpc.update(CS.carState, lead, v_cruise_setpoint)
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
mpc.update(pm, CS.carState, lead, v_cruise_setpoint)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
selfdrive/controls/tests/test_monitoring.py
+200
View File
@@ -0,0 +1,200 @@
import unittest
import numpy as np
from common.realtime import DT_CTRL, DT_DMON
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, \
_AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \
_AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \
_DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL
from selfdrive.controls.lib.gps_helpers import is_rhd_region
_TEST_TIMESPAN = 120 # seconds
_DISTRACTED_SECONDS_TO_ORANGE = _DISTRACTED_TIME - _DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
_DISTRACTED_SECONDS_TO_RED = _DISTRACTED_TIME + 1
_INVISIBLE_SECONDS_TO_ORANGE = _AWARENESS_TIME - _AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
_INVISIBLE_SECONDS_TO_RED = _AWARENESS_TIME + 1
class fake_DM_msg():
def __init__(self, is_face_detected, is_distracted=False):
self.faceOrientation = [0.,0.,0.]
self.facePosition = [0.,0.]
self.faceProb = 1. * is_face_detected
self.leftEyeProb = 1.
self.rightEyeProb = 1.
self.leftBlinkProb = 1. * is_distracted
self.rightBlinkProb = 1. * is_distracted
# driver state from neural net, 10Hz
msg_NO_FACE_DETECTED = fake_DM_msg(is_face_detected=False)
msg_ATTENTIVE = fake_DM_msg(is_face_detected=True)
msg_DISTRACTED = fake_DM_msg(is_face_detected=True, is_distracted=True)
# driver interaction with car
car_interaction_DETECTED = True
car_interaction_NOT_DETECTED = False
# openpilot state
openpilot_ENGAGED = True
openpilot_NOT_ENGAGED = False
# car standstill state
car_STANDSTILL = True
car_NOT_STANDSTILL = False
# some common state vectors
always_no_face = [msg_NO_FACE_DETECTED] * int(_TEST_TIMESPAN/DT_DMON)
always_attentive = [msg_ATTENTIVE] * int(_TEST_TIMESPAN/DT_DMON)
always_distracted = [msg_DISTRACTED] * int(_TEST_TIMESPAN/DT_DMON)
always_true = [True] * int(_TEST_TIMESPAN/DT_DMON)
always_false = [False] * int(_TEST_TIMESPAN/DT_DMON)
def run_DState_seq(driver_state_msgs, driver_car_interaction, openpilot_status, car_standstill_status):
# inputs are all 10Hz
DS = DriverStatus()
events_from_DM = []
for idx in range(len(driver_state_msgs)):
DS.get_pose(driver_state_msgs[idx], [0,0,0], 0, openpilot_status[idx])
# cal_rpy and car_speed don't matter here
# to match frequency of controlsd (100Hz)
for _ in range(int(DT_DMON/DT_CTRL)):
event_per_state = DS.update([], driver_car_interaction[idx], openpilot_status[idx], car_standstill_status[idx])
events_from_DM.append(event_per_state) # evaluate events at 10Hz for tests
assert len(events_from_DM)==len(driver_state_msgs), 'somethings wrong'
return events_from_DM
class TestMonitoring(unittest.TestCase):
# -1. rhd parser sanity check
def test_rhd_parser(self):
cities = [[32.7, -117.1, 0],\
[51.5, 0.129, 1],\
[35.7, 139.7, 1],\
[-37.8, 144.9, 1],\
[32.1, 41.74, 0],\
[55.7, 12.69, 0]]
result = []
for city in cities:
result.append(int(is_rhd_region(city[0],city[1])))
self.assertEqual(result,[int(city[2]) for city in cities])
# 0. op engaged, driver is doing fine all the time
def test_fully_aware_driver(self):
events_output = run_DState_seq(always_attentive, always_false, always_true, always_false)
self.assertTrue(np.sum([len(event) for event in events_output])==0)
# 1. op engaged, driver is distracted and does nothing
def test_fully_distracted_driver(self):
events_output = run_DState_seq(always_distracted, always_false, always_true, always_false)
self.assertTrue(len(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+\
((_DISTRACTED_PRE_TIME_TILL_TERMINAL-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL+\
((_DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_TIME+\
((_TEST_TIMESPAN-10-_DISTRACTED_TIME)/2))/DT_DMON)][0].name, 'driverDistracted')
# 2. op engaged, no face detected the whole time, no action
def test_fully_invisible_driver(self):
events_output = run_DState_seq(always_no_face, always_false, always_true, always_false)
self.assertTrue(len(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)])==0)
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PRE_TIME_TILL_TERMINAL+\
((_AWARENESS_PRE_TIME_TILL_TERMINAL-_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'preDriverUnresponsive')
self.assertEqual(events_output[int((_AWARENESS_TIME-_AWARENESS_PROMPT_TIME_TILL_TERMINAL+\
((_AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertEqual(events_output[int((_AWARENESS_TIME+\
((_TEST_TIMESPAN-10-_AWARENESS_TIME)/2))/DT_DMON)][0].name, 'driverUnresponsive')
# 3. op engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
# - should have short orange recovery time and no green afterwards; should recover rightaway on wheel touch
def test_normal_driver(self):
ds_vector = [msg_DISTRACTED] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
[msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \
[msg_DISTRACTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*2/DT_DMON))
interaction_vector = [car_interaction_NOT_DETECTED] * int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
[car_interaction_DETECTED] * (int(_TEST_TIMESPAN/DT_DMON)-int(_DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
events_output = run_DState_seq(ds_vector, interaction_vector, always_true, always_false)
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertTrue(len(events_output[int(_DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)])==0)
# 4. op engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
# driver dodges, and then touches wheel to no avail, disengages and reengages
# - orange/red alert should remain after disappearance, and only disengaging clears red
def test_biggest_comma_fan(self):
_invisible_time = 2 # seconds
ds_vector = always_distracted[:]
interaction_vector = always_false[:]
op_vector = always_true[:]
ds_vector[int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((_DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
ds_vector[int((_DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON)
interaction_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] = [True] * int(1/DT_DMON)
op_vector[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] = [False] * int(0.5/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)][0].name, 'promptDriverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)][0].name, 'driverDistracted')
self.assertEqual(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)][0].name, 'driverDistracted')
self.assertTrue(len(events_output[int((_DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)])==0)
# 5. op engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
# - both actions should clear the alert, but momentary appearence should not
def test_sometimes_transparent_commuter(self):
_visible_time = np.random.choice([1,10]) # seconds
# print _visible_time
ds_vector = always_no_face[:]*2
interaction_vector = always_false[:]*2
ds_vector[int((2*_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*_INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((_INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((_INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)])==0)
if _visible_time == 1:
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)][0].name, 'preDriverUnresponsive')
elif _visible_time == 10:
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)])==0)
else:
pass
# 6. op engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
# - only disengage will clear the alert
def test_last_second_responder(self):
_visible_time = 2 # seconds
ds_vector = always_no_face[:]
interaction_vector = always_false[:]
op_vector = always_true[:]
ds_vector[int(_INVISIBLE_SECONDS_TO_RED/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
interaction_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
op_vector[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
events_output = run_DState_seq(ds_vector, interaction_vector, op_vector, always_false)
self.assertTrue(len(events_output[int(_INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)])==0)
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)][0].name, 'promptDriverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertEqual(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)][0].name, 'driverUnresponsive')
self.assertTrue(len(events_output[int((_INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)])==0)
# 7. op not engaged, always distracted driver
# - dm should stay quiet when not engaged
def test_pure_dashcam_user(self):
events_output = run_DState_seq(always_distracted, always_false, always_false, always_false)
self.assertTrue(np.sum([len(event) for event in events_output])==0)
# 8. op engaged, car stops at traffic light, down to orange, no action, then car starts moving
# - should only reach green when stopped, but continues counting down on launch
def test_long_traffic_light_victim(self):
_redlight_time = 60 # seconds
standstill_vector = always_true[:]
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((_TEST_TIMESPAN-_redlight_time)/DT_DMON)
events_output = run_DState_seq(always_distracted, always_false, always_true, standstill_vector)
self.assertEqual(events_output[int((_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_redlight_time-0.1)/DT_DMON)][0].name, 'preDriverDistracted')
self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted')
if __name__ == "__main__":
print 'MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS
unittest.main()
selfdrive/locationd/calibrationd.py
+15 -12
View File
@@ -6,7 +6,6 @@ import numpy as np
import selfdrive.messaging as messaging
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from common.params import Params
from common.transformations.model import model_height
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
@@ -64,7 +63,7 @@ class Calibrator(object):
self.just_calibrated = True
def handle_cam_odom(self, log):
trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
trans, rot = log.trans, log.rot
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
@@ -81,7 +80,7 @@ class Calibrator(object):
else:
return None
def send_data(self, livecalibration):
def send_data(self, pm):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
@@ -92,27 +91,31 @@ class Calibrator(object):
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
livecalibration.send(cal_send.to_bytes())
pm.send('liveCalibration', cal_send)
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
cameraodometry = messaging.sub_sock(service_list['cameraOdometry'].port, addr=addr, conflate=True)
livecalibration = messaging.pub_sock(service_list['liveCalibration'].port)
def calibrationd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
while 1:
co = messaging.recv_one(cameraodometry)
sm.update()
new_vp = calibrator.handle_cam_odom(co)
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
if DEBUG and new_vp is not None:
print 'got new vp', new_vp
calibrator.send_data(livecalibration)
calibrator.send_data(pm)
def main(gctx=None, addr="127.0.0.1"):
calibrationd_thread(gctx, addr)
def main(sm=None, pm=None):
calibrationd_thread(sm, pm)
if __name__ == "__main__":
selfdrive/locationd/locationd_yawrate.cc
+49 -23
View File
@@ -10,7 +10,7 @@
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
if (dt < 0) {
@@ -19,36 +19,34 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double
prev_update_time = current_time;
}
// x = A * x;
// P = A * P * A.transpose() + dt * Q;
// Simplify because A is unity
P = P + dt * Q;
x = A * x;
P = A * P * A.transpose() + dt * Q;
double y = meas - C * x;
double S = R + C * P * C.transpose();
Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
x = x + K * y;
P = (I - K * C) * P;
}
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
if (sensor_event.getType() == 4) {
if (sensor_event.getSensor() == 5) {
sensor_data_time = current_time;
double meas = -sensor_event.getGyro().getV()[0];
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
update_state(C_gyro, R_gyro, current_time, meas);
}
}
}
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
double R = 100.0 * pow(camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
auto trans = camera_odometry.getTrans();
posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]);
camera_odometry_time = current_time;
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
@@ -59,17 +57,34 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
Localizer::Localizer() {
A << 1, 0, 0, 1;
I << 1, 0, 0, 1;
// States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
A <<
1, 1, 0, 0,
0, 1, 0, 0,
0, 0, 1, 1,
0, 0, 0, 1;
I <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
Q <<
0, 0, 0, 0,
0, pow(0.1, 2.0), 0, 0,
0, 0, 0, 0,
0, 0, pow(0.0005 / 100.0, 2.0), 0;
P <<
pow(100.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0,
0, 0, pow(100.0, 2.0), 0,
0, 0, 0, pow(100.0, 2.0);
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
C_posenet << 1, 0, 0, 0;
C_gyro << 1, 0, 1, 0;
x << 0, 0, 0, 0;
R_gyro = pow(0.05, 2.0);
R_gyro = pow(0.25, 2.0);
}
void Localizer::handle_log(cereal::Event::Reader event) {
@@ -118,20 +133,31 @@ extern "C" {
}
double localizer_get_bias(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x[1];
return loc->x[2];
}
void localizer_set_yaw(void * localizer, double yaw) {
double * localizer_get_state(void * localizer) {
Localizer * loc = (Localizer*) localizer;
loc->x[0] = yaw;
return loc->x.data();
}
void localizer_set_bias(void * localizer, double bias) {
void localizer_set_state(void * localizer, double * state) {
Localizer * loc = (Localizer*) localizer;
loc->x[1] = bias;
memcpy(loc->x.data(), state, 4 * sizeof(double));
}
double localizer_get_t(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->prev_update_time;
}
double * localizer_get_P(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->P.data();
}
void localizer_set_P(void * localizer, double * P) {
Localizer * loc = (Localizer*) localizer;
memcpy(loc->P.data(), P, 16 * sizeof(double));
}
}
selfdrive/locationd/locationd_yawrate.h
+9 -8
View File
@@ -7,28 +7,29 @@
class Localizer
{
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix2d P;
Eigen::Matrix<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
Eigen::Matrix4d A;
Eigen::Matrix4d I;
Eigen::Matrix4d Q;
Eigen::Matrix<double, 1, 4> C_posenet;
Eigen::Matrix<double, 1, 4> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
public:
Eigen::Vector2d x;
Eigen::Vector4d x;
Eigen::Matrix4d P;
double steering_angle = 0;
double car_speed = 0;
double posenet_speed = 0;
double prev_update_time = -1;
double controls_state_time = -1;
double sensor_data_time = -1;
double camera_odometry_time = -1;
Localizer();
void handle_log(cereal::Event::Reader event);
selfdrive/locationd/params_learner.cc
+1 -1
View File
@@ -45,7 +45,7 @@ ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
}
bool ParamsLearner::update(double psi, double u, double sa) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 15.)) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_ao = ao - alpha1 * ao_diff;
selfdrive/locationd/paramsd.cc
+19 -20
View File
@@ -137,33 +137,32 @@ int main(int argc, char *argv[]) {
// TODO: Fix in replay
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
// Send parameters at 10 Hz
if (save_counter % 10 == 0){
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[1]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid(posenet_invalid_count < 4);
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[2]);
live_params.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
live_params.setPosenetSpeed(localizer.posenet_speed);
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
// Save parameters every minute
// TODO: Save in seperate thread
if (save_counter % 6000 == 0) {
json11::Json json = json11::Json::object {
{"carVin", vin},
selfdrive/loggerd/uploader.py
+8 -12
View File
@@ -93,9 +93,9 @@ def is_on_hotspot():
return False
class Uploader(object):
def __init__(self, dongle_id, private_key, root):
def __init__(self, dongle_id, root):
self.dongle_id = dongle_id
self.api = Api(dongle_id, private_key)
self.api = Api(dongle_id)
self.root = root
self.upload_thread = None
@@ -146,14 +146,11 @@ class Uploader(object):
return (key, fn, 0)
if with_raw:
# then upload log files
# then upload the full log files, rear and front camera files
for name, key, fn in self.gen_upload_files():
if name == "rlog.bz2":
return (key, fn, 1)
# then upload rear and front camera files
for name, key, fn in self.gen_upload_files():
if name == "fcamera.hevc":
elif name == "fcamera.hevc":
return (key, fn, 2)
elif name == "dcamera.hevc":
return (key, fn, 3)
@@ -241,13 +238,12 @@ def uploader_fn(exit_event):
params = Params()
dongle_id = params.get("DongleId")
private_key = open("/persist/comma/id_rsa").read()
if dongle_id is None or private_key is None:
cloudlog.info("uploader missing dongle_id or private_key")
raise Exception("uploader can't start without dongle id and private key")
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
uploader = Uploader(dongle_id, private_key, ROOT)
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while True:
selfdrive/manager.py
+7 -5
View File
@@ -47,7 +47,7 @@ if __name__ == "__main__":
if is_neos:
version = int(open("/VERSION").read()) if os.path.isfile("/VERSION") else 0
revision = int(open("/REVISION").read()) if version >= 10 else 0 # Revision only present in NEOS 10 and up
neos_update_required = version < 10 or (version == 10 and revision != 4)
neos_update_required = version < 10 or (version == 10 and revision < 4)
if neos_update_required:
# update continue.sh before updating NEOS
@@ -365,6 +365,7 @@ def manager_thread():
# start frame
pm_apply_packages('enable')
system("LD_LIBRARY_PATH= appops set ai.comma.plus.offroad SU allow")
system("am start -n ai.comma.plus.frame/.MainActivity")
if os.getenv("NOBOARD") is None:
@@ -498,6 +499,11 @@ def manager_update():
update_ssh()
update_apks()
uninstall = [app for app in get_installed_apks().keys() if app in ("com.spotify.music", "com.waze")]
for app in uninstall:
cloudlog.info("uninstalling %s" % app)
os.system("pm uninstall % s" % app)
def manager_prepare():
# build cereal first
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@@ -555,16 +561,12 @@ def main():
params.put("IsMetric", "0")
if params.get("RecordFront") is None:
params.put("RecordFront", "0")
if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
if params.get("IsUploadRawEnabled") is None:
params.put("IsUploadRawEnabled", "1")
if params.get("IsUploadVideoOverCellularEnabled") is None:
params.put("IsUploadVideoOverCellularEnabled", "1")
if params.get("IsDriverMonitoringEnabled") is None:
params.put("IsDriverMonitoringEnabled", "1")
if params.get("IsGeofenceEnabled") is None:
params.put("IsGeofenceEnabled", "-1")
if params.get("SpeedLimitOffset") is None:
selfdrive/messaging.py
+22 -3
View File
@@ -84,8 +84,8 @@ def recv_one_or_none(sock):
return None
class SubMaster():
def __init__(self, services, addr="127.0.0.1"):
class SubMaster(object):
def __init__(self, services, ignore_alive=None, addr="127.0.0.1"):
self.poller = zmq.Poller()
self.frame = -1
self.updated = {s : False for s in services}
@@ -97,6 +97,12 @@ class SubMaster():
self.data = {}
self.logMonoTime = {}
self.valid = {}
if ignore_alive is not None:
self.ignore_alive = ignore_alive
else:
self.ignore_alive = []
for s in services:
# TODO: get address automatically from service_list
if addr is not None:
@@ -141,7 +147,7 @@ class SubMaster():
def all_alive(self, service_list=None):
if service_list is None: # check all
service_list = self.alive.keys()
return all(self.alive[s] for s in service_list)
return all(self.alive[s] for s in service_list if s not in self.ignore_alive)
def all_valid(self, service_list=None):
if service_list is None: # check all
@@ -152,3 +158,16 @@ class SubMaster():
if service_list is None: # check all
service_list = self.alive.keys()
return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list)
class PubMaster():
def __init__(self, services):
self.sock = {}
for s in services:
self.sock[s] = pub_sock(service_list[s].port)
def send(self, s, dat):
# accept either bytes or capnp builder
if not isinstance(dat, str):
dat = dat.to_bytes()
self.sock[s].send(dat)
selfdrive/service_list.yaml
+1 -1
View File
@@ -16,7 +16,7 @@ thermal: [8005, true, 2., 1]
can: [8006, true, 100.]
controlsState: [8007, true, 100., 100]
#liveEvent: [8008, true, 0.]
model: [8009, true, 20.]
model: [8009, true, 20., 5]
features: [8010, true, 0.]
health: [8011, true, 2., 1]
radarState: [8012, true, 20.]
selfdrive/test/test_car_models_openpilot.py → selfdrive/test/test_car_models.py
+70 -20
View File
@@ -14,6 +14,7 @@ from selfdrive.services import service_list
import selfdrive.messaging as messaging
from common.params import Params
from common.basedir import BASEDIR
from common.fingerprints import all_known_cars
from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.gm.values import CAR as GM
@@ -54,6 +55,7 @@ def get_route_logs(route_name):
with open(log_path, "w") as f:
f.write(r.content)
else:
print "failed to download test log %s" % route_name
sys.exit(-1)
routes = {
@@ -140,6 +142,10 @@ routes = {
'carFingerprint': HONDA.CRV_HYBRID,
'enableCamera': True,
},
"99e3eaed7396619e|2019-08-13--15-07-03": {
'carFingerprint': HONDA.FIT,
'enableCamera': True,
},
"2ac95059f70d76eb|2018-02-05--15-03-29": {
'carFingerprint': HONDA.ACURA_ILX,
'enableCamera': True,
@@ -351,6 +357,21 @@ routes = {
'enableCamera': False,
'enableDsu': False,
},
"2e07163a1ba9a780|2019-08-25--13-15-13": {
'carFingerprint': TOYOTA.LEXUS_IS,
'enableCamera': True,
'enableDsu': False,
},
"2e07163a1ba9a780|2019-08-29--09-35-42": {
'carFingerprint': TOYOTA.LEXUS_IS,
'enableCamera': False,
'enableDsu': False,
},
"1dd19ceed0ee2b48|2018-12-22--17-36-49": {
'carFingerprint': TOYOTA.LEXUS_IS, # 300 hybrid
'enableCamera': True,
'enableDsu': False,
},
"791340bc01ed993d|2019-03-10--16-28-08": {
'carFingerprint': SUBARU.IMPREZA,
'enableCamera': True,
@@ -370,33 +391,59 @@ passive_routes = [
#"bfa17080b080f3ec|2018-06-28--23-27-47",
]
public_routes = [
"f1b4c567731f4a1b|2018-06-06--14-43-46",
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"7ed9cdf8d0c5f43e|2018-05-17--09-31-36",
"38bfd238edecbcd7|2018-08-22--09-45-44",
"38bfd238edecbcd7|2018-08-29--22-02-15",
"b0f5a01cf604185c|2018-01-26--00-54-32",
"b0f5a01cf604185c|2018-01-26--10-54-38",
"b0f5a01cf604185c|2018-01-26--10-59-31",
"56fb1c86a9a86404|2017-11-10--10-18-43",
"b0f5a01cf604185c|2017-12-18--20-32-32",
"b0c9d2329ad1606b|2019-04-02--13-24-43",
"791340bc01ed993d|2019-03-10--16-28-08",
# TODO: replace all these with public routes
# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
non_public_routes = [
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
"b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
"5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
"265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
"53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
"c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
"1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
]
if __name__ == "__main__":
# TODO: add routes for untested cars and fail test if we have an untested car
tested_cars = [keys["carFingerprint"] for route, keys in routes.items()]
for car_model in all_known_cars():
if car_model not in tested_cars:
print "***** WARNING: %s not tested *****" % car_model
results = {}
for route, checks in routes.items():
if route not in public_routes:
print "route not public", route
if route not in non_public_routes:
get_route_logs(route)
elif "UNLOGGER_PATH" not in os.environ:
continue
get_route_logs(route)
for _ in range(3):
shutil.rmtree('/data/params')
manager.gctx = {}
@@ -420,7 +467,10 @@ if __name__ == "__main__":
# Start unlogger
print "Start unlogger"
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
if route in non_public_routes:
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
else:
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl', '--no-interactive']
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
print "Check sockets"
selfdrive/test/tests/plant/test_longitudinal.py
-1
View File
@@ -332,7 +332,6 @@ class LongitudinalControl(unittest.TestCase):
shutil.rmtree('/data/params', ignore_errors=True)
params = Params()
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
params.put("IsFcwEnabled", "1")
manager.gctx = {}
manager.prepare_managed_process('radard')
selfdrive/test/tests/process_replay/README.md
+9
View File
@@ -13,3 +13,12 @@ Currently the following processes are tested:
* plannerd
* calibrationd
## Forks
openpilot forks can use this test with their own reference logs
To generate new logs:
`./update-refs.py --no-upload`
Then, check in the new logs using git-lfs. Make sure to also include the updated `ref_commit` file.
selfdrive/test/tests/process_replay/compare_logs.py
+21 -5
View File
@@ -11,7 +11,6 @@ else:
from tools.lib.logreader import LogReader
def save_log(dest, log_msgs):
dat = ""
for msg in log_msgs:
@@ -21,25 +20,42 @@ def save_log(dest, log_msgs):
with open(dest, "w") as f:
f.write(dat)
def remove_ignored_fields(msg, ignore):
msg = msg.as_builder()
for key, val in ignore:
attr = msg
keys = key.split(".")
if msg.which() not in key and len(keys) > 1:
continue
for k in keys[:-1]:
try:
attr = getattr(msg, k)
except:
break
else:
setattr(attr, keys[-1], val)
return msg.as_reader()
def compare_logs(log1, log2, ignore=[]):
assert len(log1) == len(log2), "logs are not same length"
ignore_fields = [k for k, v in ignore]
diff = []
for msg1, msg2 in tqdm(zip(log1, log2)):
assert msg1.which() == msg2.which(), "msgs not aligned between logs"
msg1_bytes = msg1.as_builder().to_bytes()
msg2_bytes = msg2.as_builder().to_bytes()
msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes()
msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes()
if msg1_bytes != msg2_bytes:
msg1_dict = msg1.to_dict(verbose=True)
msg2_dict = msg2.to_dict(verbose=True)
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore, tolerance=0)
dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields, tolerance=0)
diff.extend(dd)
return diff
if __name__ == "__main__":
log1 = list(LogReader(sys.argv[1]))
log2 = list(LogReader(sys.argv[2]))
compare_logs(log1, log2, sys.argv[3:])
selfdrive/test/tests/process_replay/process_replay.py
+185 -86
View File
@@ -1,14 +1,15 @@
#!/usr/bin/env python2
import gc
import os
import time
import threading
import importlib
import zmq
if "CI" in os.environ:
tqdm = lambda x: x
else:
from tqdm import tqdm
from cereal import car
from cereal import car, log
from selfdrive.car.car_helpers import get_car
import selfdrive.manager as manager
import selfdrive.messaging as messaging
@@ -18,61 +19,154 @@ from collections import namedtuple
ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback'])
def fingerprint(msgs, pub_socks, sub_socks):
class FakeSocket:
def __init__(self, wait=True):
self.data = []
self.wait = wait
self.recv_called = threading.Event()
self.recv_ready = threading.Event()
def recv(self, block=None):
if block == zmq.NOBLOCK:
raise zmq.error.Again
if self.wait:
self.recv_called.set()
self.recv_ready.wait()
self.recv_ready.clear()
return self.data.pop()
def send(self, data):
if self.wait:
self.recv_called.wait()
self.recv_called.clear()
self.data.append(data)
if self.wait:
self.recv_ready.set()
def wait_for_recv(self):
self.recv_called.wait()
class DumbSocket:
def __init__(self, s=None):
if s is not None:
dat = messaging.new_message()
dat.init(s)
self.data = dat.to_bytes()
def recv(self, block=None):
return self.data
def send(self, dat):
pass
class FakeSubMaster(messaging.SubMaster):
def __init__(self, services):
super(FakeSubMaster, self).__init__(services, addr=None)
self.sock = {s: DumbSocket(s) for s in services}
self.update_called = threading.Event()
self.update_ready = threading.Event()
self.wait_on_getitem = False
def __getitem__(self, s):
# hack to know when fingerprinting is done
if self.wait_on_getitem:
self.update_called.set()
self.update_ready.wait()
self.update_ready.clear()
return self.data[s]
def update(self, timeout=-1):
self.update_called.set()
self.update_ready.wait()
self.update_ready.clear()
def update_msgs(self, cur_time, msgs):
self.update_called.wait()
self.update_called.clear()
super(FakeSubMaster, self).update_msgs(cur_time, msgs)
self.update_ready.set()
def wait_for_update(self):
self.update_called.wait()
class FakePubMaster(messaging.PubMaster):
def __init__(self, services):
self.data = {}
self.sock = {}
self.last_updated = None
for s in services:
data = messaging.new_message()
try:
data.init(s)
except:
data.init(s, 0)
self.data[s] = data.as_reader()
self.sock[s] = DumbSocket()
self.send_called = threading.Event()
self.get_called = threading.Event()
def send(self, s, dat):
self.last_updated = s
if isinstance(dat, str):
self.data[s] = log.Event.from_bytes(dat)
else:
self.data[s] = dat.as_reader()
self.send_called.set()
self.get_called.wait()
self.get_called.clear()
def wait_for_msg(self):
self.send_called.wait()
self.send_called.clear()
dat = self.data[self.last_updated]
self.get_called.set()
return dat
def fingerprint(msgs, fsm, can_sock):
print "start fingerprinting"
manager.prepare_managed_process("logmessaged")
manager.start_managed_process("logmessaged")
can = pub_socks["can"]
logMessage = messaging.sub_sock(service_list["logMessage"].port)
time.sleep(1)
messaging.drain_sock(logMessage)
# controlsd waits for a health packet before fingerprinting
msg = messaging.new_message()
msg.init("health")
pub_socks["health"].send(msg.to_bytes())
fsm.wait_on_getitem = True
# populate fake socket with data for fingerprinting
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for msg in canmsgs[:200]:
can.send(msg.as_builder().to_bytes())
can_sock.recv_called.wait()
can_sock.recv_called.clear()
can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]]
can_sock.recv_ready.set()
can_sock.wait = False
time.sleep(0.005)
log = messaging.recv_one_or_none(logMessage)
if log is not None and "fingerprinted" in log.logMessage:
break
manager.kill_managed_process("logmessaged")
# we know fingerprinting is done when controlsd sets sm['pathPlan'].sensorValid
fsm.update_called.wait()
fsm.update_called.clear()
fsm.wait_on_getitem = False
can_sock.wait = True
can_sock.data = []
fsm.update_ready.set()
print "finished fingerprinting"
def get_car_params(msgs, pub_socks, sub_socks):
sendcan = pub_socks.get("sendcan", None)
if sendcan is None:
sendcan = messaging.pub_sock(service_list["sendcan"].port)
logcan = sub_socks.get("can", None)
if logcan is None:
logcan = messaging.sub_sock(service_list["can"].port)
can = pub_socks.get("can", None)
if can is None:
can = messaging.pub_sock(service_list["can"].port)
def get_car_params(msgs, fsm, can_sock):
can = FakeSocket(wait=False)
sendcan = FakeSocket(wait=False)
time.sleep(0.5)
canmsgs = filter(lambda msg: msg.which() == "can", msgs)
for m in canmsgs[:200]:
canmsgs = filter(lambda msg: msg.which() == 'can', msgs)
for m in canmsgs[:300]:
can.send(m.as_builder().to_bytes())
_, CP = get_car(logcan, sendcan)
_, CP = get_car(can, sendcan)
Params().put("CarParams", CP.to_bytes())
time.sleep(0.5)
messaging.drain_sock(logcan)
def radar_rcv_callback(msg, CP):
if msg.which() != "can":
return []
elif CP.radarOffCan:
return ["radarState", "liveTracks"]
# hyundai and subaru don't have radar
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x475],
"hyundai": [], "chrysler": [0x2d4], "subaru": []}.get(CP.carName, None)
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
"chrysler": [0x2d4]}.get(CP.carName, None)
if radar_msgs is None:
raise NotImplementedError
@@ -80,24 +174,16 @@ def radar_rcv_callback(msg, CP):
for m in msg.can:
if m.src == 1 and m.address in radar_msgs:
return ["radarState", "liveTracks"]
return []
def plannerd_rcv_callback(msg, CP):
if msg.which() in ["model", "radarState"]:
time.sleep(0.005)
else:
time.sleep(0.002)
return {"model": ["pathPlan"], "radarState": ["plan"]}.get(msg.which(), [])
CONFIGS = [
ProcessConfig(
proc_name="controlsd",
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": []
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [],
},
ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"],
ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)],
init_callback=fingerprint,
should_recv_callback=None,
),
@@ -107,7 +193,7 @@ CONFIGS = [
"can": ["radarState", "liveTracks"],
"liveParameters": [], "controlsState": [], "model": [],
},
ignore=["logMonoTime", "radarState.cumLagMs"],
ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)],
init_callback=get_car_params,
should_recv_callback=radar_rcv_callback,
),
@@ -117,69 +203,82 @@ CONFIGS = [
"model": ["pathPlan"], "radarState": ["plan"],
"carState": [], "controlsState": [], "liveParameters": [],
},
ignore=["logMonoTime", "valid", "plan.processingDelay"],
ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)],
init_callback=get_car_params,
should_recv_callback=plannerd_rcv_callback,
should_recv_callback=None,
),
ProcessConfig(
proc_name="calibrationd",
pub_sub={
"cameraOdometry": ["liveCalibration"]
},
ignore=["logMonoTime"],
ignore=[("logMonoTime", 0), ("valid", True)],
init_callback=get_car_params,
should_recv_callback=None,
),
]
def replay_process(cfg, lr):
gc.disable() # gc can occasionally cause canparser to timeout
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
pub_socks, sub_socks = {}, {}
for pub, sub in cfg.pub_sub.iteritems():
pub_socks[pub] = messaging.pub_sock(service_list[pub].port)
for s in sub:
sub_socks[s] = messaging.sub_sock(service_list[s].port)
fsm = FakeSubMaster(pub_sockets)
fpm = FakePubMaster(sub_sockets)
args = (fsm, fpm)
if 'can' in cfg.pub_sub.keys():
can_sock = FakeSocket()
args = (fsm, fpm, can_sock)
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = filter(lambda msg: msg.which() in pub_socks.keys(), all_msgs)
pub_msgs = filter(lambda msg: msg.which() in cfg.pub_sub.keys(), all_msgs)
params = Params()
params.manager_start()
params.put("Passive", "0")
manager.gctx = {}
os.environ['NO_RADAR_SLEEP'] = "1"
manager.prepare_managed_process(cfg.proc_name)
manager.start_managed_process(cfg.proc_name)
time.sleep(3) # Wait for started process to be ready
mod = importlib.import_module(manager.managed_processes[cfg.proc_name])
thread = threading.Thread(target=mod.main, args=args)
thread.daemon = True
thread.start()
if cfg.init_callback is not None:
cfg.init_callback(all_msgs, pub_socks, sub_socks)
if 'can' not in cfg.pub_sub.keys():
can_sock = None
cfg.init_callback(all_msgs, fsm, can_sock)
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
log_msgs = []
# wait for started process to be ready
if 'can' in cfg.pub_sub.keys():
can_sock.wait_for_recv()
else:
fsm.wait_for_update()
log_msgs, msg_queue = [], []
for msg in tqdm(pub_msgs):
if cfg.should_recv_callback is not None:
recv_socks = cfg.should_recv_callback(msg, CP)
else:
recv_socks = cfg.pub_sub[msg.which()]
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
pub_socks[msg.which()].send(msg.as_builder().to_bytes())
should_recv = bool(len(recv_socks))
if len(recv_socks):
# TODO: add timeout
for sock in recv_socks:
m = messaging.recv_one(sub_socks[sock])
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())
else:
msg_queue.append(msg.as_builder())
# make these values fixed for faster comparison
m_builder = m.as_builder()
m_builder.logMonoTime = 0
m_builder.valid = True
log_msgs.append(m_builder.as_reader())
if should_recv:
fsm.update_msgs(0, msg_queue)
msg_queue = []
gc.enable()
manager.kill_managed_process(cfg.proc_name)
recv_cnt = len(recv_socks)
while recv_cnt > 0:
m = fpm.wait_for_msg()
log_msgs.append(m)
recv_cnt -= m.which() in recv_socks
return log_msgs
selfdrive/test/tests/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
e3388c62ffb80f4b3ca8721da56a581a93c44e79
8a11bcbc9833e154e10b59a8babb2b4545372f56
selfdrive/ui/spinner/Makefile
+74
View File
@@ -0,0 +1,74 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
OPENGL_LIBS = -lGLESv3
FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
OBJS = spinner.o \
../../common/framebuffer.o \
$(PHONELIBS)/nanovg/nanovg.o \
../../common/spinner.o \
opensans_semibold.o \
img_spinner_track.o \
img_spinner_comma.o
DEPS := $(OBJS:.o=.d)
.PHONY: all
all: spinner
spinner: $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
-s \
$(FRAMEBUFFER_LIBS) \
-L/system/vendor/lib64 \
$(OPENGL_LIBS) \
-lm -llog
../../common/framebuffer.o: ../../common/framebuffer.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-I$(PHONELIBS)/android_frameworks_native/include \
-I$(PHONELIBS)/android_system_core/include \
-I$(PHONELIBS)/android_hardware_libhardware/include \
-c -o '$@' '$<'
opensans_semibold.o: ../../assets/fonts/opensans_semibold.ttf
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
img_spinner_track.o: ../../assets/img_spinner_track.png
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
img_spinner_comma.o: ../../assets/img_spinner_comma.png
@echo "[ bin2o ] $@"
cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I../.. \
$(NANOVG_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f spinner $(OBJS) $(DEPS)
-include $(DEPS)
selfdrive/ui/spinner/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/spinner/spinner.c
+2 -81
View File
@@ -9,92 +9,13 @@
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include "nanovg.h"
#define NANOVG_GLES3_IMPLEMENTATION
#include "nanovg_gl.h"
#include "nanovg_gl_utils.h"
#include "common/framebuffer.h"
#include "common/spinner.h"
int main(int argc, char** argv) {
int err;
const char* spintext = NULL;
if (argc >= 2) {
spintext = argv[1];
}
// spinner
int fb_w, fb_h;
EGLDisplay display;
EGLSurface surface;
FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
&display, &surface, &fb_w, &fb_h);
assert(fb);
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
assert(vg);
int font = nvgCreateFont(vg, "Bold", "../../assets/fonts/opensans_semibold.ttf");
assert(font >= 0);
int spinner_img = nvgCreateImage(vg, "../../assets/img_spinner_track.png", 0);
assert(spinner_img >= 0);
int spinner_img_s = 360;
int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
int spinner_img_y = 260;
int spinner_img_xc = (fb_w/2);
int spinner_img_yc = (fb_h/2)-100;
int spinner_comma_img = nvgCreateImage(vg, "../../assets/img_spinner_comma.png", 0);
assert(spinner_comma_img >= 0);
for (int cnt = 0; ; cnt++) {
glClearColor(0.1, 0.1, 0.1, 1.0);
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
// background
nvgBeginPath(vg);
NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
nvgFillPaint(vg, bg);
nvgRect(vg, 0, 0, fb_w, fb_h);
nvgFill(vg);
// spin track
nvgSave(vg);
nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
nvgRotate(vg, (3.75*M_PI * cnt/120.0));
nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
nvgBeginPath(vg);
nvgFillPaint(vg, spinner_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
nvgRestore(vg);
// comma
NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
nvgBeginPath(vg);
nvgFillPaint(vg, comma_imgPaint);
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
nvgFill(vg);
// message
if (spintext) {
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
nvgFontSize(vg, 96.0f);
nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
}
nvgEndFrame(vg);
eglSwapBuffers(display, surface);
assert(glGetError() == GL_NO_ERROR);
}
spin(argc, argv);
return 0;
}
selfdrive/ui/ui.c
+54 -80
View File
@@ -41,7 +41,6 @@
#define STATUS_ENGAGED 2
#define STATUS_WARNING 3
#define STATUS_ALERT 4
#define STATUS_MAX 5
#define ALERTSIZE_NONE 0
#define ALERTSIZE_SMALL 1
@@ -122,7 +121,6 @@ typedef struct UIScene {
float mpc_y[50];
bool world_objects_visible;
mat3 warp_matrix; // transformed box -> frame.
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
float v_cruise;
@@ -253,12 +251,15 @@ typedef struct UIState {
int awake_timeout;
int volume_timeout;
int controls_timeout;
int alert_sound_timeout;
int speed_lim_off_timeout;
int is_metric_timeout;
int longitudinal_control_timeout;
int limit_set_speed_timeout;
bool controls_seen;
int status;
bool is_metric;
bool longitudinal_control;
@@ -458,6 +459,25 @@ sound_file* get_sound_file(AudibleAlert alert) {
return NULL;
}
void play_alert_sound(AudibleAlert alert) {
sound_file* sound = get_sound_file(alert);
char* error = NULL;
slplay_play(sound->uri, sound->loop, &error);
if(error) {
LOGW("error playing sound: %s", error);
}
}
void stop_alert_sound(AudibleAlert alert) {
sound_file* sound = get_sound_file(alert);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
}
}
void ui_sound_init(char **error) {
slplay_setup(error);
@@ -657,43 +677,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
s->limit_set_speed_timeout = UI_FREQ;
}
static void ui_draw_transformed_box(UIState *s, uint32_t color) {
const UIScene *scene = &s->scene;
const mat3 bbt = scene->warp_matrix;
struct {
vec3 pos;
uint32_t color;
} verts[] = {
{matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{scene->transformed_width, 0.0, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{scene->transformed_width, scene->transformed_height, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{0.0, scene->transformed_height, 1.0,}}), color},
{matvecmul3(bbt, (vec3){{0.0, 0.0, 1.0,}}), color},
};
for (int i=0; i<ARRAYSIZE(verts); i++) {
verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2];
verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2];
}
glUseProgram(s->line_program);
mat4 out_mat = matmul(device_transform,
matmul(frame_transform, s->rgb_transform));
glUniformMatrix4fv(s->line_transform_loc, 1, GL_TRUE, out_mat.v);
glEnableVertexAttribArray(s->line_pos_loc);
glVertexAttribPointer(s->line_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(verts[0]), &verts[0].pos.v[0]);
glEnableVertexAttribArray(s->line_color_loc);
glVertexAttribPointer(s->line_color_loc, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(verts[0]), &verts[0].color);
assert(glGetError() == GL_NO_ERROR);
glDrawArrays(GL_LINE_STRIP, 0, ARRAYSIZE(verts));
}
// Projects a point in car to space to the corresponding point in full frame
// image space.
vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
@@ -1619,6 +1602,9 @@ void handle_message(UIState *s, void *which) {
struct cereal_ControlsState datad;
cereal_read_ControlsState(&datad, eventd.controlsState);
s->controls_timeout = 1 * UI_FREQ;
s->controls_seen = true;
if (datad.vCruise != s->scene.v_cruise) {
s->scene.v_cruise_update_ts = eventd.logMonoTime;
}
@@ -1634,35 +1620,17 @@ void handle_message(UIState *s, void *which) {
s->scene.decel_for_model = datad.decelForModel;
s->alert_sound_timeout = 1 * UI_FREQ;
if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
char* error = NULL;
if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
sound_file* active_sound = get_sound_file(s->alert_sound);
slplay_stop_uri(active_sound->uri, &error);
if (error) {
LOGW("error stopping active sound %s", error);
}
}
sound_file* sound = get_sound_file(datad.alertSound);
slplay_play(sound->uri, sound->loop, &error);
if(error) {
LOGW("error playing sound: %s", error);
stop_alert_sound(s->alert_sound);
}
play_alert_sound(datad.alertSound);
s->alert_sound = datad.alertSound;
snprintf(s->alert_type, sizeof(s->alert_type), "%s", datad.alertType.str);
} else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none) && s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
sound_file* sound = get_sound_file(s->alert_sound);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
}
} else if ((!datad.alertSound || datad.alertSound == cereal_CarControl_HUDControl_AudibleAlert_none)
&& s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
stop_alert_sound(s->alert_sound);
s->alert_type[0] = '\0';
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
}
@@ -1734,13 +1702,6 @@ void handle_message(UIState *s, void *which) {
struct cereal_LiveCalibrationData datad;
cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
// should we still even have this?
capn_list32 warpl = datad.warpMatrix2;
capn_resolve(&warpl.p); // is this a bug?
for (int i = 0; i < 3 * 3; i++) {
s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i));
}
capn_list32 extrinsicl = datad.extrinsicMatrix;
capn_resolve(&extrinsicl.p); // is this a bug?
for (int i = 0; i < 3 * 4; i++) {
@@ -2312,18 +2273,31 @@ int main(int argc, char* argv[]) {
set_volume(s, volume);
}
// stop playing alert sounds if no controlsState msg for 1 second
if (s->alert_sound_timeout > 0) {
s->alert_sound_timeout--;
} else if (s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none){
sound_file* sound = get_sound_file(s->alert_sound);
char* error = NULL;
slplay_stop_uri(sound->uri, &error);
if(error) {
LOGW("error stopping sound: %s", error);
if (s->controls_timeout > 0) {
s->controls_timeout--;
} else {
// stop playing alert sound
if ((!s->vision_connected || (s->vision_connected && s->alert_sound_timeout == 0)) &&
s->alert_sound != cereal_CarControl_HUDControl_AudibleAlert_none) {
stop_alert_sound(s->alert_sound);
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
}
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_none;
// if visiond is still running and controlsState times out, display an alert
if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) {
s->scene.alert_size = ALERTSIZE_FULL;
update_status(s, STATUS_ALERT);
snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY");
snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive");
ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2);
s->alert_sound_timeout = 2 * UI_FREQ;
s->alert_sound = cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat;
play_alert_sound(s->alert_sound);
}
s->alert_sound_timeout--;
s->controls_seen = false;
}
read_param_bool_timeout(&s->is_metric, "IsMetric", &s->is_metric_timeout);
selfdrive/visiond/cameras/camera_frame_stream.cc
+172
View File
@@ -0,0 +1,172 @@
#include "camera_frame_stream.h"
#include <string>
#include <unistd.h>
#include <vector>
#include <string.h>
#include <czmq.h>
#include <libyuv.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/util.h"
#include "common/timing.h"
#include "common/swaglog.h"
#include "buffering.h"
extern "C" {
#include <libavcodec/avcodec.h>
}
extern volatile int do_exit;
#define FRAME_WIDTH 1164
#define FRAME_HEIGHT 874
namespace {
void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) {
assert(camera_bufs);
s->camera_bufs = camera_bufs;
}
void camera_close(CameraState *s) {
tbuffer_stop(&s->camera_tb);
}
void camera_release_buffer(void *cookie, int buf_idx) {
CameraState *s = static_cast<CameraState *>(cookie);
}
void camera_init(CameraState *s, int camera_id, unsigned int fps) {
assert(camera_id < ARRAYSIZE(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
s->frame_size = s->ci.frame_height * s->ci.frame_stride;
s->fps = fps;
tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s);
}
void run_frame_stream(DualCameraState *s) {
int err;
zsock_t *recorder_sock = zsock_new_sub(">tcp://127.0.0.1:8002", "");
assert(recorder_sock);
void *recorder_sock_raw = zsock_resolve(recorder_sock);
CameraState *const rear_camera = &s->rear;
auto *tb = &rear_camera->camera_tb;
while (!do_exit) {
zmq_msg_t msg;
err = zmq_msg_init(&msg);
assert(err == 0);
err = zmq_msg_recv(&msg, recorder_sock_raw, 0);
if(err == -1)
break;
// make copy due to alignment issues, will be freed on out of scope
size_t len = zmq_msg_size(&msg);
auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), (const uint8_t*)zmq_msg_data(&msg), len);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto frame = event.getFrame();
const int buf_idx = tbuffer_select(tb);
rear_camera->camera_bufs_metadata[buf_idx] = {
.frame_id = frame.getFrameId(),
.timestamp_eof = frame.getTimestampEof(),
.frame_length = static_cast<unsigned>(frame.getFrameLength()),
.integ_lines = static_cast<unsigned>(frame.getIntegLines()),
.global_gain = static_cast<unsigned>(frame.getGlobalGain()),
};
cl_command_queue q = rear_camera->camera_bufs[buf_idx].copy_q;
cl_mem yuv_cl = rear_camera->camera_bufs[buf_idx].buf_cl;
cl_event map_event;
void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE,
CL_MAP_WRITE, 0, frame.getImage().size(),
0, NULL, &map_event, &err);
assert(err == 0);
clWaitForEvents(1, &map_event);
clReleaseEvent(map_event);
memcpy(yuv_buf, frame.getImage().begin(), frame.getImage().size());
clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event);
clWaitForEvents(1, &map_event);
clReleaseEvent(map_event);
tbuffer_dispatch(tb, buf_idx);
err = zmq_msg_close(&msg);
assert(err == 0);
}
zsock_destroy(&recorder_sock);
}
} // namespace
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
[CAMERA_ID_IMX298] = {
.frame_width = FRAME_WIDTH,
.frame_height = FRAME_HEIGHT,
.frame_stride = FRAME_WIDTH*3,
.bayer = false,
.bayer_flip = false,
},
[CAMERA_ID_OV8865] = {
.frame_width = 1632,
.frame_height = 1224,
.frame_stride = 2040, // seems right
.bayer = false,
.bayer_flip = 3,
.hdr = false
},
};
void cameras_init(DualCameraState *s) {
memset(s, 0, sizeof(*s));
camera_init(&s->rear, CAMERA_ID_IMX298, 20);
s->rear.transform = (mat3){{
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
}};
camera_init(&s->front, CAMERA_ID_OV8865, 10);
s->front.transform = (mat3){{
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
}};
}
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats,
VisionBuf *camera_bufs_front) {
assert(camera_bufs_rear);
assert(camera_bufs_front);
int err;
// LOG("*** open front ***");
camera_open(&s->front, camera_bufs_front, false);
// LOG("*** open rear ***");
camera_open(&s->rear, camera_bufs_rear, true);
}
void cameras_close(DualCameraState *s) {
camera_close(&s->rear);
}
void cameras_run(DualCameraState *s) {
set_thread_name("frame_streaming");
run_frame_stream(s);
cameras_close(s);
}
selfdrive/visiond/cameras/camera_frame_stream.h
+56
View File
@@ -0,0 +1,56 @@
#ifndef CAMERA_FRAME_STREAM_H
#define CAMERA_FRAME_STREAM_H
#include <stdbool.h>
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#include "common/mat.h"
#include "buffering.h"
#include "common/visionbuf.h"
#include "camera_common.h"
#define FRAME_BUF_COUNT 16
#ifdef __cplusplus
extern "C" {
#endif
typedef struct CameraState {
int camera_id;
CameraInfo ci;
int frame_size;
VisionBuf *camera_bufs;
FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT];
TBuffer camera_tb;
int fps;
float digital_gain;
mat3 transform;
} CameraState;
typedef struct DualCameraState {
int ispif_fd;
CameraState rear;
CameraState front;
} DualCameraState;
void cameras_init(DualCameraState *s);
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(DualCameraState *s);
void cameras_close(DualCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
#ifdef __cplusplus
} // extern "C"
#endif
#endif
selfdrive/visiond/models/commonmodel.c
-131
View File
@@ -66,134 +66,3 @@ float sigmoid(float input) {
float softplus(float input) {
return log1p(expf(input));
}
void softmax(const float* input, float* output, size_t len) {
float max_val = -FLT_MAX;
for(int i = 0; i < len; i++) {
const float v = input[i];
if( v > max_val ) {
max_val = v;
}
}
float denominator = 0;
for(int i = 0; i < len; i++) {
float const v = input[i];
float const v_exp = expf(v - max_val);
denominator += v_exp;
output[i] = v_exp;
}
const float inv_denominator = 1. / denominator;
for(int i = 0; i < len; i++) {
output[i] *= inv_denominator;
}
}
static cereal_ModelData_PathData_ptr path_to_cereal(struct capn_segment *cs, const PathData data) {
capn_list32 poly_ptr = capn_new_list32(cs, POLYFIT_DEGREE);
for (int i=0; i<POLYFIT_DEGREE; i++) {
capn_set32(poly_ptr, i, capn_from_f32(data.poly[i]));
}
if (getenv("DEBUG_MODEL")){
capn_list32 points_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
capn_list32 stds_ptr = capn_new_list32(cs, MODEL_PATH_DISTANCE);
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
capn_set32(points_ptr, i, capn_from_f32(data.points[i]));
capn_set32(stds_ptr, i, capn_from_f32(data.stds[i]));
}
struct cereal_ModelData_PathData d = {
.points = points_ptr,
.stds = stds_ptr,
.prob = data.prob,
.std = data.std,
.poly = poly_ptr,
};
cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
cereal_write_ModelData_PathData(&d, ret);
return ret;
} else {
struct cereal_ModelData_PathData d = {
.prob = data.prob,
.std = data.std,
.poly = poly_ptr,
};
cereal_ModelData_PathData_ptr ret = cereal_new_ModelData_PathData(cs);
cereal_write_ModelData_PathData(&d, ret);
return ret;
}
}
static cereal_ModelData_LeadData_ptr lead_to_cereal(struct capn_segment *cs, const LeadData data) {
cereal_ModelData_LeadData_ptr leadp = cereal_new_ModelData_LeadData(cs);
struct cereal_ModelData_LeadData leadd = (struct cereal_ModelData_LeadData){
.dist = data.dist,
.prob = data.prob,
.std = data.std,
.relY = data.rel_y,
.relYStd = data.rel_y_std,
.relVel = data.rel_v,
.relVelStd = data.rel_v_std,
.relA = data.rel_a,
.relAStd = data.rel_a_std,
};
cereal_write_ModelData_LeadData(&leadd, leadp);
return leadp;
}
void model_publish(void* sock, uint32_t frame_id,
const mat3 transform, const ModelData data) {
struct capn rc;
capn_init_malloc(&rc);
struct capn_segment *cs = capn_root(&rc).seg;
capn_list32 input_transform_ptr = capn_new_list32(cs, 3*3);
for (int i = 0; i < 3 * 3; i++) {
capn_set32(input_transform_ptr, i, capn_from_f32(transform.v[i]));
}
capn_list32 speed_perc_ptr = capn_new_list32(cs, SPEED_PERCENTILES);
for (int i=0; i<SPEED_PERCENTILES; i++) {
capn_set32(speed_perc_ptr, i, capn_from_f32(data.speed[i]));
}
cereal_ModelData_ModelSettings_ptr settingsp = cereal_new_ModelData_ModelSettings(cs);
struct cereal_ModelData_ModelSettings settingsd = {
.inputTransform = input_transform_ptr,
};
cereal_write_ModelData_ModelSettings(&settingsd, settingsp);
cereal_ModelData_ptr modelp = cereal_new_ModelData(cs);
struct cereal_ModelData modeld = (struct cereal_ModelData){
.frameId = frame_id,
.path = path_to_cereal(cs, data.path),
.leftLane = path_to_cereal(cs, data.left_lane),
.rightLane = path_to_cereal(cs, data.right_lane),
.lead = lead_to_cereal(cs, data.lead),
.leadFuture = lead_to_cereal(cs, data.lead_future),
.settings = settingsp,
.speed = speed_perc_ptr,
};
cereal_write_ModelData(&modeld, modelp);
cereal_Event_ptr eventp = cereal_new_Event(cs);
struct cereal_Event event = {
.logMonoTime = nanos_since_boot(),
.valid = true,
.which = cereal_Event_model,
.model = modelp,
};
cereal_write_Event(&event, eventp);
capn_setp(capn_root(&rc), 0, eventp.p);
uint8_t buf[4096];
ssize_t rs = capn_write_mem(&rc, buf, sizeof(buf), 0);
zmq_send(sock, buf, rs, ZMQ_DONTWAIT);
capn_free(&rc);
}
selfdrive/visiond/models/commonmodel.h
-4
View File
@@ -12,7 +12,6 @@
extern "C" {
#endif
void softmax(const float* input, float* output, size_t len);
float softplus(float input);
float sigmoid(float input);
@@ -36,9 +35,6 @@ float *model_input_prepare(ModelInput* s, cl_command_queue q,
mat3 transform);
void model_input_free(ModelInput* s);
void model_publish(void* sock, uint32_t frame_id,
const mat3 transform, const ModelData data);
#ifdef __cplusplus
}
#endif
selfdrive/visiond/models/driving.cc
+66 -1
View File
@@ -42,6 +42,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
assert(temporal);
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
#endif
#ifdef DESIRE
s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
s->m->addDesire(s->desire, DESIRE_SIZE);
#endif
// Build Vandermonde matrix
for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
@@ -53,7 +58,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
ModelData model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock) {
mat3 transform, void* sock, float *desire_in) {
struct {
float *path;
float *left_lane;
@@ -62,6 +67,12 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
float *speed;
} net_outputs = {NULL};
#ifdef DESIRE
if (desire_in != NULL) {
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
}
#endif
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
@@ -201,3 +212,57 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
// Apply scale to output
p = p.transpose() * scale.asDiagonal();
}
void fill_path(cereal::ModelData::PathData::Builder path, const PathData path_data) {
kj::ArrayPtr<const float> poly(&path_data.poly[0], ARRAYSIZE(path_data.poly));
path.setPoly(poly);
path.setProb(path_data.prob);
path.setStd(path_data.std);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const LeadData lead_data) {
lead.setDist(lead_data.dist);
lead.setProb(lead_data.prob);
lead.setStd(lead_data.std);
lead.setRelY(lead_data.rel_y);
lead.setRelYStd(lead_data.rel_y_std);
lead.setRelVel(lead_data.rel_v);
lead.setRelVelStd(lead_data.rel_v_std);
lead.setRelA(lead_data.rel_a);
lead.setRelAStd(lead_data.rel_a_std);
}
void model_publish(void* sock, uint32_t frame_id,
const ModelData data, uint64_t timestamp_eof) {
// make msg
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setTimestampEof(timestamp_eof);
kj::ArrayPtr<const float> speed(&data.speed[0], ARRAYSIZE(data.speed));
framed.setSpeed(speed);
auto lpath = framed.initPath();
fill_path(lpath, data.path);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, data.left_lane);
auto right_lane = framed.initRightLane();
fill_path(right_lane, data.right_lane);
auto lead = framed.initLead();
fill_lead(lead, data.lead);
auto lead_future = framed.initLeadFuture();
fill_lead(lead_future, data.lead_future);
// send message
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(sock, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
selfdrive/visiond/models/driving.h
+17 -1
View File
@@ -3,25 +3,41 @@
// gate this here
#define TEMPORAL
#define DESIRE
#ifdef DESIRE
#define DESIRE_SIZE 8
#endif
#include "common/mat.h"
#include "common/modeldata.h"
#include "common/util.h"
#include "commonmodel.h"
#include "runners/run.h"
#include "cereal/gen/cpp/log.capnp.h"
#include <czmq.h>
#include <capnp/serialize.h>
typedef struct ModelState {
ModelInput in;
float *output;
RunModel *m;
#ifdef DESIRE
float *desire;
#endif
} ModelState;
void model_init(ModelState* s, cl_device_id device_id,
cl_context context, int temporal);
ModelData model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock);
mat3 transform, void* sock, float *desire_in);
void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out);
void model_publish(void* sock, uint32_t frame_id,
const ModelData data, uint64_t timestamp_eof);
#endif
selfdrive/visiond/models/monitoring.cc
+25 -1
View File
@@ -1,6 +1,7 @@
#include <string.h>
#include "monitoring.h"
#include "common/mat.h"
#include "common/timing.h"
#define MODEL_WIDTH 320
#define MODEL_HEIGHT 160
@@ -47,9 +48,32 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
return ret;
}
void monitoring_publish(void* sock, uint32_t frame_id, const MonitoringResult res) {
// make msg
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initDriverMonitoring();
framed.setFrameId(frame_id);
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
kj::ArrayPtr<const float> face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
framed.setFaceOrientation(face_orientation);
framed.setFacePosition(face_position);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
// send message
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(sock, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
void monitoring_free(MonitoringState* s) {
model_input_free(&s->in);
delete s->m;
}
selfdrive/visiond/models/monitoring.h
+6
View File
@@ -1,9 +1,14 @@
#ifndef MONITORING_H
#define MONITORING_H
#include "common/util.h"
#include "commonmodel.h"
#include "runners/run.h"
#include "cereal/gen/cpp/log.capnp.h"
#include <czmq.h>
#include <capnp/serialize.h>
#ifdef __cplusplus
extern "C" {
#endif
@@ -32,6 +37,7 @@ typedef struct MonitoringState {
void monitoring_init(MonitoringState* s, cl_device_id device_id, cl_context context);
MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height);
void monitoring_publish(void* sock, uint32_t frame_id, const MonitoringResult res);
void monitoring_free(MonitoringState* s);
#ifdef __cplusplus
selfdrive/visiond/runners/runmodel.h
+1
View File
@@ -4,6 +4,7 @@
class RunModel {
public:
virtual void addRecurrent(float *state, int state_size) {}
virtual void addDesire(float *state, int state_size) {}
virtual void execute(float *net_input_buf) {}
};
selfdrive/visiond/runners/snpemodel.cc
+14 -5
View File
@@ -86,18 +86,27 @@ SNPEModel::SNPEModel(const char *path, float *output, size_t output_size) {
}
void SNPEModel::addRecurrent(float *state, int state_size) {
recurrentBuffer = this->addExtra(state, state_size, 2);
}
void SNPEModel::addDesire(float *state, int state_size) {
desireBuffer = this->addExtra(state, state_size, 1);
}
std::unique_ptr<zdl::DlSystem::IUserBuffer> SNPEModel::addExtra(float *state, int state_size, int idx) {
// get input and output names
const auto &strListi_opt = snpe->getInputTensorNames();
if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names");
const auto &strListi = *strListi_opt;
const char *input_tensor_name = strListi.at(1);
printf("adding recurrent: %s\n", input_tensor_name);
const char *input_tensor_name = strListi.at(idx);
printf("adding index %d: %s\n", idx, input_tensor_name);
zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat;
zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
std::vector<size_t> recurrentStrides = {state_size * sizeof(float), sizeof(float)};
recurrentBuffer = ubFactory.createUserBuffer(state, state_size * sizeof(float), recurrentStrides, &userBufferEncodingFloat);
inputMap.add(input_tensor_name, recurrentBuffer.get());
std::vector<size_t> retStrides = {state_size * sizeof(float), sizeof(float)};
auto ret = ubFactory.createUserBuffer(state, state_size * sizeof(float), retStrides, &userBufferEncodingFloat);
inputMap.add(input_tensor_name, ret.get());
return ret;
}
void SNPEModel::execute(float *net_input_buf) {
selfdrive/visiond/runners/snpemodel.h
+4 -1
View File
@@ -20,6 +20,7 @@ public:
if (model_data) free(model_data);
}
void addRecurrent(float *state, int state_size);
void addDesire(float *state, int state_size);
void execute(float *net_input_buf);
private:
uint8_t *model_data = NULL;
@@ -36,8 +37,10 @@ private:
std::unique_ptr<zdl::DlSystem::IUserBuffer> outputBuffer;
float *output;
// recurrent
// recurrent and desire
std::unique_ptr<zdl::DlSystem::IUserBuffer> addExtra(float *state, int state_size, int idx);
std::unique_ptr<zdl::DlSystem::IUserBuffer> recurrentBuffer;
std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
};
#endif
selfdrive/visiond/visiond.cc
+43 -37
View File
@@ -155,6 +155,8 @@ struct VisionState {
int rgb_front_width, rgb_front_height, rgb_front_stride;
VisionBuf rgb_front_bufs[UI_BUF_COUNT];
cl_mem rgb_front_bufs_cl[UI_BUF_COUNT];
int front_meteringbox_xmin, front_meteringbox_xmax;
int front_meteringbox_ymin, front_meteringbox_ymax;
ModelState model;
ModelData model_bufs[UI_BUF_COUNT];
@@ -725,40 +727,27 @@ void* monitoring_thread(void *arg) {
double t2 = millis_since_boot();
// send driver monitoring packet
// set front camera metering target
if (res.face_prob > 0.4)
{
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initDriverMonitoring();
framed.setFrameId(frame_data.frame_id);
// junk 0s from legacy model
//kj::ArrayPtr<const float> descriptor_DEPRECATED(&res.descriptor_DEPRECATED[0], ARRAYSIZE(res.descriptor_DEPRECATED));
//framed.setDescriptor(descriptor_DEPRECATED);
//framed.setStd(res.std_DEPRECATED);
// why not use this junk space for reporting inference time instead
// framed.setStdDEPRECATED(static_cast<float>(t2-t1));
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
kj::ArrayPtr<const float> face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
framed.setFaceOrientation(face_orientation);
framed.setFacePosition(face_position);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s->monitoring_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height;
s->front_meteringbox_xmin = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72;
s->front_meteringbox_xmax = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72;
s->front_meteringbox_ymin = (res.face_position[1] + 0.5) * (s->rgb_front_height) - 72;
s->front_meteringbox_ymax = (res.face_position[1] + 0.5) * (s->rgb_front_height) + 72;
}
else // use default setting if no face
{
s->front_meteringbox_ymin = s->rgb_front_height * 0;
s->front_meteringbox_ymax = s->rgb_front_height * 2 / 3;
s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5;
s->front_meteringbox_xmax = s->rgb_front_width;
}
t2 = millis_since_boot();
// send dm packet
monitoring_publish(s->monitoring_sock_raw, frame_data.frame_id, res);
//t2 = millis_since_boot();
//LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;
}
@@ -816,11 +805,26 @@ void* frontview_thread(void *arg) {
if (cnt % 3 == 0)
#endif
{
// for driver autoexposure, use bottom right corner
const int y_start = s->rgb_front_height / 3;
const int y_end = s->rgb_front_height;
const int x_start = s->rgb_front_width * 2 / 3;
const int x_end = s->rgb_front_width;
// use driver face crop for AE
int x_start;
int x_end;
int y_start;
int y_end;
if (s->front_meteringbox_xmax > 0)
{
x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin;
x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax;
y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin;
y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax;
}
else
{
y_start = s->rgb_front_height * 0;
y_end = s->rgb_front_height * 2 / 3;
x_start = s->rgb_front_width * 3 / 5;
x_end = s->rgb_front_width;
}
uint32_t lum_binning[256] = {0,};
for (int y = y_start; y < y_end; ++y) {
@@ -1007,10 +1011,10 @@ void* processing_thread(void *arg) {
mt1 = millis_since_boot();
s->model_bufs[ui_idx] =
model_eval_frame(&s->model, q, yuv_cl, s->yuv_width, s->yuv_height,
model_transform, img_sock_raw);
model_transform, img_sock_raw, NULL);
mt2 = millis_since_boot();
model_publish(model_sock_raw, frame_id, model_transform, s->model_bufs[ui_idx]);
model_publish(model_sock_raw, frame_id, s->model_bufs[ui_idx], frame_data.timestamp_eof);
}
@@ -1073,6 +1077,8 @@ void* processing_thread(void *arg) {
posenetd.setTransStd(trans_std_vs);
kj::ArrayPtr<const float> rot_std_vs(&s->posenet.output[9], 3);
posenetd.setRotStd(rot_std_vs);
posenetd.setTimestampEof(frame_data.timestamp_eof);
posenetd.setFrameId(frame_id);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();