mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 16:23:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.7 release

Browse Source 
old-commit-hash: daf54ad54d
This commit is contained in:
Vehicle Researcher
2017-09-30 03:07:27 -07:00
parent 7a6d152ae0
commit 2c0cc6103a
130 changed files with 3557 additions and 1717 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/boardd/Makefile
+7 -2
View File
@@ -4,6 +4,7 @@ CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../..
PHONELIBS = ../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
@@ -12,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
@@ -37,6 +38,8 @@ ZMQ_LIBS = -L$(HOME)/one/external/zmq/lib/ \
CEREAL_LIBS = -L$(HOME)/one/external/capnp/lib/ \
-l:libcapnp.a -l:libcapnp_c.a -l:libkj.a
EXTRA_LIBS = -lusb-1.0 -lpthread
CXXFLAGS += -I/usr/include/libusb-1.0
CFLAGS += -I/usr/include/libusb-1.0
endif
.PHONY: all
@@ -46,6 +49,8 @@ include ../common/cereal.mk
OBJS = boardd.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
$(PHONELIBS)/json/src/json.o \
$(CEREAL_OBJS)
selfdrive/boardd/boardd.cc
+105 -27
View File
@@ -19,7 +19,9 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
@@ -38,20 +40,55 @@ bool loopback_can = false;
#define TIMEOUT 0
#define SAFETY_NOOUTPUT 0x0000
#define SAFETY_HONDA 0x0001
#define SAFETY_ALLOUTPUT 0x1337
pthread_t safety_setter_thread_handle = -1;
void *safety_setter_thread(void *s) {
char *value;
size_t value_sz = 0;
LOGW("waiting for params to set safety model");
while (1) {
const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
if (value_sz > 0) break;
usleep(100*1000);
}
LOGW("got %d bytes CarParams", value_sz);
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), value, value_sz);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
int safety_model = car_params.getSafetyModel();
LOGW("setting safety model: %d", safety_model);
pthread_mutex_lock(&usb_lock);
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
return NULL;
}
// must be called before threads or with mutex
bool usb_connect() {
int err;
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { return false; }
if (dev_handle == NULL) { goto fail; }
err = libusb_set_configuration(dev_handle, 1);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
err = libusb_claim_interface(dev_handle, 0);
if (err != 0) { return false; }
if (err != 0) { goto fail; }
if (loopback_can) {
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
@@ -60,30 +97,23 @@ bool usb_connect() {
// power off ESP
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
// forward CAN1 to CAN3...soon
//libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT);
// power on charging (may trigger a reconnection, should be okay)
#ifndef __x86_64__
libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT);
#else
LOGW("not enabling charging on x86_64");
#endif
// set UART modes for Honda Accord
/*for (int uart = 2; uart <= 3; uart++) {
// 9600 baud
libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT);
// even parity
libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT);
// callback 1
libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT);
// no output is the default
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
}
// TODO: Boardd should be able to set the baud rate
int baud = 500000;
libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1
libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0,
(unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/
// TODO: Boardd should be able to be told which safety model to use
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT);
return true;
fail:
return false;
}
void usb_retry_connect() {
@@ -93,7 +123,7 @@ void usb_retry_connect() {
}
void handle_usb_issue(int err, const char func[]) {
LOGE("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
if (err == -4) {
LOGE("lost connection");
usb_retry_connect();
@@ -113,7 +143,7 @@ void can_recv(void *s) {
do {
err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
if (err != 0) { handle_usb_issue(err, __func__); }
if (err == -8) { LOGE("overflow got 0x%x", recv); };
if (err == -8) { LOGE_100("overflow got 0x%x", recv); };
// timeout is okay to exit, recv still happened
if (err == -7) { break; }
@@ -261,6 +291,46 @@ void can_send(void *s) {
// **** threads ****
void *thermal_thread(void *crap) {
int err;
LOGD("start thermal thread");
// thermal = 8005
void *context = zmq_ctx_new();
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8005");
// run as fast as messages come in
while (!do_exit) {
// recv from thermal
zmq_msg_t msg;
zmq_msg_init(&msg);
err = zmq_msg_recv(&msg, subscriber, 0);
assert(err >= 0);
// format for board, make copy due to alignment issues, will be freed on out of scope
// copied from send thread...
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
uint16_t target_fan_speed = event.getThermal().getFanSpeed();
//LOGW("setting fan speed %d", target_fan_speed);
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT);
pthread_mutex_unlock(&usb_lock);
}
// turn the fan off when we exit
libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT);
return NULL;
}
void *can_send_thread(void *crap) {
LOGD("start send thread");
@@ -364,8 +434,16 @@ int main() {
can_recv_thread, NULL);
assert(err == 0);
pthread_t thermal_thread_handle;
err = pthread_create(&thermal_thread_handle, NULL,
thermal_thread, NULL);
assert(err == 0);
// join threads
err = pthread_join(thermal_thread_handle, NULL);
assert(err == 0);
err = pthread_join(can_recv_thread_handle, NULL);
assert(err == 0);
selfdrive/boardd/boardd.py
+4 -1
View File
@@ -20,6 +20,8 @@ except Exception:
SAFETY_NOOUTPUT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_TOYOTA_NOLIMITS = 0x1336
SAFETY_ALLOUTPUT = 0x1337
# *** serialization functions ***
@@ -33,7 +35,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = can_msg[2]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
@@ -111,6 +113,7 @@ def can_init():
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)
selfdrive/can/Makefile
+3 -2
View File
@@ -1,6 +1,7 @@
CC = clang
CXX = clang++
BASEDIR = ../..
PHONELIBS := ../../phonelibs
UNAME_S := $(shell uname -s)
@@ -53,7 +54,7 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
../../cereal/gen/cpp/log.capnp.h:
cd ../../cereal && make
libdbc.so:: parser.cc $(DBC_CCS)
libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS)
$(CXX) -fPIC -shared -o '$@' $^ \
-I. \
-I../.. \
@@ -63,7 +64,7 @@ libdbc.so:: parser.cc $(DBC_CCS)
$(CEREAL_CXXFLAGS) \
$(CEREAL_LIBS)
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
.PHONY: clean
selfdrive/can/parser_common.h → selfdrive/can/common.h
+11 -2
View File
@@ -1,13 +1,20 @@
#ifndef PARSER_COMMON_H
#define PARSER_COMMON_H
#ifndef SELFDRIVE_CAN_COMMON_H
#define SELFDRIVE_CAN_COMMON_H
#include <cstddef>
#include <cstdint>
#include <string>
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
struct SignalPackValue {
const char* name;
double value;
};
struct SignalParseOptions {
uint32_t address;
const char* name;
@@ -54,6 +61,8 @@ struct DBC {
const Msg *msgs;
};
const DBC* dbc_lookup(const std::string& dbc_name);
void dbc_register(const DBC* dbc);
#define dbc_init(dbc) \
selfdrive/can/dbc.cc
+26
View File
@@ -0,0 +1,26 @@
#include <string>
#include <vector>
#include "common.h"
namespace {
std::vector<const DBC*>& get_dbcs() {
static std::vector<const DBC*> vec;
return vec;
}
}
const DBC* dbc_lookup(const std::string& dbc_name) {
for (const auto& dbci : get_dbcs()) {
if (dbc_name == dbci->name) {
return dbci;
}
}
return NULL;
}
void dbc_register(const DBC* dbc) {
get_dbcs().push_back(dbc);
}
selfdrive/can/dbc_template.cc
+1 -1
View File
@@ -1,6 +1,6 @@
#include <cstdint>
#include "parser_common.h"
#include "common.h"
namespace {
selfdrive/can/libdbc_py.py
+51
View File
@@ -0,0 +1,51 @@
import os
import subprocess
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
typedef struct SignalPackValue {
const char* name;
double value;
} SignalPackValue;
void* canpack_init(const char* dbc_name);
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals);
""")
libdbc = ffi.dlopen(libdbc_fn)
selfdrive/can/packer.cc
+81
View File
@@ -0,0 +1,81 @@
#include <cassert>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>
#include <map>
#include "common.h"
#define WARN printf
namespace {
class CANPacker {
public:
CANPacker(const std::string& dbc_name) {
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (int i=0; i<dbc->num_msgs; i++) {
const Msg* msg = &dbc->msgs[i];
for (int j=0; j<msg->num_sigs; j++) {
const Signal* sig = &msg->sigs[j];
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
}
}
}
uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals) {
uint64_t ret = 0;
for (const auto& sigval : signals) {
std::string name = std::string(sigval.name);
double value = sigval.value;
auto sig_it = signal_lookup.find(make_pair(address, name));
if (sig_it == signal_lookup.end()) {
WARN("undefined signal %s", name.c_str());
continue;
}
auto sig = sig_it->second;
int64_t ival = (int64_t)((value - sig.offset) / sig.factor);
if (ival < 0) {
WARN("signed pack unsupported right now");
continue;
}
uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo;
uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo;
ret &= ~mask;
ret |= dat;
}
return ret;
}
private:
const DBC *dbc = NULL;
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
};
}
extern "C" {
void* canpack_init(const char* dbc_name) {
CANPacker *ret = new CANPacker(std::string(dbc_name));
return (void*)ret;
}
uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals) {
CANPacker *cp = (CANPacker*)inst;
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals));
}
}
selfdrive/can/packer.py
+37
View File
@@ -0,0 +1,37 @@
import struct
from selfdrive.can.libdbc_py import libdbc, ffi
class CANPacker(object):
def __init__(self, dbc_name):
self.packer = libdbc.canpack_init(dbc_name)
self.sig_names = {}
def pack(self, addr, values):
# values: [(signal_name, signal_value)]
values_thing = []
for name, value in values:
if name not in self.sig_names:
self.sig_names[name] = ffi.new("char[]", name)
values_thing.append({
'name': self.sig_names[name],
'value': value
})
values_c = ffi.new("SignalPackValue[]", values_thing)
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c)
def pack_bytes(self, addr, values):
return struct.pack(">Q", self.pack(addr, values))
if __name__ == "__main__":
cp = CANPacker("honda_civic_touring_2016_can")
s = cp.pack_bytes(0x30c, [
("PCM_SPEED", 123),
("PCM_GAS", 10),
])
print s.encode("hex")
selfdrive/can/parser.cc
+11 -21
View File
@@ -18,7 +18,7 @@
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "parser_common.h"
#include "common.h"
#define DEBUG(...)
// #define DEBUG printf
@@ -40,8 +40,6 @@ uint64_t read_u64_be(const uint8_t* v) {
| (uint64_t)v[7]);
}
std::vector<const DBC*> g_dbc;
bool honda_checksum(int address, uint64_t d, int l) {
int target = (d >> l) & 0xF;
@@ -77,9 +75,9 @@ struct MessageState {
for (int i=0; i < parse_sigs.size(); i++) {
auto& sig = parse_sigs[i];
int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1);
int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1);
if (sig.is_signed) {
tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
}
DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp);
@@ -123,6 +121,7 @@ struct MessageState {
};
class CANParser {
public:
CANParser(int abus, const std::string& dbc_name,
@@ -135,15 +134,10 @@ class CANParser {
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8006");
for (auto dbci : g_dbc) {
if (dbci->name == dbc_name) {
dbc = dbci;
break;
}
}
dbc = dbc_lookup(dbc_name);
assert(dbc);
for (auto &op : options) {
for (const auto& op : options) {
MessageState state = {
.address = op.address,
// .check_frequency = op.check_frequency,
@@ -178,7 +172,7 @@ class CANParser {
}
// track requested signals for this message
for (auto &sigop : sigoptions) {
for (const auto& sigop : sigoptions) {
if (sigop.address != op.address) continue;
for (int i=0; i<msg->num_sigs; i++) {
@@ -230,8 +224,8 @@ class CANParser {
void UpdateValid(uint64_t sec) {
can_valid = true;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) {
INFO("%X TIMEOUT\n", state.address);
@@ -279,8 +273,8 @@ class CANParser {
std::vector<SignalValue> query(uint64_t sec) {
std::vector<SignalValue> ret;
for (auto &kv : message_states) {
auto &state = kv.second;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (sec != 0 && state.seen != sec) continue;
for (int i=0; i<state.parse_sigs.size(); i++) {
@@ -311,10 +305,6 @@ class CANParser {
}
void dbc_register(const DBC* dbc) {
g_dbc.push_back(dbc);
}
extern "C" {
void* can_init(int bus, const char* dbc_name,
selfdrive/can/parser.py
+1 -39
View File
@@ -1,46 +1,8 @@
import os
import time
import subprocess
from collections import defaultdict
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
ffi = FFI()
ffi.cdef("""
typedef struct SignalParseOptions {
uint32_t address;
const char* name;
double default_value;
} SignalParseOptions;
typedef struct MessageParseOptions {
uint32_t address;
int check_frequency;
} MessageParseOptions;
typedef struct SignalValue {
uint32_t address;
uint16_t ts;
const char* name;
double value;
} SignalValue;
void* can_init(int bus, const char* dbc_name,
size_t num_message_options, const MessageParseOptions* message_options,
size_t num_signal_options, const SignalParseOptions* signal_options);
void can_update(void* can, uint64_t sec, bool wait);
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
""")
libdbc = ffi.dlopen(libdbc_fn)
from selfdrive.can.libdbc_py import libdbc, ffi
class CANParser(object):
def __init__(self, dbc_name, signals, checks=[], bus=0):
selfdrive/car/__init__.py
+6
View File
@@ -2,6 +2,11 @@ from common.fingerprints import fingerprint
from .honda.interface import CarInterface as HondaInterface
try:
from .toyota.interface import CarInterface as ToyotaInterface
except ImportError:
ToyotaInterface = None
try:
from .simulator.interface import CarInterface as SimInterface
except ImportError:
@@ -17,6 +22,7 @@ interfaces = {
"ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
"HONDA ACCORD 2016 TOURING": HondaInterface,
"HONDA CR-V 2016 TOURING": HondaInterface,
"TOYOTA PRIUS 2017": ToyotaInterface,
"simulator": SimInterface,
"simulator2": Sim2Interface
selfdrive/car/honda/carcontroller.py
+26 -60
View File
@@ -1,5 +1,5 @@
from collections import namedtuple
import os
import common.numpy_fast as np
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot
@@ -10,25 +10,25 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from . import hondacan
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
# hyst params... TODO: move these to VehicleParams
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
final_brake = 0.
braking = final_brake > 0.
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0.
braking = brake > 0.
# for small brake oscillations within brake_hyst_gap, don't change the brake command
if final_brake == 0.:
if brake == 0.:
brake_steady = 0.
elif final_brake > brake_steady + brake_hyst_gap:
brake_steady = final_brake - brake_hyst_gap
elif final_brake < brake_steady - brake_hyst_gap:
brake_steady = final_brake + brake_hyst_gap
final_brake = brake_steady
elif brake > brake_steady + brake_hyst_gap:
brake_steady = brake - brake_hyst_gap
elif brake < brake_steady - brake_hyst_gap:
brake_steady = brake + brake_hyst_gap
brake = brake_steady
if not civic:
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
@@ -36,10 +36,10 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
# offset the brake command for threshold in the brake system. no brake torque perceived below it
brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
brake_offset = brake_on_offset - brake_hyst_on
if final_brake > 0.0:
final_brake += brake_offset
if brake > 0.0:
brake += brake_offset
return final_brake, braking, brake_steady
return brake, braking, brake_steady
class AH:
#[alert_idx, value]
@@ -78,12 +78,9 @@ class CarController(object):
def __init__(self):
self.braking = False
self.brake_steady = 0.
self.final_brake_last = 0.
self.brake_last = 0.
# redundant safety check with the board
self.controls_allowed = False
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
@@ -94,20 +91,16 @@ class CarController(object):
return
# *** apply brake hysteresis ***
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
# *** no output if not enabled ***
if not enabled:
final_gas = 0.
final_brake = 0.
final_steer = 0.
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
if CS.pcm_acc_status:
pcm_cancel_cmd = True
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
self.final_brake_last = final_brake
brake = rate_limit(brake, self.brake_last, -2., 1./100)
self.brake_last = brake
# vehicle hud display, wait for one update from 10Hz 0x304 msg
#TODO: use enum!!
@@ -143,7 +136,8 @@ class CarController(object):
GAS_MAX = 1004
BRAKE_MAX = 1024/4
if CS.civic:
STEER_MAX = 0x1000
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value
else:
@@ -151,50 +145,22 @@ class CarController(object):
GAS_OFFSET = 328
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
apply_brake = int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
apply_steer = int(clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
# no gas if you are hitting the brake or the user is
if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
print "CANCELLING GAS", apply_brake
apply_gas = 0
# no computer brake if the gas is being pressed
if CS.car_gas > 0 and apply_brake != 0:
print "CANCELLING BRAKE"
apply_brake = 0
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
if CS.steer_not_allowed:
print "STEER ALERT, TORQUE INHIBITED"
apply_steer = 0
# *** entry into controls state ***
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
CS.cruise_buttons == 0 and not self.controls_allowed:
print "CONTROLS ARE LIVE"
self.controls_allowed = True
# *** exit from controls state on cancel, gas, or brake ***
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
print "CONTROLS ARE DEAD"
self.controls_allowed = False
# *** controls fail on steer error, brake error, or invalid can ***
if CS.steer_error:
print "STEER ERROR"
self.controls_allowed = False
if CS.brake_error:
print "BRAKE ERROR"
self.controls_allowed = False
if not CS.can_valid and self.controls_allowed: # 200 ms
print "CAN INVALID"
self.controls_allowed = False
# Send CAN commands.
can_sends = []
selfdrive/car/honda/carstate.py
+103 -40
View File
@@ -1,18 +1,54 @@
import os
import time
from cereal import car
import common.numpy_fast as np
from common.realtime import sec_since_boot
import selfdrive.messaging as messaging
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.can.parser import CANParser as CANParserC
NEW_CAN = os.getenv("OLD_CAN") is None
def parse_gear_shifter(can_gear_shifter, is_acura):
def get_can_parser(CP):
# this function generates lists for signal, messages and initial values
if can_gear_shifter == 0x1:
return "park"
elif can_gear_shifter == 0x2:
return "reverse"
if is_acura:
if can_gear_shifter == 0x3:
return "neutral"
elif can_gear_shifter == 0x4:
return "drive"
elif can_gear_shifter == 0xa:
return "sport"
else:
if can_gear_shifter == 0x4:
return "neutral"
elif can_gear_shifter == 0x8:
return "drive"
elif can_gear_shifter == 0x10:
return "sport"
elif can_gear_shifter == 0x20:
return "low"
return "unknown"
_K0 = -0.3
_K1 = -0.01879
_K2 = 0.01013
def calc_cruise_offset(offset, speed):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# constraints to solve for _K0, _K1, _K2 are:
# - speed = 0m/s, out = -0.3
# - speed = 34m/s, offset = 20, out = -0.25
# - speed = 34m/s, offset = -2.5, out = -1.8
return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
def get_can_signals(CP):
# this function generates lists for signal, messages and initial values
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
dbc_f = 'honda_civic_touring_2016_can.dbc'
signals = [
@@ -50,8 +86,9 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x296, 0),
("LEFT_BLINKER", 0x326, 0),
("RIGHT_BLINKER", 0x326, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0),
("EPB_STATE", 0x1c2, 0),
("BRAKE_HOLD_ACTIVE", 0x1A4, 0),
]
checks = [
# address, frequency
@@ -65,6 +102,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
@@ -104,8 +142,7 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
("CRUISE_SPEED_OFFSET", 0x37c, 0)
]
checks = [
(0x156, 100),
@@ -118,6 +155,7 @@ def get_can_parser(CP):
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x37c, 10),
(0x405, 3),
]
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
@@ -157,8 +195,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@@ -209,8 +245,6 @@ def get_can_parser(CP):
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
@@ -230,22 +264,23 @@ def get_can_parser(CP):
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
checks.append((0x201, 50))
if NEW_CAN:
return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
else:
return CANParser(dbc_f, signals, checks)
return dbc_f, signals, checks
def get_can_parser(CP):
dbc_f, signals, checks = get_can_signals(CP)
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
class CarState(object):
def __init__(self, CP, logcan):
self.acura = False
self.civic = False
self.accord = False
self.crv = False
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
self.civic = True
elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
self.civic = False
self.acura = True
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
# fake civic
self.accord = True
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
self.crv = True
@@ -274,10 +309,7 @@ class CarState(object):
def update(self, can_pub_main=None):
cp = self.cp
if NEW_CAN:
cp.update(int(sec_since_boot() * 1e9), False)
else:
cp.update_can(can_pub_main)
cp.update(int(sec_since_boot() * 1e9), False)
# copy can_valid
self.can_valid = cp.can_valid
@@ -294,8 +326,6 @@ class CarState(object):
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.rpm = cp.vl[0x17C]['ENGINE_RPM']
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
@@ -316,9 +346,11 @@ class CarState(object):
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel = (
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL']
self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR']
self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
@@ -328,49 +360,59 @@ class CarState(object):
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
#print self.user_gas, self.user_gas_pressed
if self.civic:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
self.gear = 0 # TODO: civic has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x326]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE']
elif self.accord:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = 0 # TODO: accord has CVT... needs rev engineering
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.crv:
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x191]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
else:
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.cruise_speed_offset = -0.3
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
elif self.acura:
can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
self.gear = cp.vl[0x1A3]['GEAR']
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
self.main_on = cp.vl[0x1A6]['MAIN_ON']
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.park_brake = 0 # TODO
self.brake_hold = 0
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura)
if self.accord:
# on the accord, this doesn't seem to include cruise control
@@ -398,4 +440,25 @@ class CarState(object):
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
self.counter_pcm = cp.vl[0x324]['COUNTER']
# carstate standalone tester
if __name__ == '__main__':
import zmq
import time
from selfdrive.services import service_list
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
class CarParams(object):
def __init__(self):
self.carFingerprint = "HONDA CIVIC 2016 TOURING"
self.enableGas = 0
self.enableCruise = 0
CP = CarParams()
CS = CarState(CP, logcan)
while 1:
CS.update()
time.sleep(0.01)
selfdrive/car/honda/interface.py
+171 -60
View File
@@ -1,19 +1,65 @@
#!/usr/bin/env python
import os
import time
import common.numpy_fast as np
import numpy as np
from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
from .carstate import CarState
from .carcontroller import CarController, AH
from selfdrive.boardd.boardd import can_capnp_to_can_list
from cereal import car
from selfdrive.services import service_list
import selfdrive.messaging as messaging
NEW_CAN = os.getenv("OLD_CAN") is None
def get_compute_gb():
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
# where -1.0 is max brake and 1.0 is max gas
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb(accel, speed):
#linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
dat = np.array([accel, speed])
if speed > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return float(m4)
return _compute_gb
# Car button codes
class CruiseButtons:
@@ -22,6 +68,7 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
class CM:
MUTE = 0
@@ -30,6 +77,7 @@ class CM:
REPEATED = 1
CONTINUOUS = 2
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
class BP:
MUTE = 0
@@ -37,12 +85,17 @@ class BP:
TRIPLE = 2
REPEATED = 1
class CarInterface(object):
def __init__(self, CP, logcan, sendcan=None):
self.logcan = logcan
self.CP = CP
self.frame = 0
self.last_enable_pressed = 0
self.last_enable_sent = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
# *** init the major players ***
@@ -54,7 +107,8 @@ class CarInterface(object):
self.CC = CarController()
if self.CS.accord:
self.accord_msg = []
# self.accord_msg = []
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint):
@@ -68,6 +122,8 @@ class CarInterface(object):
ret.radarName = "nidec"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.honda
ret.enableSteer = True
ret.enableBrake = True
@@ -75,7 +131,7 @@ class CarInterface(object):
ret.enableGas = 0x201 in fingerprint
ret.enableCruise = not ret.enableGas
# FIXME: hardcoding honda civic 2016 touring wight so it can be used to
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
@@ -86,27 +142,32 @@ class CarInterface(object):
cR_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING":
stop_and_go = True
ret.m = m_civic
ret.l = l_civic
ret.aF = aF_civic
ret.sR = 13.0
ret.steerKp, ret.steerKi = 0.8, 0.24
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
stop_and_go = False
ret.m = 3095./2.205 + std_cargo
ret.l = 2.67
ret.aF = ret.l * 0.37
ret.sR = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
# FIXME: using dongleId isn't great, better to identify the car than the Neo
is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
elif candidate == "HONDA ACCORD 2016 TOURING":
stop_and_go = False
ret.m = 3580./2.205 + std_cargo
ret.l = 2.74
ret.aF = ret.l * 0.38
ret.sR = 15.3
ret.steerKp, ret.steerKi = 0.8, 0.24
elif candidate == "HONDA CR-V 2016 TOURING":
stop_and_go = False
ret.m = 3572./2.205 + std_cargo
ret.l = 2.62
ret.aF = ret.l * 0.41
@@ -114,48 +175,54 @@ class CarInterface(object):
ret.steerKp, ret.steerKi = 0.8, 0.24
else:
raise ValueError("unsupported car %s" % candidate)
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS
ret.aR = ret.l - ret.aF
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position... all cars will have approximately similar dyn behaviors
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
# no max steer limit VS speed
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.] # max steer allowed
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
return ret
compute_gb = staticmethod(get_compute_gb())
# returns a car.CarState
def update(self):
def update(self, c):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
if NEW_CAN:
self.CS.update(can_pub_main)
else:
for a in messaging.drain_sock(self.logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80]))
if self.CS.accord:
self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
self.accord_msg = self.accord_msg[-1:]
self.CS.update(can_pub_main)
self.CS.update(can_pub_main)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
@@ -172,28 +239,17 @@ class CarInterface(object):
# TODO: units
ret.steeringAngle = self.CS.angle_steers
if self.CS.accord:
# TODO: move this into the CAN parser
ret.steeringTorque = 0
if len(self.accord_msg) > 0:
aa = map(lambda x: ord(x)&0x7f, self.accord_msg[0][2])
if len(aa) != 5 or (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f != aa[4]:
print "ACCORD MSG BAD LEN OR CHECKSUM!"
# TODO: throw an error here?
else:
st = ((aa[0]&0xF) << 5) + (aa[1]&0x1F)
if st >= 256:
st = -(512-st)
ret.steeringTorque = st
ret.steeringPressed = abs(ret.steeringTorque) > 20
else:
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
# TODO: button presses
buttonEvents = []
@@ -244,38 +300,94 @@ class CarInterface(object):
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# errors
# events
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
errors = []
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
errors.append('commIssue')
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
errors.append('steerUnavailable')
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
elif self.CS.steer_not_allowed:
errors.append('steerTempUnavailable')
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error:
errors.append('brakeUnavailable')
if not self.CS.gear_shifter_valid:
errors.append('wrongGear')
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed:
errors.append('doorOpen')
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.seatbelt:
errors.append('seatbeltNotLatched')
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
errors.append('espDisabled')
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
errors.append('wrongCarMode')
if self.CS.gear_shifter == 2:
errors.append('reverseGear')
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
ret.errors = errors
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
#if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not ret.cruiseState.enabled and \
(c.actuators.brake <= 0. or ret.vEgo < 0.3):
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
cur_time = sec_since_boot()
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
print "enabled pressed at", cur_time
self.last_enable_pressed = cur_time
enable_pressed = True
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
if self.CP.enableCruise:
# KEEP THIS EVENT LAST! send enable event if button is pressed and there are
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
# too close in time, so a no_entry will not be followed by another one.
# TODO: button press should be the only thing that triggers enble
if ((cur_time - self.last_enable_pressed) < 0.2 and
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
(enable_pressed and get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
#print ret
return ret.as_reader()
@@ -309,10 +421,10 @@ class CarInterface(object):
"chimeRepeated": (BP.MUTE, CM.REPEATED),
"chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
c.gas, c.brake, c.steeringTorque, \
c.actuators, \
c.cruiseControl.speedOverride, \
c.cruiseControl.override, \
c.cruiseControl.cancel, \
@@ -324,4 +436,3 @@ class CarInterface(object):
snd_chime = snd_chime)
self.frame += 1
return not (c.enabled and not self.CC.controls_allowed)
selfdrive/car/honda/can_parser.py → selfdrive/car/honda/old_can_parser.py
+5 -3
View File
@@ -36,15 +36,16 @@ class CANParser(object):
for _, addr, _ in signals:
self.vl[addr] = {}
self.ts[addr] = 0
self.ts[addr] = {}
self.ct[addr] = sec_since_boot()
self.ok[addr] = False
self.ok[addr] = True
self.cn[addr] = 0
self.cn_vl[addr] = 0
self.ck[addr] = False
for name, addr, ival in signals:
self.vl[addr][name] = ival
self.ts[addr][name] = 0
self._msgs = [s[1] for s in signals]
self._sgs = [s[0] for s in signals]
@@ -94,7 +95,6 @@ class CANParser(object):
self.ok[msg] = False
# update msg time stamps and counter value
self.ts[msg] = ts
self.ct[msg] = self.sec_since_boot_cached
self.cn[msg] = cn
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
@@ -107,12 +107,14 @@ class CANParser(object):
for ii in idxs:
sg = self._sgs[ii]
self.vl[msg][sg] = out[sg]
self.ts[msg][sg] = ts
# for each message, check if it's too long since last time we received it
self._check_dead_msgs()
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
self.can_valid = True
if False in self.ok.values():
#print "CAN INVALID!"
self.can_valid = False
selfdrive/common/cereal.mk
+16 -4
View File
@@ -1,14 +1,27 @@
UNAME_M ?= $(shell uname -m)
ifeq ($(UNAME_M),x86_64)
UNAME_S ?= $(shell uname -s)
CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
ifeq ($(UNAME_S),Darwin)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
$(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
$(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
else ifeq ($(UNAME_M),x86_64)
CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include
CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(ONE)/external/capnp/lib \
CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \
-l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
endif
else
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
@@ -30,4 +43,3 @@ car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \
-c -o '$@' '$<'
selfdrive/common/mat.h
+20
View File
@@ -65,4 +65,24 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
return ret;
}
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
1.0f/s, 0.0f, 0.5f,
0.0f, 1.0f/s, 0.5f,
0.0f, 0.0f, 1.0f,
}};
mat3 transform_in = {{
s, 0.0f, -0.5f*s,
0.0f, s, -0.5f*s,
0.0f, 0.0f, 1.0f,
}};
return matmul3(transform_in, matmul3(in, transform_out));
}
#endif
selfdrive/common/params.c → selfdrive/common/params.cc
+23 -2
View File
@@ -11,6 +11,18 @@
#include <stdlib.h>
#include <stdio.h>
namespace {
template <typename T>
T* null_coalesce(T* a, T* b) {
return a != NULL ? a : b;
}
static const char* default_params_path = null_coalesce(
const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
} // namespace
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size) {
int lock_fd = -1;
@@ -18,6 +30,11 @@ int write_db_value(const char* params_path, const char* key, const char* value,
int result;
char tmp_path[1024];
char path[1024];
ssize_t bytes_written;
if (params_path == NULL) {
params_path = default_params_path;
}
// Write value to temp.
result =
@@ -27,7 +44,7 @@ int write_db_value(const char* params_path, const char* key, const char* value,
}
tmp_fd = mkstemp(tmp_path);
const ssize_t bytes_written = write(tmp_fd, value, value_size);
bytes_written = write(tmp_fd, value, value_size);
if (bytes_written != value_size) {
result = -20;
goto cleanup;
@@ -79,6 +96,10 @@ int read_db_value(const char* params_path, const char* key, char** value,
int result;
char path[1024];
if (params_path == NULL) {
params_path = default_params_path;
}
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
if (result < 0) {
goto cleanup;
@@ -99,7 +120,7 @@ int read_db_value(const char* params_path, const char* key, char** value,
// Read value.
// TODO(mgraczyk): If there is a lot of contention, we can release the lock
// after opening the file, before reading.
*value = read_file(path, value_sz);
*value = static_cast<char*>(read_file(path, value_sz));
if (*value == NULL) {
result = -22;
goto cleanup;
selfdrive/common/params.h
+1 -3
View File
@@ -7,14 +7,12 @@
extern "C" {
#endif
#define PARAMS_PATH "/data/params"
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size);
// Reads a value from the params database.
// Inputs:
// params_path: The path of the database, eg /sdcard/params.
// params_path: The path of the database, or NULL to use the default.
// key: The key to read.
// value: A pointer where a newly allocated string containing the db value will
// be written.
selfdrive/common/swaglog.h
+36
View File
@@ -1,6 +1,8 @@
#ifndef SWAGLOG_H
#define SWAGLOG_H
#include "common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
@@ -24,9 +26,43 @@ void cloudlog_bind(const char* k, const char* v);
__func__, \
fmt, ## __VA_ARGS__)
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
static uint64_t __begin = 0; \
static int __printed = 0; \
static int __missed = 0; \
\
int __burst = (burst); \
int __millis = (millis); \
uint64_t __ts = nanos_since_boot(); \
\
if (!__begin) __begin = __ts; \
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
__missed = 0; \
} \
\
if (__printed < __burst) { \
cloudlog(lvl, fmt, ## __VA_ARGS__); \
__printed++; \
} else { \
__missed++; \
} \
}
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif
selfdrive/common/timing.h
+14 -1
View File
@@ -5,7 +5,7 @@
#include <time.h>
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_REALTIME
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
static inline uint64_t nanos_since_boot() {
@@ -38,4 +38,17 @@ static inline double seconds_since_epoch() {
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
// you probably should use nanos_since_boot instead
static inline uint64_t nanos_monotonic() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline uint64_t nanos_monotonic_raw() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
#endif
selfdrive/common/util.c
+11
View File
@@ -3,6 +3,10 @@
#include <string.h>
#include <assert.h>
#ifdef __linux__
#include <sys/prctl.h>
#endif
void* read_file(const char* path, size_t* out_len) {
FILE* f = fopen(path, "r");
if (!f) {
@@ -28,3 +32,10 @@ void* read_file(const char* path, size_t* out_len) {
return buf;
}
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
#endif
}
selfdrive/common/util.h
+14
View File
@@ -1,6 +1,9 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#ifndef __cplusplus
#define min(a,b) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@@ -11,6 +14,8 @@
__typeof__ (b) _b = (b); \
_a > _b ? _a : _b; })
#endif
#define clamp(a,b,c) \
({ __typeof__ (a) _a = (a); \
__typeof__ (b) _b = (b); \
@@ -19,11 +24,20 @@
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
#ifdef __cplusplus
extern "C" {
#endif
// Reads a file into a newly allocated buffer.
//
// Returns NULL on failure, otherwise the NULL-terminated file contents.
// The result must be freed by the caller.
void* read_file(const char* path, size_t* out_len);
void set_thread_name(const char* name);
#ifdef __cplusplus
}
#endif
#endif
selfdrive/common/utilpp.h
+4
View File
@@ -9,6 +9,10 @@
#include <sstream>
#include <fstream>
#ifdef __x86_64
#include <linux/limits.h>
#endif
namespace util {
inline bool starts_with(std::string s, std::string prefix) {
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define OPENPILOT_VERSION "0.3.6.1"
#define OPENPILOT_VERSION "0.3.7"
selfdrive/common/visionipc.c
+5 -4
View File
@@ -1,6 +1,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <errno.h>
@@ -126,7 +127,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
}
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds) {
for (int i=0; i<num_fds; i++) {
if (bufs[i].addr) {
@@ -180,10 +181,10 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
s->bufs_info = rp.d.stream_bufs;
s->num_bufs = rp.num_fds;
s->bufs = calloc(s->num_bufs, sizeof(VisionBuf));
s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
assert(s->bufs);
visionbufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
if (out_bufs_info) {
*out_bufs_info = s->bufs_info;
@@ -192,7 +193,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi
return 0;
}
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra) {
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
int err;
VisionPacket rp;
selfdrive/common/visionipc.h
+8 -8
View File
@@ -47,9 +47,9 @@ typedef struct VisionStreamBufs {
} buf_info;
} VisionStreamBufs;
typedef struct VisionBufExtra {
typedef struct VIPCBufExtra {
uint32_t frame_id; // only for yuv
} VisionBufExtra;
} VIPCBufExtra;
typedef union VisionPacketData {
struct {
@@ -60,7 +60,7 @@ typedef union VisionPacketData {
struct {
VisionStreamType type;
int idx;
VisionBufExtra extra;
VIPCBufExtra extra;
} stream_acq;
struct {
VisionStreamType type;
@@ -79,12 +79,12 @@ int vipc_connect(void);
int vipc_recv(int fd, VisionPacket *out_p);
int vipc_send(int fd, const VisionPacket *p);
typedef struct VisionBuf {
typedef struct VIPCBuf {
int fd;
size_t len;
void* addr;
} VisionBuf;
void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs,
} VIPCBuf;
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
int num_fds, const int* fds);
@@ -94,11 +94,11 @@ typedef struct VisionStream {
int last_idx;
int num_bufs;
VisionStreamBufs bufs_info;
VisionBuf *bufs;
VIPCBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
#ifdef __cplusplus
selfdrive/controls/controlsd.py
+413 -369
View File
@@ -2,24 +2,24 @@
import os
import json
from copy import copy
import zmq
from cereal import car, log
from common.numpy_fast import clip
from common.fingerprints import fingerprint
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
from common.params import Params
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car import get_car
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
get_events, \
create_event, \
EventTypes as ET
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -34,427 +34,472 @@ AWARENESS_TIME = 360. # 6 minutes limit without user touching steering whee
AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
# class Cal
class Calibration:
UNCALIBRATED = 0
CALIBRATED = 1
INVALID = 2
# to be used
class State():
class State:
DISABLED = 0
ENABLED = 1
SOFT_DISABLE = 2
PRE_ENABLED = 2
SOFT_DISABLING = 3
class Controls(object):
def __init__(self, gctx, rate=100):
self.rate = rate
# *** log ***
context = zmq.Context()
# True when actuators are controlled
def isActive(state):
return state in [State.ENABLED, State.SOFT_DISABLING]
# pub
self.live100 = messaging.pub_sock(context, service_list['live100'].port)
self.carstate = messaging.pub_sock(context, service_list['carState'].port)
self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# sub
self.thermal = messaging.sub_sock(context, service_list['thermal'].port)
self.health = messaging.sub_sock(context, service_list['health'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
self.CC = car.CarControl.new_message()
self.CI, self.CP = get_car(logcan, sendcan)
self.PL = Planner(self.CP)
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
self.VM = VehicleModel(self.CP)
# write CarParams
params = Params()
params.put("CarParams", self.CP.to_bytes())
# fake plan
self.plan_ts = 0
self.plan = log.Plan.new_message()
self.plan.lateralValid = False
self.plan.longitudinalValid = False
# controls enabled state
self.enabled = False
self.last_enable_request = 0
# learned angle offset
self.angle_offset = 0
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = (params.get("IsRearViewMirror") == "1")
self.v_cruise_kph = 255
# 0.0 - 1.0
self.awareness_status = 1.0
self.soft_disable_timer = None
self.overtemp = False
self.free_space = 1.0
self.cal_status = Calibration.UNCALIBRATED
self.cal_perc = 0
self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000)
def data_sample(self):
self.prof = Profiler()
self.cur_time = sec_since_boot()
# first read can and compute car states
self.CS = self.CI.update()
self.prof.checkpoint("CarInterface")
# True if system is engaged
def isEnabled(state):
return (isActive(state) or state == State.PRE_ENABLED)
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(self.thermal)
if td is not None:
# Check temperature.
self.overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
# under 15% of space free
self.free_space = td.thermal.freeSpace
# read calibration status
cal = messaging.recv_sock(self.cal)
if cal is not None:
self.cal_status = cal.liveCalibration.calStatus
self.cal_perc = cal.liveCalibration.calPerc
def data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc):
def state_transition(self):
pass # for now
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
def state_control(self):
# did it request to enable?
enable_request, enable_condition = False, False
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None:
# Check temperature
overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# reset awareness status on steering
if self.CS.steeringPressed or not self.enabled:
self.awareness_status = 1.0
elif self.enabled:
# gives the user 6 minutes
self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME)
if self.awareness_status <= 0.:
self.AM.add("driverDistracted", self.enabled)
elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
self.AM.add("preDriverDistracted", self.enabled)
# under 15% of space free
if td.thermal.freeSpace < 0.15:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# handle button presses
for b in self.CS.buttonEvents:
print b
# *** read calibration status ***
cal = messaging.recv_sock(cal)
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and self.rear_view_allowed:
self.rear_view_toggle = True
else:
self.rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
self.rear_view_toggle = not self.rear_view_toggle
if not self.CP.enableCruise and self.enabled and not b.pressed:
if b.type == "accelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
enable_request = True
# do disable on button down
if b.type == "cancel" and b.pressed:
self.AM.add("disable", self.enabled)
self.prof.checkpoint("Buttons")
# *** health checking logic ***
hh = messaging.recv_sock(self.health)
if hh is not None:
# if the board isn't allowing controls but somehow we are enabled!
# TODO: this should be in state transition with a function follower logic
if not hh.health.controlsAllowed and self.enabled:
self.AM.add("controlsMismatch", self.enabled)
# disable if the pedals are pressed while engaged, this is a user disable
if self.enabled:
if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
self.AM.add("disable", self.enabled)
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not self.CS.cruiseState.enabled and \
(self.CC.brake <= 0. or self.CS.vEgo < 0.3):
self.AM.add("cruiseDisabled", self.enabled)
if enable_request:
# check for pressed pedals
if self.CS.gasPressed or self.CS.brakePressed:
self.AM.add("pedalPressed", self.enabled)
enable_request = False
else:
print "enabled pressed at", self.cur_time
self.last_enable_request = self.cur_time
# don't engage with less than 15% free
if self.free_space < 0.15:
self.AM.add("outOfSpace", self.enabled)
enable_request = False
if self.CP.enableCruise:
enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
enable_condition = enable_request
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CP.enableCruise and self.CS.cruiseState.enabled:
self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
# *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed:
events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
self.prof.checkpoint("AdaptiveCruise")
return CS, events, cal_status, cal_perc
# *** what's the plan ***
plan_packet = self.PL.update(self.CS, self.LoC)
self.plan = plan_packet.plan
self.plan_ts = plan_packet.logMonoTime
# if user is not responsive to awareness alerts, then start a smooth deceleration
if self.awareness_status < -0.:
self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL)
self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax)
def calc_plan(CS, events, PL, LoC):
# plan runs always, independently of the state
plan_packet = PL.update(CS, LoC)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
if enable_request or enable_condition or self.enabled:
# add all alerts from car
for alert in self.CS.errors:
self.AM.add(alert, self.enabled)
# add events from planner
events += list(plan.events)
if not self.plan.longitudinalValid:
self.AM.add("radarCommIssue", self.enabled)
# disable if lead isn't close when system is active and brake is pressed to avoid
# unexpected vehicle accelerations
if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.cal_status != Calibration.CALIBRATED:
if self.cal_status == Calibration.UNCALIBRATED:
self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%')
else:
self.AM.add("calibrationInvalid", self.enabled)
return plan, plan_ts
if not self.plan.lateralValid:
# If the model is not broadcasting, assume that it is because
# the user has uploaded insufficient data for calibration.
# Other cases that would trigger this are rare and unactionable by the user.
self.AM.add("dataNeeded", self.enabled)
if self.overtemp:
self.AM.add("overheat", self.enabled)
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM):
# compute conditional state transitions and execute actions on state transitions
enabled = isEnabled(state)
# *** angle offset learning ***
if self.rk.frame % 5 == 2 and self.plan.lateralValid:
# *** run this at 20hz again ***
self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset,
self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des,
self.CS.steeringPressed)
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
# *** gas/brake PID loop ***
final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph,
self.plan.vTarget,
[self.plan.aTargetMin, self.plan.aTargetMax],
self.plan.jerkFactor, self.CP)
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# *** steering PID loop ***
final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle,
self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM)
self.prof.checkpoint("PID")
# ***** handle alerts ****
# send FCW alert if triggered by planner
if self.plan.fcw:
self.AM.add("fcw", self.enabled)
# ***** handle state transitions *****
# send a "steering required alert" if saturation count has reached the limit
if sat_flag:
self.AM.add("steerSaturated", self.enabled)
if self.enabled and self.AM.alertShouldDisable():
print "DISABLING IMMEDIATELY ON ALERT"
self.enabled = False
if self.enabled and self.AM.alertShouldSoftDisable():
if self.soft_disable_timer is None:
self.soft_disable_timer = 3 * self.rate
elif self.soft_disable_timer == 0:
print "SOFT DISABLING ON ALERT"
self.enabled = False
# DISABLED
if state == State.DISABLED:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
for e in get_events(events, [ET.NO_ENTRY]):
txt = str(cal_perc) + '%' if e == 'calibrationInProgress' else ''
AM.add(str(e) + "NoEntry", enabled, txt)
else:
self.soft_disable_timer -= 1
else:
self.soft_disable_timer = None
if get_events(events, [ET.PRE_ENABLE]):
state = State.PRE_ENABLED
else:
state = State.ENABLED
AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
if enable_condition and not self.enabled and not self.AM.alertPresent():
print "*** enabling controls"
# ENABLED
elif state == State.ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# beep for enabling
self.AM.add("enable", self.enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# enable both lateral and longitudinal controls
self.enabled = True
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.SOFT_DISABLING
soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# SOFT DISABLING
elif state == State.SOFT_DISABLING:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
# 6 minutes driver you're on
self.awareness_status = 1.0
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
# reset the PID loops
self.LaC.reset()
# start long control at actual speed
self.LoC.reset(v_pid = self.CS.vEgo)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.ENABLED
# *** push the alerts to current ***
# TODO: remove output, store them inside AM class instead
self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time)
# ***** control the car *****
self.CC.enabled = self.enabled
elif soft_disable_timer <= 0:
state = State.DISABLED
# TODO: PRE ENABLING
elif state == State.PRE_ENABLED:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
AM.add("disable", enabled)
self.CC.gas = float(final_gas)
self.CC.brake = float(final_brake)
self.CC.steeringTorque = float(final_steer)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.DISABLED
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
self.CC.cruiseControl.override = True
# always cancel if we have an interceptor
self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or
(not self.enabled and self.CS.cruiseState.enabled))
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.ENABLED
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0))
self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)
return state, soft_disable_timer, v_cruise_kph
#CC.cruiseControl.accelOverride = float(AC.a_pcm)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX))
self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
self.CC.hudControl.speedVisible = self.enabled
self.CC.hudControl.lanesVisible = self.enabled
self.CC.hudControl.leadVisible = self.plan.hasLead
self.CC.hudControl.visualAlert = self.visual_alert
self.CC.hudControl.audibleAlert = self.audible_alert
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# TODO: remove it from here and put it in state_transition
# this alert will apply next controls cycle
if not self.CI.apply(self.CC):
self.AM.add("controlsFailed", self.enabled)
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
def data_send(self):
# broadcast carControl first
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(self.CC)
self.carcontrol.send(cc_send.to_bytes())
enabled = isEnabled(state)
active = isActive(state)
self.prof.checkpoint("CarControl")
for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = copy(self.CS)
self.carstate.send(cs_send.to_bytes())
# ***** publish state to logger *****
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
rear_view_toggle = True
else:
rear_view_toggle = False
# publish controls state at 100Hz
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]:
awareness_status = 1.
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on
awareness_status -= 0.01/(AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
AM.add(e, enabled)
# if user is not responsive to awareness alerts, then start a smooth deceleration
if awareness_status < -0.:
plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed,
v_cruise_kph, plan.vTarget,
[plan.aTargetMin, plan.aTargetMax],
plan.jerkFactor, CP)
# *** steering PID loop ***
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag:
AM.add("steerSaturated", enabled)
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset):
# ***** control the car *****
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
# always cancel if we have an interceptor
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
dat.live100.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2
dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(CS.canMonoTimes)
dat.live100.planMonoTime = plan_ts
# if controls is enabled
dat.live100.enabled = isEnabled(state)
# car state
dat.live100.vEgo = CS.vEgo
dat.live100.angleSteers = CS.steeringAngle
dat.live100.steerOverride = CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(LoC.v_pid)
dat.live100.vCruise = float(v_cruise_kph)
dat.live100.upAccelCmd = float(LoC.pid.p)
dat.live100.uiAccelCmd = float(LoC.pid.i)
# lateral control state
dat.live100.yDes = float(LaC.y_des)
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
dat.live100.upSteer = float(LaC.pid.p)
dat.live100.uiSteer = float(LaC.pid.i)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(plan.vTarget)
dat.live100.aTargetMin = float(plan.aTargetMin)
dat.live100.aTargetMax = float(plan.aTargetMax)
dat.live100.jerkFactor = float(plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(angle_offset)
# lag
dat.live100.cumLagMs = -rk.remaining*1000.
live100.send(dat.to_bytes())
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
# TODO: override CS.events with all the cumulated events
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
#print [i.name for i in events]
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
dat = messaging.new_message()
dat.init('live100')
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
livempc.send(dat.to_bytes())
# show rear view camera on phone if in reverse gear or when button is pressed
dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle
dat.live100.alertText1 = self.alert_text_1
dat.live100.alertText2 = self.alert_text_2
dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0
# what packets were used to process
dat.live100.canMonoTimes = list(self.CS.canMonoTimes)
dat.live100.planMonoTime = self.plan_ts
# if controls is enabled
dat.live100.enabled = self.enabled
# car state
dat.live100.vEgo = self.CS.vEgo
dat.live100.angleSteers = self.CS.steeringAngle
dat.live100.steerOverride = self.CS.steeringPressed
# longitudinal control state
dat.live100.vPid = float(self.LoC.v_pid)
dat.live100.vCruise = float(self.v_cruise_kph)
dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd)
dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yDes = float(self.LaC.y_des)
dat.live100.angleSteersDes = float(self.LaC.angle_steers_des)
dat.live100.upSteer = float(self.LaC.Up_steer)
dat.live100.uiSteer = float(self.LaC.Ui_steer)
# processed radar state, should add a_pcm?
dat.live100.vTargetLead = float(self.plan.vTarget)
dat.live100.aTargetMin = float(self.plan.aTargetMin)
dat.live100.aTargetMax = float(self.plan.aTargetMax)
dat.live100.jerkFactor = float(self.plan.jerkFactor)
# log learned angle offset
dat.live100.angleOffset = float(self.angle_offset)
# lag
dat.live100.cumLagMs = -self.rk.remaining*1000.
self.live100.send(dat.to_bytes())
self.prof.checkpoint("Live100")
def wait(self):
# *** run loop at fixed rate ***
if self.rk.keep_time():
self.prof.display()
return CC
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
CTRLS = Controls(gctx, rate)
context = zmq.Context()
# pub
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan)
PL = Planner(CP)
LoC = LongControl(CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
# write CarParams
params = Params()
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
soft_disable_timer = 0
v_cruise_kph = 255
cal_perc = 0
cal_status = Calibration.UNCALIBRATED
rear_view_toggle = False
rear_view_allowed = params.get("IsRearViewMirror") == "1"
# 0.0 - 1.0
awareness_status = 0.
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
# learned angle offset
angle_offset = 0.
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
prof = Profiler()
while 1:
CTRLS.data_sample()
CTRLS.state_transition()
CTRLS.state_control()
CTRLS.data_send()
CTRLS.wait()
prof.reset() # avoid memory leak
# sample data and compute car events
CS, events, cal_status, cal_perc = data_sample(CI, CC, thermal, health, cal, cal_status, cal_perc)
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC)
prof.checkpoint("Plan")
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, cal_perc, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
def main(gctx=None):
@@ -462,4 +507,3 @@ def main(gctx=None):
if __name__ == "__main__":
main()
selfdrive/controls/lib/adaptivecruise.py
+2 -10
View File
@@ -109,7 +109,7 @@ def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
# maximum acceleration adjustment
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
# speeds
_A_CORR_BY_SPEED_BP = [0., 5., 20.]
_A_CORR_BY_SPEED_BP = [0., 2., 10.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
@@ -283,20 +283,12 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
class AdaptiveCruise(object):
def __init__(self):
self.last_cal = 0.
self.l1, self.l2 = None, None
self.dead = True
def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20):
def update(self, v_ego, angle_steers, v_pid, CP, l20):
if l20 is not None:
self.l1 = l20.live20.leadOne
self.l2 = l20.live20.leadTwo
# TODO: no longer has anything to do with calibration
self.last_cal = cur_time
self.dead = False
elif cur_time - self.last_cal > 0.5:
self.dead = True
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
selfdrive/controls/lib/alertmanager.py
+319 -73
View File
@@ -2,22 +2,31 @@ from cereal import car
from selfdrive.swaglog import cloudlog
import copy
class ET:
ENABLE = 0
NO_ENTRY = 1
WARNING = 2
USER_DISABLE = 3
SOFT_DISABLE = 4
IMMEDIATE_DISABLE = 5
class alert(object):
def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text):
# Priority
class PT:
HIGH = 3
MID = 2
LOW = 1
class Alert(object):
def __init__(self,
alert_text_1,
alert_text_2,
alert_priority,
visual_alert,
audible_alert,
duration_sound,
duration_hud_alert,
duration_text):
self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2
self.alert_type = alert_type
self.alert_priority = alert_priority
self.visual_alert = visual_alert if visual_alert is not None else "none"
self.audible_alert = audible_alert if audible_alert is not None else "none"
self.duration_sound = duration_sound
self.duration_hud_alert = duration_hud_alert
self.duration_text = duration_text
@@ -28,45 +37,297 @@ class alert(object):
tst.hudControl.audibleAlert = self.audible_alert
def __str__(self):
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + " " + str(self.visual_alert) + " " + str(self.audible_alert)
return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str(
self.visual_alert) + " " + str(self.audible_alert)
def __gt__(self, alert2):
return self.alert_type > alert2.alert_type
return self.alert_priority > alert2.alert_priority
class AlertManager(object):
alerts = {
"enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.),
"disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.),
"pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5),
"steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.),
"overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1),
# car errors
"commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.),
"ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.),
# Miscellaneous alerts
"enable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"disable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert(
"",
"",
PT.LOW, None, None, .1, .1, .1),
"steerSaturated": Alert(
"Take Control",
"Turn Exceeds Limit",
PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
"steerTempUnavailable": Alert(
"Take Control",
"Steer Temporarily Unavailable",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": Alert(
"Take Control ",
"User Distracted",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": Alert(
"Take Control to Regain Speed",
"User Distracted",
PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
"startup": Alert(
"Always Keep Hands on Wheel",
"Be Ready to Take Over Any Time",
PT.LOW, None, None, 0., 0., 15.),
"ethicalDilemma": Alert(
"Take Control Immediately",
"Ethical Dilemma Detected",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Temporary Unavailable",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
# Non-entry only alerts
"wrongCarModeNoEntry": Alert(
"Comma Unavailable",
"Main Switch Off",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"dataNeededNoEntry": Alert(
"Comma Unavailable",
"Data needed for calibration. Upload drive, try again",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"outOfSpaceNoEntry": Alert(
"Comma Unavailable",
"Out of Space",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
"pedalPressedNoEntry": Alert(
"Comma Unavailable",
"Pedal Pressed",
PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
"speedTooLowNoEntry": Alert(
"Comma Unavailable",
"Speed Too Low",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeHoldNoEntry": Alert(
"Comma Unavailable",
"Brake Hold Active",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"parkBrakeNoEntry": Alert(
"Comma Unavailable",
"Park Brake Engaged",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing disabling
"overheat": Alert(
"Take Control Immediately",
"System Overheated",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": Alert(
"Take Control Immediately",
"Gear not D",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": Alert(
"Take Control Immediately",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": Alert(
"Take Control Immediately",
"Calibration in Progress",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": Alert(
"Take Control Immediately",
"Door Open",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": Alert(
"Take Control Immediately",
"Seatbelt Unlatched",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": Alert(
"Take Control Immediately",
"ESP Off",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarFault": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"modelCommIssue": Alert(
"Take Control Immediately",
"Model Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsFailed": Alert(
"Take Control Immediately",
"Controls Failed",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": Alert(
"Take Control Immediately",
"Controls Mismatch",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"commIssue": Alert(
"Take Control Immediately",
"CAN Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerUnavailable": Alert(
"Take Control Immediately",
"Steer Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"brakeUnavailable": Alert(
"Take Control Immediately",
"Brake Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": Alert(
"Take Control Immediately",
"Gas Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"reverseGear": Alert(
"Take Control Immediately",
"Reverse Gear",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"cruiseDisabled": Alert(
"Take Control Immediately",
"Cruise Is Off",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
# not loud cancellations (user is in control)
"noTarget": Alert(
"Comma Canceled",
"No Close Lead",
PT.HIGH, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing non-entry
"overheatNoEntry": Alert(
"Comma Unavailable",
"System Overheated",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"wrongGearNoEntry": Alert(
"Comma Unavailable",
"Gear not D",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInvalidNoEntry": Alert(
"Comma Unavailable",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInProgressNoEntry": Alert(
"Comma Unavailable",
"Calibration in Progress: ",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"doorOpenNoEntry": Alert(
"Comma Unavailable",
"Door Open",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"seatbeltNotLatchedNoEntry": Alert(
"Comma Unavailable",
"Seatbelt Unlatched",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"espDisabledNoEntry": Alert(
"Comma Unavailable",
"ESP Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarCommIssueNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"radarFaultNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"modelCommIssueNoEntry": Alert(
"Comma Unavailable",
"Model Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsFailedNoEntry": Alert(
"Comma Unavailable",
"Controls Failed",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsMismatchNoEntry": Alert(
"Comma Unavailable",
"Controls Mismatch",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"commIssueNoEntry": Alert(
"Comma Unavailable",
"CAN Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"steerUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeUnavailableNoEntry": Alert(
"Comma Unavailable",
"Brake Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"gasUnavailableNoEntry": Alert(
"Comma Unavailable",
"Gas Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"reverseGearNoEntry": Alert(
"Comma Unavailable",
"Reverse Gear",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"cruiseDisabledNoEntry": Alert(
"Comma Unavailable",
"Cruise is Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"noTargetNoEntry": Alert(
"Comma Unavailable",
"No Close Lead",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
}
def __init__(self):
self.activealerts = []
self.current_alert = None
@@ -75,31 +336,16 @@ class AlertManager(object):
def alertPresent(self):
return len(self.activealerts) > 0
def alertShouldSoftDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts)
def alertShouldDisable(self):
return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts)
def add(self, alert_type, enabled=True, extra_text=''):
alert_type = str(alert_type)
this_alert = copy.copy(self.alerts[alert_type])
this_alert.alert_text_2 += extra_text
# downgrade the alert if we aren't enabled, except if it's FCW, which remains the same
# TODO: remove this 'if' by adding more alerts
if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \
and this_alert != self.alerts['fcw']:
this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.)
# ignore no entries if we are enabled
if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]:
return
# if new alert is higher priority, log it
if self.current_alert is None or this_alert > self.current_alert:
cloudlog.event('alert_add',
alert_type=alert_type,
enabled=enabled)
alert_type=alert_type,
enabled=enabled)
self.activealerts.append(this_alert)
self.activealerts.sort()
@@ -109,29 +355,29 @@ class AlertManager(object):
self.alert_start_time = cur_time
self.current_alert = self.activealerts[0]
print self.current_alert
alert_text_1 = ""
alert_text_2 = ""
visual_alert = "none"
audible_alert = "none"
# start with assuming no alerts
self.alert_text_1 = ""
self.alert_text_2 = ""
self.visual_alert = "none"
self.audible_alert = "none"
if self.current_alert is not None:
# ewwwww
if self.alert_start_time + self.current_alert.duration_sound > cur_time:
audible_alert = self.current_alert.audible_alert
self.audible_alert = self.current_alert.audible_alert
if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
visual_alert = self.current_alert.visual_alert
self.visual_alert = self.current_alert.visual_alert
if self.alert_start_time + self.current_alert.duration_text > cur_time:
alert_text_1 = self.current_alert.alert_text_1
alert_text_2 = self.current_alert.alert_text_2
self.alert_text_1 = self.current_alert.alert_text_1
self.alert_text_2 = self.current_alert.alert_text_2
# disable current alert
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.current_alert.duration_text) < cur_time:
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
self.current_alert.duration_text) < cur_time:
self.current_alert = None
# reset
self.activealerts = []
return alert_text_1, alert_text_2, visual_alert, audible_alert
selfdrive/controls/lib/drive_helpers.py
+30
View File
@@ -1,8 +1,38 @@
from common.numpy_fast import clip
from cereal import car
class EventTypes:
ENABLE = 'enable'
PRE_ENABLE = 'preEnable'
NO_ENTRY = 'noEntry'
WARNING = 'warning'
USER_DISABLE = 'userDisable'
SOFT_DISABLE = 'softDisable'
IMMEDIATE_DISABLE = 'immediateDisable'
def create_event(name, types):
event = car.CarEvent.new_message()
event.name = name
for t in types:
setattr(event, t, True)
return event
def get_events(events, types):
out = []
for e in events:
for t in types:
if getattr(e, t):
out.append(e.name)
return out
def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
selfdrive/controls/lib/latcontrol.py
+60 -113
View File
@@ -1,133 +1,80 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import clip, interp
from selfdrive.config import Conversions as CV
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
def calc_d_lookahead(v_ego, d_poly):
#*** this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly)>0:
pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts))/pts_len
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.1
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
states[0].x = v_ego * ACTUATORS_DELAY
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY
return states
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
#*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset
return steer_des, curvature
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max,
steer_override, sat_count, enabled, Kp, Ki, rate):
sat_count_rate = 1./rate
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
error_steer = sa_des - sa_actual
Ui_unwind_speed = 0.3/rate #.3 per second
Up_steer = error_steer*Kp
Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
output_steer_new = Ui_steer_new + Up_steer
# Anti-wind up for integrator: do not integrate if we are against the steer limits
if (
(error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
(error_steer <= 0. and
(output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
#update integrator
Ui_steer = Ui_steer_new
# unwind integrator if driver is maneuvering the steering wheel
elif steer_override:
Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
# still, intergral term should not be bigger then limits
Ui_steer = clip(Ui_steer, -steer_max, steer_max)
output_steer = Up_steer + Ui_steer
# don't run steer control if at very low speed
if v_ego < 0.3 or not enabled:
output_steer = 0.
Ui_steer = 0.
# useful to know if control is against the limit
lateral_control_sat = False
if abs(output_steer) > steer_max:
lateral_control_sat = True
output_steer = clip(output_steer, -steer_max, steer_max)
# if lateral control is saturated for a certain period of time, send an alert for taking control of the car
# wind
if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
sat_count += sat_count_rate
# unwind
else:
sat_count -= sat_count_rate
sat_flag = False
if sat_count >= sat_count_limit:
sat_flag = True
sat_count = clip(sat_count, 0, 1)
return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
class LatControl(object):
def __init__(self):
self.Up_steer = 0.
self.sat_count = 0
self.y_des = 0.0
self.lateral_control_sat = False
self.Ui_steer = 0.
self.reset()
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0)
self.setup_mpc()
self.y_des = -1 # Legacy
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init()
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0
def reset(self):
self.Ui_steer = 0.
self.pid.reset()
def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
rate = 100
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
self.mpc_updated = False
if self.last_mpc_ts + 0.001 < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
steer_max = 1.0
curvature_factor = VM.curvature_factor(v_ego)
# how far we look ahead is function of speed and desired path
d_lookahead = calc_d_lookahead(v_ego, d_poly)
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# desired lookahead offset
self.y_des = np.polyval(d_poly, d_lookahead)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR)
# calculate actual offset at the lookahead point
self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
d_lookahead, VM, angle_offset)
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width)
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
delta_desired = self.mpc_solution[0].delta[1]
self.cur_state[0].delta = delta_desired
final_steer = clip(output_steer, -steer_max, steer_max)
return final_steer, sat_flag
self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset
self.mpc_updated = True
if v_ego < 0.3 or not active:
output_steer = 0.0
self.pid.reset()
else:
steer_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steer_max
self.pid.neg_limit = -steer_max
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override)
self.sat_flag = self.pid.saturated
return output_steer
selfdrive/controls/lib/latcontrol_helpers.py
+89
View File
@@ -0,0 +1,89 @@
import numpy as np
import math
from common.numpy_fast import interp
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_d_lookahead(v_ego, d_poly):
# this function computes how far too look for lateral control
# howfar we look ahead is function of speed and how much curvy is the path
offset_lookahead = 1.
k_lookahead = 7.
# integrate abs value of second derivative of poly to get a measure of path curvature
pts_len = 50. # m
if len(d_poly) > 0:
pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len))
else:
pts = 0.
curv = np.sum(np.abs(pts)) / pts_len
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
# this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = math.radians(angle_steers - angle_offset)
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.)
return y_actual, curvature
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset
return steer_des, curvature
def compute_path_pinv():
deg = 3
x = np.arange(50.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, map(float, points))
def calc_desired_path(l_poly,
r_poly,
p_poly,
l_prob,
r_prob,
p_prob,
speed,
lane_width=None):
# this function computes the poly for the center of the lane, averaging left and right polys
if lane_width is None:
lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
# lanes in US are ~3.6m wide
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
if l_prob + r_prob > 0.01:
c_poly = ((l_poly - half_lane_poly) * l_prob +
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
c_prob = l_prob + r_prob - l_prob * r_prob
else:
c_poly = np.zeros(4)
c_prob = 0.
p_weight = 1. # predicted path weight relatively to the center of the lane
d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
return d_poly, c_poly, c_prob
selfdrive/controls/lib/lateral_mpc/Makefile
+75
View File
@@ -0,0 +1,75 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../../phonelibs
UNAME_M := $(shell uname -m)
CFLAGS = -O3 -fPIC -I.
CXXFLAGS = -O3 -fPIC -I.
QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
ifeq ($(UNAME_M),aarch64)
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
else
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
endif
OBJS = \
$(PHONELIBS)/qpoases/SRC/Bounds.o \
$(PHONELIBS)/qpoases/SRC/Constraints.o \
$(PHONELIBS)/qpoases/SRC/CyclingManager.o \
$(PHONELIBS)/qpoases/SRC/Indexlist.o \
$(PHONELIBS)/qpoases/SRC/MessageHandling.o \
$(PHONELIBS)/qpoases/SRC/QProblem.o \
$(PHONELIBS)/qpoases/SRC/QProblemB.o \
$(PHONELIBS)/qpoases/SRC/SubjectTo.o \
$(PHONELIBS)/qpoases/SRC/Utils.o \
$(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \
mpc_export/acado_qpoases_interface.o \
mpc_export/acado_integrator.o \
mpc_export/acado_solver.o \
mpc_export/acado_auxiliary_functions.o \
mpc.o
DEPS := $(OBJS:.o=.d)
.PHONY: all
all: libcommampc.so
libcommampc.so: $(OBJS)
$(CXX) -shared -o '$@' $^ -lm
%.o: %.cpp
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I mpc_export/ \
$(QPOASES_FLAGS) \
-c -o '$@' '$<'
generator: generator.cpp
$(CXX) -Wall -std=c++11 \
generator.cpp \
-o generator \
$(ACADO_FLAGS) \
$(ACADO_LIBS)
.PHONY: generate
generate: generator
./generator
.PHONY: clean
clean:
rm -f libcommampc.so generator $(OBJS) $(DEPS)
-include $(DEPS)
selfdrive/controls/lib/lateral_mpc/__init__.py
View File
selfdrive/controls/lib/lateral_mpc/generator.cpp
+137
View File
@@ -0,0 +1,137 @@
#include <acado_code_generation.hpp>
#define PI 3.1415926536
#define deg2rad(d) (d/180.0*PI)
const int controlHorizon = 50;
const double samplingTime = 0.05; // 20 Hz
using namespace std;
int main( )
{
USING_NAMESPACE_ACADO
DifferentialEquation f;
DifferentialState xx; // x position
DifferentialState yy; // y position
DifferentialState psi; // vehicle heading
DifferentialState delta;
OnlineData curvature_factor;
OnlineData v_ref; // m/s
OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
OnlineData l_prob, r_prob, p_prob;
OnlineData lane_width;
Control t;
// Equations of motion
f << dot(xx) == v_ref * cos(psi);
f << dot(yy) == v_ref * sin(psi);
f << dot(psi) == v_ref * delta * curvature_factor;
f << dot(delta) == t;
auto lr_prob = l_prob + r_prob - l_prob * r_prob;
auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
auto c_left_lane = exp(-(poly_l - yy));
auto c_right_lane = exp(poly_r - yy);
auto r_phantom = poly_l - lane_width/2.0;
auto l_phantom = poly_r + lane_width/2.0;
auto path = lr_prob * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p;
// Running cost
Function h;
// Distance errors
h << path - yy;
h << l_prob * c_left_lane;
h << r_prob * c_right_lane;
// Heading error
h << (v_ref + 1.0 ) * (angle - psi);
// Angular rate error
h << (v_ref + 1.0 ) * t;
DMatrix Q(5,5);
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(2,2) = 1.0;
Q(3,3) = 1.0;
Q(4,4) = 1.0;
// Terminal cost
Function hN;
// Distance errors
hN << path - yy;
hN << l_prob * c_left_lane;
hN << r_prob * c_right_lane;
// Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
DMatrix QN(4,4);
QN(0,0) = 1.0;
QN(1,1) = 1.0;
QN(2,2) = 1.0;
QN(3,3) = 1.0;
// Setup Optimal Control Problem
const double tStart = 0.0;
const double tEnd = samplingTime * controlHorizon;
OCP ocp( tStart, tEnd, controlHorizon );
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
ocp.minimizeLSQEndTerm(QN, hN);
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
ocp.subjectTo( -0.1 <= t <= 0.1);
ocp.setNOD(18);
OCPexport mpc(ocp);
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 250 );
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES );
mpc.set( HOTSTART_QP, YES );
mpc.set( GENERATE_TEST_FILE, NO);
mpc.set( GENERATE_MAKE_FILE, NO );
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
mpc.printDimensionsQP( );
return EXIT_SUCCESS;
}
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+30
View File
@@ -0,0 +1,30 @@
import os
import subprocess
from cffi import FFI
mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
ffi = FFI()
ffi.cdef("""
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[50];
double y[50];
double psi[50];
double delta[50];
} log_t;
void init();
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
""")
libmpc = ffi.dlopen(libmpc_fn)
selfdrive/controls/lib/lateral_mpc/mpc.c
+100
View File
@@ -0,0 +1,100 @@
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <stdio.h>
#define NX ACADO_NX /* Number of differential state variables. */
#define NXA ACADO_NXA /* Number of algebraic variables. */
#define NU ACADO_NU /* Number of control inputs. */
#define NOD ACADO_NOD /* Number of online data values. */
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
#define N ACADO_N /* Number of intervals in the horizon. */
ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;
typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[N];
double y[N];
double psi[N];
double delta[N];
} log_t;
void init(){
acado_initializeSolver();
int i;
/* Initialize the states and controls. */
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1;
/* Initialize the measurements/reference. */
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
}
void run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
int i;
for (i = 0; i <= NOD * N; i+= NOD){
acadoVariables.od[i] = curvature_factor;
acadoVariables.od[i+1] = v_ref;
acadoVariables.od[i+2] = l_poly[0];
acadoVariables.od[i+3] = l_poly[1];
acadoVariables.od[i+4] = l_poly[2];
acadoVariables.od[i+5] = l_poly[3];
acadoVariables.od[i+6] = r_poly[0];
acadoVariables.od[i+7] = r_poly[1];
acadoVariables.od[i+8] = r_poly[2];
acadoVariables.od[i+9] = r_poly[3];
acadoVariables.od[i+10] = p_poly[0];
acadoVariables.od[i+11] = p_poly[1];
acadoVariables.od[i+12] = p_poly[2];
acadoVariables.od[i+13] = p_poly[3];
acadoVariables.od[i+14] = l_prob;
acadoVariables.od[i+15] = r_prob;
acadoVariables.od[i+16] = p_prob;
acadoVariables.od[i+17] = lane_width;
}
acadoVariables.x0[0] = x0->x;
acadoVariables.x0[1] = x0->y;
acadoVariables.x0[2] = x0->psi;
acadoVariables.x0[3] = x0->delta;
acado_preparationStep();
acado_feedbackStep( );
acado_shiftStates(2, 0, 0);
acado_shiftControls( 0 );
for (i = 0; i <= N; i++){
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
return;
}
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357
size 4906
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270
size 3428
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c78a95b2550fdfdc0182add44508c59ec7af9d1c58d776dcd32ee85ea52a771e
size 8680
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:963720039f3655eb3926e5bb3bbde21d92c824245055588ebaee223223accc02
size 18490
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:10ff52f5d8dbe27def800fe490cdee82a7c054183d2fb1888752609ea00bbea1
size 1949
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:55ef4b230fd392bb43a49b2bd4fbf6e478b6f7b6144f65df97fe3a952d6b0b49
size 1822
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:dd891792d5bc3c780941a0e3d8c37829d6f4ceef554a4704017f6024e29fba20
size 194688
selfdrive/controls/lib/longcontrol.py
+72 -154
View File
@@ -1,205 +1,117 @@
import numpy as np
from common.numpy_fast import clip, interp
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.pid import PIController
STOPPING_EGO_SPEED = 0.5
STOPPING_TARGET_SPEED = 0.3
STARTING_TARGET_SPEED = 0.5
BRAKE_THRESHOLD_TO_PID = 0.2
class LongCtrlState:
#*** this function handles the long control state transitions
# long_control_state labels:
off = 0 # Off
pid = 1 # moving and tracking targets, with PID control running
off = 0 # Off
pid = 1 # moving and tracking targets, with PID control running
stopping = 2 # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
starting = 3 # starting and releasing brakes in open loop before giving back to PID
def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb):
stopping_speed = 0.5
stopping_target_speed = 0.3
starting_target_speed = 0.5
brake_threshold_to_pid = 0.2
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
output_gb, brake_pressed):
stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed))
stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \
(((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or
(brake_pressed))
if not enabled:
if not active:
long_control_state = LongCtrlState.off
else:
if long_control_state == LongCtrlState.off:
if enabled:
if active:
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.pid:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif long_control_state == LongCtrlState.stopping:
if (v_target > starting_target_speed):
if (v_target > STARTING_TARGET_SPEED):
long_control_state = LongCtrlState.starting
elif long_control_state == LongCtrlState.starting:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif output_gb >= -brake_threshold_to_pid:
elif output_gb >= -BRAKE_THRESHOLD_TO_PID:
long_control_state = LongCtrlState.pid
return long_control_state
def get_compute_gb():
# see debug/dump_accel_from_fiber.py
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb(dat):
#linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
vx = dat[1]
if vx > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return m4
return _compute_gb
# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas
compute_gb = get_compute_gb()
_KP_BP = [0., 5., 35.]
_KP_V = [1.2, 0.8, 0.5]
_KP_V = [1.2, 0.8, 0.5]
_kI_BP = [0., 35.]
_kI_V = [0.18, 0.12]
_KI_BP = [0., 35.]
_KI_V = [0.18, 0.12]
def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate):
#*** This function compute the gb pedal positions in order to track the desired speed
# proportional and integral terms. More precision at low speed
Kp = interp(v_ego, _KP_BP, _KP_V)
Ki = interp(v_ego, _kI_BP, _kI_V)
# scale Kp and Ki by jerk factor from drive_thread
Kp = (1. + jerk_factor)*Kp
Ki = (1. + jerk_factor)*Ki
# this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump
v_ego_min = 0.3
v_ego = max(v_ego, v_ego_min)
v_error = v_pid - v_ego
Up_accel_cmd = v_error*Kp
Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate
accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd
output_gb_new = compute_gb([accel_cmd_new, v_ego])
# Anti-wind up for integrator: only update integrator if we not against the throttle and brake limits
# do not wind up if we are changing gear and we are on the gas pedal
if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or
(v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and
not (v_error >= 0. and gear == 11 and output_gb_new > 0)):
#update integrator
Ui_accel_cmd = Ui_accel_cmd_new
accel_cmd = Ui_accel_cmd + Up_accel_cmd
# go from accel to pedals
output_gb = compute_gb([accel_cmd, v_ego])
output_gb = output_gb[0]
# useful to know if control is against the limit
long_control_sat = False
if output_gb > gas_max or output_gb < -brake_max:
long_control_sat = True
output_gb = clip(output_gb, -brake_max, gas_max)
return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
starting_brake_rate = 0.6 # brake_travel/s while releasing on restart
starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
starting_brake_rate = 0.6 # brake_travel/s while releasing on restart
starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
class LongControl(object):
def __init__(self):
self.long_control_state = LongCtrlState.off # initialized to off
self.long_control_sat = False
self.Up_accel_cmd = 0.
self.last_output_gb = 0.
self.reset(0.)
def __init__(self, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((_KP_BP, _KP_V),
(_KI_BP, _KI_V),
rate=100.0,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
self.Ui_accel_cmd = 0.
self.pid.reset()
self.v_pid = v_pid
def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, CP):
brake_max_bp = [0., 5., 20., 100.] # speeds
brake_max_v = [1.0, 1.0, 0.8, 0.8] # values
def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead, a_target,
jerk_factor, CP):
# brake and gas limits
brake_max = interp(v_ego, brake_max_bp, brake_max_v)
# actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
# TODO: not every time
if CP.enableGas:
gas_max_bp = [0., 100.] # speeds
gas_max_v = [0.6, 0.6] # values
gas_max = interp(v_ego, gas_max_bp, gas_max_v)
else:
gas_max = 0
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
output_gb = self.last_output_gb
rate = 100
# limit max target speed based on cruise setting:
v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC
v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS)
# limit max target speed based on cruise setting
v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS)
rate = 100.0
max_speed_delta_up = a_target[1] * 1.0 / rate
max_speed_delta_down = a_target[0] * 1.0 / rate
max_speed_delta_up = a_target[1]*1.0/rate
max_speed_delta_down = a_target[0]*1.0/rate
# *** long control substate transitions
self.long_control_state = long_control_state_trans(enabled, self.long_control_state, v_ego, v_target, self.v_pid, output_gb)
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
v_target, self.v_pid, output_gb, brake_pressed)
# *** long control behavior based on state
# TODO: move this to drive_helpers
# disabled
if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego # do nothing
self.v_pid = v_ego # do nothing
output_gb = 0.
self.Ui_accel_cmd = 0.
self.pid.reset()
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
if ((self.v_pid > v_ego + overshoot_allowance) and
(v_target < self.v_pid)):
if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)):
self.v_pid = max(v_target, v_ego + overshoot_allowance)
elif ((self.v_pid < v_ego - overshoot_allowance) and
(v_target > self.v_pid)):
elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)):
self.v_pid = min(v_target, v_ego - overshoot_allowance)
# move v_pid no faster than allowed accel limits
@@ -215,25 +127,31 @@ class LongControl(object):
max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V)
self.v_pid = min(self.v_pid, v_ego + max_speed_error)
# TODO: removed anti windup on gear change, does it matter?
output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \
self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, 0, rate)
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor)
# intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego
if v_ego > 0. or output_gb > -brake_stopping_target:
output_gb -= stopping_brake_rate/rate
output_gb -= stopping_brake_rate / rate
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego
self.Ui_accel_cmd = 0.
self.pid.reset()
# intention is to move again, release brake fast before handling control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += starting_brake_rate/rate
output_gb += starting_brake_rate / rate
self.v_pid = v_ego
self.Ui_accel_cmd = starting_Ui
self.pid.reset()
self.pid.i = starting_Ui
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
return final_gas, final_brake
selfdrive/controls/lib/pathplanner.py
+34 -68
View File
@@ -1,66 +1,9 @@
import math
import numpy as np
from common.numpy_fast import interp
import selfdrive.messaging as messaging
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
def compute_path_pinv():
deg = 3
x = np.arange(50.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, map(float, points))
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
_LANE_WIDTH_V = [3., 3.8]
# break points of speed
_LANE_WIDTH_BP = [0., 31.]
def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
#*** this function computes the poly for the center of the lane, averaging left and right polys
lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
# lanes in US are ~3.6m wide
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
if l_prob + r_prob > 0.01:
c_poly = ((l_poly - half_lane_poly) * l_prob +
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
c_prob = math.sqrt((l_prob**2 + r_prob**2) / 2.)
else:
c_poly = np.zeros(4)
c_prob = 0.
p_weight = 1. # predicted path weight relatively to the center of the lane
d_poly = list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight))
return d_poly, c_poly, c_prob
class OptPathPlanner(object):
def __init__(self, model):
self.model = model
self.dead = True
self.d_poly = [0., 0., 0., 0.]
self.last_model = 0.
self._path_pinv = compute_path_pinv()
def update(self, cur_time, v_ego, md):
if md is not None:
# simple compute of the center of the lane
pts = [(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)]
self.d_poly = model_polyfit(pts, self._path_pinv)
self.last_model = cur_time
self.dead = False
elif cur_time - self.last_model > 0.5:
self.dead = True
class PathPlanner(object):
def __init__(self):
self.dead = True
self.d_poly = [0., 0., 0., 0.]
self.c_poly = [0., 0., 0., 0.]
self.c_prob = 0.
@@ -68,24 +11,47 @@ class PathPlanner(object):
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
self._path_pinv = compute_path_pinv()
def update(self, cur_time, v_ego, md):
self.lane_width_estimate = 3.7
self.lane_width_certainty = 1.0
self.lane_width = 3.7
def update(self, v_ego, md):
if md is not None:
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.model.leftLane.prob # left line prob
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.model.leftLane.prob # left line prob
r_prob = md.model.rightLane.prob # right line prob
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
lane_width_diff = abs(self.lane_width - current_lane_width)
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
r_prob *= lane_r_prob
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
self.lead_var = md.model.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
self.last_model = cur_time
self.dead = False
elif cur_time - self.last_model > 0.5:
self.dead = True
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob
selfdrive/controls/lib/pid.py
+91
View File
@@ -0,0 +1,91 @@
import numpy as np
from common.numpy_fast import clip, interp
import numbers
class PIController(object):
def __init__(self, k_p, k_i, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
self._k_p = k_p
self._k_i = k_i
self.pos_limit = pos_limit
self.neg_limit = neg_limit
self.sat_count_rate = 1.0 / rate
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.sat_limit = sat_limit
self.jerk_factor = 0.0
self.convert = convert
self.reset()
@property
def k_p(self):
if isinstance(self._k_p, numbers.Number):
kp = self._k_p
else:
kp = interp(self.speed, self._k_p[0], self._k_p[1])
return (1.0 + self.jerk_factor) * kp
@property
def k_i(self):
if isinstance(self._k_i, numbers.Number):
ki = self._k_i
else:
ki = interp(self.speed, self._k_i[0], self._k_i[1])
return (1.0 + self.jerk_factor) * ki
def _check_saturation(self, control, override, error):
saturated = (control < self.neg_limit) or (control > self.pos_limit)
if saturated and not override and abs(error) > 0.1:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate
self.sat_count = clip(self.sat_count, 0.0, 1.0)
return self.sat_count > self.sat_limit
def reset(self):
self.p = 0.0
self.i = 0.0
self.sat_count = 0.0
self.saturated = False
self.control = 0
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False):
self.speed = speed
self.jerk_factor = jerk_factor
error = float(setpoint - measurement)
self.p = error * self.k_p
if override:
self.i -= self.i_unwind_rate * float(np.sign(self.i))
else:
i = self.i + error * self.k_i * self.i_rate
control = self.p + i
if self.convert is not None:
control = self.convert(control, speed=self.speed)
# Update when changing i will move the control away from the limits
# or when i will move towards the sign of the error
if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
(error <= 0 and (control >= self.neg_limit or i > 0.0)):
self.i = i
control = self.p + self.i
if self.convert is not None:
control = self.convert(control, speed=self.speed)
if check_saturation:
self.saturated = self._check_saturation(control, override, error)
else:
self.saturated = False
self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control
selfdrive/controls/lib/planner.py
+43 -30
View File
@@ -1,80 +1,93 @@
#!/usr/bin/env python
import os
import zmq
import numpy as np
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from common.realtime import sec_since_boot
from common.params import Params
from selfdrive.swaglog import cloudlog
from cereal import car
from selfdrive.controls.lib.pathplanner import OptPathPlanner
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
from selfdrive.controls.lib.fcw import ForwardCollisionWarning
_DT = 0.01 # 100Hz
class Planner(object):
def __init__(self, CP):
context = zmq.Context()
self.CP = CP
self.live20 = messaging.sub_sock(context, service_list['live20'].port)
self.model = messaging.sub_sock(context, service_list['model'].port)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.last_md_ts = 0
self.last_l20_ts = 0
self.last_model = 0.
self.last_l20 = 0.
self.model_dead = True
self.radar_dead = True
self.radar_errors = []
if os.getenv("OPT") is not None:
self.PP = OptPathPlanner(self.model)
else:
self.PP = PathPlanner()
self.PP = PathPlanner()
self.AC = AdaptiveCruise()
self.FCW = ForwardCollisionWarning(_DT)
# this runs whenever we get a packet that can change the plan
def update(self, CS, LoC):
cur_time = sec_since_boot()
md = messaging.recv_sock(self.model)
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
if cur_time - self.last_model > 0.5:
self.model_dead = True
l20 = messaging.recv_sock(self.live20)
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
self.PP.update(cur_time, CS.vEgo, md)
self.PP.update(CS.vEgo, md)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.PP.dead
if plan_send.plan.lateralValid:
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.AC.dead
if plan_send.plan.longitudinalValid:
plan_send.plan.vTarget = float(self.AC.v_target_lead)
plan_send.plan.aTargetMin = float(self.AC.a_target[0])
plan_send.plan.aTargetMax = float(self.AC.a_target[1])
plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
plan_send.plan.hasLead = self.AC.has_lead
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vTarget = float(self.AC.v_target_lead)
plan_send.plan.aTargetMin = float(self.AC.a_target[0])
plan_send.plan.aTargetMax = float(self.AC.a_target[1])
plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
plan_send.plan.hasLead = self.AC.has_lead
# compute risk of collision events: fcw
self.FCW.process(CS, self.AC)
selfdrive/controls/lib/vehicle_model.py
+26 -23
View File
@@ -1,33 +1,32 @@
#!/usr/bin/env python
import numpy as np
from numpy.linalg import inv
from selfdrive.car.honda.interface import CarInterface
## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
# dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
# Xdot = A*X + B*U
# where X = [v, r], with v and r lateral speed and rotational speed, respectively
# and U is the steering angle (controller input)
#
#
# A depends on longitudinal speed, u, and vehicle parameters CP
def create_dyn_state_matrices(u, CP):
A = np.zeros((2,2))
B = np.zeros((2,1))
A[0,0] = - (CP.cF + CP.cR)/(CP.m*u)
A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u
A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u)
A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u)
B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR
B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR
A = np.zeros((2, 2))
B = np.zeros((2, 1))
A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u)
A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u
A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u)
A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u)
B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR
B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR
return A, B
def kin_ss_sol(sa, u, CP):
# kinematic solution, useful when speed ~ 0
K = np.zeros((2,1))
K[0,0] = CP.aR / CP.sR / CP.l * u
K[1,0] = 1. / CP.sR / CP.l * u
K = np.zeros((2, 1))
K[0, 0] = CP.aR / CP.sR / CP.l * u
K[1, 0] = 1. / CP.sR / CP.l * u
return K * sa
@@ -36,13 +35,15 @@ def dyn_ss_sol(sa, u, CP):
A, B = create_dyn_state_matrices(u, CP)
return - np.matmul(inv(A), B) * sa
def calc_slip_factor(CP):
# the slip factor is a measure of how the curvature changes with speed
# it's positive for Oversteering vehicle, negative (usual case) otherwise
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
class VehicleModel(object):
def __init__(self, CP, init_state=np.asarray([[0.],[0.]])):
def __init__(self, CP, init_state=np.asarray([[0.], [0.]])):
self.dt = 0.1
lookahead = 2. # s
self.steps = int(lookahead / self.dt)
@@ -54,7 +55,7 @@ class VehicleModel(object):
self.state = state
def steady_state_sol(self, sa, u):
# if the speed is too small we can't use the dynamic model
# if the speed is too small we can't use the dynamic model
# (tire slip is undefined), we then use the kinematic model
if u > 0.1:
return dyn_ss_sol(sa, u, self.CP)
@@ -63,13 +64,14 @@ class VehicleModel(object):
def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions
return self.curvature_factor(u) * sa / self.CP.sR
def curvature_factor(self, u):
sf = calc_slip_factor(self.CP)
return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l
return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l
def get_steer_from_curvature(self, curv, u):
# this function is the exact inverse of calc_curvature, returning steer angle given curvature
sf = calc_slip_factor(self.CP)
return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv
return curv * self.CP.sR * 1.0 / self.curvature_factor(u)
def state_prediction(self, sa, u):
# U is the matrix of the controls
@@ -79,9 +81,10 @@ class VehicleModel(object):
if __name__ == '__main__':
from selfdrive.car.toyota.interface import CarInterface
# load car params
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
print CP
VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15)
print calc_slip_factor(CP)
selfdrive/controls/radard.py
+5 -4
View File
@@ -7,7 +7,7 @@ from collections import defaultdict
from fastcluster import linkage_vector
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -84,7 +84,7 @@ def radard_thread(gctx=None):
tracks = defaultdict(dict)
# Kalman filter stuff:
# Kalman filter stuff:
ekfv = EKFV1D()
speedSensorV = SimpleSensor(XV, 1, 2)
@@ -121,7 +121,7 @@ def radard_thread(gctx=None):
last_md_ts = md.logMonoTime
# *** get path prediction from the model ***
PP.update(sec_since_boot(), v_ego, md)
PP.update(v_ego, md)
# run kalman filter only if prob is high enough
if PP.lead_prob > 0.7:
@@ -156,7 +156,7 @@ def radard_thread(gctx=None):
continue
rpt = ar_pts[ids]
# align v_ego by a fixed time to align it with the radar measurement
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0])
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
@@ -224,6 +224,7 @@ def radard_thread(gctx=None):
dat.init('live20')
dat.live20.mdMonoTime = last_md_ts
dat.live20.canMonoTimes = list(rr.canMonoTimes)
dat.live20.radarErrors = list(rr.errors)
dat.live20.l100MonoTime = last_l100_ts
if lead_len > 0:
lead_clusters[0].toLive20(dat.live20.leadOne)
selfdrive/debug/can_printer.py
+15 -6
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python
import os
import sys
import struct
from collections import defaultdict
from common.realtime import sec_since_boot
@@ -8,14 +9,14 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
def can_printer():
def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"):
context = zmq.Context()
logcan = messaging.sub_sock(context, service_list['can'].port)
logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", 0))
canbus = int(os.getenv("CAN", bus))
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
@@ -27,10 +28,18 @@ def can_printer():
dd = chr(27) + "[2J"
dd += "%5.2f\n" % (sec_since_boot() - start)
for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))):
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
if k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
print dd
lp = sec_since_boot()
if __name__ == "__main__":
can_printer()
if len(sys.argv) > 3:
can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3])
elif len(sys.argv) > 2:
can_printer(int(sys.argv[1]), int(sys.argv[2]))
elif len(sys.argv) > 1:
can_printer(int(sys.argv[1]))
else:
can_printer()
selfdrive/debug/dump.py
+1 -1
View File
@@ -27,5 +27,5 @@ if __name__ == "__main__":
if args.raw:
hexdump(sock.recv())
else:
print messaging.recv_sock(sock)
print messaging.recv_one(sock)
selfdrive/debug/getframes/getframes.py
+6 -6
View File
@@ -42,26 +42,26 @@ typedef struct VisionStreamBufs {
} buf_info;
} VisionStreamBufs;
typedef struct VisionBuf {
typedef struct VIPCBuf {
int fd;
size_t len;
void* addr;
} VisionBuf;
} VIPCBuf;
typedef struct VisionBufExtra {
typedef struct VIPCBufExtra {
uint32_t frame_id; // only for yuv
} VisionBufExtra;
} VIPCBufExtra;
typedef struct VisionStream {
int ipc_fd;
int last_idx;
int num_bufs;
VisionStreamBufs bufs_info;
VisionBuf *bufs;
VIPCBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
"""
selfdrive/debug/test_carcontroller.py
+3 -2
View File
@@ -28,6 +28,7 @@ if __name__ == "__main__":
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
CI, CP = get_car(logcan, sendcan)
CC = car.CarControl.new_message()
rk = Ratekeeper(100)
@@ -56,11 +57,11 @@ if __name__ == "__main__":
print axis_values, button_values
# **** handle car ****
CS = CI.update()
CS = CI.update(CC)
#print CS
CC = car.CarControl.new_message()
CC.enabled = True
CC.gas = float(np.clip(-axis_values[1], 0, 1.0))
selfdrive/debug/test_carstate.py
+3 -1
View File
@@ -1,6 +1,7 @@
#!/usr/bin/env python
import os
import zmq
from cereal import car
import selfdrive.messaging as messaging
from selfdrive.services import service_list
@@ -36,8 +37,9 @@ def test_loop():
"False"]
while 1:
CC = car.CarControl.new_message()
# read CAN
CS = CI.update()
CS = CI.update(CC)
while eval(states[state]) == True:
state += 1
selfdrive/logcatd/Makefile
+1
View File
@@ -2,6 +2,7 @@ CC = clang
CXX = clang++
PHONELIBS = ../../phonelibs
BASEDIR = ../..
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
selfdrive/loggerd/config.py
+6 -1
View File
@@ -1,4 +1,9 @@
import os
ROOT = '/data/media/0/realdata/'
if os.environ.get('LOGGERD_ROOT', False):
ROOT = os.environ['LOGGERD_ROOT']
print("Custom loggerd root: ", ROOT)
else:
ROOT = '/data/media/0/realdata/'
SEGMENT_LENGTH = 60
selfdrive/loggerd/loggerd
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:51d01404e86f6e1f1d2ecea67bfea72c8e568f03b4f839b4480b40c27993e685
size 1367088
oid sha256:2aaef8016fb65911037340c5a7a4c340b7cc49ea67723d01c1129246f8221bbe
size 1364512
selfdrive/manager.py
+115 -55
View File
@@ -1,30 +1,36 @@
#!/usr/bin/env python
import os
import time
# check if NEOS update is required
while 1:
if ((not os.path.isfile("/VERSION")
or int(open("/VERSION").read()) < 3)
and not os.path.isfile("/data/media/0/noupdate")):
os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
else:
break
if os.path.isfile("/init.qcom.rc"):
# check if NEOS update is required
while 1:
if ((not os.path.isfile("/VERSION")
or int(open("/VERSION").read()) < 3)
and not os.path.isfile("/data/media/0/noupdate")):
os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
else:
break
time.sleep(10)
import sys
import time
import importlib
import subprocess
import signal
import traceback
import usb1
from multiprocessing import Process
from selfdrive.services import service_list
from common.basedir import BASEDIR
sys.path.append(os.path.join(BASEDIR, "pyextra"))
os.environ['BASEDIR'] = BASEDIR
import usb1
import hashlib
import zmq
from setproctitle import setproctitle
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from selfdrive.thermal import read_thermal
@@ -36,22 +42,20 @@ from common.params import Params
from selfdrive.loggerd.config import ROOT
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
# comment out anything you don't want to run
managed_processes = {
"uploader": "selfdrive.loggerd.uploader",
"controlsd": "selfdrive.controls.controlsd",
"radard": "selfdrive.controls.radard",
"loggerd": ("loggerd", ["./loggerd"]),
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged",
"tombstoned": "selfdrive.tombstoned",
"logcatd": ("logcatd", ["./logcatd"]),
"proclogd": ("proclogd", ["./proclogd"]),
"boardd": ("boardd", ["./boardd"]), # switch to c++ boardd
"ui": ("ui", ["./ui"]),
"visiond": ("visiond", ["./visiond"]),
"sensord": ("sensord", ["./sensord"]), }
"logcatd": ("selfdrive/logcatd", ["./logcatd"]),
"proclogd": ("selfdrive/proclogd", ["./proclogd"]),
"boardd": ("selfdrive/boardd", ["./boardd"]), # switch to c++ boardd
"ui": ("selfdrive/ui", ["./ui"]),
"visiond": ("selfdrive/visiond", ["./visiond"]),
"sensord": ("selfdrive/sensord", ["./sensord"]), }
running = {}
def get_running():
@@ -116,13 +120,28 @@ def start_managed_process(name):
running[name] = Process(name=name, target=launcher, args=(proc, gctx))
else:
pdir, pargs = proc
cwd = os.path.join(BASEDIR, "selfdrive")
if pdir is not None:
cwd = os.path.join(cwd, pdir)
cwd = os.path.join(BASEDIR, pdir)
cloudlog.info("starting process %s" % name)
running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd))
running[name].start()
def prepare_managed_process(p):
proc = managed_processes[p]
if isinstance(proc, basestring):
# import this python
cloudlog.info("preimporting %s" % proc)
importlib.import_module(proc)
else:
# build this process
cloudlog.info("building %s" % (proc,))
try:
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0]))
except subprocess.CalledProcessError:
# make clean if the build failed
cloudlog.info("building %s failed, make clean" % (proc, ))
subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0]))
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0]))
def kill_managed_process(name):
if name not in running or name not in managed_processes:
return
@@ -212,9 +231,67 @@ def system(cmd):
except subprocess.CalledProcessError, e:
cloudlog.event("running failed",
cmd=e.cmd,
output=e.output,
output=e.output[-1024:],
returncode=e.returncode)
# TODO: this is not proper gating for EON
try:
from smbus2 import SMBus
EON = True
except ImportError:
EON = False
def setup_eon_fan():
if not EON:
return
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
bus = SMBus(7, force=True)
bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts
bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable
bus.write_byte_data(0x21, 0x02, 0x2) # needed?
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
bus.close()
last_eon_fan_val = None
def set_eon_fan(val):
global last_eon_fan_val
if not EON:
return
if last_eon_fan_val is None or last_eon_fan_val != val:
bus = SMBus(7, force=True)
bus.write_byte_data(0x21, 0x04, 0x2)
bus.write_byte_data(0x21, 0x03, (val*2)+1)
bus.write_byte_data(0x21, 0x04, 0x4)
bus.close()
last_eon_fan_val = val
# temp thresholds to control fan speed - high hysteresis
_TEMP_THRS_H = [50., 65., 80., 10000]
# temp thresholds to control fan speed - low hysteresis
_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
# fan speed options
_FAN_SPEEDS = [0, 16384, 32768, 65535]
def handle_fan(max_temp, fan_speed):
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp)
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp)
if new_speed_h > fan_speed:
# update speed if using the high thresholds results in fan speed increment
fan_speed = new_speed_h
elif new_speed_l < fan_speed:
# update speed if using the low thresholds results in fan speed decrement
fan_speed = new_speed_l
set_eon_fan(fan_speed/16384)
return fan_speed
def manager_thread():
global baseui_running
@@ -233,24 +310,18 @@ def manager_thread():
start_managed_process("ui")
manage_baseui(True)
panda = False
# do this before panda flashing
setup_eon_fan()
if os.getenv("NOBOARD") is None:
# *** wait for the board ***
panda = wait_for_device() == 0x2300
# flash the device
if os.getenv("NOPROG") is None:
# flash the board
boarddir = os.path.join(BASEDIR, "panda/board/")
mkfile = "Makefile" if panda else "Makefile.legacy"
print "using", mkfile
system("cd %s && make -f %s" % (boarddir, mkfile))
start_managed_process("boardd")
from panda import ensure_st_up_to_date
ensure_st_up_to_date()
start_managed_process("boardd")
started = False
logger_dead = False
count = 0
fan_speed = 0
# set 5 second timeout on health socket
# 5x slower than expected
@@ -274,12 +345,15 @@ def manager_thread():
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
thermal_sock.send(msg.to_bytes())
print msg
# TODO: add car battery voltage check
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
fan_speed = handle_fan(max_temp, fan_speed)
msg.thermal.fanSpeed = fan_speed
thermal_sock.send(msg.to_bytes())
print msg
# uploader is gated based on the phone temperature
if max_temp > 85.0:
@@ -352,21 +426,7 @@ def manager_prepare():
# build all processes
os.chdir(os.path.dirname(os.path.abspath(__file__)))
for p in managed_processes:
proc = managed_processes[p]
if isinstance(proc, basestring):
# import this python
cloudlog.info("preimporting %s" % proc)
importlib.import_module(proc)
else:
# build this process
cloudlog.info("building %s" % (proc,))
try:
subprocess.check_call(["make", "-j4"], cwd=proc[0])
except subprocess.CalledProcessError:
# make clean if the build failed
cloudlog.info("building %s failed, make clean" % (proc, ))
subprocess.check_call(["make", "clean"], cwd=proc[0])
subprocess.check_call(["make", "-j4"], cwd=proc[0])
prepare_managed_process(p)
# install apks
installed = get_installed_apks()
selfdrive/proclogd/Makefile
+1
View File
@@ -2,6 +2,7 @@ CC = clang
CXX = clang++
PHONELIBS = ../../phonelibs
BASEDIR = ../..
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
selfdrive/radar/nidec/interface.py
+31 -45
View File
@@ -2,8 +2,7 @@
import os
import numpy as np
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.can.parser import CANParser as CANParserC
from selfdrive.can.parser import CANParser
from selfdrive.boardd.boardd import can_capnp_to_can_list
@@ -14,29 +13,27 @@ import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
NEW_CAN = os.getenv("OLD_CAN") is None
def _create_radard_can_parser():
def _create_nidec_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
['REL_SPEED'] * 16, radar_messages * 4,
[255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
checks = zip(radar_messages, [20]*16)
radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446)
signals = zip(['RADAR_STATE'] +
['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
['REL_SPEED'] * 16,
[0x400] + radar_messages[1:] * 4,
[0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
checks = zip([0x445], [20])
if NEW_CAN:
return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 1)
else:
return CANParser(dbc_f, signals, checks)
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
class RadarInterface(object):
def __init__(self):
# radar
self.pts = {}
self.track_id = 0
self.radar_fault = False
# Nidec
self.rcp = _create_radard_can_parser()
self.rcp = _create_nidec_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
@@ -44,39 +41,19 @@ class RadarInterface(object):
def update(self):
canMonoTimes = []
if NEW_CAN:
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
if 0x445 in updated_messages:
break
else:
can_pub_radar = []
# TODO: can hang if no packets show up
while 1:
for a in messaging.drain_sock(self.logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
# only run on the 0x445 packets, used for timing
if any(x[0] == 0x445 for x in can_pub_radar):
break
updated_messages = self.rcp.update_can(can_pub_radar)
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("notValid")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
if 0x445 in updated_messages:
break
for ii in updated_messages:
cpt = self.rcp.vl[ii]
#print cpt
if cpt['LONG_DIST'] < 255:
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
@@ -90,6 +67,15 @@ class RadarInterface(object):
if ii in self.pts:
del self.pts[ii]
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
if self.radar_fault:
errors.append("fault")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
ret.points = self.pts.values()
return ret
selfdrive/sensord/sensord
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:fc4b862553d2ec9d63740b554997e5069808c531d9d722b4ce2cf83a2f4429ed
size 935360
oid sha256:ba62e6a2641999da0ae564166ebbcf01e1922f69dbcf9d48dc94c3bb0886e558
size 939496
selfdrive/service_list.yaml
+3
View File
@@ -43,6 +43,9 @@ lidarPts: [8030, true]
procLog: [8031, true]
gpsLocationExternal: [8032, true]
ubloxGnss: [8033, true]
clocks: [8034, true]
liveMpc: [8035, true]
testModel: [8040, false]
# manager -- base process to manage starting and stopping of all others
selfdrive/test/plant/maneuver.py
+2
View File
@@ -66,6 +66,8 @@ class Maneuver(object):
jerk_factor=last_live100.jerkFactor,
a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax)
print "maneuver end"
return (None, plot)
selfdrive/test/plant/plant.py
+17 -16
View File
@@ -13,10 +13,10 @@ import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.config import CruiseButtons
from selfdrive.car.honda.hondacan import fix
from selfdrive.car.honda.carstate import get_can_parser
from selfdrive.car.honda.carstate import get_can_signals
from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.car.honda.old_can_parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from cereal import car
@@ -81,6 +81,11 @@ def get_car_can_parser():
]
return CANParser(dbc_f, signals, checks)
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
class Plant(object):
messaging_initialized = False
@@ -142,11 +147,12 @@ class Plant(object):
return float(self.rk.frame) / self.rate
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
cp2 = get_can_parser(CP)
sgs = cp2._sgs
msgs = cp2._msgs
cks_msgs = cp2.msgs_ck
gen_dbc, gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_msgs = []
@@ -212,10 +218,8 @@ class Plant(object):
self.user_brake, self.steer_error, self.brake_error,
self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
self.pedal_gas, self.cruise_setting,
# left_blinker, right_blinker, counter
0,0,0,
# interceptor_gas
0,0]
# append one more zero for gas interceptor
0,0,0,0]
# TODO: publish each message at proper frequency
can_msgs = []
@@ -236,15 +240,12 @@ class Plant(object):
# add the radar message
# TODO: use the DBC
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel*16.0) + \
to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \
to_3s_byte(int(v_rel*32.0)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
selfdrive/test/plant/plant_ui.py
Executable
+122
View File
@@ -0,0 +1,122 @@
#!/usr/bin/env python
import pygame
from plant import Plant
from selfdrive.config import CruiseButtons
import numpy as np
import selfdrive.messaging as messaging
import math
CAR_WIDTH = 2.0
CAR_LENGTH = 4.5
METER = 8
def rot_center(image, angle):
"""rotate an image while keeping its center and size"""
orig_rect = image.get_rect()
rot_image = pygame.transform.rotate(image, angle)
rot_rect = orig_rect.copy()
rot_rect.center = rot_image.get_rect().center
rot_image = rot_image.subsurface(rot_rect).copy()
return rot_image
def car_w_color(c):
car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH))
car.set_alpha(0)
car.fill((10,10,10))
car.set_alpha(128)
pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1)
return car
if __name__ == "__main__":
pygame.init()
display = pygame.display.set_mode((1000, 1000))
pygame.display.set_caption('Plant UI')
car = car_w_color((255,0,255))
leadcar = car_w_color((255,0,0))
carx, cary, heading = 10.0, 50.0, 0.0
plant = Plant(100, distance_lead = 40.0)
control_offset = 2.0
control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10)
def pt_to_car(pt):
x,y = pt
x -= carx
y -= cary
rx = x * math.cos(-heading) + y * -math.sin(-heading)
ry = x * math.sin(-heading) + y * math.cos(-heading)
return rx, ry
def pt_from_car(pt):
x,y = pt
rx = x * math.cos(heading) + y * -math.sin(heading)
ry = x * math.sin(heading) + y * math.cos(heading)
rx += carx
ry += cary
return rx, ry
while 1:
if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25:
cruise_buttons = CruiseButtons.RES_ACCEL
else:
cruise_buttons = 0
md = messaging.new_message()
md.init('model')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0]*50
x.prob = 0.0
x.std = 1.0
car_pts = map(pt_to_car, control_pts)
print car_pts
car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
md.model.path.prob = 1.0
Plant.model.send(md.to_bytes())
plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False)
display.fill((10,10,10))
carx += plant.speed * plant.ts * math.cos(heading)
cary += plant.speed * plant.ts * math.sin(heading)
# positive steering angle = steering right
print plant.angle_steer
heading += plant.angle_steer * plant.ts
print heading
# draw my car
display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER))
# draw control pts
for x,y in control_pts:
pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2)
# draw path
path_pts = zip(np.arange(0, 50), md.model.path.points)
for x,y in path_pts:
x,y = pt_from_car((x,y))
pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1)
"""
# draw lead car
dl = (plant.distance_lead - plant.distance) + 4.5
lx = carx + dl * math.cos(heading)
ly = cary + dl * math.sin(heading)
display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER))
"""
pygame.display.flip()
selfdrive/test/plant/runtest.sh
-14
View File
@@ -1,14 +0,0 @@
#!/bin/bash
export OPTEST=1
export OLD_CAN=1
pushd ../../controls
./controlsd.py &
pid1=$!
./radard.py &
pid2=$!
trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT
popd
mkdir -p out
MPLBACKEND=svg ./runtracks.py out
selfdrive/test/plant/runtracks.py
-207
View File
@@ -1,207 +0,0 @@
#!/usr/bin/env python
import sys
import time, json
from selfdrive.test.plant import plant
from selfdrive.config import Conversions as CV, CruiseButtons as CB
from maneuver import *
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 20mph, grade change +10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'while cruising at 20mph, grade change -10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
duration=30.,
initial_speed = 60. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 35.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 25.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
duration=30.,
initial_speed = 0.,
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)]
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
duration=25.,
initial_speed=30.,
lead_relevancy=True,
initial_distance_lead=49.,
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
duration=70.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
duration=30.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=4.,
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
duration=70.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
)
]
css_style = """
.maneuver_title {
font-size: 24px;
text-align: center;
}
.maneuver_graph {
width: 100%;
}
"""
def main(output_dir):
view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
for i, man in enumerate(maneuvers):
view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
view_html += "</tr>"
with open(os.path.join(output_dir, "index.html"), "w") as f:
f.write(view_html)
for i, man in enumerate(maneuvers):
score, plot = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2))
if __name__ == "__main__":
if len(sys.argv) <= 1:
print "Usage:", sys.argv[0], "<output_dir>"
exit(1)
main(sys.argv[1])
selfdrive/test/test_openpilot.py
+19 -19
View File
@@ -42,25 +42,25 @@ def with_processes(processes):
return wrap
return wrapper
@phone_only
@with_processes(['controlsd', 'radard'])
def test_controls():
from selfdrive.test.plant.plant import Plant
# start the fake car for 2 seconds
plant = Plant(100)
for i in range(200):
if plant.rk.frame >= 20 and plant.rk.frame <= 25:
cruise_buttons = CruiseButtons.RES_ACCEL
# rolling forward
assert plant.speed > 0
else:
cruise_buttons = 0
plant.step(cruise_buttons = cruise_buttons)
plant.close()
# assert that we stopped
assert plant.speed == 0.0
#@phone_only
#@with_processes(['controlsd', 'radard'])
#def test_controls():
# from selfdrive.test.plant.plant import Plant
#
# # start the fake car for 2 seconds
# plant = Plant(100)
# for i in range(200):
# if plant.rk.frame >= 20 and plant.rk.frame <= 25:
# cruise_buttons = CruiseButtons.RES_ACCEL
# # rolling forward
# assert plant.speed > 0
# else:
# cruise_buttons = 0
# plant.step(cruise_buttons = cruise_buttons)
# plant.close()
#
# # assert that we stopped
# assert plant.speed == 0.0
@phone_only
@with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd'])
selfdrive/test/tests/plant/__init__.py
View File
selfdrive/test/tests/plant/test_longitudinal.py
Executable
+255
View File
@@ -0,0 +1,255 @@
#!/usr/bin/env python
import os
os.environ['OLD_CAN'] = '1'
os.environ['NOCRASH'] = '1'
import time
import unittest
import shutil
import matplotlib
matplotlib.use('svg')
from selfdrive.config import Conversions as CV, CruiseButtons as CB
from selfdrive.test.plant.maneuver import Maneuver
import selfdrive.manager as manager
def create_dir(path):
try:
os.makedirs(path)
except OSError:
pass
maneuvers = [
Maneuver(
'while cruising at 40 mph, change cruise speed to 50mph',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.RES_ACCEL, 10.), (0, 10.1),
(CB.RES_ACCEL, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 60 mph, change cruise speed to 50mph',
duration=30.,
initial_speed=60. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3),
(CB.DECEL_SET, 10.), (0, 10.1),
(CB.DECEL_SET, 10.2), (0, 10.3)]
),
Maneuver(
'while cruising at 20mph, grade change +10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., 1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'while cruising at 20mph, grade change -10%',
duration=25.,
initial_speed=20. * CV.MPH_TO_MS,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)],
grade_values = [0., 0., -1.0],
grade_breakpoints = [0., 10., 11.]
),
Maneuver(
'approaching a 40mph car while cruising at 60mph from 100m away',
duration=30.,
initial_speed = 60. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=100.,
speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'approaching a 0mph car while cruising at 40mph from 150m away',
duration=30.,
initial_speed = 40. * CV.MPH_TO_MS,
lead_relevancy=True,
initial_distance_lead=150.,
speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 100.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 35.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
duration=50.,
initial_speed = 20.,
lead_relevancy=True,
initial_distance_lead=35.,
speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS],
speed_lead_breakpoints = [0., 15., 25.0],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)]
),
Maneuver(
'starting at 0mph, approaching a stopped car 100m away',
duration=30.,
initial_speed = 0.,
lead_relevancy=True,
initial_distance_lead=100.,
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9)]
),
Maneuver(
"following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s",
duration=25.,
initial_speed=30.,
lead_relevancy=True,
initial_distance_lead=49.,
speed_lead_values=[30.,30.,29.,31.,29.,31.,29.],
speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel",
duration=70.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0.,10.],
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"green light: stopped behind lead car, lead car accelerates at 1.5 m/s",
duration=30.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=4.,
speed_lead_values=[0, 0 , 45],
speed_lead_breakpoints=[0, 10., 40.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"stop and go with 1m/s2 lead decel and accel, with full stops",
duration=70.,
initial_speed=0.,
lead_relevancy=True,
initial_distance_lead=20.,
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 10.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
),
Maneuver(
"accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2",
duration=30.,
initial_speed=10.,
lead_relevancy=True,
initial_distance_lead=10.,
speed_lead_values=[20., 0.],
speed_lead_breakpoints=[1., 11.],
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
(CB.RES_ACCEL, 1.6), (0.0, 1.7),
(CB.RES_ACCEL, 1.8), (0.0, 1.9),
(CB.RES_ACCEL, 2.0), (0.0, 2.1),
(CB.RES_ACCEL, 2.2), (0.0, 2.3)]
)
]
def setup_output():
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
if not os.path.exists(os.path.join(output_dir, "index.html")):
# write test output header
css_style = """
.maneuver_title {
font-size: 24px;
text-align: center;
}
.maneuver_graph {
width: 100%;
}
"""
view_html = "<html><head><style>%s</style></head><body><table>" % (css_style,)
for i, man in enumerate(maneuvers):
view_html += "<tr><td class='maneuver_title' colspan=5><div>%s</div></td></tr><tr>" % (man.title,)
for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']:
view_html += "<td><img class='maneuver_graph' src='%s'/></td>" % (os.path.join("maneuver" + str(i+1).zfill(2), c), )
view_html += "</tr>"
create_dir(output_dir)
with open(os.path.join(output_dir, "index.html"), "w") as f:
f.write(view_html)
class LongitudinalControl(unittest.TestCase):
@classmethod
def setUpClass(cls):
setup_output()
shutil.rmtree('/data/params', ignore_errors=True)
manager.gctx = {}
manager.prepare_managed_process('radard')
manager.prepare_managed_process('controlsd')
manager.start_managed_process('radard')
manager.start_managed_process('controlsd')
@classmethod
def tearDownClass(cls):
manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd')
time.sleep(5)
# hack
def test_longitudinal_setup(self):
pass
WORKERS = 8
def run_maneuver_worker(k):
output_dir = os.path.join(os.getcwd(), 'out/longitudinal')
for i, man in enumerate(maneuvers[k::WORKERS]):
score, plot = man.evaluate()
plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2))
for k in xrange(WORKERS):
setattr(LongitudinalControl,
"test_longitudinal_maneuvers_%d" % (k+1),
lambda self, k=k: run_maneuver_worker(k))
if __name__ == "__main__":
unittest.main()
selfdrive/ui/Makefile
+2 -1
View File
@@ -55,9 +55,10 @@ ui: $(OBJS)
$(OPENGL_LIBS) \
-lcutils -lm -llog
../common/framebuffer.o: ../common/framebuffer.cc
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
-I$(PHONELIBS)/android_frameworks_native/include \
-I$(PHONELIBS)/android_system_core/include \
-I$(PHONELIBS)/android_hardware_libhardware/include \
selfdrive/ui/ui.c
+95 -24
View File
@@ -38,16 +38,18 @@ typedef struct UIScene {
int frontview;
uint8_t *bgr_ptr;
int big_box_x, big_box_y, big_box_width, big_box_height;
int transformed_width, transformed_height;
uint64_t model_ts;
ModelData model;
float mpc_x[50];
float mpc_y[50];
bool world_objects_visible;
// TODO(mgraczyk): Remove and use full frame for everything.
mat3 warp_matrix; // transformed box -> big_box.
mat3 warp_matrix; // transformed box -> frame.
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
float v_cruise;
@@ -88,14 +90,16 @@ typedef struct UIState {
void *livecalibration_sock_raw;
zsock_t *live20_sock;
void *live20_sock_raw;
zsock_t *livempc_sock;
void *livempc_sock_raw;
// vision state
bool vision_connected;
bool vision_connect_firstrun;
int ipc_fd;
VisionBuf bufs[UI_BUF_COUNT];
VisionBuf front_bufs[UI_BUF_COUNT];
VIPCBuf bufs[UI_BUF_COUNT];
VIPCBuf front_bufs[UI_BUF_COUNT];
int cur_vision_idx;
int cur_vision_front_idx;
@@ -228,6 +232,11 @@ static void ui_init(UIState *s) {
assert(s->live20_sock);
s->live20_sock_raw = zsock_resolve(s->live20_sock);
s->livempc_sock = zsock_new_sub(">tcp://127.0.0.1:8035", "");
assert(s->livempc_sock);
s->livempc_sock_raw = zsock_resolve(s->livempc_sock);
s->ipc_fd = -1;
// init display
@@ -271,8 +280,7 @@ static void ui_init(UIState *s) {
// intrinsic_matrix and returns true. Otherwise returns false.
static bool try_load_intrinsics(mat3 *intrinsic_matrix) {
char *value;
const int result =
read_db_value("/data/params", "CloudCalibration", &value, NULL);
const int result = read_db_value(NULL, "CloudCalibration", &value, NULL);
if (result == 0) {
JsonNode* calibration_json = json_decode(value);
@@ -309,18 +317,14 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
assert(num_back_fds == UI_BUF_COUNT);
assert(num_front_fds == UI_BUF_COUNT);
visionbufs_load(s->bufs, &back_bufs, num_back_fds, back_fds);
visionbufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds);
vipc_bufs_load(s->bufs, &back_bufs, num_back_fds, back_fds);
vipc_bufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds);
s->cur_vision_idx = -1;
s->cur_vision_front_idx = -1;
s->scene = (UIScene){
.frontview = 0,
.big_box_x = ui_info.big_box_x,
.big_box_y = ui_info.big_box_y,
.big_box_width = ui_info.big_box_width,
.big_box_height = ui_info.big_box_height,
.transformed_width = ui_info.transformed_width,
.transformed_height = ui_info.transformed_height,
.front_box_x = ui_info.front_box_x,
@@ -344,7 +348,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
}};
char *value;
const int result = read_db_value("/data/params", "IsMetric", &value, NULL);
const int result = read_db_value(NULL, "IsMetric", &value, NULL);
if (result == 0) {
s->is_metric = value[0] == '1';
free(value);
@@ -392,9 +396,8 @@ static void ui_draw_transformed_box(UIState *s, uint32_t color) {
};
for (int i=0; i<ARRAYSIZE(verts); i++) {
verts[i].pos.v[0] = scene->big_box_x + verts[i].pos.v[0] / verts[i].pos.v[2];
verts[i].pos.v[1] = s->rgb_height - (scene->big_box_y +
verts[i].pos.v[1] / verts[i].pos.v[2]);
verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2];
verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2];
}
glUseProgram(s->line_program);
@@ -469,6 +472,51 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co
nvgRestore(s->vg);
}
static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, size_t num_points,
NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
// path coords are worked out in rgb-box space
nvgTranslate(s->vg, 240.0f, 0.0);
// zooom in 2x
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2);
nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
nvgBeginPath(s->vg);
nvgStrokeColor(s->vg, color);
nvgStrokeWidth(s->vg, 2);
bool started = false;
for (int i=0; i<num_points; i++) {
float px = x_coords[i];
float py = y_coords[i];
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
}
nvgStroke(s->vg);
nvgRestore(s->vg);
}
static void draw_path(UIState *s, const float *points, float off,
NVGcolor color) {
const UIScene *scene = &s->scene;
@@ -669,6 +717,8 @@ static void ui_draw_world(UIState *s) {
draw_model_path(
s, scene->model.right_lane,
nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128));
draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255));
}
if (scene->lead_status) {
@@ -939,7 +989,7 @@ static void ui_update(UIState *s) {
// poll for events
while (true) {
zmq_pollitem_t polls[5] = {{0}};
zmq_pollitem_t polls[6] = {{0}};
polls[0].socket = s->live100_sock_raw;
polls[0].events = ZMQ_POLLIN;
polls[1].socket = s->livecalibration_sock_raw;
@@ -948,15 +998,19 @@ static void ui_update(UIState *s) {
polls[2].events = ZMQ_POLLIN;
polls[3].socket = s->live20_sock_raw;
polls[3].events = ZMQ_POLLIN;
int num_polls = 4;
polls[4].socket = s->livempc_sock_raw;
polls[4].events = ZMQ_POLLIN;
int num_polls = 5;
if (s->vision_connected) {
assert(s->ipc_fd >= 0);
polls[4].fd = s->ipc_fd;
polls[4].events = ZMQ_POLLIN;
polls[5].fd = s->ipc_fd;
polls[5].events = ZMQ_POLLIN;
num_polls++;
}
int ret = zmq_poll(polls, num_polls, 0);
if (ret < 0) {
LOGW("poll failed (%d)", ret);
@@ -969,7 +1023,7 @@ static void ui_update(UIState *s) {
// awake on any activity
set_awake(s, true);
if (s->vision_connected && polls[4].revents) {
if (s->vision_connected && polls[5].revents) {
// vision ipc event
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
@@ -1021,7 +1075,7 @@ static void ui_update(UIState *s) {
} else {
// zmq messages
void* which = NULL;
for (int i=0; i<4; i++) {
for (int i=0; i<5; i++) {
if (polls[i].revents) {
which = polls[i].socket;
break;
@@ -1085,7 +1139,7 @@ static void ui_update(UIState *s) {
cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration);
// should we still even have this?
capn_list32 warpl = datad.warpMatrix;
capn_list32 warpl = datad.warpMatrix2;
capn_resolve(&warpl.p); // is this a bug?
for (int i = 0; i < 3 * 3; i++) {
s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i));
@@ -1100,6 +1154,23 @@ static void ui_update(UIState *s) {
} else if (eventd.which == cereal_Event_model) {
s->scene.model_ts = eventd.logMonoTime;
s->scene.model = read_model(eventd.model);
} else if (eventd.which == cereal_Event_liveMpc) {
struct cereal_LiveMpcData datad;
cereal_read_LiveMpcData(&datad, eventd.liveMpc);
capn_list32 x_list = datad.x;
capn_resolve(&x_list.p);
for (int i = 0; i < 50; i++){
s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i));
}
capn_list32 y_list = datad.y;
capn_resolve(&y_list.p);
for (int i = 0; i < 50; i++){
s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i));
}
}
capn_free(&ctx);
selfdrive/visiond/visiond
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e2dc36a454c0447387826dd0de64ee47e170012df0ec041189ae77d64aa6de3f
size 16407976
oid sha256:a5078532dfc830e278a83b885c19f8e7a2a7073ea3f3bb389f2323702f40cb6b
size 13285152