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

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

Browse Source
This commit is contained in:
dragonpilot
2020-01-17 22:31:23 +10:00
parent 1d8aec0fe3 cc35879b9d
commit 2eb80acd9b
195 changed files with 5662 additions and 4369 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/athena/test.py
+9 -10
View File
@@ -32,10 +32,8 @@ class TestAthenadMethods(unittest.TestCase):
assert dispatcher["echo"]("bob") == "bob"
def test_getMessage(self):
try:
with self.assertRaises(TimeoutError) as _:
dispatcher["getMessage"]("controlsState")
except TimeoutError:
pass
def send_thermal():
messaging.context = messaging.Context()
@@ -50,6 +48,7 @@ class TestAthenadMethods(unittest.TestCase):
p = Process(target=send_thermal)
p.start()
time.sleep(0.1)
try:
thermal = dispatcher["getMessage"]("thermal")
assert thermal['thermal']
@@ -60,7 +59,7 @@ class TestAthenadMethods(unittest.TestCase):
print(dispatcher["listDataDirectory"]())
@with_http_server
def test_do_upload(self):
def test_do_upload(self, host):
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
Path(fn).touch()
@@ -71,14 +70,14 @@ class TestAthenadMethods(unittest.TestCase):
except requests.exceptions.ConnectionError:
pass
item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
resp = athenad._do_upload(item)
self.assertEqual(resp.status_code, 201)
finally:
os.unlink(fn)
@with_http_server
def test_uploadFileToUrl(self):
def test_uploadFileToUrl(self, host):
not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {})
self.assertEqual(not_exists_resp, 404)
@@ -86,9 +85,9 @@ class TestAthenadMethods(unittest.TestCase):
Path(fn).touch()
try:
resp = dispatcher["uploadFileToUrl"]("qlog.bz2", "http://localhost:44444/qlog.bz2", {})
resp = dispatcher["uploadFileToUrl"]("qlog.bz2", f"{host}/qlog.bz2", {})
self.assertEqual(resp['enqueued'], 1)
self.assertDictContainsSubset({"path": fn, "url": "http://localhost:44444/qlog.bz2", "headers": {}}, resp['item'])
self.assertDictContainsSubset({"path": fn, "url": f"{host}/qlog.bz2", "headers": {}}, resp['item'])
self.assertIsNotNone(resp['item'].get('id'))
self.assertEqual(athenad.upload_queue.qsize(), 1)
finally:
@@ -96,10 +95,10 @@ class TestAthenadMethods(unittest.TestCase):
os.unlink(fn)
@with_http_server
def test_upload_handler(self):
def test_upload_handler(self, host):
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
Path(fn).touch()
item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
end_event = threading.Event()
thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
selfdrive/athena/test_helpers.py
+23 -6
View File
@@ -1,4 +1,7 @@
import http.server
import multiprocessing
import queue
import random
import requests
import socket
import time
@@ -70,27 +73,41 @@ class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler):
self.send_response(201, "Created")
self.end_headers()
def http_server(port_queue, **kwargs):
while 1:
try:
port = random.randrange(40000, 50000)
port_queue.put(port)
http.server.test(**kwargs, port=port)
except OSError as e:
if e.errno == 98:
continue
def with_http_server(func):
@wraps(func)
def inner(*args, **kwargs):
p = Process(target=http.server.test,
port_queue = multiprocessing.Queue()
host = '127.0.0.1'
p = Process(target=http_server,
args=(port_queue,),
kwargs={
'HandlerClass': HTTPRequestHandler,
'port': 44444,
'bind': '127.0.0.1'})
'bind': host})
p.start()
now = time.time()
port = None
while 1:
if time.time() - now > 5:
raise Exception('HTTP Server did not start')
try:
requests.put('http://localhost:44444/qlog.bz2', data='')
port = port_queue.get(timeout=0.1)
requests.put(f'http://{host}:{port}/qlog.bz2', data='')
break
except requests.exceptions.ConnectionError:
except (requests.exceptions.ConnectionError, queue.Empty):
time.sleep(0.1)
try:
return func(*args, **kwargs)
return func(*args, f'http://{host}:{port}', **kwargs)
finally:
p.terminate()
selfdrive/boardd/boardd.cc
+47 -39
View File
@@ -21,6 +21,7 @@
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/util.h"
#include "common/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
@@ -35,8 +36,10 @@
#define MAX_IR_POWER 0.5f
#define MIN_IR_POWER 0.0f
#define CUTOFF_GAIN 0.015625f // iso400
#define SATURATE_GAIN 0.0625f // iso1600
#define CUTOFF_GAIN 0.015625f // iso400
#define SATURATE_GAIN 0.0625f // iso1600
#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a')
#define VOLTAGE_K 0.091 // LPF gain for 5s tau (dt/tau / (dt/tau + 1))
namespace {
@@ -62,14 +65,19 @@ bool loopback_can = false;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
bool is_pigeon = false;
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs
const uint32_t VBATT_START_CHARGING = 11500;
const uint32_t VBATT_PAUSE_CHARGING = 10500;
const float VBATT_START_CHARGING = 11.5;
const float VBATT_PAUSE_CHARGING = 11.0;
float voltage_f = 12.5; // filtered voltage
uint32_t no_ignition_cnt = 0;
bool connected_once = false;
bool ignition_last = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
bool safety_setter_thread_initialized = false;
pthread_t safety_setter_thread_handle;
bool pigeon_thread_initialized = false;
pthread_t pigeon_thread_handle;
bool pigeon_needs_init;
void pigeon_init();
@@ -132,10 +140,7 @@ void *safety_setter_thread(void *s) {
pthread_mutex_lock(&usb_lock);
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
// set if long_control is allowed by openpilot. Hardcoded to True for now
libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT);
safety_setter_thread_initialized = false;
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT);
@@ -146,13 +151,12 @@ void *safety_setter_thread(void *s) {
// must be called before threads or with mutex
bool usb_connect() {
int err;
int err, err2;
unsigned char hw_query[1] = {0};
unsigned char fw_ver_buf[64];
unsigned char fw_sig_buf[128];
unsigned char fw_sig_hex_buf[16];
unsigned char serial_buf[16];
const char *fw_ver;
const char *serial;
int fw_ver_sz = 0;
int serial_sz = 0;
ignition_last = false;
@@ -171,12 +175,17 @@ bool usb_connect() {
}
// get panda fw
err = libusb_control_transfer(dev_handle, 0xc0, 0xd6, 0, 0, fw_ver_buf, 64, TIMEOUT);
if (err > 0) {
fw_ver = (const char *)fw_ver_buf;
fw_ver_sz = err;
write_db_value(NULL, "PandaFirmware", fw_ver, fw_ver_sz);
printf("panda fw: %.*s\n", fw_ver_sz, fw_ver);
err = libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, fw_sig_buf, 64, TIMEOUT);
err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT);
if ((err == 64) && (err2 == 64)) {
printf("FW signature read\n");
write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128);
for (size_t i = 0; i < 8; i++){
fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX(fw_sig_buf[i] >> 4);
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX(fw_sig_buf[i] & 0xF);
}
write_db_value(NULL, "PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16);
}
else { goto fail; }
@@ -208,9 +217,10 @@ bool usb_connect() {
if (is_pigeon) {
LOGW("panda with gps detected");
pigeon_needs_init = true;
if (pigeon_thread_handle == -1) {
if (!pigeon_thread_initialized) {
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
assert(err == 0);
pigeon_thread_initialized = true;
}
}
@@ -291,6 +301,8 @@ void can_recv(PubSocket *publisher) {
// return if length is 0
if (recv <= 0) {
return;
} else if (recv == RECV_SIZE) {
LOGW("Receive buffer full");
}
// create message
@@ -298,7 +310,6 @@ void can_recv(PubSocket *publisher) {
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(start_time);
size_t num_msg = recv / 0x10;
auto canData = event.initCan(num_msg);
// populate message
@@ -332,6 +343,7 @@ void can_health(PubSocket *publisher) {
uint32_t uptime;
uint32_t voltage;
uint32_t current;
uint32_t can_rx_errs;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
@@ -357,6 +369,11 @@ void can_health(PubSocket *publisher) {
} while(cnt != sizeof(health));
pthread_mutex_unlock(&usb_lock);
if (spoofing_started) {
health.ignition_line = 1;
}
voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f; // LPF
#ifndef DragonBTG
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
@@ -377,13 +394,15 @@ void can_health(PubSocket *publisher) {
#ifndef __x86_64__
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
if ((no_ignition_exp || (health.voltage < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
printf("TURN OFF CHARGING!\n");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
printf("POWER DOWN DEVICE\n");
system("service call power 17 i32 0 i32 1");
}
if (!no_ignition_exp && (health.voltage > VBATT_START_CHARGING) && !cdp_mode) {
if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) {
printf("TURN ON CHARGING!\n");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
@@ -412,15 +431,15 @@ void can_health(PubSocket *publisher) {
// clear VIN, CarParams, and set new safety on car start
if (ignition && !ignition_last) {
int result = delete_db_value(NULL, "CarVin");
assert((result == 0) || (result == ERR_NO_VALUE));
result = delete_db_value(NULL, "CarParams");
assert((result == 0) || (result == ERR_NO_VALUE));
if (safety_setter_thread_handle == -1) {
if (!safety_setter_thread_initialized) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
assert(err == 0);
safety_setter_thread_initialized = true;
}
}
@@ -465,15 +484,12 @@ void can_health(PubSocket *publisher) {
healthData.setUptime(health.uptime);
healthData.setVoltage(health.voltage);
healthData.setCurrent(health.current);
if (spoofing_started) {
healthData.setIgnitionLine(true);
} else {
healthData.setIgnitionLine(health.ignition_line);
}
healthData.setIgnitionLine(health.ignition_line);
healthData.setIgnitionCan(health.ignition_can);
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
healthData.setHasGps(is_pigeon);
healthData.setCanRxErrs(health.can_rx_errs);
healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs);
@@ -853,14 +869,6 @@ void *pigeon_thread(void *crap) {
return NULL;
}
int set_realtime_priority(int level) {
// should match python using chrt
struct sched_param sa;
memset(&sa, 0, sizeof(sa));
sa.sched_priority = level;
return sched_setscheduler(getpid(), SCHED_FIFO, &sa);
}
}
int main() {
selfdrive/camerad/main.cc
-8
View File
@@ -680,14 +680,6 @@ void* visionserver_client_thread(void* arg) {
} else {
assert(false);
}
if (stream_type == VISION_STREAM_RGB_BACK ||
stream_type == VISION_STREAM_RGB_FRONT) {
/*stream_bufs->buf_info.ui_info = (VisionUIInfo){
.transformed_width = s->model.in.transformed_width,
.transformed_height = s->model.in.transformed_height,
};*/
}
vipc_send(fd, &rep);
streams[stream_type].subscribed = true;
} else if (p.type == VIPC_STREAM_RELEASE) {
selfdrive/car/car_helpers.py
+10 -6
View File
@@ -3,6 +3,7 @@ from common.params import Params, put_nonblocking
from common.basedir import BASEDIR
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions
from selfdrive.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint
@@ -57,12 +58,14 @@ def only_toyota_left(candidate_cars):
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
# **** for use live only ****
def fingerprint(logcan, sendcan, has_relay):
if has_relay:
# Vin query only reliably works thorugh OBDII
vin = get_vin(logcan, sendcan, 1)
bus = 1
addr, vin = get_vin(logcan, sendcan, bus)
_, car_fw = get_fw_versions(logcan, sendcan, bus)
else:
vin = VIN_UNKNOWN
_, car_fw = set(), []
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
@@ -127,18 +130,19 @@ def fingerprint(logcan, sendcan, has_relay):
put_nonblocking("DragonCarVIN", vin)
cloudlog.warning("fingerprinted %s", car_fingerprint)
return car_fingerprint, finger, vin
return car_fingerprint, finger, vin, car_fw
def get_car(logcan, sendcan, has_relay=False):
candidate, fingerprints, vin = fingerprint(logcan, sendcan, has_relay)
candidate, fingerprints, vin, car_fw = fingerprint(logcan, sendcan, has_relay)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
candidate = "mock"
CarInterface, CarController = interfaces[candidate]
car_params = CarInterface.get_params(candidate, fingerprints, vin, has_relay)
car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw)
car_params.carVin = vin
car_params.carFw = car_fw
return CarInterface(car_params, CarController), car_params
selfdrive/car/chrysler/interface.py
+1 -2
View File
@@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.chrysler
selfdrive/car/fingerprints.py
+30 -14
View File
@@ -1,27 +1,43 @@
import os
from common.basedir import BASEDIR
def get_attr_from_cars(attr):
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car models
# - values are attr values from all car folders
result = {}
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
try:
car_name = car_folder.split('/')[-1]
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=[attr])
if hasattr(values, attr):
attr_values = getattr(values, attr)
else:
continue
for f, v in attr_values.items():
result[f] = v
except (ImportError, IOError):
pass
return result
def get_fw_versions_list():
return get_attr_from_cars('FW_VERSIONS')
def get_fingerprint_list():
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car models for which we have a fingerprint
# - values are lists dicts of messages that constitute the unique
# CAN fingerprint of each car model and all its variants
fingerprints = {}
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
try:
car_name = car_folder.split('/')[-1]
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=['FINGERPRINTS'])
if hasattr(values, 'FINGERPRINTS'):
car_fingerprints = values.FINGERPRINTS
else:
continue
for f, v in car_fingerprints.items():
fingerprints[f] = v
except (ImportError, IOError):
pass
return fingerprints
return get_attr_from_cars('FINGERPRINTS')
FW_VERSIONS = get_fw_versions_list()
_FINGERPRINTS = get_fingerprint_list()
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
selfdrive/car/ford/interface.py
+1 -2
View File
@@ -34,13 +34,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.ford
selfdrive/car/fw_versions.py
Executable
+192
View File
@@ -0,0 +1,192 @@
#!/usr/bin/env python3
import traceback
import struct
from tqdm import tqdm
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
from selfdrive.swaglog import cloudlog
from selfdrive.car.fingerprints import FW_VERSIONS
import panda.python.uds as uds
from cereal import car
Ecu = car.CarParams.Ecu
def p16(val):
return struct.pack("!H", val)
TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0])
TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0])
SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT])
SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.DEFAULT])
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01'
TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01'
OBD_VERSION_REQUEST = b'\x09\x04'
OBD_VERSION_RESPONSE = b'\x49\x04'
REQUESTS = [
# Honda
(
[UDS_VERSION_REQUEST],
[UDS_VERSION_RESPONSE]
),
# Toyota
(
[SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST],
[SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE]
),
(
[SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST],
[SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE]
),
(
[TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST],
[TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE]
)
]
def chunks(l, n=128):
for i in range(0, len(l), n):
yield l[i:i + n]
def match_fw_to_car(fw_versions):
candidates = FW_VERSIONS
invalid = []
for candidate, fws in candidates.items():
for ecu, expected_versions in fws.items():
ecu_type = ecu[0]
addr = ecu[1:]
found_version = fw_versions.get(addr, None)
# Allow DSU not being present
if ecu_type in [Ecu.unknown, Ecu.dsu] and found_version is None:
continue
if found_version not in expected_versions:
invalid.append(candidate)
break
return set(candidates.keys()) - set(invalid)
def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False):
ecu_types = {}
# Extract ECU adresses to query from fingerprints
# ECUs using a subadress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
versions = FW_VERSIONS
if extra is not None:
versions.update(extra)
for c in versions.values():
for ecu_type, addr, sub_addr in c.keys():
a = (addr, sub_addr)
if a not in ecu_types:
ecu_types[a] = ecu_type
if sub_addr is None:
parallel_addrs.append(a)
else:
addrs.append([a])
addrs.insert(0, parallel_addrs)
fw_versions = {}
for i, addr in enumerate(tqdm(addrs, disable=not progress)):
for addr_chunk in chunks(addr):
for request, response in REQUESTS:
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, addr_chunk, request, response, debug=debug)
t = 2 * timeout if i == 0 else timeout
fw_versions.update(query.get_data(t))
except Exception:
cloudlog.warning(f"FW query exception: {traceback.format_exc()}")
# Build capnp list to put into CarParams
car_fw = []
for addr, version in fw_versions.items():
f = car.CarParams.CarFw.new_message()
f.ecu = ecu_types[addr]
f.fwVersion = version
f.address = addr[0]
if addr[1] is not None:
f.subAddress = addr[1]
car_fw.append(f)
candidates = match_fw_to_car(fw_versions)
return candidates, car_fw
if __name__ == "__main__":
import time
import argparse
import cereal.messaging as messaging
from selfdrive.car.vin import get_vin
parser = argparse.ArgumentParser(description='Get firmware version of ECUs')
parser.add_argument('--scan', action='store_true')
parser.add_argument('--debug', action='store_true')
args = parser.parse_args()
logcan = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
extra = None
if args.scan:
extra = {"DEBUG": {}}
# Honda
for i in range(256):
extra["DEBUG"][(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = []
extra["DEBUG"][(Ecu.unknown, 0x700 + i, None)] = []
extra["DEBUG"][(Ecu.unknown, 0x750, i)] = []
time.sleep(1.)
t = time.time()
print("Getting vin...")
addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
print(f"VIN: {vin}")
print("Getting VIN took %.3f s" % (time.time() - t))
print()
t = time.time()
candidates, fw_vers = get_fw_versions(logcan, sendcan, 1, extra=extra, debug=args.debug, progress=True)
print()
print("Found FW versions")
print("{")
for version in fw_vers:
subaddr = None if version.subAddress == 0 else hex(version.subAddress)
print(f" (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]")
print("}")
print()
print("Possible matches:", candidates)
print("Getting fw took %.3f s" % (time.time() - t))
selfdrive/car/gm/interface.py
+1 -2
View File
@@ -43,12 +43,11 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.enableCruise = False
selfdrive/car/honda/carcontroller.py
+31 -16
View File
@@ -1,14 +1,16 @@
from collections import namedtuple
from cereal import car
from common.realtime import DT_CTRL
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip
from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import AH, CruiseButtons, CAR
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD
from opendbc.can.packer import CANPacker
from common.params import Params
params = Params()
VisualAlert = car.CarControl.HUDControl.VisualAlert
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
@@ -58,25 +60,25 @@ def process_hud_alert(hud_alert):
fcw_display = 0
steer_required = 0
acc_alert = 0
if hud_alert == AH.NONE: # no alert
pass
elif hud_alert == AH.FCW: # FCW
fcw_display = hud_alert[1]
elif hud_alert == AH.STEER: # STEER
steer_required = hud_alert[1]
else: # any other ACC alert
acc_alert = hud_alert[1]
# priority is: FCW, steer required, all others
if hud_alert == VisualAlert.fcw:
fcw_display = VISUAL_HUD[hud_alert.raw]
elif hud_alert == VisualAlert.steerRequired:
steer_required = VISUAL_HUD[hud_alert.raw]
else:
acc_alert = VISUAL_HUD[hud_alert.raw]
return fcw_display, steer_required, acc_alert
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "mini_car", "car", "X4",
"lanes", "fcw", "acc_alert", "steer_required", "dashed_lanes"])
["pcm_accel", "v_cruise", "car",
"lanes", "fcw", "acc_alert", "steer_required", "dashed_lanes"])
class CarController():
def __init__(self, dbc_name):
def __init__(self, dbc_name, CP):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
@@ -84,6 +86,11 @@ class CarController():
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
self.eps_modified = False
for fw in CP.carFw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
print("EPS FW MODIFIED!")
self.eps_modified = True
# dragonpilot
self.turning_signal_timer = 0
@@ -109,7 +116,7 @@ class CarController():
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
@@ -127,8 +134,8 @@ class CarController():
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
0xc1, hud_lanes, fcw_display, acc_alert, steer_required, CS.lkMode)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required, CS.lkMode)
# **** process the car messages ****
@@ -137,9 +144,11 @@ class CarController():
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
STEER_MAX = 0x7FFF
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
STEER_MAX = 0x1400
else:
STEER_MAX = 0x1000
@@ -148,6 +157,12 @@ class CarController():
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
if apply_steer > 0xA00:
apply_steer = (apply_steer - 0xA00) / 2 + 0xA00
elif apply_steer < -0xA00:
apply_steer = (apply_steer + 0xA00) / 2 - 0xA00
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
selfdrive/car/honda/carstate.py
+2 -2
View File
@@ -41,8 +41,8 @@ def get_can_signals(CP):
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("STEER_ANGLE", "STEERING_SENSORS", 0),
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
("STEER_TORQUE_MOTOR", "STEER_STATUS", 0),
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
("GEAR", "GEARBOX", 0),
@@ -336,7 +336,7 @@ class CarState():
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.steer_torque_motor = cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
selfdrive/car/honda/hondacan.py
+1 -1
View File
@@ -54,7 +54,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
'PCM_GAS': hud.pcm_accel,
'CRUISE_SPEED': hud.v_cruise,
'ENABLE_MINI_CAR': hud.mini_car,
'ENABLE_MINI_CAR': 1,
'HUD_LEAD': hud.car,
'HUD_DISTANCE': 3, # max distance setting on display
'IMPERIAL_UNIT': int(not is_metric),
selfdrive/car/honda/interface.py
+15 -12
View File
@@ -8,14 +8,14 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, ECU, ECU_FINGERPRINT, FINGERPRINTS
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, ECU, ECU_FINGERPRINT, FINGERPRINTS
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
params = Params()
A_ACC_MAX = max(_A_CRUISE_MAX_V)
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
@@ -93,7 +93,7 @@ class CarInterface(CarInterfaceBase):
self.CC = None
if CarController is not None:
self.CC = CarController(self.cp.dbc_name)
self.CC = CarController(self.cp.dbc_name, CP)
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
self.compute_gb = get_compute_gb_acura()
@@ -139,22 +139,21 @@ class CarInterface(CarInterfaceBase):
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "honda"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe
rdr_bus = 0 if has_relay else 2
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
ret.radarOffCan = True
ret.openpilotLongitudinalControl = False
else:
ret.safetyModel = car.CarParams.SafetyModel.honda
ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
ret.enableGasInterceptor = 0x201 in fingerprint[0]
ret.openpilotLongitudinalControl = ret.enableCamera
@@ -173,6 +172,11 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
eps_modified = False
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
eps_modified = True
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
stop_and_go = True
ret.mass = CivicParams.MASS
@@ -181,7 +185,8 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
tire_stiffness_factor = 1.
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]]
ret.lateralTuning.pid.kf = 0.00006
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
@@ -600,8 +605,6 @@ class CarInterface(CarInterfaceBase):
else:
hud_v_cruise = 255
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
@@ -613,7 +616,7 @@ class CarInterface(CarInterfaceBase):
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
hud_alert=hud_alert)
hud_alert=c.hudControl.visualAlert)
self.frame += 1
return can_sends
selfdrive/car/honda/values.py
+41 -21
View File
@@ -1,6 +1,7 @@
from cereal import car
from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
# Car button codes
@@ -10,28 +11,18 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
class AH:
#[alert_idx, value]
# See dbc files for info on values"
NONE = [0, 0]
FCW = [1, 1]
STEER = [2, 1]
BRAKE_PRESSED = [3, 10]
GEAR_NOT_D = [4, 6]
SEATBELT = [5, 5]
SPEED_TOO_HIGH = [6, 8]
# See dbc files for info on values"
VISUAL_HUD = {
VisualAlert.none: AH.NONE,
VisualAlert.fcw: AH.FCW,
VisualAlert.steerRequired: AH.STEER,
VisualAlert.brakePressed: AH.BRAKE_PRESSED,
VisualAlert.wrongGear: AH.GEAR_NOT_D,
VisualAlert.seatbeltUnbuckled: AH.SEATBELT,
VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH}
VisualAlert.none: 0,
VisualAlert.fcw: 1,
VisualAlert.steerRequired: 1,
VisualAlert.brakePressed: 10,
VisualAlert.wrongGear: 6,
VisualAlert.seatbeltUnbuckled: 5,
VisualAlert.speedTooHigh: 8}
class ECU:
CAM = 0
CAM = Ecu.fwdCamera
class CAR:
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
@@ -88,9 +79,8 @@ FINGERPRINTS = {
CAR.CRV: [{
57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
}],
# msg 1115 has seen with len 2 and 4, so ignore it
CAR.CRV_5G: [{
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
}],
CAR.CRV_HYBRID: [{
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5
@@ -141,6 +131,36 @@ for c in FINGERPRINTS:
for d in DIAG_MSGS:
FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
# TODO: Figure out what is relevant
FW_VERSIONS = {
CAR.CIVIC: {
(Ecu.unknown, 0x18da10f1, None): [b'37805-5AA-L660\x00\x00'],
(Ecu.unknown, 0x18da1ef1, None): [b'28101-5CG-A050\x00\x00'],
(Ecu.unknown, 0x18da28f1, None): [b'57114-TBA-A550\x00\x00'],
(Ecu.eps, 0x18da30f1, None): [b'39990-TBA-A030\x00\x00', b'39990-TBA,A030\x00\x00'],
(Ecu.unknown, 0x18da53f1, None): [b'77959-TBA-A030\x00\x00'],
(Ecu.unknown, 0x18da60f1, None): [b'78109-TBC-A310\x00\x00'],
(Ecu.unknown, 0x18dab0f1, None): [b'36161-TBC-A030\x00\x00'],
(Ecu.unknown, 0x18daeff1, None): [b'38897-TBA-A020\x00\x00'],
},
CAR.ACCORD: {
(Ecu.unknown, 0x18da10f1, None): [b'37805-6B2-A650\x00\x00'],
(Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'],
(Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00'],
(Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00'],
(Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'],
(Ecu.eps, 0x18da30f1, None): [b'39990-TVA-A150\x00\x00'],
(Ecu.unknown, 0x18da3af1, None): [b'39390-TVA-A020\x00\x00'],
(Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00'],
(Ecu.unknown, 0x18da60f1, None): [b'78109-TVC-A210\x00\x00'],
(Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'],
(Ecu.unknown, 0x18dab0f1, None): [b'36802-TVA-A160\x00\x00'],
(Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00'],
(Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'],
}
}
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
selfdrive/car/hyundai/interface.py
+1 -2
View File
@@ -38,13 +38,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai
selfdrive/car/interfaces.py
+1 -1
View File
@@ -18,7 +18,7 @@ class CarInterfaceBase():
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
raise NotImplementedError
# returns a car.CarState, pass in car.CarControl
selfdrive/car/isotp_parallel_query.py
+128
View File
@@ -0,0 +1,128 @@
import time
from collections import defaultdict
from functools import partial
import cereal.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.boardd.boardd import can_list_to_can_capnp
from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr
class IsoTpParallelQuery():
def __init__(self, sendcan, logcan, bus, addrs, request, response, functional_addr=False, debug=False):
self.sendcan = sendcan
self.logcan = logcan
self.bus = bus
self.request = request
self.response = response
self.debug = debug
self.functional_addr = functional_addr
self.real_addrs = []
for a in addrs:
if isinstance(a, tuple):
self.real_addrs.append(a)
else:
self.real_addrs.append((a, None))
self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0]) for tx_addr in self.real_addrs}
self.msg_buffer = defaultdict(list)
def rx(self):
"""Drain can socket and sort messages into buffers based on address"""
can_packets = messaging.drain_sock(self.logcan, wait_for_one=True)
for packet in can_packets:
for msg in packet.can:
if msg.src == self.bus:
if self.functional_addr:
if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF):
fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32)
self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src))
elif msg.address in self.msg_addrs.values():
self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
def _can_tx(self, tx_addr, dat, bus):
"""Helper function to send single message"""
msg = [tx_addr, 0, dat, bus]
self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan'))
def _can_rx(self, addr, sub_addr=None):
"""Helper function to retrieve message with specified address and subadress from buffer"""
keep_msgs = []
if sub_addr is None:
msgs = self.msg_buffer[addr]
else:
# Filter based on subadress
msgs = []
for m in self.msg_buffer[addr]:
first_byte = m[2][0]
if first_byte == sub_addr:
msgs.append(m)
else:
keep_msgs.append(m)
self.msg_buffer[addr] = keep_msgs
return msgs
def _drain_rx(self):
messaging.drain_sock(self.logcan)
self.msg_buffer = defaultdict(list)
def get_data(self, timeout):
self._drain_rx()
# Create message objects
msgs = {}
request_counter = {}
request_done = {}
for tx_addr, rx_addr in self.msg_addrs.items():
# rx_addr not set when using functional tx addr
id_addr = rx_addr or tx_addr[0]
sub_addr = tx_addr[1]
can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr, self.bus, sub_addr=sub_addr, debug=self.debug)
max_len = 8 if sub_addr is None else 7
msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
msg.send(self.request[0])
msgs[tx_addr] = msg
request_counter[tx_addr] = 0
request_done[tx_addr] = False
results = {}
start_time = time.time()
while True:
self.rx()
if all(request_done.values()):
break
for tx_addr, msg in msgs.items():
dat = msg.recv()
if not dat:
continue
counter = request_counter[tx_addr]
expected_response = self.response[counter]
response_valid = dat[:len(expected_response)] == expected_response
if response_valid:
if counter + 1 < len(self.request):
msg.send(self.request[counter + 1])
request_counter[tx_addr] += 1
else:
results[tx_addr] = dat[len(expected_response):]
request_done[tx_addr] = True
else:
request_done[tx_addr] = True
cloudlog.warning(f"iso-tp query bad response: 0x{bytes.hex(dat)}")
if time.time() - start_time > timeout:
break
return results
selfdrive/car/mock/interface.py
+1 -1
View File
@@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase):
return accel
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
selfdrive/car/subaru/interface.py
+1 -2
View File
@@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "subaru"
ret.radarOffCan = True
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.subaru
selfdrive/car/toyota/interface.py
+1 -2
View File
@@ -47,13 +47,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.toyota
selfdrive/car/toyota/values.py
+31 -3
View File
@@ -1,4 +1,6 @@
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
@@ -34,9 +36,9 @@ class CAR:
class ECU:
CAM = 0 # camera
DSU = 1 # driving support unit
APGS = 2 # advanced parking guidance system
CAM = Ecu.fwdCamera # camera
DSU = Ecu.dsu # driving support unit
APGS = Ecu.apgs # advanced parking guidance system
# addr: (ecu, cars, bus, 1/freq*100, vl)
@@ -261,6 +263,32 @@ FINGERPRINTS = {
}],
}
FW_VERSIONS = {
CAR.COROLLA_TSS2: {
(Ecu.engine, 0x700, None): [b'\x01896630ZG5000\x00\x00\x00\x00'],
(Ecu.eps, 0x7a1, None): [b'\x018965B12350\x00\x00\x00\x00\x00\x00'],
(Ecu.esp, 0x7b0, None): [b'\x01F152602280\x00\x00\x00\x00\x00\x00'],
(Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301100\x00\x00\x00\x00'],
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00'],
},
CAR.PRIUS: {
(Ecu.engine, 0x700, None): [b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00'],
(Ecu.eps, 0x7a1, None): [b'8965B47023\x00\x00\x00\x00\x00\x00'],
(Ecu.esp, 0x7b0, None): [b'F152647416\x00\x00\x00\x00\x00\x00'],
(Ecu.dsu, 0x791, None): [b'881514703100\x00\x00\x00\x00'],
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4702100\x00\x00\x00\x00'],
},
CAR.RAV4: {
(Ecu.engine, 0x7e0, None): [b'\x02342Q2100\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00'],
(Ecu.eps, 0x7a1, None): [b'8965B42083\x00\x00\x00\x00\x00\x00'],
(Ecu.esp, 0x7b0, None): [b'F15260R103\x00\x00\x00\x00\x00\x00'],
(Ecu.dsu, 0x791, None): [b'881514201400\x00\x00\x00\x00'],
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4202100\x00\x00\x00\x00'],
}
}
STEER_THRESHOLD = 100
DBC = {
selfdrive/car/vin.py
+23 -94
View File
@@ -1,104 +1,33 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
import traceback
import cereal.messaging as messaging
from panda.python.uds import FUNCTIONAL_ADDRS
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
from selfdrive.swaglog import cloudlog
VIN_REQUEST = b'\x09\x02'
VIN_RESPONSE = b'\x49\x02\x01'
VIN_UNKNOWN = "0" * 17
# sanity checks on response messages from vin query
def is_vin_response_valid(can_dat, step, cnt):
if len(can_dat) != 8:
# ISO-TP meesages are all 8 bytes
return False
if step == 0:
# VIN does not fit in a single message and it's 20 bytes of data
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
return False
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
for i in range(retry):
try:
query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [VIN_REQUEST], [VIN_RESPONSE], functional_addr=True, debug=debug)
for addr, vin in query.get_data(timeout).items():
return addr[0], vin.decode()
print(f"vin query retry ({i+1}) ...")
except Exception:
cloudlog.warning(f"VIN query exception: {traceback.format_exc()}")
if step == 1 and cnt == 0:
# first response after a CONTINUE query is sent
if can_dat[0] != 0x21:
return False
if step == 1 and cnt == 1:
# second response after a CONTINUE query is sent
if can_dat[0] != 0x22:
return False
return True
class VinQuery():
def __init__(self, bus):
self.bus = bus
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
self.query_ext_msgs = [[0x18DB33F1, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
[0x18DA10f1, 0, b'\x30'.ljust(8, b"\x00"), bus]]
self.query_nor_msgs = [[0x7df, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
[0x7e0, 0, b'\x30'.ljust(8, b"\x00"), bus]]
self.cnts = [1, 2] # number of messages to wait for at each iteration
self.step = 0
self.cnt = 0
self.responded = False
self.never_responded = True
self.dat = b""
self.got_vin = False
self.vin = VIN_UNKNOWN
def check_response(self, msg):
# have we got a VIN query response?
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
self.never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(msg.dat, self.step, self.cnt):
self.dat += bytes(msg.dat[2:]) if self.step == 0 else bytes(msg.dat[1:])
self.cnt += 1
if self.cnt == self.cnts[self.step]:
self.responded = True
self.step += 1
if self.step == len(self.cnts):
self.got_vin = True
def send_query(self, sendcan):
# keep sending VIN query if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if self.never_responded or (self.responded and not self.got_vin):
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
self.responded = False
self.cnt = 0
def get_vin(self):
if self.got_vin:
try:
self.vin = self.dat[3:].decode('utf8')
except UnicodeDecodeError:
pass # have seen unexpected non-unicode characters
return self.vin
def get_vin(logcan, sendcan, bus, query_time=1.):
vin_query = VinQuery(bus)
frame = 0
# 1s max of VIN query time
while frame < query_time * 100 and not vin_query.got_vin:
a = messaging.get_one_can(logcan)
for can in a.can:
vin_query.check_response(can)
if vin_query.got_vin:
break
vin_query.send_query(sendcan)
frame += 1
return vin_query.get_vin()
return 0, VIN_UNKNOWN
if __name__ == "__main__":
logcan = messaging.sub_sock('can')
import time
sendcan = messaging.pub_sock('sendcan')
print(get_vin(logcan, sendcan, 0))
logcan = messaging.sub_sock('can')
time.sleep(1)
addr, vin = get_vin(logcan, sendcan, 1, debug=False)
print(hex(addr), vin)
selfdrive/car/volkswagen/interface.py
+1 -2
View File
@@ -42,12 +42,11 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
ret = car.CarParams.new_message()
ret.carFingerprint = candidate
ret.isPandaBlack = has_relay
ret.carVin = vin
if candidate == CAR.GOLF:
# Set common MQB parameters that will apply globally
selfdrive/clocksd/.gitignore
+1
View File
@@ -0,0 +1 @@
clocksd
selfdrive/clocksd/SConscript
+2
View File
@@ -0,0 +1,2 @@
Import('env', 'common', 'messaging')
env.Program('clocksd.cc', LIBS=['diag', 'time_genoff', common, messaging, 'capnp', 'zmq', 'kj'])
selfdrive/clocksd/clocksd.cc
+72
View File
@@ -0,0 +1,72 @@
#include <stdio.h>
#include <stdint.h>
#include <sys/resource.h>
#include <sys/timerfd.h>
#include <sys/time.h>
#include <utils/Timers.h>
#include <capnp/serialize.h>
#include "messaging.hpp"
#include "common/timing.h"
#include "cereal/gen/cpp/log.capnp.h"
namespace {
int64_t arm_cntpct() {
int64_t v;
asm volatile("mrs %0, cntpct_el0" : "=r"(v));
return v;
}
}
int main() {
setpriority(PRIO_PROCESS, 0, -13);
int err = 0;
Context *context = Context::create();
PubSocket* clock_publisher = PubSocket::create(context, "clocks");
assert(clock_publisher != NULL);
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
assert(timerfd >= 0);
struct itimerspec spec = {0};
spec.it_interval.tv_sec = 1;
spec.it_interval.tv_nsec = 0;
spec.it_value.tv_sec = 1;
spec.it_value.tv_nsec = 0;
err = timerfd_settime(timerfd, 0, &spec, 0);
assert(err == 0);
uint64_t expirations = 0;
while ((err = read(timerfd, &expirations, sizeof(expirations)))) {
if (err < 0) break;
uint64_t boottime = nanos_since_boot();
uint64_t monotonic = nanos_monotonic();
uint64_t monotonic_raw = nanos_monotonic_raw();
uint64_t wall_time = nanos_since_epoch();
uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(boottime);
auto clocks = event.initClocks();
clocks.setBootTimeNanos(boottime);
clocks.setMonotonicNanos(monotonic);
clocks.setMonotonicRawNanos(monotonic_raw);
clocks.setWallTimeNanos(wall_time);
clocks.setModemUptimeMillis(modem_uptime_v);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
clock_publisher->send((char*)bytes.begin(), bytes.size());
}
close(timerfd);
delete clock_publisher;
return 0;
}
selfdrive/common/framebuffer.cc
+6 -5
View File
@@ -1,4 +1,3 @@
#include <cstdio>
#include <cstdlib>
#include <cassert>
@@ -39,7 +38,6 @@ extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
extern "C" FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h) {
status_t status;
int success;
@@ -131,11 +129,14 @@ extern "C" FramebufferState* framebuffer_init(
const char brightness_level[] = BACKLIGHT_LEVEL;
write(brightness_fd, brightness_level, strlen(brightness_level));
if (out_display) *out_display = s->display;
if (out_surface) *out_surface = s->surface;
if (out_w) *out_w = w;
if (out_h) *out_h = h;
return s;
}
extern "C" void framebuffer_swap(FramebufferState *s) {
eglSwapBuffers(s->display, s->surface);
assert(glGetError() == GL_NO_ERROR);
}
selfdrive/common/framebuffer.h
+1 -1
View File
@@ -11,10 +11,10 @@ typedef struct FramebufferState FramebufferState;
FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h);
void framebuffer_set_power(FramebufferState *s, int mode);
void framebuffer_swap(FramebufferState *s);
/* Display power modes */
enum {
selfdrive/common/params.cc
-4
View File
@@ -243,10 +243,6 @@ int read_db_value(const char* params_path, const char* key, char** value,
goto cleanup;
}
// Remove one for null byte.
if (value_sz != NULL) {
*value_sz -= 1;
}
result = 0;
cleanup:
selfdrive/common/util.c
+4 -2
View File
@@ -19,7 +19,7 @@ void* read_file(const char* path, size_t* out_len) {
long f_len = ftell(f);
rewind(f);
char* buf = calloc(f_len + 1, 1);
char* buf = (char*)calloc(f_len, 1);
assert(buf);
size_t num_read = fread(buf, f_len, 1, f);
@@ -31,7 +31,7 @@ void* read_file(const char* path, size_t* out_len) {
}
if (out_len) {
*out_len = f_len + 1;
*out_len = f_len;
}
return buf;
@@ -54,6 +54,8 @@ int set_realtime_priority(int level) {
memset(&sa, 0, sizeof(sa));
sa.sched_priority = level;
return sched_setscheduler(tid, SCHED_FIFO, &sa);
#else
return -1;
#endif
}
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.7-release"
#define COMMA_VERSION "0.7.1-release"
selfdrive/common/visionimg.h
+3 -6
View File
@@ -1,13 +1,12 @@
#ifndef VISIONIMG_H
#define VISIONIMG_H
#ifdef QCOM
#include "common/visionbuf.h"
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#endif
#include "common/visionbuf.h"
#undef Status
#ifdef __cplusplus
extern "C" {
@@ -26,11 +25,9 @@ typedef struct VisionImg {
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
#ifdef QCOM
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
#endif
#ifdef __cplusplus
} // extern "C"
selfdrive/controls/controlsd.py
+14 -6
View File
@@ -76,7 +76,7 @@ def events_to_bytes(events):
return ret
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params):
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
@@ -91,6 +91,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa
# Check for CAN timeout
if not can_strs:
can_error_counter += 1
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
@@ -156,7 +157,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa
#if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
# events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_perc, mismatch_counter
return CS, events, cal_perc, mismatch_counter, can_error_counter
def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
@@ -334,7 +335,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
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,
last_blinker_frame, is_ldw_enabled):
last_blinker_frame, is_ldw_enabled, can_error_counter):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
@@ -431,6 +432,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk
"startMonoTime": int(start_time * 1e9),
"mapValid": sm['plan'].mapValid,
"forceDecel": bool(force_decel),
"canErrorCounter": can_error_counter,
}
if CP.lateralTuning.which() == 'pid':
@@ -549,6 +551,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
events_prev = []
@@ -589,11 +592,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params)
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
if not sm.alive['plan'] and sm.alive['pathPlan']: # only plan not being received: radar not communicating
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not sm.all_alive_and_valid():
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
@@ -618,6 +623,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
# events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if not dragon_toyota_stock_dsu:
# Only allow engagement with brake pressed when stopped behind another stopped car
@@ -658,7 +665,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
# Publish data
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, last_blinker_frame, is_ldw_enabled)
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent")
rk.monitor_time()
selfdrive/controls/lib/alerts.py
+22 -1
View File
@@ -412,6 +412,13 @@ ALERTS = [
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"radarCommIssue",
"TAKE CONTROL IMMEDIATELY",
"Radar Communication Issue",
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
Alert(
"radarCanError",
"TAKE CONTROL IMMEDIATELY",
@@ -661,6 +668,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
Alert(
"radarCommIssueNoEntry",
"openpilot Unavailable",
"Radar Communication Issue",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
Alert(
"internetConnectivityNeededNoEntry",
"openpilot Unavailable",
@@ -741,11 +755,18 @@ ALERTS = [
Alert(
"lowMemoryPermanent",
"RAM Memory Critically Low",
"RAM Critically Low",
"Reboot your EON",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
Alert(
"carUnrecognizedPermanent",
"Dashcam Mode",
"Car Unrecognized",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
Alert(
"vehicleModelInvalid",
"Vehicle Parameter Identification Failed",
selfdrive/controls/lib/driver_monitor.py
+6 -6
View File
@@ -22,9 +22,9 @@ _PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
_METRIC_THRESHOLD = 0.4
_METRIC_THRESHOLD_SLACK = 0.55
_METRIC_THRESHOLD_STRICT = 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)
_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
_PITCH_NATURAL_OFFSET = 0.02 # 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
@@ -146,13 +146,13 @@ class DriverStatus():
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()
# positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
pitch_error *= _PITCH_WEIGHT
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
selfdrive/controls/lib/pathplanner.py
+18 -14
View File
@@ -14,9 +14,11 @@ from common.params import Params
LaneChangeState = log.PathPlan.LaneChangeState
LaneChangeDirection = log.PathPlan.LaneChangeDirection
LOG_MPC = os.environ.get('LOG_MPC', False)
LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
DESIRES = {
LaneChangeDirection.none: {
LaneChangeState.off: log.PathPlan.Desire.none,
@@ -124,6 +126,7 @@ class PathPlanner():
# Lane change logic
lane_change_direction = LaneChangeDirection.none
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if not active:
self.lane_change_state = LaneChangeState.off
@@ -137,10 +140,9 @@ class PathPlanner():
elif sm['carState'].rightBlinker:
lane_change_direction = LaneChangeDirection.right
if lane_change_direction == LaneChangeDirection.left:
torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
else:
torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \
(sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
@@ -168,14 +170,16 @@ class PathPlanner():
self.dragon_auto_lc_timer = None
# State transitions
# off
if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker:
self.lane_change_state = LaneChangeState.off
elif self.lane_change_state == LaneChangeState.preLaneChange and torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
@@ -183,14 +187,14 @@ class PathPlanner():
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
self.lane_change_state = LaneChangeState.preLaneChange
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
# when finishing, we reset timer to none.
self.dragon_auto_lc_timer = None
# Don't allow starting lane change below 37 mph / 59.5kph
if (v_ego < self.dragon_assisted_lc_min_mph * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange):
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0
else:
selfdrive/controls/lib/planner.py
+14 -5
View File
@@ -27,7 +27,8 @@ _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
# need fast accel at very low speed for stop and go
# make sure these accelerations are smaller than mpc limits
_A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4]
_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4]
_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4]
_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
# Lookup table for turns
@@ -38,9 +39,13 @@ _A_TOTAL_MAX_BP = [20., 40.]
SPEED_PERCENTILE_IDX = 7
def calc_cruise_accel_limits(v_ego):
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
if following:
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
else:
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
return np.vstack([a_cruise_min, a_cruise_max])
@@ -80,6 +85,7 @@ class Planner():
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
# dragonpilot
self.dragon_slow_on_curve = True
@@ -132,6 +138,7 @@ class Planner():
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if self.dragon_slow_on_curve and len(sm['model'].path.poly):
path = list(sm['model'].path.poly)
@@ -152,8 +159,8 @@ class Planner():
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
if enabled and not self.first_loop:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
@@ -252,3 +259,5 @@ class Planner():
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
selfdrive/controls/tests/test_following_distance.py
+1 -1
View File
@@ -38,7 +38,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
first = True
while t < t_end:
# Run cruise control
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
selfdrive/locationd/calibrationd.py
+64 -23
View File
@@ -10,19 +10,23 @@ from selfdrive.swaglog import cloudlog
from common.params import Params, put_nonblocking
from common.transformations.model import model_height
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
eon_intrinsics, get_calib_from_vp, H, W
get_calib_from_vp, H, W, FOCAL
MPH_TO_MS = 0.44704
MIN_SPEED_FILTER = 15 * MPH_TO_MS
MAX_SPEED_STD = 1.5
MAX_YAW_RATE_FILTER = np.radians(2) # per second
INPUTS_NEEDED = 300 # allow to update VP every so many frames
INPUTS_WANTED = 600 # We want a little bit more than we need for stability
WRITE_CYCLES = 400 # write every 400 cycles
# This is all 20Hz, blocks needed for efficiency
BLOCK_SIZE = 100
INPUTS_NEEDED = 5 # allow to update VP every so many frames
INPUTS_WANTED = 20 # We want a little bit more than we need for stability
WRITE_CYCLES = 10 # write every 1000 cycles
VP_INIT = np.array([W/2., H/2.])
# These validity corners were chosen by looking at 1000
# and taking most extreme cases with some margin.
VP_VALIDITY_CORNERS = np.array([[W//2 - 150, 280], [W//2 + 150, 540]])
VP_VALIDITY_CORNERS = np.array([[W//2 - 120, 300], [W//2 + 120, 520]])
DEBUG = os.getenv("DEBUG") is not None
@@ -31,13 +35,29 @@ def is_calibration_valid(vp):
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
def sanity_clip(vp):
if np.isnan(vp).any():
vp = VP_INIT
return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)]
def intrinsics_from_vp(vp):
return np.array([
[FOCAL, 0., vp[0]],
[ 0., FOCAL, vp[1]],
[ 0., 0., 1.]])
class Calibrator():
def __init__(self, param_put=False):
self.param_put = param_put
self.vp = copy.copy(VP_INIT)
self.vps = []
self.vps = np.zeros((INPUTS_WANTED, 2))
self.idx = 0
self.block_idx = 0
self.valid_blocks = 0
self.cal_status = Calibration.UNCALIBRATED
self.write_counter = 0
self.just_calibrated = False
# Read calibration
@@ -46,14 +66,19 @@ class Calibrator():
try:
calibration_params = json.loads(calibration_params)
self.vp = np.array(calibration_params["vanishing_point"])
self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist()
if not np.isfinite(self.vp).all():
self.vp = copy.copy(VP_INIT)
self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
self.valid_blocks = calibration_params['valid_blocks']
if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0:
self.valid_blocks = 0
self.update_status()
except Exception:
cloudlog.exception("CalibrationParams file found but error encountered")
def update_status(self):
start_status = self.cal_status
if len(self.vps) < INPUTS_NEEDED:
if self.valid_blocks < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
else:
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
@@ -63,19 +88,28 @@ class Calibrator():
if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
self.just_calibrated = True
def handle_cam_odom(self, log):
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))
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
if ((trans[0] > MIN_SPEED_FILTER) and
(trans_std[0] < MAX_SPEED_STD) and
(abs(rot[2]) < MAX_YAW_RATE_FILTER)):
# intrinsics are not eon intrinsics, since this is calibrated frame
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
self.vps.append(new_vp)
self.vps = self.vps[-INPUTS_WANTED:]
self.vp = np.mean(self.vps, axis=0)
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
self.vp = sanity_clip(raw_vp)
self.update_status()
self.write_counter += 1
if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
cal_params = {"vanishing_point": list(self.vp),
"valid_points": len(self.vps)}
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_vp
else:
@@ -88,7 +122,7 @@ class Calibrator():
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
@@ -104,15 +138,22 @@ def calibrationd_thread(sm=None, pm=None):
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
send_counter = 0
while 1:
sm.update()
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
if sm.updated['cameraOdometry']:
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
sm['cameraOdometry'].rot,
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].rotStd)
if DEBUG and new_vp is not None:
print('got new vp', new_vp)
calibrator.send_data(pm)
# decimate outputs for efficiency
if (send_counter % 5) == 0:
calibrator.send_data(pm)
send_counter += 1
def main(sm=None, pm=None):
selfdrive/locationd/locationd_yawrate.cc
+2 -2
View File
@@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
}
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = 100.0 * pow(camera_odometry.getRotStd()[2], 2);
double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
@@ -73,7 +73,7 @@ Localizer::Localizer() {
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;
0, 0, pow(0.005 / 100.0, 2.0), 0;
P <<
pow(100.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0,
selfdrive/loggerd/SConscript
+11 -5
View File
@@ -1,6 +1,12 @@
Import('env', 'messaging', 'common', 'visionipc')
env.Program(['loggerd.cc', 'logger.c', 'raw_logger.cc', 'encoder.c'], LIBS=[
'zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
Import('env', 'arch', 'messaging', 'common', 'visionipc')
src = ['loggerd.cc', 'logger.c']
libs = ['zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
'avformat', 'avcodec', 'swscale', 'avutil',
'OmxVenc', 'OmxCore', 'yuv',
'bz2', 'cutils', common, 'json', messaging, visionipc])
'yuv', 'bz2', common, 'json', messaging, visionipc]
if arch == "aarch64":
src += ['encoder.c', 'raw_logger.cc']
libs += ['OmxVenc', 'OmxCore', 'cutils']
env.Program(src, LIBS=libs)
selfdrive/loggerd/config.py
+12 -2
View File
@@ -9,11 +9,21 @@ else:
SEGMENT_LENGTH = 60
def get_available_percent():
def get_available_percent(default=None):
try:
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
except OSError:
available_percent = 100.0
available_percent = default
return available_percent
def get_available_bytes(default=None):
try:
statvfs = os.statvfs(ROOT)
available_bytes = statvfs.f_bavail * statvfs.f_frsize
except OSError:
available_bytes = default
return available_bytes
selfdrive/loggerd/deleter.py
+3 -3
View File
@@ -3,15 +3,15 @@ import os
import shutil
import threading
from selfdrive.swaglog import cloudlog
from selfdrive.loggerd.config import ROOT, get_available_percent
from selfdrive.loggerd.config import ROOT, get_available_bytes
from selfdrive.loggerd.uploader import listdir_by_creation
def deleter_thread(exit_event):
while not exit_event.is_set():
available_percent = get_available_percent()
available_bytes = get_available_bytes()
if available_percent < 10.0:
if available_bytes is not None and available_bytes < (5 * 1024 * 1024 * 1024):
# remove the earliest directory we can
dirs = listdir_by_creation(ROOT)
for delete_dir in dirs:
selfdrive/loggerd/loggerd.cc
+10
View File
@@ -25,7 +25,10 @@
#include <zmq.h>
#include <yaml-cpp/yaml.h>
#include <capnp/serialize.h>
#ifdef QCOM
#include <cutils/properties.h>
#endif
#include "common/version.h"
#include "common/timing.h"
@@ -38,6 +41,11 @@
#include "logger.h"
#include "messaging.hpp"
#ifndef QCOM
// no encoder on PC
#define DISABLE_ENCODER
#endif
#ifndef DISABLE_ENCODER
#include "encoder.h"
@@ -420,6 +428,7 @@ kj::Array<capnp::word> gen_init_data() {
init.setKernelVersion(util::read_file("/proc/version"));
#ifdef QCOM
{
std::vector<std::pair<std::string, std::string> > properties;
property_list(append_property, (void*)&properties);
@@ -431,6 +440,7 @@ kj::Array<capnp::word> gen_init_data() {
lentry.setValue(properties[i].second);
}
}
#endif
const char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {
selfdrive/loggerd/tests/test_deleter.py
+11 -3
View File
@@ -1,6 +1,7 @@
import os
import time
import threading
import unittest
from collections import namedtuple
import selfdrive.loggerd.deleter as deleter
@@ -8,7 +9,8 @@ from common.timeout import Timeout, TimeoutException
from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks'])
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize'])
class TestDeleter(UploaderTestCase):
def fake_statvfs(self, d):
@@ -17,7 +19,7 @@ class TestDeleter(UploaderTestCase):
def setUp(self):
self.f_type = "fcamera.hevc"
super(TestDeleter, self).setUp()
self.fake_stats = Stats(f_bavail=0, f_blocks=10)
self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096)
deleter.os.statvfs = self.fake_statvfs
deleter.ROOT = self.root
@@ -68,7 +70,9 @@ class TestDeleter(UploaderTestCase):
def test_no_delete_when_available_space(self):
f_path = self.make_file_with_data(self.seg_dir, self.f_type)
self.fake_stats = Stats(f_bavail=10, f_blocks=10)
block_size = 4096
available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free
self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size)
self.start_thread()
@@ -98,3 +102,7 @@ class TestDeleter(UploaderTestCase):
self.join_thread()
self.assertTrue(os.path.exists(f_path), "File deleted when locked")
if __name__ == "__main__":
unittest.main()
selfdrive/loggerd/uploader.py
+3 -1
View File
@@ -68,13 +68,15 @@ def is_on_wifi():
# ConnectivityManager.getActiveNetworkInfo()
try:
result = android.parse_service_call_string(["connectivity", "2"])
if result is None:
return True
return 'WIFI' in result
except AttributeError:
return False
def is_on_hotspot():
try:
result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8')
result = subprocess.check_output(["ifconfig", "wlan0"], stderr=subprocess.STDOUT, encoding='utf8')
result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
is_android = result.startswith('192.168.43.')
selfdrive/manager.py
+50 -28
View File
@@ -5,11 +5,13 @@ import sys
import fcntl
import errno
import signal
import shutil
import subprocess
import datetime
from selfdrive.dragonpilot.dragonconf import dragonpilot_set_params
from common.basedir import BASEDIR
from common.android import ANDROID
sys.path.append(os.path.join(BASEDIR, "pyextra"))
os.environ['BASEDIR'] = BASEDIR
@@ -22,7 +24,7 @@ try:
except FileExistsError:
pass
if os.path.isfile('/EON'):
if ANDROID:
os.chmod("/dev/shm", 0o777)
def unblock_stdout():
@@ -75,7 +77,11 @@ if not prebuilt:
# run scons
env = os.environ.copy()
env['SCONS_PROGRESS'] = "1"
scons = subprocess.Popen(["scons", "-j4"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
env['SCONS_CACHE'] = "1"
nproc = os.cpu_count()
j_flag = "" if nproc is None else "-j%d" % (nproc - 1)
scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
# Read progress from stderr and update spinner
while scons.poll() is None:
@@ -97,8 +103,12 @@ if not prebuilt:
if scons.returncode != 0:
if retry:
print("scons build failed, make clean")
print("scons build failed, cleaning in")
for i in range(3,-1,-1):
print("....%d" % i)
time.sleep(1)
subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
shutil.rmtree("/tmp/scons_cache")
else:
raise RuntimeError("scons build failed")
else:
@@ -140,6 +150,7 @@ managed_processes = {
"paramsd": ("selfdrive/locationd", ["./paramsd"]),
"camerad": ("selfdrive/camerad", ["./camerad"]),
"sensord": ("selfdrive/sensord", ["./sensord"]),
"clocksd": ("selfdrive/clocksd", ["./clocksd"]),
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
"updated": "selfdrive.updated",
"monitoringd": ("selfdrive/modeld", ["./monitoringd"]),
@@ -148,6 +159,7 @@ managed_processes = {
"shutdownd": "selfdrive.dragonpilot.shutdownd.shutdownd",
"appd": "selfdrive.dragonpilot.appd.appd",
}
daemon_processes = {
"manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
}
@@ -165,35 +177,45 @@ interrupt_processes = []
# processes to end with SIGKILL instead of SIGTERM
kill_processes = ['sensord', 'paramsd']
# processes to end if thermal conditions exceed Green parameters
green_temp_processes = ['uploader']
persistent_processes = [
'thermald',
'logmessaged',
'logcatd',
'tombstoned',
'uploader',
'ui',
'updated',
'shutdownd',
'appd',
'uploader',
]
if ANDROID:
persistent_processes += [
'logcatd',
'tombstoned',
'updated',
'shutdownd',
'appd',
]
car_started_processes = [
'controlsd',
'plannerd',
'loggerd',
'sensord',
'radard',
'calibrationd',
'paramsd',
'camerad',
'modeld',
'monitoringd',
'proclogd',
'ubloxd',
'gpsd',
'deleter',
'dashcamd',
]
if ANDROID:
car_started_processes += [
'sensord',
'clocksd',
'gpsd',
'monitoringd',
'deleter',
'dashcamd',
]
def register_managed_process(name, desc, car_started=False):
global managed_processes, car_started_processes, persistent_processes
@@ -310,7 +332,8 @@ def kill_managed_process(name):
def cleanup_all_processes(signal, frame):
cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
pm_apply_packages('disable')
if ANDROID:
pm_apply_packages('disable')
for name in list(running.keys()):
kill_managed_process(name)
@@ -372,8 +395,9 @@ def manager_thread():
start_managed_process(p)
# start frame
pm_apply_packages('enable')
start_frame()
if ANDROID:
pm_apply_packages('enable')
start_frame()
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
@@ -383,11 +407,15 @@ def manager_thread():
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
# uploader is gated based on the phone temperature
# heavyweight batch processes are gated on favorable thermal conditions
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
kill_managed_process("uploader")
for p in green_temp_processes:
if p in persistent_processes:
kill_managed_process(p)
else:
start_managed_process("uploader")
for p in green_temp_processes:
if p in persistent_processes:
start_managed_process(p)
if msg.thermal.freeSpace < 0.05:
logger_dead = True
@@ -437,13 +465,6 @@ def main():
# disable bluetooth
os.system('service call bluetooth_manager 8')
# support additional internal only extensions
try:
import selfdrive.manager_extensions
selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member
except ImportError:
pass
params = Params()
params.manager_start()
@@ -489,7 +510,8 @@ def main():
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
update_apks()
if ANDROID:
update_apks()
manager_init()
manager_prepare(spinner)
spinner.close()
selfdrive/modeld/SConscript
+19 -8
View File
@@ -1,25 +1,36 @@
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
lenv = env.Clone()
libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc]
common_src = [
"models/commonmodel.c",
"runners/snpemodel.cc",
"transforms/loadyuv.c",
"transforms/transform.c"]
if arch == "aarch64":
libs += ['gsl', 'CB', 'gnustl_shared']
else:
libs += ['symphony-cpu', 'pthread']
common = env.Object([
"models/commonmodel.c",
"runners/snpemodel.cc",
"transforms/loadyuv.c",
"transforms/transform.c"])
if FindFile('libtensorflow.so', env['LIBPATH']):
# for tensorflow support
common_src += ['runners/tfmodel.cc']
libs += ['tensorflow']
# tell runners to use it
lenv['CFLAGS'].append("-DUSE_TF_MODEL")
lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
env.Program('_monitoringd', [
common = lenv.Object(common_src)
lenv.Program('_monitoringd', [
"monitoringd.cc",
"models/monitoring.cc",
]+common, LIBS=libs)
env.Program('_modeld', [
lenv.Program('_modeld', [
"modeld.cc",
"models/driving.cc",
"models/posenet.cc",
]+common, LIBS=libs)
selfdrive/modeld/modeld
+1 -1
View File
@@ -1,4 +1,4 @@
#!/bin/sh
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
exec ./_modeld
selfdrive/modeld/modeld.cc
+19 -53
View File
@@ -1,18 +1,11 @@
#include <stdio.h>
#include <stdlib.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "models/driving.h"
#include "models/posenet.h"
volatile sig_atomic_t do_exit = 0;
@@ -122,13 +115,26 @@ int main(int argc, char **argv) {
cl_command_queue q;
{
// TODO: refactor this
cl_platform_id platform_id = NULL;
cl_platform_id platform_id[2];
cl_uint num_devices;
cl_uint num_platforms;
err = clGetPlatformIDs(1, &platform_id, &num_platforms);
err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms);
assert(err == 0);
err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1,
#ifdef QCOM
int clPlatform = 0;
#else
// don't use nvidia on pc, it's broken
// TODO: write this nicely
int clPlatform = num_platforms-1;
#endif
char cBuffer[1024];
clGetPlatformInfo(platform_id[clPlatform], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
LOGD("got %d opencl platform(s), using %s", num_platforms, cBuffer);
err = clGetDeviceIDs(platform_id[clPlatform], CL_DEVICE_TYPE_DEFAULT, 1,
&device_id, &num_devices);
assert(err == 0);
@@ -141,9 +147,7 @@ int main(int argc, char **argv) {
// init the models
ModelState model;
PosenetState posenet;
model_init(&model, device_id, context, true);
posenet_init(&posenet);
LOGW("models loaded, modeld starting");
// debayering does a 2x downscale
@@ -213,54 +217,18 @@ int main(int argc, char **argv) {
// TODO: don't make copies!
memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);
ModelData model_buf =
ModelDataRaw model_buf =
model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
model_transform, NULL, vec_desire);
mt2 = millis_since_boot();
model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof);
posenet_publish(posenet_sock, extra.frame_id, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
last = mt1;
}
// push the frame to the posenet
// TODO: This doesn't always have to run
double pt1 = 0, pt2 = 0, pt3 = 0;
pt1 = millis_since_boot();
posenet_push(&posenet, (uint8_t*)buf->addr, buf_info.width);
pt2 = millis_since_boot();
// posenet runs every 5
if (extra.frame_id % 5 == 0) {
posenet_eval(&posenet);
// send posenet event
{
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto posenetd = event.initCameraOdometry();
kj::ArrayPtr<const float> trans_vs(&posenet.output[0], 3);
posenetd.setTrans(trans_vs);
kj::ArrayPtr<const float> rot_vs(&posenet.output[3], 3);
posenetd.setRot(rot_vs);
kj::ArrayPtr<const float> trans_std_vs(&posenet.output[6], 3);
posenetd.setTransStd(trans_std_vs);
kj::ArrayPtr<const float> rot_std_vs(&posenet.output[9], 3);
posenetd.setRotStd(rot_std_vs);
posenetd.setTimestampEof(extra.timestamp_eof);
posenetd.setFrameId(extra.frame_id);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
posenet_sock->send((char*)bytes.begin(), bytes.size());
}
pt3 = millis_since_boot();
LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1));
}
}
visionbuf_free(&yuv_ion);
}
@@ -268,9 +236,7 @@ int main(int argc, char **argv) {
visionstream_destroy(&stream);
delete model_sock;
delete posenet_sock;
posenet_free(&posenet);
model_free(&model);
LOG("joining live_thread");
selfdrive/modeld/models/commonmodel.c
+28 -28
View File
@@ -5,57 +5,57 @@
#include "common/mat.h"
#include "common/timing.h"
void model_input_init(ModelInput* s, int width, int height,
void frame_init(ModelFrame* frame, int width, int height,
cl_device_id device_id, cl_context context) {
int err;
s->device_id = device_id;
s->context = context;
frame->device_id = device_id;
frame->context = context;
transform_init(&s->transform, context, device_id);
s->transformed_width = width;
s->transformed_height = height;
transform_init(&frame->transform, context, device_id);
frame->transformed_width = width;
frame->transformed_height = height;
s->transformed_y_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
s->transformed_width*s->transformed_height, NULL, &err);
frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
frame->transformed_width*frame->transformed_height, NULL, &err);
assert(err == 0);
s->transformed_u_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
s->transformed_v_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
s->net_input_size = ((width*height*3)/2)*sizeof(float);
s->net_input = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
s->net_input_size, (void*)NULL, &err);
frame->net_input_size = ((width*height*3)/2)*sizeof(float);
frame->net_input = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
frame->net_input_size, (void*)NULL, &err);
assert(err == 0);
loadyuv_init(&s->loadyuv, context, device_id, s->transformed_width, s->transformed_height);
loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height);
}
float *model_input_prepare(ModelInput* s, cl_command_queue q,
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform) {
int err;
int i = 0;
transform_queue(&s->transform, q,
transform_queue(&frame->transform, q,
yuv_cl, width, height,
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
s->transformed_width, s->transformed_height,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
frame->transformed_width, frame->transformed_height,
transform);
loadyuv_queue(&s->loadyuv, q,
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
s->net_input);
float *net_input_buf = (float *)clEnqueueMapBuffer(q, s->net_input, CL_TRUE,
CL_MAP_READ, 0, s->net_input_size,
loadyuv_queue(&frame->loadyuv, q,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
frame->net_input);
float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE,
CL_MAP_READ, 0, frame->net_input_size,
0, NULL, NULL, &err);
clFinish(q);
return net_input_buf;
}
void model_input_free(ModelInput* s) {
transform_destroy(&s->transform);
loadyuv_destroy(&s->loadyuv);
void frame_free(ModelFrame* frame) {
transform_destroy(&frame->transform);
loadyuv_destroy(&frame->loadyuv);
}
selfdrive/modeld/models/commonmodel.h
+5 -6
View File
@@ -4,7 +4,6 @@
#include <CL/cl.h>
#include "common/mat.h"
#include "common/modeldata.h"
#include "transforms/transform.h"
#include "transforms/loadyuv.h"
@@ -15,7 +14,7 @@ extern "C" {
float softplus(float input);
float sigmoid(float input);
typedef struct ModelInput {
typedef struct ModelFrame {
cl_device_id device_id;
cl_context context;
@@ -26,14 +25,14 @@ typedef struct ModelInput {
LoadYUVState loadyuv;
cl_mem net_input;
size_t net_input_size;
} ModelInput;
} ModelFrame;
void model_input_init(ModelInput* s, int width, int height,
void frame_init(ModelFrame* frame, int width, int height,
cl_device_id device_id, cl_context context);
float *model_input_prepare(ModelInput* s, cl_command_queue q,
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform);
void model_input_free(ModelInput* s);
void frame_free(ModelFrame* frame);
#ifdef __cplusplus
}
selfdrive/modeld/models/driving.cc
+172 -183
View File
@@ -2,26 +2,20 @@
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/timing.h"
#include "driving.h"
#define MODEL_WIDTH 512
#define MODEL_HEIGHT 256
#define MODEL_NAME "driving_model_dlc"
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define SPEED_BUCKETS 100
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + OTHER_META_SIZE + DESIRE_PRED_SIZE)
#define PATH_IDX 0
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 1
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 1
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
#define META_IDX LONG_A_IDX + TIME_DISTANCE*2
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
#else
@@ -33,15 +27,19 @@
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
model_input_init(&s->in, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float));
const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output = (float*)malloc(output_size * sizeof(float));
memset(s->output, 0, output_size * sizeof(float));
s->m = new DefaultRunModel("../../models/driving_model.dlc", s->output, output_size, USE_GPU_RUNTIME);
s->output = (float*)calloc(output_size, sizeof(float));
s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
#ifdef TEMPORAL
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;
@@ -56,18 +54,11 @@ 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,
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock, float *desire_in) {
struct {
float *path;
float *left_lane;
float *right_lane;
float *lead;
float *speed;
float *meta;
} net_outputs = {NULL};
#ifdef DESIRE
if (desire_in != NULL) {
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
@@ -76,124 +67,36 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
//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);
float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
s->m->execute(s->input_frames);
#ifdef DUMP_YUV
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
fwrite(new_frame_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
fclose(dump_yuv_file);
assert(1==2);
#endif
//printf("readinggggg \n");
//FILE *f = fopen("goof_frame", "r");
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
//fclose(f);
//sleep(1);
//printf("%i \n",OUTPUT_SIZE);
//printf("%i \n",MDN_GROUP_SIZE);
s->m->execute(net_input_buf);
// net outputs
net_outputs.path = &s->output[0];
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
//net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
net_outputs.meta = &s->output[OUTPUT_SIZE - OTHER_META_SIZE - DESIRE_PRED_SIZE];
ModelData model = {0};
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
model.path.points[i] = net_outputs.path[i];
model.left_lane.points[i] = net_outputs.left_lane[i] + 1.8;
model.right_lane.points[i] = net_outputs.right_lane[i] - 1.8;
model.path.stds[i] = softplus(net_outputs.path[MODEL_PATH_DISTANCE + i]);
model.left_lane.stds[i] = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + i]);
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
}
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE]);
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE]);
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE]);
model.path.prob = 1.;
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
model.right_lane.prob = sigmoid(net_outputs.right_lane[MODEL_PATH_DISTANCE*2]);
poly_fit(model.path.points, model.path.stds, model.path.poly);
poly_fit(model.left_lane.points, model.left_lane.stds, model.left_lane.poly);
poly_fit(model.right_lane.points, model.right_lane.stds, model.right_lane.poly);
const double max_dist = 140.0;
const double max_rel_vel = 10.0;
// Every output distribution from the MDN includes the probabilties
// of it representing a current lead car, a lead car in 2s
// or a lead car in 4s
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]);
model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
mdn_max_idx = i;
}
}
model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
// get speed percentiles numbers represent 5th, 15th, ... 95th percentile
for (int i=0; i < SPEED_PERCENTILES; i++) {
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
}
//float sum = 0;
//for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
// sum += net_outputs.speed[idx];
// int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
// if (idx_percentile < SPEED_PERCENTILES ){
// model.speed[idx_percentile] = ((float)idx)/2.0;
// }
//}
// make sure no percentiles are skipped
//for (int i=SPEED_PERCENTILES-1; i > 0; i--){
// if (model.speed[i-1] > model.speed[i]){
// model.speed[i-1] = model.speed[i];
// }
//}
for (int i=0; i<OTHER_META_SIZE + DESIRE_PRED_SIZE; i++) {
model.meta[i] = net_outputs.meta[i];
}
return model;
ModelDataRaw net_outputs;
net_outputs.path = &s->output[PATH_IDX];
net_outputs.left_lane = &s->output[LL_IDX];
net_outputs.right_lane = &s->output[RL_IDX];
net_outputs.lead = &s->output[LEAD_IDX];
net_outputs.long_x = &s->output[LONG_X_IDX];
net_outputs.long_v = &s->output[LONG_V_IDX];
net_outputs.long_a = &s->output[LONG_A_IDX];
net_outputs.meta = &s->output[META_IDX];
net_outputs.pose = &s->output[POSE_IDX];
return net_outputs;
}
void model_free(ModelState* s) {
free(s->output);
model_input_free(&s->in);
free(s->input_frames);
frame_free(&s->frame);
delete s->m;
}
@@ -224,31 +127,52 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
}
void fill_path(cereal::ModelData::PathData::Builder path, const PathData path_data) {
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
float points_arr[MODEL_PATH_DISTANCE];
float stds_arr[MODEL_PATH_DISTANCE];
float poly_arr[POLYFIT_DEGREE];
float std;
float prob;
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
points_arr[i] = data[i] + offset;
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]);
}
if (has_prob) {
prob = sigmoid(data[MODEL_PATH_DISTANCE*2]);
} else {
prob = 1.0;
}
std = softplus(data[MODEL_PATH_DISTANCE]);
poly_fit(points_arr, stds_arr, poly_arr);
if (std::getenv("DEBUG")){
kj::ArrayPtr<const float> stds(&path_data.stds[0], ARRAYSIZE(path_data.stds));
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
path.setStds(stds);
kj::ArrayPtr<const float> points(&path_data.points[0], ARRAYSIZE(path_data.points));
kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
path.setPoints(points);
}
kj::ArrayPtr<const float> poly(&path_data.poly[0], ARRAYSIZE(path_data.poly));
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setProb(path_data.prob);
path.setStd(path_data.std);
path.setProb(prob);
path.setStd(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 fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) {
const double x_scale = 10.0;
const double y_scale = 10.0;
lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE]));
lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
}
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
@@ -260,39 +184,104 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
meta.setDesirePrediction(desire_pred);
}
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) {
// just doing 10 vals, 1 every sec for now
float speed_arr[TIME_DISTANCE/10];
float accel_arr[TIME_DISTANCE/10];
for (int i=0; i<TIME_DISTANCE/10; i++) {
speed_arr[i] = long_v_data[i*10];
accel_arr[i] = long_a_data[i*10];
}
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
longi.setSpeeds(speed);
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
longi.setAccelerations(accel);
}
void model_publish(PubSocket *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());
const ModelDataRaw net_outputs, 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);
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, net_outputs.path, false, 0);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, net_outputs.left_lane, true, 1.8);
auto right_lane = framed.initRightLane();
fill_path(right_lane, net_outputs.right_lane, true, -1.8);
auto longi = framed.initLongitudinal();
fill_longi(longi, net_outputs.long_v, net_outputs.long_a);
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);
auto meta = framed.initMeta();
fill_meta(meta, data.meta);
// send message
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
sock->send((char*)bytes.begin(), bytes.size());
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
mdn_max_idx = i;
}
}
auto lead = framed.initLead();
fill_lead(lead, net_outputs.lead, mdn_max_idx);
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
mdn_max_idx = i;
}
}
auto lead_future = framed.initLeadFuture();
fill_lead(lead_future, net_outputs.lead, mdn_max_idx);
auto meta = framed.initMeta();
fill_meta(meta, net_outputs.meta);
// send message
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
sock->send((char*)bytes.begin(), bytes.size());
}
void posenet_publish(PubSocket *sock, uint32_t frame_id,
const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
float trans_arr[3];
float trans_std_arr[3];
float rot_arr[3];
float rot_std_arr[3];
for (int i =0; i < 3; i++) {
trans_arr[i] = net_outputs.pose[i];
trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
}
auto posenetd = event.initCameraOdometry();
kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3);
posenetd.setTrans(trans_vs);
kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3);
posenetd.setRot(rot_vs);
kj::ArrayPtr<const float> trans_std_vs(&trans_std_arr[0], 3);
posenetd.setTransStd(trans_std_vs);
kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3);
posenetd.setRotStd(rot_std_vs);
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(frame_id);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
sock->send((char*)bytes.begin(), bytes.size());
}
selfdrive/modeld/models/driving.h
+41 -4
View File
@@ -9,8 +9,13 @@
#define DESIRE_SIZE 8
#endif
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/mat.h"
#include "common/modeldata.h"
#include "common/util.h"
#include "commonmodel.h"
@@ -21,10 +26,40 @@
#include <capnp/serialize.h>
#include "messaging.hpp"
#define MODEL_WIDTH 512
#define MODEL_HEIGHT 256
#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
#define MODEL_NAME "supercombo_dlc"
#define MODEL_PATH_DISTANCE 192
#define POLYFIT_DEGREE 4
#define SPEED_PERCENTILES 10
#define DESIRE_PRED_SIZE 32
#define OTHER_META_SIZE 4
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define TIME_DISTANCE 100
#define POSE_SIZE 12
struct ModelDataRaw {
float *path;
float *left_lane;
float *right_lane;
float *lead;
float *long_x;
float *long_v;
float *long_a;
float *meta;
float *pose;
};
typedef struct ModelState {
ModelInput in;
ModelFrame frame;
float *output;
float *input_frames;
RunModel *m;
#ifdef DESIRE
float *desire;
@@ -33,12 +68,14 @@ typedef struct 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,
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
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(PubSocket* sock, uint32_t frame_id,
const ModelData data, uint64_t timestamp_eof);
const ModelDataRaw data, uint64_t timestamp_eof);
void posenet_publish(PubSocket* sock, uint32_t frame_id,
const ModelDataRaw data, uint64_t timestamp_eof);
#endif
selfdrive/modeld/models/posenet.cc
-58
View File
@@ -1,58 +0,0 @@
#include <string.h>
#include <math.h>
#include "posenet.h"
void posenet_init(PosenetState *s) {
s->input = (float*)malloc(2*200*532*sizeof(float));
s->m = new DefaultRunModel("../../models/posenet.dlc", s->output, sizeof(s->output)/sizeof(float), USE_GPU_RUNTIME);
}
void posenet_push(PosenetState *s, uint8_t *yuv_ptr_y, int yuv_width) {
// move second frame to first frame
memmove(&s->input[0], &s->input[1], sizeof(float)*(200*532*2 - 1));
// fill posenet input
float a;
// posenet uses a half resolution cropped frame
// with upper left corner: [50, 237] and
// bottom right corner: [1114, 637]
// So the resulting crop is 532 X 200
for (int y=237; y<637; y+=2) {
int yy = (y-237)/2;
for (int x = 50; x < 1114; x+=2) {
int xx = (x-50)/2;
a = 0;
a += yuv_ptr_y[yuv_width*(y+0) + (x+1)];
a += yuv_ptr_y[yuv_width*(y+1) + (x+1)];
a += yuv_ptr_y[yuv_width*(y+0) + (x+0)];
a += yuv_ptr_y[yuv_width*(y+1) + (x+0)];
// The posenet takes a normalized image input
// like the driving model so [0,255] is remapped
// to [-1,1]
s->input[(yy*532+xx)*2 + 1] = (a/512.0 - 1.0);
}
}
}
void posenet_eval(PosenetState *s) {
s->m->execute(s->input);
// fix stddevs
for (int i = 6; i < 12; i++) {
s->output[i] = log1p(exp(s->output[i])) + 1e-6;
}
// to radians
for (int i = 3; i < 6; i++) {
s->output[i] = M_PI * s->output[i] / 180.0;
}
// to radians
for (int i = 9; i < 12; i++) {
s->output[i] = M_PI * s->output[i] / 180.0;
}
}
void posenet_free(PosenetState *s) {
delete s->m;
free(s->input);
}
selfdrive/modeld/models/posenet.h
-27
View File
@@ -1,27 +0,0 @@
#ifndef POSENET_H
#define POSENET_H
#include <stdint.h>
#include "runners/run.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct PosenetState {
float output[12];
float *input;
RunModel *m;
} PosenetState;
void posenet_init(PosenetState *s);
void posenet_push(PosenetState *s, uint8_t *yuv_ptr_y, int yuv_width);
void posenet_eval(PosenetState *s);
void posenet_free(PosenetState *s);
#ifdef __cplusplus
}
#endif
#endif
selfdrive/modeld/monitoringd
+1 -1
View File
@@ -1,5 +1,5 @@
#!/bin/sh
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/"
exec ./_monitoringd
selfdrive/modeld/runners/run.h
+6 -3
View File
@@ -7,9 +7,12 @@
#ifdef QCOM
#define DefaultRunModel SNPEModel
#else
#define DefaultRunModel SNPEModel
/* #include "tfmodel.h" */
/* #define DefaultRunModel TFModel */
#ifdef USE_TF_MODEL
#include "tfmodel.h"
#define DefaultRunModel TFModel
#else
#define DefaultRunModel SNPEModel
#endif
#endif
#endif
selfdrive/pandad.py
+34 -23
View File
@@ -4,19 +4,29 @@ import os
import time
from selfdrive.swaglog import cloudlog
from panda import Panda, PandaDFU, BASEDIR
from panda import Panda, PandaDFU, BASEDIR, build_st
def get_expected_version():
with open(os.path.join(BASEDIR, "VERSION")) as f:
repo_version = f.read()
repo_version += "-EON" if os.path.isfile('/EON') else "-DEV"
return repo_version
def get_firmware_fn():
signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
if False: #os.path.exists(signed_fn):
cloudlog.info("Using prebuilt signed firmware")
return signed_fn
else:
cloudlog.info("Building panda firmware")
fn = "obj/panda.bin"
build_st(fn, clean=False)
return os.path.join(BASEDIR, "board", fn)
def get_expected_signature(fw_fn=None):
if fw_fn is None:
fw_fn = get_firmware_fn()
return Panda.get_signature_from_firmware(fw_fn)
def update_panda():
repo_version = get_expected_version()
panda = None
panda_dfu = None
@@ -37,27 +47,28 @@ def update_panda():
panda_dfu = PandaDFU(panda_dfu[0])
panda_dfu.recover()
print("waiting for board...")
time.sleep(1)
fw_fn = get_firmware_fn()
fw_signature = get_expected_signature(fw_fn)
try:
serial = panda.get_serial()[0].decode("utf-8")
except Exception:
serial = None
current_version = "bootstub" if panda.bootstub else panda.get_version()
cloudlog.warning("Panda %s connected, version: %s, expected %s" % (serial, current_version, repo_version))
if panda.bootstub or not current_version.startswith(repo_version):
panda_version = "bootstub" if panda.bootstub else panda.get_version()
panda_signature = "bootstub" if panda.bootstub else panda.get_signature()
cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
serial,
panda_version,
panda_signature.hex(),
fw_signature.hex(),
))
if panda.bootstub or panda_signature != fw_signature:
cloudlog.info("Panda firmware out of date, update required")
#signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
#if os.path.exists(signed_fn):
# cloudlog.info("Flashing signed firmware")
# panda.flash(fn=signed_fn)
#else:
cloudlog.info("Building and flashing unsigned firmware")
panda.flash()
panda.flash(fw_fn)
cloudlog.info("Done flashing")
if panda.bootstub:
@@ -69,8 +80,8 @@ def update_panda():
cloudlog.info("Panda still not booting, exiting")
raise AssertionError
version = panda.get_version()
if not version.startswith(repo_version):
panda_signature = panda.get_signature()
if panda_signature != fw_signature:
cloudlog.info("Version mismatch after flashing, exiting")
raise AssertionError
selfdrive/registration.py
+13 -2
View File
@@ -7,6 +7,7 @@ from selfdrive.version import version, terms_version, training_version, get_git_
from common.android import get_imei, get_serial, get_subscriber_info
from common.api import api_get
from common.params import Params
from common.file_helpers import mkdirs_exists_ok
def register():
params = Params()
@@ -18,6 +19,17 @@ def register():
params.put("GitRemote", get_git_remote())
params.put("SubscriberInfo", get_subscriber_info())
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
# do not erase your persist partition
if not os.path.isfile("/persist/comma/id_rsa.pub"):
cloudlog.warning("generating your personal RSA key")
mkdirs_exists_ok("/persist/comma")
assert os.system("openssl genrsa -out /persist/comma/id_rsa.tmp 2048") == 0
assert os.system("openssl rsa -in /persist/comma/id_rsa.tmp -pubout -out /persist/comma/id_rsa.tmp.pub") == 0
os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
# make key readable by app users (ai.comma.plus.offroad)
os.chmod('/persist/comma/', 0o755)
os.chmod('/persist/comma/id_rsa', 0o744)
@@ -36,7 +48,7 @@ def register():
try:
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
imei=get_imei(), serial=get_serial(), public_key=public_key, register_token=register_token)
imei=get_imei(0), imei2=get_imei(1), serial=get_serial(), public_key=public_key, register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"], dongleauth["access_token"]
@@ -51,5 +63,4 @@ def register():
return None
if __name__ == "__main__":
print(api_get("").text)
print(register())
selfdrive/sensord/gpsd.cc
-65
View File
@@ -7,7 +7,6 @@
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/timerfd.h>
#include <sys/resource.h>
#include <pthread.h>
@@ -31,8 +30,6 @@ volatile sig_atomic_t do_exit = 0;
namespace {
pthread_t clock_thread_handle;
Context *gps_context;
PubSocket *gps_publisher;
PubSocket *gps_location_publisher;
@@ -181,61 +178,6 @@ int64_t arm_cntpct() {
return v;
}
// TODO: move this out of here
void* clock_thread(void* args) {
int err = 0;
PubSocket* clock_publisher = PubSocket::create(gps_context, "clocks");
assert(clock_publisher != NULL);
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
assert(timerfd >= 0);
struct itimerspec spec = {0};
spec.it_interval.tv_sec = 1;
spec.it_interval.tv_nsec = 0;
spec.it_value.tv_sec = 1;
spec.it_value.tv_nsec = 0;
err = timerfd_settime(timerfd, 0, &spec, 0);
assert(err == 0);
uint64_t expirations = 0;
while ((err = read(timerfd, &expirations, sizeof(expirations)))) {
if (err < 0) break;
if (do_exit) break;
uint64_t boottime = nanos_since_boot();
uint64_t monotonic = nanos_monotonic();
uint64_t monotonic_raw = nanos_monotonic_raw();
uint64_t wall_time = nanos_since_epoch();
uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(boottime);
auto clocks = event.initClocks();
clocks.setBootTimeNanos(boottime);
clocks.setMonotonicNanos(monotonic);
clocks.setMonotonicRawNanos(monotonic_raw);
clocks.setWallTimeNanos(wall_time);
clocks.setModemUptimeMillis(modem_uptime_v);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
clock_publisher->send((char*)bytes.begin(), bytes.size());
}
close(timerfd);
delete clock_publisher;
return NULL;
}
}
int main() {
@@ -249,15 +191,8 @@ int main() {
rawgps_init();
err = pthread_create(&clock_thread_handle, NULL,
clock_thread, NULL);
assert(err == 0);
while(!do_exit) pause();
err = pthread_join(clock_thread_handle, NULL);
assert(err == 0);
rawgps_destroy();
gps_destroy();
selfdrive/sensord/sensors.cc
+159 -147
View File
@@ -38,6 +38,7 @@
#define SENSOR_LIGHT 7
volatile sig_atomic_t do_exit = 0;
volatile sig_atomic_t re_init_sensors = 0;
namespace {
@@ -47,179 +48,190 @@ void set_do_exit(int sig) {
void sigpipe_handler(int sig) {
LOGE("SIGPIPE received");
re_init_sensors = true;
}
void sensor_loop() {
LOG("*** sensor loop");
Context * c = Context::create();
PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents");
assert(sensor_events_sock != NULL);
struct sensors_poll_device_t* device;
struct sensors_module_t* module;
hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
sensors_open(&module->common, &device);
// required
struct sensor_t const* list;
int count = module->get_sensors_list(module, &list);
LOG("%d sensors found", count);
if (getenv("SENSOR_TEST")) {
exit(count);
}
for (int i = 0; i < count; i++) {
LOGD("sensor %4d: %4d %60s %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay);
}
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0);
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0);
device->activate(device, SENSOR_ACCELEROMETER, 0);
device->activate(device, SENSOR_MAGNETOMETER, 0);
device->activate(device, SENSOR_GYRO, 0);
device->activate(device, SENSOR_PROXIMITY, 0);
device->activate(device, SENSOR_LIGHT, 0);
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1);
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1);
device->activate(device, SENSOR_ACCELEROMETER, 1);
device->activate(device, SENSOR_MAGNETOMETER, 1);
device->activate(device, SENSOR_GYRO, 1);
device->activate(device, SENSOR_PROXIMITY, 1);
device->activate(device, SENSOR_LIGHT, 1);
device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10));
device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100));
device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
device->setDelay(device, SENSOR_GYRO, ms2ns(10));
device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100));
device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
static const size_t numEvents = 16;
sensors_event_t buffer[numEvents];
while (!do_exit) {
int n = device->poll(device, buffer, numEvents);
if (n == 0) continue;
if (n < 0) {
LOG("sensor_loop poll failed: %d", n);
continue;
Context * c = Context::create();
PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents");
assert(sensor_events_sock != NULL);
struct sensors_poll_device_t* device;
struct sensors_module_t* module;
hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
sensors_open(&module->common, &device);
// required
struct sensor_t const* list;
int count = module->get_sensors_list(module, &list);
LOG("%d sensors found", count);
if (getenv("SENSOR_TEST")) {
exit(count);
}
int log_events = 0;
for (int i=0; i < n; i++) {
switch (buffer[i].type) {
case SENSOR_TYPE_ACCELEROMETER:
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_TYPE_MAGNETIC_FIELD:
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE:
case SENSOR_TYPE_PROXIMITY:
case SENSOR_TYPE_LIGHT:
log_events++;
break;
default:
continue;
}
for (int i = 0; i < count; i++) {
LOGD("sensor %4d: %4d %60s %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay);
}
uint64_t log_time = nanos_since_boot();
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0);
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0);
device->activate(device, SENSOR_ACCELEROMETER, 0);
device->activate(device, SENSOR_MAGNETOMETER, 0);
device->activate(device, SENSOR_GYRO, 0);
device->activate(device, SENSOR_PROXIMITY, 0);
device->activate(device, SENSOR_LIGHT, 0);
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(log_time);
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1);
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1);
device->activate(device, SENSOR_ACCELEROMETER, 1);
device->activate(device, SENSOR_MAGNETOMETER, 1);
device->activate(device, SENSOR_GYRO, 1);
device->activate(device, SENSOR_PROXIMITY, 1);
device->activate(device, SENSOR_LIGHT, 1);
auto sensor_events = event.initSensorEvents(log_events);
device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10));
device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100));
device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
device->setDelay(device, SENSOR_GYRO, ms2ns(10));
device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100));
device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
int log_i = 0;
for (int i = 0; i < n; i++) {
static const size_t numEvents = 16;
sensors_event_t buffer[numEvents];
const sensors_event_t& data = buffer[i];
switch (data.type) {
case SENSOR_TYPE_ACCELEROMETER:
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_TYPE_MAGNETIC_FIELD:
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE:
case SENSOR_TYPE_PROXIMITY:
case SENSOR_TYPE_LIGHT:
break;
default:
while (!do_exit) {
int n = device->poll(device, buffer, numEvents);
if (n == 0) continue;
if (n < 0) {
LOG("sensor_loop poll failed: %d", n);
continue;
}
auto log_event = sensor_events[log_i];
log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID);
log_event.setVersion(data.version);
log_event.setSensor(data.sensor);
log_event.setType(data.type);
log_event.setTimestamp(data.timestamp);
switch (data.type) {
case SENSOR_TYPE_ACCELEROMETER: {
auto svec = log_event.initAcceleration();
kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
svec.setV(vs);
svec.setStatus(data.acceleration.status);
break;
}
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: {
auto svec = log_event.initMagneticUncalibrated();
// assuming the uncalib and bias floats are contiguous in memory
kj::ArrayPtr<const float> vs(&data.uncalibrated_magnetic.uncalib[0], 6);
svec.setV(vs);
break;
}
case SENSOR_TYPE_MAGNETIC_FIELD: {
auto svec = log_event.initMagnetic();
kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
svec.setV(vs);
svec.setStatus(data.magnetic.status);
break;
}
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: {
auto svec = log_event.initGyroUncalibrated();
// assuming the uncalib and bias floats are contiguous in memory
kj::ArrayPtr<const float> vs(&data.uncalibrated_gyro.uncalib[0], 6);
svec.setV(vs);
break;
}
case SENSOR_TYPE_GYROSCOPE: {
auto svec = log_event.initGyro();
kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
svec.setV(vs);
svec.setStatus(data.gyro.status);
break;
}
case SENSOR_TYPE_PROXIMITY: {
log_event.setProximity(data.distance);
break;
}
case SENSOR_TYPE_LIGHT:
log_event.setLight(data.light);
break;
int log_events = 0;
for (int i=0; i < n; i++) {
switch (buffer[i].type) {
case SENSOR_TYPE_ACCELEROMETER:
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_TYPE_MAGNETIC_FIELD:
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE:
case SENSOR_TYPE_PROXIMITY:
case SENSOR_TYPE_LIGHT:
log_events++;
break;
default:
continue;
}
}
log_i++;
uint64_t log_time = nanos_since_boot();
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(log_time);
auto sensor_events = event.initSensorEvents(log_events);
int log_i = 0;
for (int i = 0; i < n; i++) {
const sensors_event_t& data = buffer[i];
switch (data.type) {
case SENSOR_TYPE_ACCELEROMETER:
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_TYPE_MAGNETIC_FIELD:
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
case SENSOR_TYPE_GYROSCOPE:
case SENSOR_TYPE_PROXIMITY:
case SENSOR_TYPE_LIGHT:
break;
default:
continue;
}
auto log_event = sensor_events[log_i];
log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID);
log_event.setVersion(data.version);
log_event.setSensor(data.sensor);
log_event.setType(data.type);
log_event.setTimestamp(data.timestamp);
switch (data.type) {
case SENSOR_TYPE_ACCELEROMETER: {
auto svec = log_event.initAcceleration();
kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
svec.setV(vs);
svec.setStatus(data.acceleration.status);
break;
}
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: {
auto svec = log_event.initMagneticUncalibrated();
// assuming the uncalib and bias floats are contiguous in memory
kj::ArrayPtr<const float> vs(&data.uncalibrated_magnetic.uncalib[0], 6);
svec.setV(vs);
break;
}
case SENSOR_TYPE_MAGNETIC_FIELD: {
auto svec = log_event.initMagnetic();
kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
svec.setV(vs);
svec.setStatus(data.magnetic.status);
break;
}
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: {
auto svec = log_event.initGyroUncalibrated();
// assuming the uncalib and bias floats are contiguous in memory
kj::ArrayPtr<const float> vs(&data.uncalibrated_gyro.uncalib[0], 6);
svec.setV(vs);
break;
}
case SENSOR_TYPE_GYROSCOPE: {
auto svec = log_event.initGyro();
kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
svec.setV(vs);
svec.setStatus(data.gyro.status);
break;
}
case SENSOR_TYPE_PROXIMITY: {
log_event.setProximity(data.distance);
break;
}
case SENSOR_TYPE_LIGHT:
log_event.setLight(data.light);
break;
}
log_i++;
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
sensor_events_sock->send((char*)bytes.begin(), bytes.size());
if (re_init_sensors){
LOGE("Resetting sensors");
re_init_sensors = false;
break;
}
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
sensor_events_sock->send((char*)bytes.begin(), bytes.size());
delete sensor_events_sock;
delete c;
}
}
delete sensor_events_sock;
delete c;
}
}
}// Namespace end
int main(int argc, char *argv[]) {
setpriority(PRIO_PROCESS, 0, -13);
selfdrive/test/longitudinal_maneuvers/plant.py
+3 -1
View File
@@ -272,6 +272,7 @@ class Plant():
'INTERCEPTOR_GAS',
'INTERCEPTOR_GAS2',
'IMPERIAL_UNIT',
'MOTOR_TORQUE',
])
vls = vls_tuple(
self.speed_sensor(speed),
@@ -304,7 +305,8 @@ class Plant():
0, # Brake hold
0, # Interceptor feedback
0, # Interceptor 2 feedback
False
False,
0,
)
# TODO: publish each message at proper frequency
selfdrive/test/process_replay/process_replay.py
+15 -9
View File
@@ -159,11 +159,11 @@ def get_car_params(msgs, fsm, can_sock):
_, CP = get_car(can, sendcan)
Params().put("CarParams", CP.to_bytes())
def radar_rcv_callback(msg, CP):
def radar_rcv_callback(msg, CP, cfg, fsm):
if msg.which() != "can":
return []
return [], False
elif CP.radarOffCan:
return ["radarState", "liveTracks"]
return ["radarState", "liveTracks"], True
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
"chrysler": [0x2d4]}.get(CP.carName, None)
@@ -173,8 +173,15 @@ 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 []
return ["radarState", "liveTracks"], True
return [], False
def calibration_rcv_callback(msg, CP, cfg, fsm):
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
# should_recv always true to increment frame
recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else []
return recv_socks, True
CONFIGS = [
ProcessConfig(
@@ -215,7 +222,7 @@ CONFIGS = [
},
ignore=[("logMonoTime", 0), ("valid", True)],
init_callback=get_car_params,
should_recv_callback=None,
should_recv_callback=calibration_rcv_callback,
),
]
@@ -263,12 +270,11 @@ def replay_process(cfg, lr):
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)
recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm)
else:
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]
should_recv = bool(len(recv_socks))
should_recv = bool(len(recv_socks))
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
89304bdcab73fa43a8dd39cab93bc4ea4c9cbbdb
b60841eb6cf09037200bc2daacf0c9cf69b358fe
selfdrive/test/test_car_models.py
+124 -113
View File
@@ -39,8 +39,6 @@ def wait_for_socket(name, timeout=10.0):
break
time.sleep(0.5)
if r is None:
sys.exit(-1)
return r
def get_route_logs(route_name):
@@ -405,42 +403,48 @@ passive_routes = [
#"bfa17080b080f3ec|2018-06-28--23-27-47",
]
forced_dashcam_routes = [
# Ford fusion
"f1b4c567731f4a1b|2018-04-18--11-29-37",
"f1b4c567731f4a1b|2018-04-30--10-15-35",
]
# 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
"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__":
@@ -458,96 +462,103 @@ if __name__ == "__main__":
elif "UNLOGGER_PATH" not in os.environ:
continue
for _ in range(3):
shutil.rmtree('/data/params')
manager.gctx = {}
params = Params()
params.manager_start()
if route in passive_routes:
params.put("Passive", "1")
else:
params.put("Passive", "0")
shutil.rmtree('/data/params')
manager.gctx = {}
params = Params()
params.manager_start()
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
print("testing ", route, " ", checks['carFingerprint'])
print("Preparing processes")
manager.prepare_managed_process("radard")
manager.prepare_managed_process("controlsd")
manager.prepare_managed_process("plannerd")
print("Starting processes")
manager.start_managed_process("radard")
manager.start_managed_process("controlsd")
manager.start_managed_process("plannerd")
time.sleep(2)
if route in passive_routes:
params.put("Passive", "1")
else:
params.put("Passive", "0")
# Start unlogger
print("Start unlogger")
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,carEvents,carParams', '--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,carEvents,carParams', '--no-interactive']
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
print("testing ", route, " ", checks['carFingerprint'])
print("Preparing processes")
manager.prepare_managed_process("radard")
manager.prepare_managed_process("controlsd")
manager.prepare_managed_process("plannerd")
print("Starting processes")
manager.start_managed_process("radard")
manager.start_managed_process("controlsd")
manager.start_managed_process("plannerd")
time.sleep(2)
print("Check sockets")
controls_state_result = wait_for_socket('controlsState', timeout=30)
radarstate_result = wait_for_socket('radarState', timeout=30)
plan_result = wait_for_socket('plan', timeout=30)
# Start unlogger
print("Start unlogger")
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,carEvents,carParams', '--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,carEvents,carParams', '--no-interactive']
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
if route not in passive_routes: # TODO The passive routes have very flaky models
path_plan_result = wait_for_socket('pathPlan', timeout=30)
else:
path_plan_result = True
print("Check sockets")
controls_state_result = wait_for_socket('controlsState', timeout=30)
carstate_result = wait_for_socket('carState', timeout=30)
has_camera = checks.get('enableCamera', False)
if (route not in passive_routes) and (route not in forced_dashcam_routes) and has_camera:
controls_state_result = controls_state_result and wait_for_socket('sendcan', timeout=30)
print("Check if everything is running")
running = manager.get_running()
controlsd_running = running['controlsd'].is_alive()
radard_running = running['radard'].is_alive()
plannerd_running = running['plannerd'].is_alive()
radarstate_result = wait_for_socket('radarState', timeout=30)
plan_result = wait_for_socket('plan', timeout=30)
manager.kill_managed_process("controlsd")
manager.kill_managed_process("radard")
manager.kill_managed_process("plannerd")
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
if route not in passive_routes: # TODO The passive routes have very flaky models
path_plan_result = wait_for_socket('pathPlan', timeout=30)
else:
path_plan_result = True
sockets_ok = all([
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
controlsd_running, radard_running, plannerd_running
])
params_ok = True
failures = []
carstate_result = wait_for_socket('carState', timeout=30)
if not controlsd_running:
failures.append('controlsd')
if not radard_running:
failures.append('radard')
if not radarstate_result:
failures.append('radarState')
if not controls_state_result:
failures.append('controlsState')
if not plan_result:
failures.append('plan')
if not path_plan_result:
failures.append('pathPlan')
print("Check if everything is running")
running = manager.get_running()
controlsd_running = running['controlsd'].is_alive()
radard_running = running['radard'].is_alive()
plannerd_running = running['plannerd'].is_alive()
try:
car_params = car.CarParams.from_bytes(params.get("CarParams"))
for k, v in checks.items():
if not v == getattr(car_params, k):
params_ok = False
failures.append(k)
except:
params_ok = False
manager.kill_managed_process("controlsd")
manager.kill_managed_process("radard")
manager.kill_managed_process("plannerd")
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
if sockets_ok and params_ok:
print("Success")
results[route] = True, failures
break
else:
print("Failure")
results[route] = False, failures
sockets_ok = all([
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
controlsd_running, radard_running, plannerd_running
])
params_ok = True
failures = []
time.sleep(2)
if not controlsd_running:
failures.append('controlsd')
if not radard_running:
failures.append('radard')
if not radarstate_result:
failures.append('radarState')
if not controls_state_result:
failures.append('controlsState')
if not plan_result:
failures.append('plan')
if not path_plan_result:
failures.append('pathPlan')
try:
car_params = car.CarParams.from_bytes(params.get("CarParams"))
for k, v in checks.items():
if not v == getattr(car_params, k):
params_ok = False
failures.append(k)
except Exception:
params_ok = False
if sockets_ok and params_ok:
print("Success")
results[route] = True, failures
else:
print("Failure")
results[route] = False, failures
break
time.sleep(2)
for route in results:
print(results[route])
selfdrive/test/test_openpilot.py
+23 -29
View File
@@ -149,11 +149,11 @@ def test_athena():
assert False, f"Athena did not start within {timeout} seconds"
time.sleep(0.5)
def athena_post(payload, max_retries=5):
def athena_post(payload, max_retries=5, wait=5):
tries = 0
while 1:
try:
return requests.post(
resp = requests.post(
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
headers={
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
@@ -162,29 +162,26 @@ def test_athena():
data=json.dumps(payload),
timeout=30
)
resp_json = resp.json()
if resp_json.get('error'):
raise Exception(resp_json['error'])
return resp_json
except Exception as e:
print(e)
time.sleep(5.0)
time.sleep(wait)
tries += 1
if tries == max_retries:
raise
else:
print(f'athena_post failed {e}. retrying...')
def expect_athena_registers(timeout=60):
now = time.time()
while 1:
resp = athena_post({
"method": "echo",
"params": ["hello"],
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
if resp_json.get('result') == "hello":
break
elif time.time() - now > timeout:
assert False, f"Athena did not become available within {timeout} seconds."
else:
time.sleep(5.0)
def expect_athena_registers():
resp = athena_post({
"method": "echo",
"params": ["hello"],
"id": 0,
"jsonrpc": "2.0"
}, max_retries=12, wait=5)
assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
try:
athenad_pid = expect_athena_starts()
@@ -204,9 +201,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert 'sim_id' in resp_json['result'], resp_json['result']
assert resp.get('result'), resp
assert 'sim_id' in resp['result'], resp['result']
print("ATHENA: takeSnapshot")
resp = athena_post({
@@ -214,9 +210,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert resp_json['result']['jpegBack'], resp_json['result']
assert resp.get('result'), resp
assert resp['result']['jpegBack'], resp['result']
@with_processes(["thermald"])
def test_athena_thermal():
@@ -227,9 +222,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert resp_json['result']['thermal'], resp_json['result']
assert resp.get('result'), resp
assert resp['result']['thermal'], resp['result']
test_athena_thermal()
finally:
try:
selfdrive/thermald.py
+39 -20
View File
@@ -6,6 +6,7 @@ import datetime
import psutil
from smbus2 import SMBus
from cereal import log
from common.android import ANDROID
from common.basedir import BASEDIR
from common.params import Params, put_nonblocking
from common.realtime import sec_since_boot, DT_TRML
@@ -15,9 +16,9 @@ from selfdrive.version import terms_version, training_version
from selfdrive.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.loggerd.config import get_available_percent
from selfdrive.pandad import get_expected_version
from selfdrive.pandad import get_expected_signature
FW_VERSION = get_expected_version()
FW_SIGNATURE = get_expected_signature()
params = Params()
import subprocess
@@ -33,10 +34,13 @@ with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file:
OFFROAD_ALERTS = json.load(json_file)
def read_tz(x, clip=True):
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
ret = int(f.read())
if clip:
ret = max(0, ret)
try:
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
ret = int(f.read())
if clip:
ret = max(0, ret)
except FileNotFoundError:
return 0
return ret
@@ -163,7 +167,7 @@ def thermald_thread():
should_start_prev = False
is_uno = (read_tz(29, clip=False) < -1000)
if is_uno:
if is_uno or not ANDROID:
handle_fan = handle_fan_uno
else:
setup_eon_fan()
@@ -193,20 +197,28 @@ def thermald_thread():
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
msg.thermal.freeSpace = get_available_percent() / 100.0 # disk space
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = int(f.read())
with open("/sys/class/power_supply/usb/present") as f:
msg.thermal.usbOnline = bool(int(f.read()))
try:
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = int(f.read())
with open("/sys/class/power_supply/usb/present") as f:
msg.thermal.usbOnline = bool(int(f.read()))
except FileNotFoundError:
pass
# Fake battery levels on uno for frame
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
# dragonpilot ip Mod
# update ip every 10 seconds
@@ -295,8 +307,9 @@ def thermald_thread():
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get("CompletedTrainingVersion") == training_version
fw_version = params.get("PandaFirmware", encoding="utf8")
fw_version_match = fw_version is None or fw_version.startswith(FW_VERSION) # don't show alert is no panda is connected (None)
panda_signature = params.get("PandaFirmware")
fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
should_start = ignition
@@ -333,12 +346,18 @@ def thermald_thread():
params.delete("Offroad_TemperatureTooHigh")
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
selfdrive/ui/SConscript
+13 -3
View File
@@ -1,5 +1,15 @@
Import('env', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
env.Program('_ui', ['ui.cc', 'slplay.c', '#phonelibs/nanovg/nanovg.c'],
src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c']
libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, 'OpenCL', gpucommon, visionipc]
if arch == "aarch64":
src += ['sound.cc', 'slplay.c']
libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid']
else:
src += ['linux.cc']
libs += ['EGL', 'pthread', 'X11-xcb', 'xcb', 'X11', 'glfw']
env.Program('_ui', src,
LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'],
LIBS=[common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', 'GLESv3', 'EGL', cereal, 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'json', messaging, 'CB', 'OpenCL', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', gpucommon, visionipc])
LIBS=libs)
selfdrive/ui/linux.cc
+100
View File
@@ -0,0 +1,100 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include "ui.hpp"
#define GLFW_INCLUDE_ES2
#define GLFW_INCLUDE_GLEXT
#include <GLFW/glfw3.h>
typedef struct FramebufferState FramebufferState;
typedef struct TouchState TouchState;
#define FALSE 0
#define TRUE 1
#include <xcb/xcb.h>
#include <X11/Xlib-xcb.h>
extern "C" {
FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
int *out_w, int *out_h) {
glfwInit();
glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_ES_API);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
glfwWindowHint(GLFW_RESIZABLE, 0);
GLFWwindow* window;
window = glfwCreateWindow(1920, 1080, "ui", NULL, NULL);
if (!window) {
printf("glfwCreateWindow failed\n");
}
glfwMakeContextCurrent(window);
glfwSwapInterval(0);
// clear screen
glClearColor(0.2f, 0.2f, 0.2f, 1.0f );
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
framebuffer_swap((FramebufferState*)window);
if (out_w) *out_w = 1920;
if (out_h) *out_h = 1080;
return (FramebufferState*)window;
}
void framebuffer_set_power(FramebufferState *s, int mode) {
}
void framebuffer_swap(FramebufferState *s) {
glfwSwapBuffers((GLFWwindow*)s);
}
void touch_init(TouchState *s) {
printf("touch_init\n");
}
int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
return -1;
}
int touch_read(TouchState *s, int* out_x, int* out_y) {
return -1;
}
}
#include "sound.hpp"
void ui_sound_init() {}
void ui_sound_destroy() {}
void set_volume(int volume) {}
void play_alert_sound(AudibleAlert alert) {}
void stop_alert_sound(AudibleAlert alert) {}
#include "common/visionimg.h"
#include <sys/mman.h>
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
unsigned int texture;
glGenTextures(1, &texture);
glBindTexture(GL_TEXTURE_2D, texture);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph);
glGenerateMipmap(GL_TEXTURE_2D);
*pkhr = (EGLImageKHR *)1; // not NULL
return texture;
}
void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
// empty
}
selfdrive/ui/paint.cc
+1050
View File
File diff suppressed because it is too large Load Diff
selfdrive/ui/sound.cc
+85
View File
@@ -0,0 +1,85 @@
#include <stdlib.h>
#include "sound.hpp"
#include "common/swaglog.h"
typedef struct {
AudibleAlert alert;
const char* uri;
bool loop;
} sound_file;
extern "C"{
#include "slplay.h"
}
void set_volume(int volume) {
char volume_change_cmd[64];
sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume);
// 5 second timeout at 60fps
int volume_changed = system(volume_change_cmd);
}
sound_file sound_table[] = {
{ cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_repeat.wav", true },
{ cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false },
{ cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false },
};
sound_file* get_sound_file(AudibleAlert alert) {
for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
if (s->alert == alert) {
return s;
}
}
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 = NULL;
slplay_setup(&error);
if (error) goto fail;
for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
slplay_create_player_for_uri(s->uri, &error);
if (error) goto fail;
}
return;
fail:
LOGW(error);
exit(1);
}
void ui_sound_destroy() {
slplay_destroy();
}
selfdrive/ui/sound.hpp
+17
View File
@@ -0,0 +1,17 @@
#ifndef __SOUND_HPP
#define __SOUND_HPP
#include "cereal/gen/c/log.capnp.h"
typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert;
void ui_sound_init();
void ui_sound_destroy();
void set_volume(int volume);
void play_alert_sound(AudibleAlert alert);
void stop_alert_sound(AudibleAlert alert);
#endif
selfdrive/ui/ui.cc
+50 -1760
View File
File diff suppressed because it is too large Load Diff
selfdrive/ui/ui.hpp
+253
View File
@@ -0,0 +1,253 @@
#ifndef _UI_H
#define _UI_H
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include "nanovg.h"
#include "common/mat.h"
#include "common/visionipc.h"
#include "common/framebuffer.h"
#include "common/modeldata.h"
#include "messaging.hpp"
#include "cereal/gen/c/log.capnp.h"
#include "sound.hpp"
#define STATUS_STOPPED 0
#define STATUS_DISENGAGED 1
#define STATUS_ENGAGED 2
#define STATUS_WARNING 3
#define STATUS_ALERT 4
#define ALERTSIZE_NONE 0
#define ALERTSIZE_SMALL 1
#define ALERTSIZE_MID 2
#define ALERTSIZE_FULL 3
#ifndef QCOM
#define UI_60FPS
#endif
#define UI_BUF_COUNT 4
//#define SHOW_SPEEDLIMIT 1
//#define DEBUG_TURN
const int vwp_w = 1920;
const int vwp_h = 1080;
const int nav_w = 640;
const int nav_ww= 760;
const int sbr_w = 300;
const int bdr_s = 30;
const int box_x = sbr_w+bdr_s;
const int box_y = bdr_s;
const int box_w = vwp_w-sbr_w-(bdr_s*2);
const int box_h = vwp_h-(bdr_s*2);
const int viz_w = vwp_w-(bdr_s*2);
const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
const int UI_FREQ = 30; // Hz
const int MODEL_PATH_MAX_VERTICES_CNT = 98;
const int MODEL_LANE_PATH_CNT = 3;
const int TRACK_POINTS_MAX_CNT = 50 * 2;
const int SET_SPEED_NA = 255;
const uint8_t bg_colors[][4] = {
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff},
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff},
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff},
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff},
};
typedef struct UIScene {
int frontview;
int fullview;
int transformed_width, transformed_height;
ModelData model;
float mpc_x[50];
float mpc_y[50];
bool world_objects_visible;
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
float v_cruise;
uint64_t v_cruise_update_ts;
float v_ego;
bool decel_for_model;
float speedlimit;
bool speedlimit_valid;
bool map_valid;
float curvature;
int engaged;
bool engageable;
bool monitoring_active;
bool uilayout_sidebarcollapsed;
bool uilayout_mapenabled;
// responsive layout
int ui_viz_rx;
int ui_viz_rw;
int ui_viz_ro;
int lead_status;
float lead_d_rel, lead_y_rel, lead_v_rel;
int front_box_x, front_box_y, front_box_width, front_box_height;
uint64_t alert_ts;
char alert_text1[1024];
char alert_text2[1024];
uint8_t alert_size;
float alert_blinkingrate;
float awareness_status;
// Used to show gps planner status
bool gps_planner_active;
} UIScene;
typedef struct {
float x, y;
}vertex_data;
typedef struct {
vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
int cnt;
} model_path_vertices_data;
typedef struct {
vertex_data v[TRACK_POINTS_MAX_CNT];
int cnt;
} track_vertices_data;
typedef struct UIState {
pthread_mutex_t lock;
pthread_cond_t bg_cond;
// framebuffer
FramebufferState *fb;
int fb_w, fb_h;
EGLDisplay display;
EGLSurface surface;
// NVG
NVGcontext *vg;
// fonts and images
int font_courbd;
int font_sans_regular;
int font_sans_semibold;
int font_sans_bold;
int img_wheel;
int img_turn;
int img_face;
int img_map;
// sockets
Context *ctx;
SubSocket *model_sock;
SubSocket *controlsstate_sock;
SubSocket *livecalibration_sock;
SubSocket *radarstate_sock;
SubSocket *map_data_sock;
SubSocket *uilayout_sock;
Poller * poller;
int active_app;
// vision state
bool vision_connected;
bool vision_connect_firstrun;
int ipc_fd;
VIPCBuf bufs[UI_BUF_COUNT];
VIPCBuf front_bufs[UI_BUF_COUNT];
int cur_vision_idx;
int cur_vision_front_idx;
GLuint frame_program;
GLuint frame_texs[UI_BUF_COUNT];
EGLImageKHR khr[UI_BUF_COUNT];
void *priv_hnds[UI_BUF_COUNT];
GLuint frame_front_texs[UI_BUF_COUNT];
EGLImageKHR khr_front[UI_BUF_COUNT];
void *priv_hnds_front[UI_BUF_COUNT];
GLint frame_pos_loc, frame_texcoord_loc;
GLint frame_texture_loc, frame_transform_loc;
GLuint line_program;
GLint line_pos_loc, line_color_loc;
GLint line_transform_loc;
int rgb_width, rgb_height, rgb_stride;
size_t rgb_buf_len;
mat4 rgb_transform;
int rgb_front_width, rgb_front_height, rgb_front_stride;
size_t rgb_front_buf_len;
UIScene scene;
bool awake;
// timeouts
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;
bool limit_set_speed;
float speed_lim_off;
bool is_ego_over_limit;
char alert_type[64];
AudibleAlert alert_sound;
int alert_size;
float alert_blinking_alpha;
bool alert_blinked;
float light_sensor;
int touch_fd;
// Hints for re-calculations and redrawing
bool model_changed;
bool livempc_or_radarstate_changed;
GLuint frame_vao[2], frame_vbo[2], frame_ibo[2];
mat4 rear_frame_mat, front_frame_mat;
model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
track_vertices_data track_vertices[2];
} UIState;
// API
void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
const char* va_text1, const char* va_text2);
void ui_draw(UIState *s);
void ui_nvg_init(UIState *s);
#endif
selfdrive/updated.py
+343 -33
View File
@@ -1,58 +1,368 @@
#!/usr/bin/env python3
# simple service that waits for network access and tries to update every hour
# Safe Update: A simple service that waits for network access and tries to
# update every 10 minutes. It's intended to make the OP update process more
# robust against Git repository corruption. This service DOES NOT try to fix
# an already-corrupt BASEDIR Git repo, only prevent it from happening.
#
# During normal operation, both onroad and offroad, the update process makes
# no changes to the BASEDIR install of OP. All update attempts are performed
# in a disposable staging area provided by OverlayFS. It assumes the deleter
# process provides enough disk space to carry out the process.
#
# If an update succeeds, a flag is set, and the update is swapped in at the
# next reboot. If an update is interrupted or otherwise fails, the OverlayFS
# upper layer and metadata can be discarded before trying again.
#
# The swap on boot is triggered by launch_chffrplus.sh
# gated on the existence of $FINALIZED/.overlay_consistent and also the
# existence and mtime of $BASEDIR/.overlay_init.
#
# Other than build byproducts, BASEDIR should not be modified while this
# service is running. Developers modifying code directly in BASEDIR should
# disable this service.
import os
import datetime
import subprocess
import time
import psutil
from stat import S_ISREG, S_ISDIR, S_ISLNK, S_IMODE, ST_MODE, ST_INO, ST_UID, ST_GID, ST_ATIME, ST_MTIME
import shutil
import signal
from pathlib import Path
import fcntl
import threading
from cffi import FFI
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.swaglog import cloudlog
STAGING_ROOT = "/data/safe_staging"
OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper")
OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata")
OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged")
FINALIZED = os.path.join(STAGING_ROOT, "finalized")
NICE_LOW_PRIORITY = ["nice", "-n", "19"]
def main(gctx=None):
SHORT = os.getenv("SHORT") is not None
# Workaround for the EON/termux build of Python having os.link removed.
ffi = FFI()
ffi.cdef("int link(const char *oldpath, const char *newpath);")
libc = ffi.dlopen(None)
class WaitTimeHelper:
ready_event = threading.Event()
shutdown = False
def __init__(self):
signal.signal(signal.SIGTERM, self.graceful_shutdown)
signal.signal(signal.SIGINT, self.graceful_shutdown)
signal.signal(signal.SIGHUP, self.update_now)
def graceful_shutdown(self, signum, frame):
# umount -f doesn't appear effective in avoiding "device busy" on EON,
# so don't actually die until the next convenient opportunity in main().
cloudlog.info("caught SIGINT/SIGTERM, dismounting overlay at next opportunity")
self.shutdown = True
self.ready_event.set()
def update_now(self, signum, frame):
cloudlog.info("caught SIGHUP, running update check immediately")
self.ready_event.set()
def wait_between_updates(ready_event):
ready_event.clear()
if SHORT:
ready_event.wait(timeout=10)
else:
ready_event.wait(timeout=60 * 10)
def link(src, dest):
# Workaround for the EON/termux build of Python having os.link removed.
return libc.link(src.encode(), dest.encode())
def run(cmd, cwd=None):
return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
def remove_consistent_flag():
os.system("sync")
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
try:
consistent_file.unlink()
except FileNotFoundError:
pass
os.system("sync")
def set_consistent_flag():
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
os.system("sync")
consistent_file.touch()
os.system("sync")
def set_update_available_params(new_version=False):
params = Params()
while True:
time_wrong = datetime.datetime.now().year < 2019
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
if ping_failed or time_wrong:
time.sleep(60)
continue
# download application update
try:
r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT).decode('utf8')
except subprocess.CalledProcessError as e:
cloudlog.event("git fetch failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode)
time.sleep(60)
continue
cloudlog.info("git fetch success: %s", r)
# Write update available param
try:
cur_hash = subprocess.check_output(["git", "rev-parse", "HEAD"]).rstrip()
upstream_hash = subprocess.check_output(["git", "rev-parse", "@{u}"]).rstrip()
params.put("UpdateAvailable", str(int(cur_hash != upstream_hash)))
except:
params.put("UpdateAvailable", "0")
t = datetime.datetime.now().isoformat()
params.put("LastUpdateTime", t.encode('utf8'))
if new_version:
# Write latest release notes to param
try:
# r = subprocess.check_output(["git", "--no-pager", "show", "@{u}:RELEASES.md"])
r = subprocess.check_output(["curl", "-H", "'Cache-Control: no-cache'", "-s", "https://raw.githubusercontent.com/dragonpilot-community/dragonpilot/docs/CHANGELOG.md"])
r = r[:r.find(b'\n\n')] # Slice latest release notes
params.put("ReleaseNotes", r + b"\n")
except:
params.put("ReleaseNotes", "")
#try:
# with open(os.path.join(FINALIZED, "RELEASES.md"), "rb") as f:
# r = f.read()
# r = r[:r.find(b'\n\n')] # Slice latest release notes
# params.put("ReleaseNotes", r + b"\n")
#except Exception:
# params.put("ReleaseNotes", "")
params.put("UpdateAvailable", "1")
t = datetime.datetime.now().isoformat()
params.put("LastUpdateTime", t.encode('utf8'))
time.sleep(60*60)
def dismount_ovfs():
if os.path.ismount(OVERLAY_MERGED):
cloudlog.error("unmounting existing overlay")
run(["umount", "-l", OVERLAY_MERGED])
def init_ovfs():
cloudlog.info("preparing new safe staging area")
Params().put("UpdateAvailable", "0")
remove_consistent_flag()
dismount_ovfs()
if os.path.isdir(STAGING_ROOT):
shutil.rmtree(STAGING_ROOT)
for dirname in [STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED, FINALIZED]:
os.mkdir(dirname, 0o755)
if not os.lstat(BASEDIR).st_dev == os.lstat(OVERLAY_MERGED).st_dev:
raise RuntimeError("base and overlay merge directories are on different filesystems; not valid for overlay FS!")
# Remove consistent flag from current BASEDIR so it's not copied over
if os.path.isfile(os.path.join(BASEDIR, ".overlay_consistent")):
os.remove(os.path.join(BASEDIR, ".overlay_consistent"))
# We sync FS object atimes (which EON doesn't use) and mtimes, but ctimes
# are outside user control. Make sure Git is set up to ignore system ctimes,
# because they change when we make hard links during finalize. Otherwise,
# there is a lot of unnecessary churn. This appears to be a common need on
# OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/
run(["git", "config", "core.trustctime", "false"], BASEDIR)
# We are temporarily using copytree to copy the directory, which also changes
# inode numbers. Ignore those changes too.
run(["git", "config", "core.checkStat", "minimal"], BASEDIR)
# Leave a timestamped canary in BASEDIR to check at startup. The EON clock
# should be correct by the time we get here. If the init file disappears, or
# critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can
# assume that BASEDIR has used for local development or otherwise modified,
# and skips the update activation attempt.
Path(os.path.join(BASEDIR, ".overlay_init")).touch()
overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}"
run(["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED])
def inodes_in_tree(search_dir):
"""Given a search root, produce a dictionary mapping of inodes to relative
pathnames of regular files (no directories, symlinks, or special files)."""
inode_map = {}
for root, dirs, files in os.walk(search_dir, topdown=True):
for file_name in files:
full_path_name = os.path.join(root, file_name)
st = os.lstat(full_path_name)
if S_ISREG(st[ST_MODE]):
inode_map[st[ST_INO]] = full_path_name
return inode_map
def dup_ovfs_object(inode_map, source_obj, target_dir):
"""Given a relative pathname to copy, and a new target root, duplicate the
source object in the target root, using hardlinks for regular files."""
source_full_path = os.path.join(OVERLAY_MERGED, source_obj)
st = os.lstat(source_full_path)
target_full_path = os.path.join(target_dir, source_obj)
if S_ISREG(st[ST_MODE]):
# Hardlink all regular files; ownership and permissions are shared.
link(inode_map[st[ST_INO]], target_full_path)
else:
# Recreate all directories and symlinks; copy ownership and permissions.
if S_ISDIR(st[ST_MODE]):
os.mkdir(os.path.join(FINALIZED, source_obj), S_IMODE(st[ST_MODE]))
elif S_ISLNK(st[ST_MODE]):
os.symlink(os.readlink(source_full_path), target_full_path)
os.chmod(target_full_path, S_IMODE(st[ST_MODE]), follow_symlinks=False)
else:
# Ran into a FIFO, socket, etc. Should not happen in OP install dir.
# Ignore without copying for the time being; revisit later if needed.
cloudlog.error("can't copy this file type: %s" % source_full_path)
os.chown(target_full_path, st[ST_UID], st[ST_GID], follow_symlinks=False)
# Sync target mtimes to the cached lstat() value from each source object.
# Restores shared inode mtimes after linking, fixes symlinks and dirs.
os.utime(target_full_path, (st[ST_ATIME], st[ST_MTIME]), follow_symlinks=False)
def finalize_from_ovfs_hardlink():
"""Take the current OverlayFS merged view and finalize a copy outside of
OverlayFS, ready to be swapped-in at BASEDIR. Copy using hardlinks"""
cloudlog.info("creating finalized version of the overlay")
# The "copy" is done with hardlinks, but since the OverlayFS merge looks
# like a different filesystem, and hardlinks can't cross filesystems, we
# have to borrow a source pathname from the upper or lower layer.
inode_map = inodes_in_tree(BASEDIR)
inode_map.update(inodes_in_tree(OVERLAY_UPPER))
shutil.rmtree(FINALIZED)
os.umask(0o077)
os.mkdir(FINALIZED)
for root, dirs, files in os.walk(OVERLAY_MERGED, topdown=True):
for obj_name in dirs:
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
for obj_name in files:
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
cloudlog.info("done finalizing overlay")
def finalize_from_ovfs_copy():
"""Take the current OverlayFS merged view and finalize a copy outside of
OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree"""
cloudlog.info("creating finalized version of the overlay")
shutil.rmtree(FINALIZED)
shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True)
cloudlog.info("done finalizing overlay")
def attempt_update():
cloudlog.info("attempting git update inside staging overlay")
git_fetch_output = run(NICE_LOW_PRIORITY + ["git", "fetch"], OVERLAY_MERGED)
cloudlog.info("git fetch success: %s", git_fetch_output)
cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip()
upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip()
new_version = cur_hash != upstream_hash
git_fetch_result = len(git_fetch_output) > 0 and (git_fetch_output != "Failed to add the host to the list of known hosts (/data/data/com.termux/files/home/.ssh/known_hosts).\n")
cloudlog.info("comparing %s to %s" % (cur_hash, upstream_hash))
if new_version or git_fetch_result:
cloudlog.info("Running update")
if new_version:
cloudlog.info("git reset in progress")
r = [
run(NICE_LOW_PRIORITY + ["git", "reset", "--hard", "@{u}"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "clean", "-xdf"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "submodule", "init"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "submodule", "update"], OVERLAY_MERGED),
]
cloudlog.info("git reset success: %s", '\n'.join(r))
# Un-set the validity flag to prevent the finalized tree from being
# activated later if the finalize step is interrupted
remove_consistent_flag()
finalize_from_ovfs_copy()
# Make sure the validity flag lands on disk LAST, only when the local git
# repo and OP install are in a consistent state.
set_consistent_flag()
cloudlog.info("update successful!")
else:
cloudlog.info("nothing new from git at this time")
set_update_available_params(new_version=new_version)
def main(gctx=None):
overlay_init_done = False
wait_helper = WaitTimeHelper()
params = Params()
if not os.geteuid() == 0:
raise RuntimeError("updated must be launched as root!")
# Set low io priority
p = psutil.Process()
if psutil.LINUX:
p.ionice(psutil.IOPRIO_CLASS_BE, value=7)
ov_lock_fd = open('/tmp/safe_staging_overlay.lock', 'w')
try:
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
except IOError:
raise RuntimeError("couldn't get overlay lock; is another updated running?")
while True:
time_wrong = datetime.datetime.now().year < 2019
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
# Wait until we have a valid datetime to initialize the overlay
if not (ping_failed or time_wrong):
try:
# If the git directory has modifcations after we created the overlay
# we need to recreate the overlay
if overlay_init_done:
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
if len(new_files.splitlines()):
cloudlog.info(".git directory changed, recreating overlay")
overlay_init_done = False
if not overlay_init_done:
init_ovfs()
overlay_init_done = True
if params.get("IsOffroad") == b"1":
attempt_update()
else:
cloudlog.info("not running updater, openpilot running")
except subprocess.CalledProcessError as e:
cloudlog.event(
"update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
overlay_init_done = False
except Exception:
cloudlog.exception("uncaught updated exception, shouldn't happen")
overlay_init_done = False
wait_between_updates(wait_helper.ready_event)
if wait_helper.shutdown:
break
# We've been signaled to shut down
dismount_ovfs()
if __name__ == "__main__":
main()
selfdrive/version.py
+2 -2
View File
@@ -48,7 +48,7 @@ try:
dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"], encoding='utf8')
commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"], encoding='utf8').rstrip()
origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch], encoding='utf8').rstrip()
cloudlog.event("dirty comma branch", vesion=version, dirty=dirty, origin=origin, branch=branch, dirty_files=dirty_files, commit=commit, origin_commit=origin_commit)
cloudlog.event("dirty comma branch", version=version, dirty=dirty, origin=origin, branch=branch, dirty_files=dirty_files, commit=commit, origin_commit=origin_commit)
else:
dirty = True
@@ -59,7 +59,7 @@ except subprocess.CalledProcessError:
pass
dirty = True
training_version = b"0.1.0"
training_version = b"0.2.0"
terms_version = b"2"
if __name__ == "__main__":