Vehicle Researcher
8 years ago
parent
e2acd7bd05
commit
46e0ee7750
67 changed files with 1942 additions and 773 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:027af259a0b06dc7a719241a74ed9d2d63c13beb49905db1008a908afdbdb1a3 |
||||
size 8993895 |
||||
oid sha256:31f6d7340f0beed300d7ba8789dcfed8d8fd70aeb815bd430f031587c026b9b2 |
||||
size 9479160 |
||||
|
||||
@ -1 +1 @@ |
||||
Subproject commit d584cdd46ca398aace9f1a7ee6c967a8c7602e8e |
||||
Subproject commit b77861eb00d45e25af501f78bbed155d2df06159 |
||||
@ -0,0 +1 @@ |
||||
Subproject commit e0738376db27d208603d7e63dd465e003ca06325 |
||||
@ -0,0 +1 @@ |
||||
boardd |
||||
@ -0,0 +1,72 @@ |
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
PHONELIBS := ../../phonelibs
|
||||
|
||||
UNAME_S := $(shell uname -s)
|
||||
UNAME_M := $(shell uname -m)
|
||||
|
||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||
-Werror=incompatible-pointer-types \
|
||||
-Werror=int-conversion \
|
||||
-Werror=return-type \
|
||||
-Werror=format-extra-args \
|
||||
-Wno-deprecated-declarations
|
||||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
|
||||
ifeq ($(UNAME_S),Darwin) |
||||
CEREAL_LIBS := -L /usr/local/lib -lkj -lcapnp
|
||||
ZMQ_LIBS = -L/usr/local/lib -lzmq
|
||||
else ifeq ($(OPTEST),1) |
||||
ZMQ_LIBS = -lzmq
|
||||
CEREAL_LIBS = -lcapnp -lkj
|
||||
else ifeq ($(UNAME_M),x86_64) |
||||
EXTERNAL := ../../external
|
||||
ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
|
||||
ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a
|
||||
CEREAL_CXXFLAGS := -I$(EXTERNAL)/capnp/include
|
||||
CEREAL_LIBS := -L$(EXTERNAL)/capnp/lib -l:libcapnp.a -l:libkj.a
|
||||
else ifeq ($(UNAME_M),aarch64) |
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
|
||||
endif |
||||
|
||||
OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH')
|
||||
|
||||
DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc)
|
||||
DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES))
|
||||
|
||||
CWD := $(shell pwd)
|
||||
|
||||
.PHONY: all |
||||
all: libdbc.so |
||||
|
||||
ifeq ($(UNAME_M),aarch64) |
||||
include ../common/cereal.mk |
||||
endif |
||||
|
||||
# make sure cereal is built
|
||||
libdbc.so:: ../../cereal/gen/cpp/log.capnp.h |
||||
|
||||
../../cereal/gen/cpp/log.capnp.h: |
||||
cd ../../cereal && make
|
||||
|
||||
libdbc.so:: parser.cc $(DBC_CCS) |
||||
$(CXX) -fPIC -shared -o '$@' $^ \
|
||||
-I. \
|
||||
-I../.. \
|
||||
$(CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(CEREAL_LIBS)
|
||||
|
||||
dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc |
||||
PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
|
||||
|
||||
.PHONY: clean |
||||
clean: |
||||
rm -rf libdbc.so*
|
||||
rm -f dbc_out/*.cc
|
||||
@ -0,0 +1,2 @@ |
||||
*.cc |
||||
|
||||
@ -0,0 +1,51 @@ |
||||
#include <cstdint> |
||||
|
||||
#include "parser_common.h" |
||||
|
||||
namespace { |
||||
|
||||
{% for address, msg_name, sigs in msgs %} |
||||
const Signal sigs_{{address}}[] = { |
||||
{% for sig in sigs %} |
||||
{ |
||||
{% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %}
|
||||
.name = "{{sig.name}}", |
||||
.b1 = {{b1}}, |
||||
.b2 = {{sig.size}}, |
||||
.bo = {{64 - (b1 + sig.size)}}, |
||||
.is_signed = {{"true" if sig.is_signed else "false"}}, |
||||
.factor = {{sig.factor}}, |
||||
.offset = {{sig.offset}}, |
||||
{% if checksum_type == "honda" and sig.name == "CHECKSUM" %} |
||||
.type = SignalType::HONDA_CHECKSUM, |
||||
{% elif checksum_type == "honda" and sig.name == "COUNTER" %} |
||||
.type = SignalType::HONDA_COUNTER, |
||||
{% else %} |
||||
.type = SignalType::DEFAULT, |
||||
{% endif %} |
||||
}, |
||||
{% endfor %} |
||||
}; |
||||
{% endfor %} |
||||
|
||||
const Msg msgs[] = { |
||||
{% for address, msg_name, sigs in msgs %} |
||||
{% set address_hex = "0x%X" % address %} |
||||
{ |
||||
.name = "{{msg_name}}", |
||||
.address = {{address_hex}}, |
||||
.num_sigs = ARRAYSIZE(sigs_{{address}}), |
||||
.sigs = sigs_{{address}}, |
||||
}, |
||||
{% endfor %} |
||||
}; |
||||
|
||||
} |
||||
|
||||
const DBC {{dbc.name}} = { |
||||
.name = "{{dbc.name}}", |
||||
.num_msgs = ARRAYSIZE(msgs), |
||||
.msgs = msgs, |
||||
}; |
||||
|
||||
dbc_init({{dbc.name}}) |
||||
@ -0,0 +1,441 @@ |
||||
#include <cstdio> |
||||
#include <cstdint> |
||||
#include <cassert> |
||||
#include <cstring> |
||||
|
||||
#include <unistd.h> |
||||
#include <fcntl.h> |
||||
#include <sys/stat.h> |
||||
#include <sys/mman.h> |
||||
|
||||
#include <string> |
||||
#include <vector> |
||||
#include <algorithm> |
||||
#include <unordered_map> |
||||
|
||||
#include <zmq.h> |
||||
|
||||
#include <capnp/serialize.h> |
||||
#include "cereal/gen/cpp/log.capnp.h" |
||||
|
||||
#include "parser_common.h" |
||||
|
||||
#define DEBUG(...) |
||||
// #define DEBUG printf
|
||||
#define INFO printf |
||||
|
||||
|
||||
#define MAX_BAD_COUNTER 5 |
||||
|
||||
namespace { |
||||
|
||||
uint64_t read_u64_be(const uint8_t* v) { |
||||
return (((uint64_t)v[0] << 56) |
||||
| ((uint64_t)v[1] << 48) |
||||
| ((uint64_t)v[2] << 40) |
||||
| ((uint64_t)v[3] << 32) |
||||
| ((uint64_t)v[4] << 24) |
||||
| ((uint64_t)v[5] << 16) |
||||
| ((uint64_t)v[6] << 8) |
||||
| (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; |
||||
|
||||
DEBUG("check checksum %16lx %d", d, l); |
||||
|
||||
// remove checksum from calculation
|
||||
d &= ~(0xFLL << l); |
||||
|
||||
int s = 0; |
||||
while (address > 0) { s += (address & 0xF); address >>= 4; } |
||||
while (d > 0) { s += (d & 0xF); d >>= 4; } |
||||
s = 8-s; |
||||
s &= 0xF; |
||||
|
||||
DEBUG(" %d = %d\n", target, s); |
||||
return target == s; |
||||
} |
||||
|
||||
struct MessageState { |
||||
uint32_t address; |
||||
|
||||
std::vector<Signal> parse_sigs; |
||||
std::vector<double> vals; |
||||
|
||||
uint64_t seen; |
||||
uint64_t check_threshold; |
||||
|
||||
uint8_t counter; |
||||
uint8_t counter_fail; |
||||
|
||||
bool parse(uint64_t sec, uint64_t dat) { |
||||
for (int i=0; i < parse_sigs.size(); i++) { |
||||
auto& sig = parse_sigs[i]; |
||||
|
||||
int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1); |
||||
if (sig.is_signed) { |
||||
tmp -= (tmp >> (sig.b2-1)) ? (1<<sig.b2) : 0; //signed
|
||||
} |
||||
|
||||
DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp); |
||||
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) { |
||||
if (!honda_checksum(address, dat, sig.bo)) { |
||||
INFO("%X CHECKSUM FAIL\n", address); |
||||
return false; |
||||
} |
||||
} else if (sig.type == SignalType::HONDA_COUNTER) { |
||||
if (!honda_update_counter(tmp)) { |
||||
return false; |
||||
} |
||||
} |
||||
|
||||
vals[i] = tmp * sig.factor + sig.offset; |
||||
} |
||||
seen = sec; |
||||
|
||||
return true; |
||||
} |
||||
|
||||
|
||||
bool honda_update_counter(int64_t v) { |
||||
uint8_t old_counter = counter; |
||||
counter = v; |
||||
if (((old_counter+1) & 3) != v) { |
||||
counter_fail += 1; |
||||
if (counter_fail > 1) { |
||||
INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v); |
||||
} |
||||
if (counter_fail >= MAX_BAD_COUNTER) { |
||||
return false; |
||||
} |
||||
} else if (counter_fail > 0) { |
||||
counter_fail--; |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
}; |
||||
|
||||
class CANParser { |
||||
public: |
||||
CANParser(int abus, const std::string& dbc_name, |
||||
const std::vector<MessageParseOptions> &options, |
||||
const std::vector<SignalParseOptions> &sigoptions) |
||||
: bus(abus) { |
||||
// connect to can on 8006
|
||||
context = zmq_ctx_new(); |
||||
subscriber = zmq_socket(context, ZMQ_SUB); |
||||
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; |
||||
} |
||||
} |
||||
assert(dbc); |
||||
|
||||
for (auto &op : options) { |
||||
MessageState state = { |
||||
.address = op.address, |
||||
// .check_frequency = op.check_frequency,
|
||||
}; |
||||
|
||||
// msg is not valid if a message isn't received for 10 consecutive steps
|
||||
if (op.check_frequency > 0) { |
||||
state.check_threshold = (1000000000ULL / op.check_frequency) * 10; |
||||
} |
||||
|
||||
|
||||
const Msg* msg = NULL; |
||||
for (int i=0; i<dbc->num_msgs; i++) { |
||||
if (dbc->msgs[i].address == op.address) { |
||||
msg = &dbc->msgs[i]; |
||||
break; |
||||
} |
||||
} |
||||
if (!msg) { |
||||
fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str()); |
||||
assert(false); |
||||
} |
||||
|
||||
// track checksums and for this message
|
||||
for (int i=0; i<msg->num_sigs; i++) { |
||||
const Signal *sig = &msg->sigs[i]; |
||||
if (sig->type == HONDA_CHECKSUM |
||||
|| sig->type == HONDA_COUNTER) { |
||||
state.parse_sigs.push_back(*sig); |
||||
state.vals.push_back(0); |
||||
} |
||||
} |
||||
|
||||
// track requested signals for this message
|
||||
for (auto &sigop : sigoptions) { |
||||
if (sigop.address != op.address) continue; |
||||
|
||||
for (int i=0; i<msg->num_sigs; i++) { |
||||
const Signal *sig = &msg->sigs[i]; |
||||
if (strcmp(sig->name, sigop.name) == 0 |
||||
&& sig->type != HONDA_CHECKSUM |
||||
&& sig->type != HONDA_COUNTER) { |
||||
state.parse_sigs.push_back(*sig); |
||||
state.vals.push_back(sigop.default_value); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
} |
||||
|
||||
message_states[state.address] = state; |
||||
} |
||||
} |
||||
|
||||
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) { |
||||
int msg_count = cans.size(); |
||||
|
||||
DEBUG("got %d messages\n", msg_count); |
||||
|
||||
// parse the messages
|
||||
for (int i = 0; i < msg_count; i++) { |
||||
auto cmsg = cans[i]; |
||||
if (cmsg.getSrc() != bus) { |
||||
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
|
||||
continue; |
||||
} |
||||
auto state_it = message_states.find(cmsg.getAddress()); |
||||
if (state_it == message_states.end()) { |
||||
// DEBUG("skip %d: not specified\n", cmsg.getAddress());
|
||||
continue; |
||||
} |
||||
|
||||
if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
|
||||
uint8_t dat[8] = {0}; |
||||
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); |
||||
|
||||
uint64_t p = read_u64_be(dat); |
||||
|
||||
DEBUG(" proc %X: %lx\n", cmsg.getAddress(), p); |
||||
|
||||
state_it->second.parse(sec, p); |
||||
} |
||||
} |
||||
|
||||
void UpdateValid(uint64_t sec) { |
||||
can_valid = true; |
||||
for (auto &kv : message_states) { |
||||
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); |
||||
} |
||||
can_valid = false; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void update(uint64_t sec, bool wait) { |
||||
int err; |
||||
|
||||
// recv from can
|
||||
zmq_msg_t msg; |
||||
zmq_msg_init(&msg); |
||||
|
||||
// multiple recv is fine
|
||||
bool first = wait; |
||||
while (1) { |
||||
if (first) { |
||||
err = zmq_msg_recv(&msg, subscriber, 0); |
||||
first = false; |
||||
} else { |
||||
err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT); |
||||
} |
||||
if (err < 0) break; |
||||
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
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)); |
||||
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(amsg); |
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||
|
||||
auto cans = event.getCan(); |
||||
|
||||
UpdateCans(sec, cans); |
||||
} |
||||
|
||||
UpdateValid(sec); |
||||
|
||||
} |
||||
|
||||
std::vector<SignalValue> query(uint64_t sec) { |
||||
std::vector<SignalValue> ret; |
||||
|
||||
for (auto &kv : message_states) { |
||||
auto &state = kv.second; |
||||
if (sec != 0 && state.seen != sec) continue; |
||||
|
||||
for (int i=0; i<state.parse_sigs.size(); i++) { |
||||
const Signal &sig = state.parse_sigs[i]; |
||||
ret.push_back((SignalValue){ |
||||
.address = state.address, |
||||
.name = sig.name, |
||||
.value = state.vals[i], |
||||
}); |
||||
} |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
bool can_valid = false; |
||||
|
||||
private: |
||||
const int bus; |
||||
// zmq vars
|
||||
void *context = NULL; |
||||
void *subscriber = NULL; |
||||
|
||||
const DBC *dbc = NULL; |
||||
std::unordered_map<uint32_t, MessageState> message_states; |
||||
}; |
||||
|
||||
} |
||||
|
||||
void dbc_register(const DBC* dbc) { |
||||
g_dbc.push_back(dbc); |
||||
} |
||||
|
||||
extern "C" { |
||||
|
||||
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) { |
||||
CANParser* ret = new CANParser(bus, std::string(dbc_name), |
||||
(message_options ? std::vector<MessageParseOptions>(message_options, message_options+num_message_options) |
||||
: std::vector<MessageParseOptions>{}), |
||||
(signal_options ? std::vector<SignalParseOptions>(signal_options, signal_options+num_signal_options) |
||||
: std::vector<SignalParseOptions>{})); |
||||
return (void*)ret; |
||||
} |
||||
|
||||
void can_update(void* can, uint64_t sec, bool wait) { |
||||
CANParser* cp = (CANParser*)can; |
||||
cp->update(sec, wait); |
||||
} |
||||
|
||||
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) { |
||||
CANParser* cp = (CANParser*)can; |
||||
|
||||
if (out_can_valid) { |
||||
*out_can_valid = cp->can_valid; |
||||
} |
||||
|
||||
const std::vector<SignalValue> values = cp->query(sec); |
||||
if (out_values) { |
||||
std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values); |
||||
} |
||||
return values.size(); |
||||
}; |
||||
|
||||
} |
||||
|
||||
#ifdef TEST |
||||
|
||||
int main(int argc, char** argv) { |
||||
CANParser cp(0, "honda_civic_touring_2016_can", |
||||
std::vector<MessageParseOptions>{ |
||||
// address, check_frequency
|
||||
{0x14a, 100}, |
||||
{0x158, 100}, |
||||
{0x17c, 100}, |
||||
{0x191, 100}, |
||||
{0x1a4, 50}, |
||||
{0x326, 10}, |
||||
{0x1b0, 50}, |
||||
{0x1d0, 50}, |
||||
{0x305, 10}, |
||||
{0x324, 10}, |
||||
{0x405, 3}, |
||||
{0x18f, 0}, |
||||
{0x130, 0}, |
||||
{0x296, 0}, |
||||
{0x30c, 0}, |
||||
}, |
||||
std::vector<SignalParseOptions>{ |
||||
// sig_name, sig_address, default
|
||||
{0x158, "XMISSION_SPEED", 0}, |
||||
{0x1d0, "WHEEL_SPEED_FL", 0}, |
||||
{0x1d0, "WHEEL_SPEED_FR", 0}, |
||||
{0x1d0, "WHEEL_SPEED_RL", 0}, |
||||
{0x14a, "STEER_ANGLE", 0}, |
||||
{0x18f, "STEER_TORQUE_SENSOR", 0}, |
||||
{0x191, "GEAR", 0}, |
||||
{0x1b0, "WHEELS_MOVING", 1}, |
||||
{0x405, "DOOR_OPEN_FL", 1}, |
||||
{0x405, "DOOR_OPEN_FR", 1}, |
||||
{0x405, "DOOR_OPEN_RL", 1}, |
||||
{0x405, "DOOR_OPEN_RR", 1}, |
||||
{0x324, "CRUISE_SPEED_PCM", 0}, |
||||
{0x305, "SEATBELT_DRIVER_LAMP", 1}, |
||||
{0x305, "SEATBELT_DRIVER_LATCHED", 0}, |
||||
{0x17c, "BRAKE_PRESSED", 0}, |
||||
{0x130, "CAR_GAS", 0}, |
||||
{0x296, "CRUISE_BUTTONS", 0}, |
||||
{0x1a4, "ESP_DISABLED", 1}, |
||||
{0x30c, "HUD_LEAD", 0}, |
||||
{0x1a4, "USER_BRAKE", 0}, |
||||
{0x18f, "STEER_STATUS", 5}, |
||||
{0x1d0, "WHEEL_SPEED_RR", 0}, |
||||
{0x1b0, "BRAKE_ERROR_1", 1}, |
||||
{0x1b0, "BRAKE_ERROR_2", 1}, |
||||
{0x191, "GEAR_SHIFTER", 0}, |
||||
{0x326, "MAIN_ON", 0}, |
||||
{0x17c, "ACC_STATUS", 0}, |
||||
{0x17c, "PEDAL_GAS", 0}, |
||||
{0x296, "CRUISE_SETTING", 0}, |
||||
{0x326, "LEFT_BLINKER", 0}, |
||||
{0x326, "RIGHT_BLINKER", 0}, |
||||
{0x324, "COUNTER", 0}, |
||||
{0x17c, "ENGINE_RPM", 0}, |
||||
}); |
||||
|
||||
|
||||
|
||||
const std::string log_fn = "dats.bin"; |
||||
|
||||
int log_fd = open(log_fn.c_str(), O_RDONLY, 0); |
||||
assert(log_fd >= 0); |
||||
|
||||
off_t log_size = lseek(log_fd, 0, SEEK_END); |
||||
lseek(log_fd, 0, SEEK_SET); |
||||
|
||||
void* log_data = mmap(NULL, log_size, PROT_READ, MAP_PRIVATE, log_fd, 0); |
||||
assert(log_data); |
||||
|
||||
auto words = kj::arrayPtr((const capnp::word*)log_data, log_size/sizeof(capnp::word)); |
||||
while (words.size() > 0) { |
||||
capnp::FlatArrayMessageReader reader(words); |
||||
|
||||
auto evt = reader.getRoot<cereal::Event>(); |
||||
auto cans = evt.getCan(); |
||||
|
||||
cp.UpdateCans(0, cans); |
||||
|
||||
words = kj::arrayPtr(reader.getEnd(), words.end()); |
||||
} |
||||
|
||||
munmap(log_data, log_size); |
||||
|
||||
close(log_fd); |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
#endif |
||||
@ -0,0 +1,166 @@ |
||||
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; |
||||
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) |
||||
|
||||
class CANParser(object): |
||||
def __init__(self, dbc_name, signals, checks=[], bus=0): |
||||
self.can_valid = True |
||||
self.vl = defaultdict(dict) |
||||
|
||||
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals) |
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [ |
||||
{ |
||||
'address': sig_address, |
||||
'name': sig_names[sig_name], |
||||
'default_value': sig_default, |
||||
} for sig_name, sig_address, sig_default in signals]) |
||||
|
||||
message_options = dict((address, 0) for _, address, _ in signals) |
||||
message_options.update(dict(checks)) |
||||
|
||||
message_options_c = ffi.new("MessageParseOptions[]", [ |
||||
{ |
||||
'address': address, |
||||
'check_frequency': freq, |
||||
} for address, freq in message_options.iteritems()]) |
||||
|
||||
self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, |
||||
len(signal_options_c), signal_options_c) |
||||
|
||||
self.p_can_valid = ffi.new("bool*") |
||||
|
||||
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL) |
||||
self.can_values = ffi.new("SignalValue[%d]" % value_count) |
||||
self.update_vl(0) |
||||
# print "===" |
||||
|
||||
def update_vl(self, sec): |
||||
|
||||
can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values) |
||||
assert can_values_len <= len(self.can_values) |
||||
|
||||
self.can_valid = self.p_can_valid[0] |
||||
|
||||
# print can_values_len |
||||
ret = set() |
||||
for i in xrange(can_values_len): |
||||
cv = self.can_values[i] |
||||
address = cv.address |
||||
# print hex(cv.address), ffi.string(cv.name) |
||||
self.vl[address][ffi.string(cv.name)] = cv.value |
||||
ret.add(address) |
||||
return ret |
||||
|
||||
def update(self, sec, wait): |
||||
libdbc.can_update(self.can, sec, wait) |
||||
return self.update_vl(sec) |
||||
|
||||
if __name__ == "__main__": |
||||
from common.realtime import sec_since_boot |
||||
|
||||
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) |
||||
|
||||
# cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1) |
||||
|
||||
signals = [ |
||||
("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default |
||||
("WHEEL_SPEED_FL", 0x1d0, 0), |
||||
("WHEEL_SPEED_FR", 0x1d0, 0), |
||||
("WHEEL_SPEED_RL", 0x1d0, 0), |
||||
("STEER_ANGLE", 0x14a, 0), |
||||
("STEER_TORQUE_SENSOR", 0x18f, 0), |
||||
("GEAR", 0x191, 0), |
||||
("WHEELS_MOVING", 0x1b0, 1), |
||||
("DOOR_OPEN_FL", 0x405, 1), |
||||
("DOOR_OPEN_FR", 0x405, 1), |
||||
("DOOR_OPEN_RL", 0x405, 1), |
||||
("DOOR_OPEN_RR", 0x405, 1), |
||||
("CRUISE_SPEED_PCM", 0x324, 0), |
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1), |
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0), |
||||
("BRAKE_PRESSED", 0x17c, 0), |
||||
("CAR_GAS", 0x130, 0), |
||||
("CRUISE_BUTTONS", 0x296, 0), |
||||
("ESP_DISABLED", 0x1a4, 1), |
||||
("HUD_LEAD", 0x30c, 0), |
||||
("USER_BRAKE", 0x1a4, 0), |
||||
("STEER_STATUS", 0x18f, 5), |
||||
("WHEEL_SPEED_RR", 0x1d0, 0), |
||||
("BRAKE_ERROR_1", 0x1b0, 1), |
||||
("BRAKE_ERROR_2", 0x1b0, 1), |
||||
("GEAR_SHIFTER", 0x191, 0), |
||||
("MAIN_ON", 0x326, 0), |
||||
("ACC_STATUS", 0x17c, 0), |
||||
("PEDAL_GAS", 0x17c, 0), |
||||
("CRUISE_SETTING", 0x296, 0), |
||||
("LEFT_BLINKER", 0x326, 0), |
||||
("RIGHT_BLINKER", 0x326, 0), |
||||
("COUNTER", 0x324, 0), |
||||
("ENGINE_RPM", 0x17C, 0) |
||||
] |
||||
checks = [ |
||||
(0x14a, 100), # address, frequency |
||||
(0x158, 100), |
||||
(0x17c, 100), |
||||
(0x191, 100), |
||||
(0x1a4, 50), |
||||
(0x326, 10), |
||||
(0x1b0, 50), |
||||
(0x1d0, 50), |
||||
(0x305, 10), |
||||
(0x324, 10), |
||||
(0x405, 3), |
||||
] |
||||
|
||||
cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0) |
||||
print cp.vl |
||||
|
||||
while True: |
||||
cp.update(int(sec_since_boot()*1e9), True) |
||||
print cp.vl |
||||
print cp.can_valid |
||||
time.sleep(0.01) |
||||
@ -0,0 +1,63 @@ |
||||
#ifndef PARSER_COMMON_H |
||||
#define PARSER_COMMON_H |
||||
|
||||
#include <cstddef> |
||||
#include <cstdint> |
||||
|
||||
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) |
||||
|
||||
|
||||
|
||||
struct SignalParseOptions { |
||||
uint32_t address; |
||||
const char* name; |
||||
double default_value; |
||||
}; |
||||
|
||||
struct MessageParseOptions { |
||||
uint32_t address; |
||||
int check_frequency; |
||||
}; |
||||
|
||||
struct SignalValue { |
||||
uint32_t address; |
||||
const char* name; |
||||
double value; |
||||
}; |
||||
|
||||
|
||||
enum SignalType { |
||||
DEFAULT, |
||||
HONDA_CHECKSUM, |
||||
HONDA_COUNTER, |
||||
}; |
||||
|
||||
struct Signal { |
||||
const char* name; |
||||
int b1, b2, bo; |
||||
bool is_signed; |
||||
double factor, offset; |
||||
SignalType type; |
||||
}; |
||||
|
||||
struct Msg { |
||||
const char* name; |
||||
uint32_t address; |
||||
size_t num_sigs; |
||||
const Signal *sigs; |
||||
}; |
||||
|
||||
struct DBC { |
||||
const char* name; |
||||
size_t num_msgs; |
||||
const Msg *msgs; |
||||
}; |
||||
|
||||
void dbc_register(const DBC* dbc); |
||||
|
||||
#define dbc_init(dbc) \ |
||||
static void __attribute__((constructor)) do_dbc_init_ ## dbc(void) { \
|
||||
dbc_register(&dbc); \
|
||||
} |
||||
|
||||
#endif |
||||
@ -0,0 +1,32 @@ |
||||
#!/usr/bin/env python |
||||
import os |
||||
import sys |
||||
|
||||
import jinja2 |
||||
|
||||
import opendbc |
||||
from common.dbc import dbc |
||||
|
||||
if len(sys.argv) != 3: |
||||
print "usage: %s dbc_path struct_path" % (sys.argv[0],) |
||||
sys.exit(0) |
||||
|
||||
dbc_fn = sys.argv[1] |
||||
out_fn = sys.argv[2] |
||||
|
||||
template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") |
||||
|
||||
can_dbc = dbc(dbc_fn) |
||||
|
||||
with open(template_fn, "r") as template_f: |
||||
template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) |
||||
|
||||
msgs = [(address, msg_name, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first |
||||
for address, ((msg_name, _), msg_sigs) in sorted(can_dbc.msgs.iteritems()) if msg_sigs] |
||||
|
||||
checksum_type = "honda" if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura") else None |
||||
|
||||
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len) |
||||
|
||||
with open(out_fn, "w") as out_f: |
||||
out_f.write(parser_code) |
||||
@ -0,0 +1,30 @@ |
||||
from common.fingerprints import fingerprint |
||||
|
||||
from .honda.interface import CarInterface as HondaInterface |
||||
|
||||
try: |
||||
from .simulator.interface import CarInterface as SimInterface |
||||
except ImportError: |
||||
SimInterface = None |
||||
|
||||
try: |
||||
from .simulator2.interface import CarInterface as Sim2Interface |
||||
except ImportError: |
||||
Sim2Interface = None |
||||
|
||||
interfaces = { |
||||
"HONDA CIVIC 2016 TOURING": HondaInterface, |
||||
"ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface, |
||||
"HONDA ACCORD 2016 TOURING": HondaInterface, |
||||
"HONDA CR-V 2016 TOURING": HondaInterface, |
||||
|
||||
"simulator": SimInterface, |
||||
"simulator2": Sim2Interface |
||||
} |
||||
|
||||
def get_car(logcan, sendcan=None): |
||||
candidate, fingerprints = fingerprint(logcan) |
||||
interface_cls = interfaces[candidate] |
||||
params = interface_cls.get_params(candidate, fingerprints) |
||||
|
||||
return interface_cls(params, logcan, sendcan), params |
||||
@ -1 +1 @@ |
||||
#define OPENPILOT_VERSION "0.3.3" |
||||
#define OPENPILOT_VERSION "0.3.4" |
||||
|
||||
@ -0,0 +1,66 @@ |
||||
import numpy as np |
||||
from common.realtime import sec_since_boot |
||||
|
||||
#Time to collisions greater than 5s are iognored |
||||
MAX_TTC = 5. |
||||
|
||||
def calc_ttc(l1): |
||||
# if l1 is None, return max ttc immediately |
||||
if not l1: |
||||
return MAX_TTC |
||||
# this function returns the time to collision (ttc), assuming that |
||||
# ARel will stay constant TODO: review this assumptions |
||||
# change sign to rel quantities as it's going to be easier for calculations |
||||
vRel = -l1.vRel |
||||
aRel = -l1.aRel |
||||
|
||||
# assuming that closing gap ARel comes from lead vehicle decel, |
||||
# then limit ARel so that v_lead will get to zero in no sooner than t_decel. |
||||
# This helps underweighting ARel when v_lead is close to zero. |
||||
t_decel = 2. |
||||
aRel = np.minimum(aRel, l1.vLead/t_decel) |
||||
|
||||
# delta of the quadratic equation to solve for ttc |
||||
delta = vRel**2 + 2 * l1.dRel * aRel |
||||
|
||||
# assign an arbitrary high ttc value if there is no solution to ttc |
||||
if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1): |
||||
ttc = MAX_TTC |
||||
else: |
||||
ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC) |
||||
return ttc |
||||
|
||||
class ForwardCollisionWarning(object): |
||||
def __init__(self, dt): |
||||
self.last_active = 0. |
||||
self.violation_time = 0. |
||||
self.active = False |
||||
self.dt = dt # time step |
||||
|
||||
def process(self, CS, AC): |
||||
# send an fcw alert if the violation time > violation_thrs |
||||
violation_thrs = 0.3 # fcw turns on after a continuous violation for this time |
||||
fcw_t_delta = 5. # no more than one fcw alert within this time |
||||
a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw |
||||
a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw |
||||
ttc_thrs = 2.5 # ttc threshold for fcw |
||||
v_fcw_min = 2. # no fcw below 2m/s |
||||
steer_angle_th = 40. # deg, no fcw above this steer angle |
||||
cur_time = sec_since_boot() |
||||
|
||||
ttc = calc_ttc(AC.l1) |
||||
a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off |
||||
|
||||
# increase violation time if we want to decelerate quite fast |
||||
if AC.l1 and ( \ |
||||
(CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \ |
||||
and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \ |
||||
and AC.l1.fcw): |
||||
self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs) |
||||
else: |
||||
self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0) |
||||
|
||||
# fire FCW |
||||
self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta) |
||||
if self.active: |
||||
self.last_active = cur_time |
||||
@ -0,0 +1,84 @@ |
||||
#!/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 |
||||
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 |
||||
|
||||
if os.getenv("OPT") is not None: |
||||
self.PP = OptPathPlanner(self.model) |
||||
else: |
||||
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 |
||||
l20 = messaging.recv_sock(self.live20) |
||||
if l20 is not None: |
||||
self.last_l20_ts = l20.logMonoTime |
||||
|
||||
self.PP.update(cur_time, 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) |
||||
|
||||
# **** send the plan **** |
||||
plan_send = messaging.new_message() |
||||
plan_send.init('plan') |
||||
|
||||
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) |
||||
|
||||
# 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 |
||||
|
||||
# compute risk of collision events: fcw |
||||
self.FCW.process(CS, self.AC) |
||||
plan_send.plan.fcw = bool(self.FCW.active) |
||||
|
||||
self.plan.send(plan_send.to_bytes()) |
||||
return plan_send |
||||
@ -1,81 +0,0 @@ |
||||
#!/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, set_realtime_priority |
||||
from common.params import Params |
||||
|
||||
from selfdrive.swaglog import cloudlog |
||||
from cereal import car |
||||
|
||||
from selfdrive.controls.lib.pathplanner import PathPlanner |
||||
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise |
||||
|
||||
def plannerd_thread(gctx): |
||||
context = zmq.Context() |
||||
poller = zmq.Poller() |
||||
|
||||
carstate = messaging.sub_sock(context, service_list['carState'].port, poller) |
||||
live20 = messaging.sub_sock(context, service_list['live20'].port) |
||||
model = messaging.sub_sock(context, service_list['model'].port) |
||||
|
||||
plan = messaging.pub_sock(context, service_list['plan'].port) |
||||
|
||||
# wait for stats about the car to come in from controls |
||||
cloudlog.info("plannerd is waiting for CarParams") |
||||
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) |
||||
cloudlog.info("plannerd got CarParams") |
||||
|
||||
CS = None |
||||
PP = PathPlanner(model) |
||||
AC = AdaptiveCruise(live20) |
||||
|
||||
# start the loop |
||||
set_realtime_priority(2) |
||||
|
||||
# this runs whenever we get a packet that can change the plan |
||||
while True: |
||||
polld = poller.poll(timeout=1000) |
||||
for sock, mode in polld: |
||||
if mode != zmq.POLLIN or sock != carstate: |
||||
continue |
||||
|
||||
cur_time = sec_since_boot() |
||||
CS = messaging.recv_sock(carstate).carState |
||||
|
||||
PP.update(cur_time, CS.vEgo) |
||||
|
||||
# LoC.v_pid -> CS.vEgo |
||||
# TODO: is this change okay? |
||||
AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP) |
||||
|
||||
# **** send the plan **** |
||||
plan_send = messaging.new_message() |
||||
plan_send.init('plan') |
||||
|
||||
# lateral plan |
||||
plan_send.plan.lateralValid = not PP.dead |
||||
if plan_send.plan.lateralValid: |
||||
plan_send.plan.dPoly = map(float, PP.d_poly) |
||||
|
||||
# longitudal plan |
||||
plan_send.plan.longitudinalValid = not AC.dead |
||||
if plan_send.plan.longitudinalValid: |
||||
plan_send.plan.vTarget = float(AC.v_target_lead) |
||||
plan_send.plan.aTargetMin = float(AC.a_target[0]) |
||||
plan_send.plan.aTargetMax = float(AC.a_target[1]) |
||||
plan_send.plan.jerkFactor = float(AC.jerk_factor) |
||||
plan_send.plan.hasLead = AC.has_lead |
||||
|
||||
plan.send(plan_send.to_bytes()) |
||||
|
||||
|
||||
def main(gctx=None): |
||||
plannerd_thread(gctx) |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:11408b2637323c15dfccdb0b6059cb45f9073ac700ab1efe233f43ccb61184a7 |
||||
size 1357296 |
||||
oid sha256:706438daafdc304a4e31539dd6bd01f2ea6b349990756ff80d79728ae5a849a6 |
||||
size 1365184 |
||||
|
||||
@ -0,0 +1 @@ |
||||
sensord |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:925d26ccc75ec4d7051b68fe98a929536c5224693f4e375fe20fb604299bc079 |
||||
oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb |
||||
size 918928 |
||||
|
||||
@ -1,122 +0,0 @@ |
||||
#!/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() |
||||
|
||||
|
||||
@ -1,12 +1,14 @@ |
||||
#!/bin/bash |
||||
|
||||
export OPTEST=1 |
||||
export OLD_CAN=1 |
||||
|
||||
pushd ../../controls |
||||
./controlsd.py & |
||||
pid1=$! |
||||
./radard.py & |
||||
pid2=$! |
||||
./plannerd.py & |
||||
pid3=$! |
||||
trap "trap - SIGTERM && kill $pid1 && kill $pid2 && kill $pid3" SIGINT SIGTERM EXIT |
||||
trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT |
||||
popd |
||||
mkdir -p out |
||||
MPLBACKEND=svg ./runtracks.py out |
||||
|
||||
@ -0,0 +1 @@ |
||||
ui |
||||
@ -0,0 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:295a6226b3db60adfae573bcaf5760c0bed0d21b9942f5e6a06831a177b20c38 |
||||
size 205056 |
||||
@ -0,0 +1,77 @@ |
||||
#include <stdio.h> |
||||
#include <stdlib.h> |
||||
#include <stdbool.h> |
||||
#include <math.h> |
||||
#include <unistd.h> |
||||
#include <assert.h> |
||||
|
||||
#include <GLES3/gl3.h> |
||||
#include <EGL/eglext.h> |
||||
|
||||
#include "nanovg.h" |
||||
#define NANOVG_GLES3_IMPLEMENTATION |
||||
#include "nanovg_gl.h" |
||||
#include "nanovg_gl_utils.h" |
||||
|
||||
#include "common/framebuffer.h" |
||||
|
||||
|
||||
int main(int argc, char** argv) { |
||||
int err; |
||||
|
||||
const char* spintext = NULL; |
||||
if (argc >= 2) { |
||||
spintext = argv[1]; |
||||
} |
||||
|
||||
// spinner
|
||||
int fb_w, fb_h; |
||||
EGLDisplay display; |
||||
EGLSurface surface; |
||||
FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false, |
||||
&display, &surface, &fb_w, &fb_h); |
||||
assert(fb); |
||||
|
||||
NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES); |
||||
assert(vg); |
||||
|
||||
int font = nvgCreateFont(vg, "Bold", "../../assets/courbd.ttf"); |
||||
assert(font >= 0); |
||||
|
||||
for (int cnt = 0; ; cnt++) { |
||||
glClearColor(0.1, 0.1, 0.1, 1.0); |
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); |
||||
|
||||
glEnable(GL_BLEND); |
||||
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); |
||||
nvgBeginFrame(vg, fb_w, fb_h, 1.0f); |
||||
|
||||
|
||||
for (int k=0; k<3; k++) { |
||||
float ang = (2*M_PI * (float)cnt / 120.0) + (k / 3.0) * 2*M_PI; |
||||
|
||||
nvgBeginPath(vg); |
||||
nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 255)); |
||||
nvgStrokeWidth(vg, 5); |
||||
|
||||
nvgMoveTo(vg, fb_w/2 + 50 * cosf(ang), fb_h/2 + 50 * sinf(ang)); |
||||
nvgLineTo(vg, fb_w/2 + 15 * cosf(ang), fb_h/2 + 15 * sinf(ang)); |
||||
nvgMoveTo(vg, fb_w/2 - 15 * cosf(ang), fb_h/2 - 15 * sinf(ang)); |
||||
nvgLineTo(vg, fb_w/2 - 50 * cosf(ang), fb_h/2 - 50 * sinf(ang)); |
||||
nvgStroke(vg); |
||||
} |
||||
|
||||
if (spintext) { |
||||
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); |
||||
nvgFontSize(vg, 96.0f); |
||||
nvgText(vg, fb_w / 2, fb_h*2/3, spintext, NULL);
|
||||
} |
||||
|
||||
nvgEndFrame(vg); |
||||
|
||||
eglSwapBuffers(display, surface); |
||||
assert(glGetError() == GL_NO_ERROR); |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:d4b8e99120b0e6f61b56e514e8b607ef797a6046c1cc2fffafe30c9231a65671 |
||||
size 16370760 |
||||
oid sha256:61b61563109853f59a5764cc520fef8d9f0d8d2a845ebbcf9381b6a64761f655 |
||||
size 16399784 |
||||
|
||||
Loading…
Reference in new issue