openpilot v0.4.3 release
parent
19010d3766
commit
9a411ebf32
|
@ -0,0 +1,18 @@
|
|||
# How to contribute
|
||||
|
||||
Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use.
|
||||
|
||||
Most open source development activity is coordinated through our [slack](https://slack.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/)
|
||||
|
||||
## Getting Started
|
||||
|
||||
* Join our slack [slack.comma.ai](https://slack.comma.ai)
|
||||
* Make sure you have a [GitHub account](https://github.com/signup/free)
|
||||
* Fork the repository on GitHub
|
||||
|
||||
## Car Ports (openpilot)
|
||||
|
||||
We've released a guide for porting to Toyota cars [here](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6)
|
||||
|
||||
If you port openpilot to a substantially new car, you might be eligible for a bounty. See our bounties at [comma.ai/bounties.html](https://comma.ai/bounties.html)
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
Copyright (c) 2016, Comma.ai, Inc.
|
||||
Copyright (c) 2018, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
Welcome to chffrplus
|
||||
======
|
||||
|
||||
[chffrplus](https://github.com/commaai/chffrplus) is an open source dashcam.
|
||||
|
||||
This is the shipping reference software for the comma EON Dashcam DevKit. It keeps many of the niceities of [openpilot](https://github.com/commaai/openpilot), like high quality sensors, great camera, and good autostart and stop. Though unlike openpilot, it cannot control your car. chffrplus can interface with your car through a [panda](https://shop.comma.ai/products/panda-obd-ii-dongle), but just like our dashcam app [chffr](https://getchffr.com/), it is read only.
|
||||
|
||||
It integrates with the rest of the comma ecosystem, so you can view your drives on the [chffr](https://getchffr.com/) app for Android or iOS, and reverse engineer your car with [cabana](https://community.comma.ai/cabana/?demo=1).
|
||||
|
||||
|
||||
Hardware
|
||||
------
|
||||
|
||||
Right now chffrplus supports the [EON Dashcam DevKit](https://shop.comma.ai/products/eon-dashcam-devkit) for hardware to run on.
|
||||
|
||||
Install chffrplus on a EON device by entering ``https://chffrplus.comma.ai`` during NEOS setup.
|
||||
|
||||
|
||||
User Data / chffr Account / Crash Reporting
|
||||
------
|
||||
|
||||
By default chffrplus creates an account and includes a client for chffr, our dashcam app.
|
||||
|
||||
It's open source software, so you are free to disable it if you wish.
|
||||
|
||||
It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
|
||||
It does not log the user facing camera or the microphone.
|
||||
|
||||
By using it, you agree to [our privacy policy](https://beta.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data.
|
||||
|
||||
|
||||
Licensing
|
||||
------
|
||||
|
||||
chffrplus is released under the MIT license.
|
||||
|
12
RELEASES.md
12
RELEASES.md
|
@ -1,3 +1,15 @@
|
|||
Version 0.4.3 (2018-03-13)
|
||||
==========================
|
||||
* Add HDR and autofocus
|
||||
* Update UI aesthetic
|
||||
* Grey panda works in Waze
|
||||
* Add alpha support for 2017 Honda Pilot
|
||||
* Slight increase in acceleration response from stop
|
||||
* Switch CAN sending to use CANPacker
|
||||
* Fix pulsing acceleration regression on Honda
|
||||
* Fix openpilot bugs when stock system is in use
|
||||
* Change starting logic for chffrplus to use battery voltage
|
||||
|
||||
Version 0.4.2 (2018-02-05)
|
||||
==========================
|
||||
* Add alpha support for 2017 Lexus RX Hybrid
|
||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -24,7 +24,7 @@ APKS = {
|
|||
'src': 'https://apkcache.s3.amazonaws.com/com.waze_1021278.apk',
|
||||
'src_sha256': 'f00957e93e2389f9e30502ac54994b98ac769314b0963c263d4e8baa625ab0c2',
|
||||
'patch': 'com.waze.apkpatch',
|
||||
'out_sha256': '9ec8b0ea3c78c666342865b1bfb66e368a3f5c911df2ad12835206ec8b19f444'
|
||||
'out_sha256': 'fee880a91a44c738442cd05fd1b6d9b5817cbf755aa61c86325ada2bc443d5cf'
|
||||
},
|
||||
'com.spotify.music': {
|
||||
'src': 'https://apkcache.s3.amazonaws.com/com.spotify.music_24382006.apk',
|
||||
|
|
|
@ -207,6 +207,7 @@ struct CarControl {
|
|||
brake @1: Float32;
|
||||
# range from -1.0 - 1.0
|
||||
steer @2: Float32;
|
||||
steerAngle @3: Float32;
|
||||
}
|
||||
|
||||
struct CruiseControl {
|
||||
|
|
131
cereal/log.capnp
131
cereal/log.capnp
|
@ -117,6 +117,8 @@ struct FrameData {
|
|||
frameLength @3 :Int32;
|
||||
integLines @4 :Int32;
|
||||
globalGain @5 :Int32;
|
||||
lensPos @11 :Int32;
|
||||
lensSag @12 :Float32;
|
||||
image @6 :Data;
|
||||
|
||||
frameType @7 :FrameType;
|
||||
|
@ -515,6 +517,7 @@ struct Plan {
|
|||
aCruise @17 :Float32;
|
||||
vTarget @3 :Float32;
|
||||
vTargetFuture @14 :Float32;
|
||||
vMax @20 :Float32;
|
||||
aTargetMinDEPRECATED @4 :Float32;
|
||||
aTargetMaxDEPRECATED @5 :Float32;
|
||||
aTarget @18 :Float32;
|
||||
|
@ -582,6 +585,8 @@ struct LiveLocationData {
|
|||
|
||||
positionECEF @18 :List(Float64);
|
||||
poseQuatECEF @19 :List(Float32);
|
||||
pitchCalibration @20 :Float32;
|
||||
yawCalibration @21 :Float32;
|
||||
|
||||
struct Accuracy {
|
||||
pNEDError @0 :List(Float32);
|
||||
|
@ -599,6 +604,7 @@ struct LiveLocationData {
|
|||
kalman @1;
|
||||
orbslam @2;
|
||||
timing @3;
|
||||
dummy @4;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1283,6 +1289,11 @@ struct UbloxGnss {
|
|||
fitInterval @35 :Float64;
|
||||
|
||||
toc @36 :Float64;
|
||||
|
||||
ionoCoeffsValid @37 :Bool;
|
||||
ionoAlpha @38 :List(Float64);
|
||||
ionoBeta @39 :List(Float64);
|
||||
|
||||
}
|
||||
|
||||
struct IonoData {
|
||||
|
@ -1341,7 +1352,8 @@ struct GPSPlannerPoints {
|
|||
points @1 :List(ECEFPoint);
|
||||
valid @2 :Bool;
|
||||
trackName @3 :Text;
|
||||
instructionProgress @4 :Float32;
|
||||
speedLimit @4 :Float32;
|
||||
accelTarget @5 :Float32;
|
||||
}
|
||||
|
||||
struct GPSPlannerPlan {
|
||||
|
@ -1349,22 +1361,29 @@ struct GPSPlannerPlan {
|
|||
poly @1 :List(Float32);
|
||||
trackName @2 :Text;
|
||||
speed @3 :Float32;
|
||||
acceleration @4 :Float32;
|
||||
points @5 :List(ECEFPoint);
|
||||
}
|
||||
|
||||
struct TrafficSigns {
|
||||
struct TrafficEvent @0xacfa74a094e62626 {
|
||||
type @0 :Type;
|
||||
distance @1 :Float32;
|
||||
action @2 :Action;
|
||||
resuming @3 :Bool;
|
||||
|
||||
enum Type {
|
||||
light @0;
|
||||
stopSign @0;
|
||||
lightRed @1;
|
||||
lightYellow @2;
|
||||
lightGreen @3;
|
||||
stopLight @4;
|
||||
}
|
||||
|
||||
enum Action {
|
||||
none @0;
|
||||
yield @1;
|
||||
stop @2;
|
||||
resumeReady @3;
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1378,6 +1397,97 @@ struct OrbslamCorrection {
|
|||
numInliers @5 :UInt32;
|
||||
}
|
||||
|
||||
struct OrbObservation {
|
||||
observationMonoTime @0 :UInt64;
|
||||
normalizedCoordinates @1 :List(Float32);
|
||||
locationECEF @2 :List(Float64);
|
||||
matchDistance @3: UInt32;
|
||||
}
|
||||
|
||||
struct UiNavigationEvent {
|
||||
type @0: Type;
|
||||
status @1: Status;
|
||||
distanceTo @2: Float32;
|
||||
endRoadPoint @3: ECEFPoint;
|
||||
|
||||
enum Type {
|
||||
none @0;
|
||||
laneChangeLeft @1;
|
||||
laneChangeRight @2;
|
||||
mergeLeft @3;
|
||||
mergeRight @4;
|
||||
turnLeft @5;
|
||||
turnRight @6;
|
||||
}
|
||||
|
||||
enum Status {
|
||||
none @0;
|
||||
passive @1;
|
||||
approaching @2;
|
||||
active @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct UiLayoutState {
|
||||
activeApp @0 :App;
|
||||
sidebarCollapsed @1 :Bool;
|
||||
mapEnabled @2 :Bool;
|
||||
|
||||
enum App {
|
||||
home @0;
|
||||
music @1;
|
||||
nav @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct Joystick {
|
||||
# convenient for debug and live tuning
|
||||
axes @0: List(Float32);
|
||||
buttons @1: List(Bool);
|
||||
}
|
||||
|
||||
struct OrbOdometry {
|
||||
# timing first
|
||||
startMonoTime @0 :UInt64;
|
||||
endMonoTime @1 :UInt64;
|
||||
|
||||
# fundamental matrix and error
|
||||
f @2: List(Float64);
|
||||
err @3: Float64;
|
||||
|
||||
# number of inlier points
|
||||
inliers @4: Int32;
|
||||
|
||||
# for debug only
|
||||
# indexed by endMonoTime features
|
||||
# value is startMonoTime feature match
|
||||
# -1 if no match
|
||||
matches @5: List(Int16);
|
||||
}
|
||||
|
||||
struct OrbFeatures {
|
||||
timestampEof @0 :UInt64;
|
||||
# transposed arrays of normalized image coordinates
|
||||
# len(xs) == len(ys) == len(descriptors) * 32
|
||||
xs @1 :List(Float32);
|
||||
ys @2 :List(Float32);
|
||||
descriptors @3 :Data;
|
||||
octaves @4 :List(Int8);
|
||||
}
|
||||
|
||||
struct OrbKeyFrame {
|
||||
# this is a globally unique id for the KeyFrame
|
||||
id @0: UInt64;
|
||||
|
||||
# this is the location of the KeyFrame
|
||||
pos @1: ECEFPoint;
|
||||
|
||||
# these are the features in the world
|
||||
# len(dpos) == len(descriptors) * 32
|
||||
dpos @2 :List(ECEFPoint);
|
||||
descriptors @3 :Data;
|
||||
}
|
||||
|
||||
struct Event {
|
||||
# in nanoseconds?
|
||||
logMonoTime @0 :UInt64;
|
||||
|
@ -1425,9 +1535,20 @@ struct Event {
|
|||
gpsPlannerPoints @40 :GPSPlannerPoints;
|
||||
gpsPlannerPlan @41 :GPSPlannerPlan;
|
||||
applanixRaw @42 :Data;
|
||||
trafficSigns @43 :List(TrafficSigns);
|
||||
trafficEvents @43 :List(TrafficEvent);
|
||||
liveLocationTiming @44 :LiveLocationData;
|
||||
orbslamCorrection @45 :OrbslamCorrection;
|
||||
orbslamCorrectionDEPRECATED @45 :OrbslamCorrection;
|
||||
liveLocationCorrected @46 :LiveLocationData;
|
||||
orbObservation @47 :List(OrbObservation);
|
||||
gpsLocationExternal @48 :GpsLocationData;
|
||||
location @49 :LiveLocationData;
|
||||
uiNavigationEvent @50 :UiNavigationEvent;
|
||||
liveLocationKalman @51 :LiveLocationData;
|
||||
testJoystick @52 :Joystick;
|
||||
orbOdometry @53 :OrbOdometry;
|
||||
orbFeatures @54 :OrbFeatures;
|
||||
applanixLocation @55 :LiveLocationData;
|
||||
orbKeyFrame @56 :OrbKeyFrame;
|
||||
uiLayoutState @57 :UiLayoutState;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -14,43 +14,43 @@ class TOYOTA:
|
|||
COROLLA = "TOYOTA COROLLA 2017"
|
||||
LEXUS_RXH = "LEXUS RX HYBRID 2017"
|
||||
|
||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||
|
||||
_FINGERPRINTS = {
|
||||
HONDA.ACURA_ILX: {
|
||||
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5,
|
||||
HONDA.ACURA_ILX: [{
|
||||
1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5,
|
||||
# sent messages
|
||||
0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5,
|
||||
},
|
||||
HONDA.ACURA_RDX: {
|
||||
0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5,
|
||||
}],
|
||||
HONDA.ACURA_RDX: [{
|
||||
57L: 3, 145L: 8, 229L: 4, 308L: 5, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 398L: 3, 399L: 6, 404L: 4, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 892L: 8, 923L: 2, 929L: 4, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1034L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1365L: 5, 1424L: 5, 1729L: 1
|
||||
},
|
||||
HONDA.CIVIC: {
|
||||
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5,
|
||||
}],
|
||||
HONDA.CIVIC: [{
|
||||
1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5,
|
||||
# sent messages
|
||||
0xe4: 5, 0x1fa: 8, 0x200: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
|
||||
},
|
||||
HONDA.CRV: {
|
||||
0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
|
||||
}],
|
||||
HONDA.CRV: [{
|
||||
57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8,
|
||||
# sent messages
|
||||
0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5,
|
||||
},
|
||||
}],
|
||||
HONDA.ODYSSEY: [{
|
||||
57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5
|
||||
},
|
||||
# Odyssey Elite
|
||||
{
|
||||
57L: 3, 148L: 8, 228L: 5, 229L: 4, 304L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 440L: 8, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5
|
||||
}
|
||||
],
|
||||
HONDA.PILOT: {
|
||||
}],
|
||||
HONDA.PILOT: [{
|
||||
1600L: 5, 1027L: 5, 1668L: 5, 1029L: 8, 1601L: 8, 777L: 8, 891L: 8, 1036L: 8, 399L: 7, 1424L: 5, 145L: 8, 660L: 8, 985L: 3, 1616L: 5, 538L: 3, 795L: 8, 542L: 7, 773L: 7, 800L: 8, 545L: 5, 546L: 3, 419L: 8, 420L: 8, 422L: 8, 1064L: 7, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 819L: 7, 308L: 5, 821L: 5, 57L: 3, 965L: 8, 316L: 8, 829L: 5, 1088L: 8, 1089L: 8, 963L: 8, 837L: 5, 966L: 8, 929L: 8, 780L: 8, 923L: 2, 1613L: 5, 334L: 8, 463L: 8, 464L: 8, 1618L: 5, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 856L: 7, 804L: 8, 1612L: 5, 476L: 4, 1125L: 8, 344L: 8, 1296L: 8, 379L: 8, 228L: 5, 229L: 4, 871L: 8, 892L: 8, 490L: 8, 808L: 8, 882L: 2, 884L: 7, 967L: 8, 506L: 8, 507L: 1, 380L: 8,
|
||||
},
|
||||
TOYOTA.RAV4: {
|
||||
}],
|
||||
TOYOTA.RAV4: [{
|
||||
36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||
},
|
||||
TOYOTA.RAV4H: {
|
||||
}],
|
||||
TOYOTA.RAV4H: [{
|
||||
36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||
},
|
||||
}],
|
||||
TOYOTA.PRIUS: [{
|
||||
36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||
},
|
||||
|
@ -61,14 +61,13 @@ _FINGERPRINTS = {
|
|||
# Taiwanese Prius Prime
|
||||
{
|
||||
36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 845L: 5, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1264L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||
}
|
||||
],
|
||||
TOYOTA.COROLLA: {
|
||||
}],
|
||||
TOYOTA.COROLLA: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
TOYOTA.LEXUS_RXH: {
|
||||
}],
|
||||
TOYOTA.LEXUS_RXH: [{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
|
||||
},
|
||||
}],
|
||||
}
|
||||
|
||||
# support additional internal only fingerprints
|
||||
|
@ -97,10 +96,10 @@ def eliminate_incompatible_cars(msg, candidate_cars):
|
|||
compatible_cars = []
|
||||
for car_name in candidate_cars:
|
||||
car_fingerprints = _FINGERPRINTS[car_name]
|
||||
if not isinstance(car_fingerprints, list):
|
||||
car_fingerprints = [car_fingerprints]
|
||||
|
||||
for fingerprint in car_fingerprints:
|
||||
fingerprint.update(_DEBUG_ADDRESS) # add alien debug address
|
||||
|
||||
if is_valid_for_fingerprint(msg, fingerprint):
|
||||
compatible_cars.append(car_name)
|
||||
break
|
||||
|
|
|
@ -6,7 +6,7 @@ class Profiler(object):
|
|||
self.cp = {}
|
||||
self.cp_ignored = []
|
||||
self.iter = 0
|
||||
self.start_time = time.clock()
|
||||
self.start_time = time.time()
|
||||
self.last_time = self.start_time
|
||||
self.tot = 0.
|
||||
|
||||
|
@ -15,14 +15,14 @@ class Profiler(object):
|
|||
self.cp = {}
|
||||
self.cp_ignored = []
|
||||
self.iter = 0
|
||||
self.start_time = time.clock()
|
||||
self.start_time = time.time()
|
||||
self.last_time = self.start_time
|
||||
|
||||
def checkpoint(self, name, ignore=False):
|
||||
# ignore flag needed when benchmarking threads with ratekeeper
|
||||
if not self.enabled:
|
||||
return
|
||||
tt = time.clock()
|
||||
tt = time.time()
|
||||
if name not in self.cp:
|
||||
self.cp[name] = 0.
|
||||
if ignore:
|
||||
|
@ -37,11 +37,10 @@ class Profiler(object):
|
|||
return
|
||||
self.iter += 1
|
||||
print "******* Profiling *******"
|
||||
for n in self.cp:
|
||||
ms = self.cp[n]
|
||||
for n, ms in sorted(self.cp.items(), key=lambda x: -x[1]):
|
||||
if n in self.cp_ignored:
|
||||
print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED"
|
||||
print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED"
|
||||
else:
|
||||
print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100)
|
||||
print "%30s: %7.2f percent: %3.0f" % (n, ms*1000.0, ms/self.tot*100)
|
||||
print "Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)
|
||||
|
||||
|
|
|
@ -0,0 +1,104 @@
|
|||
import numpy as np
|
||||
"""
|
||||
Coordinate transformation module. All methods accept arrays as input
|
||||
with each row as a position.
|
||||
"""
|
||||
|
||||
|
||||
|
||||
a = 6378137
|
||||
b = 6356752.3142
|
||||
esq = 6.69437999014 * 0.001
|
||||
e1sq = 6.73949674228 * 0.001
|
||||
|
||||
|
||||
def geodetic2ecef(geodetic):
|
||||
geodetic = np.array(geodetic)
|
||||
input_shape = geodetic.shape
|
||||
geodetic = np.atleast_2d(geodetic)
|
||||
lat = (np.pi/180)*geodetic[:,0]
|
||||
lon = (np.pi/180)*geodetic[:,1]
|
||||
alt = geodetic[:,2]
|
||||
|
||||
xi = np.sqrt(1 - esq * np.sin(lat)**2)
|
||||
x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
|
||||
y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
|
||||
z = (a / xi * (1 - esq) + alt) * np.sin(lat)
|
||||
ecef = np.array([x, y, z]).T
|
||||
return ecef.reshape(input_shape)
|
||||
|
||||
|
||||
def ecef2geodetic(ecef):
|
||||
"""
|
||||
Convert ECEF coordinates to geodetic using ferrari's method
|
||||
"""
|
||||
def ferrari(x, y, z):
|
||||
# ferrari's method
|
||||
r = np.sqrt(x * x + y * y)
|
||||
Esq = a * a - b * b
|
||||
F = 54 * b * b * z * z
|
||||
G = r * r + (1 - esq) * z * z - esq * Esq
|
||||
C = (esq * esq * F * r * r) / (pow(G, 3))
|
||||
S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C))
|
||||
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
|
||||
Q = np.sqrt(1 + 2 * esq * esq * P)
|
||||
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
|
||||
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
|
||||
U = np.sqrt(pow((r - esq * r_0), 2) + z * z)
|
||||
V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
|
||||
Z_0 = b * b * z / (a * V)
|
||||
h = U * (1 - b * b / (a * V))
|
||||
lat = (180/np.pi)*np.arctan((z + e1sq * Z_0) / r)
|
||||
lon = (180/np.pi)*np.arctan2(y, x)
|
||||
return lat, lon, h
|
||||
|
||||
geodetic = []
|
||||
ecef = np.array(ecef)
|
||||
input_shape = ecef.shape
|
||||
ecef = np.atleast_2d(ecef)
|
||||
for p in ecef:
|
||||
geodetic.append(ferrari(*p))
|
||||
geodetic = np.array(geodetic)
|
||||
return geodetic.reshape(input_shape)
|
||||
|
||||
|
||||
|
||||
class LocalCoord(object):
|
||||
"""
|
||||
Allows conversions to local frames. In this case NED.
|
||||
That is: North East Down from the start position in
|
||||
meters.
|
||||
"""
|
||||
def __init__(self, init_geodetic, init_ecef):
|
||||
self.init_ecef = init_ecef
|
||||
lat, lon, _ = (np.pi/180)*init_geodetic
|
||||
self.ned2ecef_matrix = np.array([[-np.sin(lat)*np.cos(lon), -np.sin(lon), -np.cos(lat)*np.cos(lon)],
|
||||
[-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)],
|
||||
[np.cos(lat), 0, -np.sin(lat)]])
|
||||
self.ecef2ned_matrix = self.ned2ecef_matrix.T
|
||||
|
||||
@classmethod
|
||||
def from_geodetic(self, init_geodetic):
|
||||
init_ecef = geodetic2ecef(init_geodetic)
|
||||
return LocalCoord(init_geodetic, init_ecef)
|
||||
|
||||
@classmethod
|
||||
def from_ecef(self, init_ecef):
|
||||
init_geodetic = ecef2geodetic(init_ecef)
|
||||
return LocalCoord(init_geodetic, init_ecef)
|
||||
|
||||
|
||||
def ecef2ned(self, ecef):
|
||||
return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T
|
||||
|
||||
def ned2ecef(self, ned):
|
||||
# Transpose so that init_ecef will broadcast correctly for 1d or 2d ned.
|
||||
return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef)
|
||||
|
||||
def geodetic2ned(self, geodetic):
|
||||
ecef = geodetic2ecef(geodetic)
|
||||
return self.ecef2ned(ecef)
|
||||
|
||||
def ned2geodetic(self, ned):
|
||||
ecef = self.ned2ecef(ned)
|
||||
return ecef2geodetic(ecef)
|
|
@ -0,0 +1,44 @@
|
|||
#!/usr/bin/bash
|
||||
|
||||
if [ -z "$PASSIVE" ]; then
|
||||
export PASSIVE="1"
|
||||
fi
|
||||
|
||||
function launch {
|
||||
# apply update
|
||||
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
|
||||
git reset --hard @{u} &&
|
||||
git clean -xdf &&
|
||||
exec "${BASH_SOURCE[0]}"
|
||||
fi
|
||||
|
||||
# no cpu rationing for now
|
||||
echo 0-3 > /dev/cpuset/background/cpus
|
||||
echo 0-3 > /dev/cpuset/system-background/cpus
|
||||
echo 0-3 > /dev/cpuset/foreground/boost/cpus
|
||||
echo 0-3 > /dev/cpuset/foreground/cpus
|
||||
echo 0-3 > /dev/cpuset/android/cpus
|
||||
|
||||
# check if NEOS update is required
|
||||
while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do
|
||||
# wait for network
|
||||
(cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$!
|
||||
until ping -W 1 -c 1 8.8.8.8; do sleep 1; done
|
||||
kill $spin_pid
|
||||
|
||||
# update NEOS
|
||||
curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater
|
||||
sleep 10
|
||||
done
|
||||
|
||||
export PYTHONPATH="$PWD"
|
||||
|
||||
# start manager
|
||||
cd selfdrive
|
||||
./manager.py
|
||||
|
||||
# if broken, keep on screen error
|
||||
while true; do sleep 1; done
|
||||
}
|
||||
|
||||
launch
|
|
@ -1,39 +1,5 @@
|
|||
#!/usr/bin/bash
|
||||
|
||||
function launch {
|
||||
# apply update
|
||||
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
|
||||
git reset --hard @{u} &&
|
||||
git clean -xdf &&
|
||||
exec "${BASH_SOURCE[0]}"
|
||||
fi
|
||||
export PASSIVE="0"
|
||||
exec ./launch_chffrplus.sh
|
||||
|
||||
# no cpu rationing for now
|
||||
echo 0-3 > /dev/cpuset/background/cpus
|
||||
echo 0-3 > /dev/cpuset/system-background/cpus
|
||||
echo 0-3 > /dev/cpuset/foreground/boost/cpus
|
||||
echo 0-3 > /dev/cpuset/foreground/cpus
|
||||
echo 0-3 > /dev/cpuset/android/cpus
|
||||
|
||||
# wait for network
|
||||
(cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$!
|
||||
until ping -W 1 -c 1 8.8.8.8; do sleep 1; done
|
||||
kill $spin_pid
|
||||
|
||||
# check if NEOS update is required
|
||||
while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do
|
||||
curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater
|
||||
sleep 10
|
||||
done
|
||||
|
||||
export PYTHONPATH="$PWD"
|
||||
|
||||
# start manager
|
||||
cd selfdrive
|
||||
./manager.py
|
||||
|
||||
# if broken, keep on screen error
|
||||
while true; do sleep 1; done
|
||||
}
|
||||
|
||||
launch
|
||||
|
|
|
@ -14,4 +14,5 @@ enum34==1.1.1
|
|||
sympy==1.1.1
|
||||
filterpy==1.0.0
|
||||
smbus2==0.2.0
|
||||
pyflakes==1.5.0
|
||||
-e git+https://github.com/commaai/le_python.git#egg=Logentries
|
||||
|
|
Binary file not shown.
Binary file not shown.
After Width: | Height: | Size: 16 KiB |
Binary file not shown.
After Width: | Height: | Size: 51 KiB |
Binary file not shown.
After Width: | Height: | Size: 41 KiB |
Binary file not shown.
After Width: | Height: | Size: 52 KiB |
Binary file not shown.
After Width: | Height: | Size: 53 KiB |
Binary file not shown.
After Width: | Height: | Size: 26 KiB |
|
@ -525,10 +525,25 @@ void pigeon_init() {
|
|||
pigeon_send("\xB5\x62\x06\x1E\x00\x00\x24\x72");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
|
||||
|
||||
LOGW("grey panda is ready to fly");
|
||||
}
|
||||
|
||||
static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto ublox_raw = event.initUbloxRaw(alen);
|
||||
memcpy(ublox_raw.begin(), dat, alen);
|
||||
|
||||
// send to ubloxRaw
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
|
||||
}
|
||||
|
||||
|
||||
void *pigeon_thread(void *crap) {
|
||||
// ubloxRaw = 8042
|
||||
|
@ -543,22 +558,6 @@ void *pigeon_thread(void *crap) {
|
|||
if (pigeon_needs_init) {
|
||||
pigeon_needs_init = false;
|
||||
pigeon_init();
|
||||
} else {
|
||||
// send periodic messages
|
||||
if (cnt%3000 == 0) {
|
||||
for (unsigned char sv = 1; sv < 33; ++sv){
|
||||
const unsigned char buffer[5] = {0x0B, 0x31, 0x01, 0x00, sv};
|
||||
unsigned char CK_A = 0;
|
||||
unsigned char CK_B = 0;
|
||||
for(int i=0;i<5;i++) {
|
||||
CK_A = CK_A + buffer[i];
|
||||
CK_B = CK_B + CK_A;
|
||||
}
|
||||
const unsigned char msg[9] = {0xB5, 0x62, 0x0B, 0x31, 0x01, 0x00, sv, CK_A, CK_B};
|
||||
_pigeon_send((const char *)msg, 9);
|
||||
}
|
||||
pigeon_send("\xB5\x62\x0b\x02\x00\x00\x0d\x32");
|
||||
}
|
||||
}
|
||||
int alen = 0;
|
||||
while (alen < 0xfc0) {
|
||||
|
@ -572,17 +571,7 @@ void *pigeon_thread(void *crap) {
|
|||
alen += len;
|
||||
}
|
||||
if (alen > 0) {
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto ublox_raw = event.initUbloxRaw(alen);
|
||||
memcpy(ublox_raw.begin(), dat, alen);
|
||||
|
||||
// send to ubloxRaw
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
|
||||
pigeon_publish_raw(publisher, dat, alen);
|
||||
}
|
||||
|
||||
// 10ms
|
||||
|
|
|
@ -101,7 +101,7 @@ def can_init():
|
|||
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
|
||||
handle = device.open()
|
||||
handle.claimInterface(0)
|
||||
handle.controlWrite(0x40, 0xdc, SAFETY_HONDA, 0, b'')
|
||||
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
|
||||
|
||||
if handle is None:
|
||||
print "CAN NOT FOUND"
|
||||
|
|
|
@ -1,12 +1,11 @@
|
|||
import os
|
||||
import sys
|
||||
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_call(["make"], stdout=sys.stderr, cwd=can_dir)
|
||||
subprocess.check_call(["make"], cwd=can_dir)
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef("""
|
||||
|
@ -67,7 +66,8 @@ typedef struct {
|
|||
|
||||
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, bool sendcan);
|
||||
size_t num_signal_options, const SignalParseOptions* signal_options, bool sendcan,
|
||||
const char* tcp_addr);
|
||||
|
||||
void can_update(void* can, uint64_t sec, bool wait);
|
||||
|
||||
|
|
|
@ -3,6 +3,7 @@ import numbers
|
|||
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
|
||||
class CANPacker(object):
|
||||
def __init__(self, dbc_name):
|
||||
self.packer = libdbc.canpack_init(dbc_name)
|
||||
|
@ -24,6 +25,9 @@ class CANPacker(object):
|
|||
# values: [(signal_name, signal_value)]
|
||||
|
||||
values_thing = []
|
||||
if isinstance(values, dict):
|
||||
values = values.items()
|
||||
|
||||
for name, value in values:
|
||||
if name not in self.sig_names:
|
||||
self.sig_names[name] = ffi.new("char[]", name)
|
||||
|
@ -42,7 +46,9 @@ class CANPacker(object):
|
|||
size = self.address_to_size[addr]
|
||||
else:
|
||||
addr, size = self.name_to_address_and_size[addr]
|
||||
r = struct.pack(">Q", self.pack(addr, values, counter))
|
||||
|
||||
val = self.pack(addr, values, counter)
|
||||
r = struct.pack(">Q", val)
|
||||
return addr, r[:size]
|
||||
|
||||
def make_can_msg(self, addr, bus, values, counter=-1):
|
||||
|
|
|
@ -142,18 +142,23 @@ class CANParser {
|
|||
CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions,
|
||||
bool sendcan)
|
||||
bool sendcan, const std::string& tcp_addr)
|
||||
: bus(abus) {
|
||||
// connect to can on 8006
|
||||
context = zmq_ctx_new();
|
||||
subscriber = zmq_socket(context, ZMQ_SUB);
|
||||
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
|
||||
|
||||
std::string tcp_addr_str;
|
||||
|
||||
if (sendcan) {
|
||||
zmq_connect(subscriber, "tcp://127.0.0.1:8017");
|
||||
tcp_addr_str = "tcp://" + tcp_addr + ":8017";
|
||||
} else {
|
||||
zmq_connect(subscriber, "tcp://127.0.0.1:8006");
|
||||
tcp_addr_str = "tcp://" + tcp_addr + ":8006";
|
||||
}
|
||||
const char *tcp_addr_char = tcp_addr_str.c_str();
|
||||
|
||||
zmq_connect(subscriber, tcp_addr_char);
|
||||
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
|
@ -331,12 +336,12 @@ 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,
|
||||
bool sendcan) {
|
||||
bool sendcan, const char* tcp_addr) {
|
||||
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>{}), sendcan);
|
||||
: std::vector<SignalParseOptions>{}), sendcan, std::string(tcp_addr));
|
||||
return (void*)ret;
|
||||
}
|
||||
|
||||
|
|
|
@ -5,11 +5,12 @@ import numbers
|
|||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
class CANParser(object):
|
||||
def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False):
|
||||
def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False, tcp_addr="127.0.0.1"):
|
||||
self.can_valid = True
|
||||
self.vl = defaultdict(dict)
|
||||
self.ts = defaultdict(dict)
|
||||
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.msg_name_to_addres = {}
|
||||
self.address_to_msg_name = {}
|
||||
|
@ -39,10 +40,6 @@ class CANParser(object):
|
|||
|
||||
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
|
||||
|
||||
# Set default values by name
|
||||
for sig_name, sig_address, sig_default in signals:
|
||||
self.vl[self.address_to_msg_name[sig_address]][sig_name] = sig_default
|
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [
|
||||
{
|
||||
'address': sig_address,
|
||||
|
@ -60,7 +57,7 @@ class CANParser(object):
|
|||
} for msg_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, sendcan)
|
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr)
|
||||
|
||||
self.p_can_valid = ffi.new("bool*")
|
||||
|
||||
|
|
|
@ -6,6 +6,7 @@ from common.numpy_fast import clip
|
|||
from . import hondacan
|
||||
from .values import AH
|
||||
from common.fingerprints import HONDA as CAR
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
|
||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
|
@ -52,16 +53,17 @@ def process_hud_alert(hud_alert):
|
|||
|
||||
|
||||
HUDData = namedtuple("HUDData",
|
||||
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
|
||||
"lanes", "beep", "X8", "chime", "acc_alert"])
|
||||
["pcm_accel", "v_cruise", "mini_car", "car", "X4",
|
||||
"lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
class CarController(object):
|
||||
def __init__(self, enable_camera=True):
|
||||
def __init__(self, dbc_name, enable_camera=True):
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.enable_camera = enable_camera
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, sendcan, enabled, CS, frame, actuators, \
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||
|
@ -85,36 +87,33 @@ class CarController(object):
|
|||
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
#TODO: use enum!!
|
||||
if hud_show_lanes:
|
||||
hud_lanes = 0x04
|
||||
hud_lanes = 1
|
||||
else:
|
||||
hud_lanes = 0x00
|
||||
hud_lanes = 0
|
||||
|
||||
# TODO: factor this out better
|
||||
if enabled:
|
||||
if hud_show_car:
|
||||
hud_car = 0xe0
|
||||
hud_car = 2
|
||||
else:
|
||||
hud_car = 0xd0
|
||||
hud_car = 1
|
||||
else:
|
||||
hud_car = 0xc0
|
||||
hud_car = 0
|
||||
|
||||
#print chime, alert_id, hud_alert
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
|
||||
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 0x01, hud_car,
|
||||
0xc1, 0x41, hud_lanes + steer_required,
|
||||
int(snd_beep), 0x48, (snd_chime << 5) + fcw_display, acc_alert)
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
|
||||
0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
|
||||
|
||||
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
|
||||
print "INVALID HUD", hud
|
||||
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
|
||||
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x40, 0, 0, 0, 0)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# *** compute control surfaces ***
|
||||
GAS_MAX = 1004
|
||||
BRAKE_MAX = 1024/4
|
||||
if CS.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.PILOT):
|
||||
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
|
||||
|
@ -123,10 +122,9 @@ class CarController(object):
|
|||
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
|
||||
else:
|
||||
STEER_MAX = 0xF00
|
||||
GAS_OFFSET = 328
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
|
||||
apply_gas = clip(actuators.gas, 0., 1.)
|
||||
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
||||
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||
|
||||
|
@ -139,24 +137,23 @@ class CarController(object):
|
|||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.extend(hondacan.create_steering_control(apply_steer, CS.CP.carFingerprint, idx))
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CS.CP.carFingerprint, idx))
|
||||
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
idx = (frame / 2) % 4
|
||||
can_sends.append(
|
||||
hondacan.create_brake_command(apply_brake, pcm_override,
|
||||
pcm_cancel_cmd, hud.chime, idx))
|
||||
hondacan.create_brake_command(self.packer, apply_brake, pcm_override,
|
||||
pcm_cancel_cmd, hud.chime, hud.fcw, idx))
|
||||
if not CS.brake_only:
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
|
||||
can_sends.append(hondacan.create_gas_command(gas_amount, idx))
|
||||
can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame/10) % 4
|
||||
can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.CP.carFingerprint, idx))
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx))
|
||||
|
||||
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
|
||||
if CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||
|
|
|
@ -8,22 +8,25 @@ import numpy as np
|
|||
|
||||
|
||||
def parse_gear_shifter(can_gear_shifter, car_fingerprint):
|
||||
# TODO: Use values from DBC to parse this field
|
||||
|
||||
# TODO: Use VAL from DBC to parse this field
|
||||
if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY):
|
||||
if can_gear_shifter == 0x1:
|
||||
return "park"
|
||||
elif can_gear_shifter == 0x2:
|
||||
return "reverse"
|
||||
|
||||
if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY):
|
||||
if can_gear_shifter == 0x3:
|
||||
elif can_gear_shifter == 0x3:
|
||||
return "neutral"
|
||||
elif can_gear_shifter == 0x4:
|
||||
return "drive"
|
||||
elif can_gear_shifter == 0xa:
|
||||
return "sport"
|
||||
|
||||
elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX):
|
||||
if can_gear_shifter == 0x4:
|
||||
if can_gear_shifter == 0x1:
|
||||
return "park"
|
||||
elif can_gear_shifter == 0x2:
|
||||
return "reverse"
|
||||
elif can_gear_shifter == 0x4:
|
||||
return "neutral"
|
||||
elif can_gear_shifter == 0x8:
|
||||
return "drive"
|
||||
|
@ -31,8 +34,8 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint):
|
|||
return "sport"
|
||||
elif can_gear_shifter == 0x20:
|
||||
return "low"
|
||||
|
||||
elif car_fingerprint in (CAR.PILOT):
|
||||
# TODO: neutral?
|
||||
if can_gear_shifter == 0x8:
|
||||
return "reverse"
|
||||
elif can_gear_shifter == 0x4:
|
||||
|
@ -215,7 +218,7 @@ class CarState(object):
|
|||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
|
||||
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
||||
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS + self.v_weight * self.v_wheel
|
||||
|
||||
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_x = np.matrix([[speed], [0.0]])
|
||||
|
|
|
@ -4,7 +4,6 @@ import common.numpy_fast as np
|
|||
from selfdrive.config import Conversions as CV
|
||||
from common.fingerprints import HONDA as CAR
|
||||
|
||||
|
||||
# *** Honda specific ***
|
||||
def can_cksum(mm):
|
||||
s = 0
|
||||
|
@ -29,56 +28,89 @@ def make_can_msg(addr, dat, idx, alt):
|
|||
return [addr, 0, dat, alt]
|
||||
|
||||
|
||||
def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
|
||||
def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chime, fcw, idx):
|
||||
"""Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
|
||||
pump_on = apply_brake > 0
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
|
||||
pcm_fault_cmd = False
|
||||
amount = struct.pack("!H", (apply_brake << 6) + pump_on)
|
||||
msg = amount + struct.pack("BBB", (pcm_override << 4) |
|
||||
(pcm_fault_cmd << 2) |
|
||||
(pcm_cancel_cmd << 1) | brake_rq, 0x80,
|
||||
brakelights << 7) + chr(chime) + "\x00"
|
||||
return make_can_msg(0x1fa, msg, idx, 0)
|
||||
|
||||
values = {
|
||||
"COMPUTER_BRAKE": apply_brake,
|
||||
"COMPUTER_BRAKE_REQUEST": pump_on,
|
||||
"CRUISE_OVERRIDE": pcm_override,
|
||||
"CRUISE_FAULT_CMD": pcm_fault_cmd,
|
||||
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
|
||||
"COMPUTER_BRAKE_REQUEST_2": brake_rq,
|
||||
"SET_ME_0X80": 0x80,
|
||||
"BRAKE_LIGHTS": brakelights,
|
||||
"CHIME": chime,
|
||||
"FCW": fcw << 1, # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work
|
||||
}
|
||||
return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
|
||||
|
||||
|
||||
def create_gas_command(gas_amount, idx):
|
||||
def create_gas_command(packer, gas_amount, idx):
|
||||
"""Creates a CAN message for the Honda DBC GAS_COMMAND."""
|
||||
msg = struct.pack("!H", gas_amount)
|
||||
return make_can_msg(0x200, msg, idx, 0)
|
||||
enable = gas_amount > 0.001
|
||||
|
||||
def create_steering_control(apply_steer, car_fingerprint, idx):
|
||||
values = {"ENABLE": enable}
|
||||
|
||||
if enable:
|
||||
values["GAS_COMMAND"] = gas_amount * 255.
|
||||
values["GAS_COMMAND2"] = gas_amount * 255.
|
||||
|
||||
return packer.make_can_msg("GAS_COMMAND", 0, values, idx)
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, car_fingerprint, idx):
|
||||
"""Creates a CAN message for the Honda DBC STEERING_CONTROL."""
|
||||
commands = []
|
||||
if car_fingerprint in (CAR.CRV, CAR.ACURA_RDX):
|
||||
msg_0x194 = struct.pack("!h", apply_steer << 4) + ("\x80" if apply_steer != 0 else "\x00")
|
||||
commands.append(make_can_msg(0x194, msg_0x194, idx, 0))
|
||||
else:
|
||||
msg_0xe4 = struct.pack("!h", apply_steer) + ("\x80\x00" if apply_steer != 0 else "\x00\x00")
|
||||
commands.append(make_can_msg(0xe4, msg_0xe4, idx, 0))
|
||||
return commands
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer,
|
||||
"STEER_TORQUE_REQUEST": apply_steer != 0,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_CONTROL", 0, values, idx)
|
||||
|
||||
|
||||
def create_ui_commands(pcm_speed, hud, car_fingerprint, idx):
|
||||
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
|
||||
"""Creates an iterable of CAN messages for the UIs."""
|
||||
commands = []
|
||||
pcm_speed_real = np.clip(int(round(pcm_speed / 0.002759506)), 0,
|
||||
64000) # conversion factor from dbc file
|
||||
msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
|
||||
hud.v_cruise, hud.X2, hud.car, hud.X4)
|
||||
commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
|
||||
|
||||
msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
|
||||
commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
|
||||
# TODO: Why is X4 always 0xc1? Not implemented yet in canpacker
|
||||
acc_hud_values = {
|
||||
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
|
||||
'PCM_GAS': hud.pcm_accel,
|
||||
'CRUISE_SPEED': hud.v_cruise,
|
||||
'ENABLE_MINI_CAR': hud.mini_car,
|
||||
'HUD_LEAD': hud.car,
|
||||
'SET_ME_X03': 0x03,
|
||||
'SET_ME_X03_2': 0x03,
|
||||
'SET_ME_X01': 0x01,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
|
||||
|
||||
lkas_hud_values = {
|
||||
'SET_ME_X41': 0x41,
|
||||
'SET_ME_X48': 0x48,
|
||||
'STEERING_REQUIRED': hud.steer_required,
|
||||
'SOLID_LANES': hud.lanes,
|
||||
'BEEP': hud.beep,
|
||||
}
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', 0, lkas_hud_values, idx))
|
||||
|
||||
if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY):
|
||||
msg_0x35e = chr(0) * 7
|
||||
commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
|
||||
msg_0x39f = (chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0))
|
||||
commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
|
||||
commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx))
|
||||
|
||||
radar_hud_values = {
|
||||
'ACC_ALERTS': hud.acc_alert,
|
||||
'LEAD_SPEED': 0x1fe, # What are these magic values
|
||||
'LEAD_STATE': 0x7,
|
||||
'LEAD_DISTANCE': 0x1e,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx))
|
||||
return commands
|
||||
|
||||
|
||||
def create_radar_commands(v_ego, car_fingerprint, idx):
|
||||
"""Creates an iterable of CAN messages for the radar system."""
|
||||
commands = []
|
||||
|
|
|
@ -97,7 +97,7 @@ class CarInterface(object):
|
|||
# sending if read only is False
|
||||
if sendcan is not None:
|
||||
self.sendcan = sendcan
|
||||
self.CC = CarController(CP.enableCamera)
|
||||
self.CC = CarController(self.cp.dbc_name, CP.enableCamera)
|
||||
|
||||
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||
self.compute_gb = get_compute_gb_acura()
|
||||
|
@ -106,6 +106,10 @@ class CarInterface(object):
|
|||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
# limit the pcm accel cmd if:
|
||||
# - v_ego exceeds v_target, or
|
||||
# - a_ego exceeds a_target and v_ego is close to v_target
|
||||
|
||||
eA = a_ego - a_target
|
||||
valuesA = [1.0, 0.1]
|
||||
bpA = [0.3, 1.1]
|
||||
|
@ -114,9 +118,17 @@ class CarInterface(object):
|
|||
valuesV = [1.0, 0.1]
|
||||
bpV = [0.0, 0.5]
|
||||
|
||||
valuesRangeV = [1., 0.]
|
||||
bpRangeV = [-1., 0.]
|
||||
|
||||
# only limit if v_ego is close to v_target
|
||||
speedLimiter = interp(eV, bpV, valuesV)
|
||||
accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV))
|
||||
|
||||
# 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 help in quicker restart
|
||||
return float(max(0.714, a_target / A_ACC_MAX)) * min(interp(eA, bpA, valuesA), interp(eV, bpV, valuesV))
|
||||
|
||||
return float(max(0.714, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint):
|
||||
|
@ -174,7 +186,7 @@ class CarInterface(object):
|
|||
ret.steerRatio = 15.3
|
||||
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
|
||||
is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310']
|
||||
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.1], [0.03]] if is_fw_modified else [[0.8], [0.24]]
|
||||
|
||||
ret.longitudinalKpBP = [0., 5., 35.]
|
||||
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||
|
@ -431,8 +443,11 @@ class CarInterface(object):
|
|||
|
||||
# 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.:
|
||||
# non loud alert if cruise disbales below 25mph as expected (+ a little margin)
|
||||
if ret.vEgo < self.CP.minEnableSpeed + 2.:
|
||||
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
|
||||
else:
|
||||
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
||||
if self.CS.CP.carFingerprint != CAR.CIVIC and ret.vEgo < 0.001:
|
||||
events.append(create_event('manualRestart', [ET.WARNING]))
|
||||
|
|
|
@ -26,7 +26,7 @@ class AH:
|
|||
#[alert_idx, value]
|
||||
# See dbc files for info on values"
|
||||
NONE = [0, 0]
|
||||
FCW = [1, 0x8]
|
||||
FCW = [1, 1]
|
||||
STEER = [2, 1]
|
||||
BRAKE_PRESSED = [3, 10]
|
||||
GEAR_NOT_D = [4, 6]
|
||||
|
|
|
@ -1,26 +1,36 @@
|
|||
from common.numpy_fast import clip
|
||||
from common.numpy_fast import clip, interp, int_rnd
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
||||
create_steer_command, create_ui_command, \
|
||||
create_ipas_steer_command, create_accel_command, \
|
||||
create_fcw_command
|
||||
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
# Accel limits
|
||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||
ACCEL_MAX = 1500 # 1.5 m/s2
|
||||
ACCEL_MIN = -3000 # 3 m/s2
|
||||
ACCEL_MAX = 1.5 # 1.5 m/s2
|
||||
ACCEL_MIN = -3.0 # 3 m/s2
|
||||
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||
|
||||
# Steer torque limits
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
# Steer angle limits
|
||||
ANGLE_MAX_BP = [0., 5.]
|
||||
ANGLE_MAX_V = [510., 300.]
|
||||
ANGLE_DELTA_BP = [0., 5.]
|
||||
ANGLE_DELTA_V = [3., 1.]
|
||||
|
||||
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
||||
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
||||
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
|
||||
0x383]
|
||||
|
||||
|
||||
def accel_hysteresis(accel, accel_steady, enabled):
|
||||
|
||||
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
|
||||
|
@ -56,35 +66,65 @@ def process_hud_alert(hud_alert, audible_alert):
|
|||
|
||||
return steer, fcw, sound1, sound2
|
||||
|
||||
def ipas_state_transition(steer_angle_enabled, enabled, ipas_state, ipas_reset_counter):
|
||||
|
||||
if enabled and not steer_angle_enabled:
|
||||
#ipas_reset_counter = max(0, ipas_reset_counter - 1)
|
||||
#if ipas_reset_counter == 0:
|
||||
# steer_angle_enabled = True
|
||||
#else:
|
||||
# steer_angle_enabled = False
|
||||
#return steer_angle_enabled, ipas_reset_counter
|
||||
return True, 0
|
||||
|
||||
elif enabled and steer_angle_enabled:
|
||||
if steer_angle_enabled and ipas_state != 3:
|
||||
ipas_reset_counter += 1
|
||||
else:
|
||||
ipas_reset_counter = 0
|
||||
if ipas_reset_counter > 10: # try every 0.1s
|
||||
steer_angle_enabled = False
|
||||
return steer_angle_enabled, ipas_reset_counter
|
||||
|
||||
else:
|
||||
return False, 0
|
||||
|
||||
|
||||
class CarController(object):
|
||||
def __init__(self, car_fingerprint, enable_camera, enable_dsu, enable_apg):
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg):
|
||||
self.braking = False
|
||||
# redundant safety check with the board
|
||||
self.controls_allowed = True
|
||||
self.last_steer = 0.
|
||||
self.last_steer = 0
|
||||
self.last_angle = 0
|
||||
self.accel_steady = 0.
|
||||
self.car_fingerprint = car_fingerprint
|
||||
self.alert_active = False
|
||||
self.last_standstill = False
|
||||
self.standstill_req = False
|
||||
self.angle_control = False
|
||||
|
||||
self.steer_angle_enabled = False
|
||||
self.ipas_reset_counter = 0
|
||||
|
||||
self.fake_ecus = set()
|
||||
if enable_camera: self.fake_ecus.add(ECU.CAM)
|
||||
if enable_dsu: self.fake_ecus.add(ECU.DSU)
|
||||
if enable_apg: self.fake_ecus.add(ECU.APGS)
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, sendcan, enabled, CS, frame, actuators,
|
||||
pcm_cancel_cmd, hud_alert, audible_alert):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
# gas and brake
|
||||
apply_accel = actuators.gas - actuators.brake
|
||||
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
|
||||
apply_accel = int(round(clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)))
|
||||
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
# steer torque
|
||||
apply_steer = int(round(actuators.steer * STEER_MAX))
|
||||
|
||||
max_lim = min(max(CS.steer_torque_motor + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX)
|
||||
|
@ -98,15 +138,31 @@ class CarController(object):
|
|||
else:
|
||||
apply_steer = clip(apply_steer, self.last_steer - STEER_DELTA_UP, min(self.last_steer + STEER_DELTA_DOWN, STEER_DELTA_UP))
|
||||
|
||||
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
|
||||
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
|
||||
# only cut torque when steer state is a known fault
|
||||
if not enabled or CS.steer_state in [18, 50]:
|
||||
apply_steer = 0
|
||||
|
||||
self.steer_angle_enabled, self.ipas_reset_counter = \
|
||||
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_state, self.ipas_reset_counter)
|
||||
#print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_state
|
||||
|
||||
# steer angle
|
||||
if self.steer_angle_enabled:
|
||||
apply_angle = actuators.steerAngle
|
||||
angle_lim = int_rnd(interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V))
|
||||
apply_angle = clip(apply_angle, -angle_lim, angle_lim)
|
||||
|
||||
angle_rate_lim = int_rnd(interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V))
|
||||
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
|
||||
else:
|
||||
apply_angle = CS.angle_steers
|
||||
|
||||
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
|
||||
pcm_cancel_cmd = 1
|
||||
|
||||
# dropping torque immediately might cause eps to temp fault. On the other hand, safety_toyota
|
||||
# cuts steer torque immediately anyway TODO: monitor if this is a real issue
|
||||
if not enabled or CS.steer_error:
|
||||
apply_steer = 0
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.standstill and not self.last_standstill:
|
||||
self.standstill_req = True
|
||||
|
@ -115,6 +171,7 @@ class CarController(object):
|
|||
self.standstill_req = False
|
||||
|
||||
self.last_steer = apply_steer
|
||||
self.last_angle = apply_angle
|
||||
self.last_accel = apply_accel
|
||||
self.last_standstill = CS.standstill
|
||||
|
||||
|
@ -127,15 +184,21 @@ class CarController(object):
|
|||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
if ECU.CAM in self.fake_ecus:
|
||||
can_sends.append(create_steer_command(apply_steer, frame))
|
||||
if self.angle_control:
|
||||
can_sends.append(create_steer_command(self.packer, 0., frame))
|
||||
else:
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, frame))
|
||||
|
||||
if ECU.APGS in self.fake_ecus:
|
||||
can_sends.append(create_ipas_steer_command(apply_steer))
|
||||
if self.angle_control:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled))
|
||||
else:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, 0, 0))
|
||||
|
||||
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
|
||||
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
|
||||
if ECU.DSU in self.fake_ecus:
|
||||
can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd, self.standstill_req))
|
||||
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req))
|
||||
else:
|
||||
can_sends.append(create_accel_command(0, pcm_cancel_cmd, False))
|
||||
|
||||
|
@ -157,8 +220,8 @@ class CarController(object):
|
|||
send_ui = False
|
||||
|
||||
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
||||
can_sends.append(create_ui_command(steer, sound1, sound2))
|
||||
can_sends.append(create_fcw_command(fcw))
|
||||
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2))
|
||||
can_sends.append(create_fcw_command(self.packer, fcw))
|
||||
|
||||
#*** static msgs ***
|
||||
|
||||
|
|
|
@ -75,6 +75,7 @@ def get_can_parser(CP):
|
|||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
|
||||
("LKA_STATE", "EPS_STATUS", 0),
|
||||
("IPAS_STATE", "EPS_STATUS", 1),
|
||||
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
|
||||
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
|
||||
]
|
||||
|
@ -125,9 +126,6 @@ class CarState(object):
|
|||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
self.steer_error = False
|
||||
self.brake_error = 0
|
||||
|
||||
can_gear = cp.vl["GEAR_PACKET"]['GEAR']
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
|
@ -160,7 +158,11 @@ class CarState(object):
|
|||
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
|
||||
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] == 50
|
||||
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
|
||||
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
|
||||
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [2, 10]
|
||||
self.ipas_state = cp.vl['EPS_STATUS']['IPAS_STATE']
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
|
||||
|
|
|
@ -33,7 +33,7 @@ class CarInterface(object):
|
|||
# sending if read only is False
|
||||
if sendcan is not None:
|
||||
self.sendcan = sendcan
|
||||
self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
|
@ -76,11 +76,15 @@ class CarInterface(object):
|
|||
if candidate == CAR.PRIUS:
|
||||
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 14.5 # TODO: find exact value for Prius
|
||||
ret.steerRatio = 15.0
|
||||
ret.mass = 3045./2.205 + std_cargo
|
||||
ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.4], [0.01]]
|
||||
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
ret.steerRateCost = 2.
|
||||
ret.steerRateCost = 1.5
|
||||
|
||||
f = 1.43353663
|
||||
tireStiffnessFront_civic *= f
|
||||
tireStiffnessRear_civic *= f
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H]:
|
||||
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.65
|
||||
|
@ -150,7 +154,6 @@ class CarInterface(object):
|
|||
print "ECU Camera Simulated: ", ret.enableCamera
|
||||
print "ECU DSU Simulated: ", ret.enableDsu
|
||||
print "ECU APGS Simulated: ", ret.enableApgs
|
||||
ret.enableGas = True
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
ret.stoppingControl = False
|
||||
|
|
|
@ -28,62 +28,73 @@ def create_video_target(frame, addr):
|
|||
return make_can_msg(addr, msg, 1, True)
|
||||
|
||||
|
||||
def create_ipas_steer_command(steer):
|
||||
|
||||
def create_ipas_steer_command(packer, steer, enabled):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
if steer < 0:
|
||||
move = 0x60
|
||||
steer = 0xfff + steer + 1
|
||||
direction = 3
|
||||
elif steer > 0:
|
||||
move = 0x20
|
||||
direction = 1
|
||||
else:
|
||||
move = 0x40
|
||||
direction = 2
|
||||
|
||||
mode = 0x30 if steer else 0x10
|
||||
mode = 3 if enabled else 1
|
||||
|
||||
steer_h = (steer & 0xF00) >> 8
|
||||
steer_l = steer & 0xff
|
||||
values = {
|
||||
"STATE": mode,
|
||||
"DIRECTION_CMD": direction,
|
||||
"ANGLE": steer,
|
||||
"SET_ME_X10": 0x10,
|
||||
"SET_ME_X40": 0x40
|
||||
}
|
||||
return packer.make_can_msg("STEERING_IPAS", 0, values)
|
||||
|
||||
msg = struct.pack("!BBBBBBB", mode | steer_h, steer_l, 0x10, 0x00, move, 0x40, 0x00)
|
||||
|
||||
return make_can_msg(0x266, msg, 0, True)
|
||||
|
||||
def create_steer_command(steer, raw_cnt):
|
||||
def create_steer_command(packer, steer, raw_cnt):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
# from 0x80 to 0xff
|
||||
counter = ((raw_cnt & 0x3f) << 1) | 0x80
|
||||
if steer != 0:
|
||||
counter |= 1
|
||||
|
||||
# hud
|
||||
# 00 => Regular
|
||||
# 40 => Actively Steering (with beep)
|
||||
# 80 => Actively Steering (without beep)
|
||||
hud = 0x00
|
||||
|
||||
msg = struct.pack("!BhB", counter, steer, hud)
|
||||
|
||||
return make_can_msg(0x2e4, msg, 0, True)
|
||||
values = {
|
||||
"STEER_REQUEST": abs(steer) > 0.001,
|
||||
"STEER_TORQUE_CMD": steer,
|
||||
"COUNTER": raw_cnt,
|
||||
"SET_ME_1": 1,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_LKA", 0, values)
|
||||
|
||||
|
||||
def create_accel_command(accel, pcm_cancel, standstill_req):
|
||||
def create_accel_command(packer, accel, pcm_cancel, standstill_req):
|
||||
# TODO: find the exact canceling bit
|
||||
state = 0x40 if standstill_req else 0xC0
|
||||
state += pcm_cancel # this allows automatic restart from hold without driver cmd
|
||||
|
||||
msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
|
||||
|
||||
return make_can_msg(0x343, msg, 0, True)
|
||||
|
||||
def create_fcw_command(fcw):
|
||||
|
||||
msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00)
|
||||
|
||||
return make_can_msg(0x411, msg, 0, False)
|
||||
values = {
|
||||
"ACCEL_CMD": accel,
|
||||
"SET_ME_X63": 0x63,
|
||||
"SET_ME_1": 1,
|
||||
"RELEASE_STANDSTILL": not standstill_req,
|
||||
"CANCEL_REQ": pcm_cancel,
|
||||
}
|
||||
return packer.make_can_msg("ACC_CONTROL", 0, values)
|
||||
|
||||
|
||||
def create_ui_command(steer, sound1, sound2):
|
||||
def create_fcw_command(packer, fcw):
|
||||
values = {
|
||||
"FCW": fcw,
|
||||
"SET_ME_X20": 0x20,
|
||||
"SET_ME_X10": 0x10,
|
||||
"SET_ME_X80": 0x80,
|
||||
}
|
||||
return packer.make_can_msg("ACC_HUD", 0, values)
|
||||
|
||||
msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00,
|
||||
sound1, 0x2C, 0x38, 0x02)
|
||||
return make_can_msg(0x412, msg, 0, False)
|
||||
|
||||
def create_ui_command(packer, steer, sound1, sound2):
|
||||
values = {
|
||||
"RIGHT_LINE": 1,
|
||||
"LEFT_LINE": 1,
|
||||
"SET_ME_X0C": 0x0c,
|
||||
"SET_ME_X2C": 0x2c,
|
||||
"SET_ME_X38": 0x38,
|
||||
"SET_ME_X02": 0x02,
|
||||
"SET_ME_X01": 1,
|
||||
"SET_ME_X01_2": 1,
|
||||
"REPEATED_BEEPS": sound1,
|
||||
"TWO_BEEPS": sound2,
|
||||
"LDA_ALERT": steer,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_HUD", 0, values)
|
||||
|
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.4.2-openpilot"
|
||||
#define COMMA_VERSION "0.4.3-release"
|
||||
|
|
|
@ -48,7 +48,9 @@ typedef struct VisionStreamBufs {
|
|||
} VisionStreamBufs;
|
||||
|
||||
typedef struct VIPCBufExtra {
|
||||
uint32_t frame_id; // only for yuv
|
||||
// only for yuv
|
||||
uint32_t frame_id;
|
||||
uint64_t timestamp_eof;
|
||||
} VIPCBufExtra;
|
||||
|
||||
typedef union VisionPacketData {
|
||||
|
|
|
@ -273,7 +273,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
|||
CP, PL.lead_1)
|
||||
|
||||
# *** steering PID loop ***
|
||||
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
|
||||
actuators.steer, actuators.steerAngle = 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
|
||||
|
@ -454,6 +454,11 @@ def controlsd_thread(gctx, rate=100):
|
|||
|
||||
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
|
||||
|
||||
# if stock camera is connected, then force passive behavior
|
||||
if not CP.enableCamera:
|
||||
passive = True
|
||||
sendcan = None
|
||||
|
||||
if CI is None:
|
||||
raise Exception("unsupported car")
|
||||
|
||||
|
|
|
@ -72,55 +72,55 @@ class AlertManager(object):
|
|||
|
||||
"fcw": Alert(
|
||||
"Brake!",
|
||||
"Risk of Collision",
|
||||
"Risk of collision detected",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
|
||||
|
||||
"steerSaturated": Alert(
|
||||
"Take Control",
|
||||
"Turn Exceeds Limit",
|
||||
AlertStatus.userPrompt, AlertSize.full,
|
||||
"TAKE CONTROL",
|
||||
"Turn exceeds steering limit",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
|
||||
|
||||
"steerTempUnavailable": Alert(
|
||||
"Take Control",
|
||||
"Steer Temporarily Unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.full,
|
||||
"TAKE CONTROL",
|
||||
"Steering temporarily unavailable",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"preDriverDistracted": Alert(
|
||||
"Take Control",
|
||||
"User Distracted",
|
||||
AlertStatus.userPrompt, AlertSize.full,
|
||||
"TAKE CONTROL",
|
||||
"User appears distracted",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
|
||||
|
||||
"driverDistracted": Alert(
|
||||
"Take Control to Regain Speed",
|
||||
"User Distracted",
|
||||
"TAKE CONTROL TO REGAIN SPEED",
|
||||
"User appears distracted",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
|
||||
|
||||
"startup": Alert(
|
||||
"Always Keep Hands on Wheel",
|
||||
"Be Ready to Take Over Any Time",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Always keep hands on wheel",
|
||||
"Be ready to take over at any time",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, None, None, 0., 0., 15.),
|
||||
|
||||
"ethicalDilemma": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Ethical Dilemma Detected",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"steerTempUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Steer Temporary Unavailable",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Steering temporarily unavailable",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
"manualRestart": Alert(
|
||||
"Take Control",
|
||||
"Resume Driving Manually",
|
||||
"TAKE CONTROL",
|
||||
"Resume driving manually",
|
||||
AlertStatus.userPrompt, AlertSize.full,
|
||||
Priority.LOW, None, None, 0., 0., .2),
|
||||
|
||||
|
@ -128,157 +128,157 @@ class AlertManager(object):
|
|||
"wrongCarModeNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Main Switch Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
"dataNeededNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Data needed for calibration. Upload drive, try again",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
"outOfSpaceNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Out of Space",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Out of storage space",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
"pedalPressedNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Pedal Pressed",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Pedal pressed during attempt",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"speedTooLowNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Speed Too Low",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Speed too low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"brakeHoldNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Brake Hold Active",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Brake hold active",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"parkBrakeNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Park Brake Engaged",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Park brake engaged",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"lowSpeedLockoutNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing soft disabling
|
||||
"overheat": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"System Overheated",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"wrongGear": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gear not D",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"calibrationInvalid": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"calibrationInProgress": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"doorOpen": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Door Open",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"seatbeltNotLatched": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"espDisabled": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"ESP Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
# Cancellation alerts causing immediate disabling
|
||||
"radarCommIssue": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"radarFault": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"modelCommIssue": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Model Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"controlsFailed": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Failed",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"controlsMismatch": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Mismatch",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"commIssue": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"CAN Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"steerUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Steer Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"brakeUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Brake Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"gasUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gas Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"reverseGear": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Reverse Gear",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"cruiseDisabled": Alert(
|
||||
"Take Control Immediately",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Is Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
@ -286,141 +286,141 @@ class AlertManager(object):
|
|||
# not loud cancellations (user is in control)
|
||||
"noTarget": Alert(
|
||||
"Comma Canceled",
|
||||
"No Close Lead",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"No close lead car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"speedTooLow": Alert(
|
||||
"Comma Canceled",
|
||||
"Speed Too Low",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Speed too low",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing non-entry
|
||||
"overheatNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"System Overheated",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"System overheated",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"wrongGearNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Gear not D",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"calibrationInvalidNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"calibrationInProgressNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"doorOpenNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Door Open",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Door open",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"seatbeltNotLatchedNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Seatbelt unlatched",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"espDisabledNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"ESP Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"radarCommIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"radarFaultNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"modelCommIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Model Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"controlsFailedNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Controls Failed",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"commIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"CAN Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"steerUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Steer Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"brakeUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Brake Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"gasUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Gas Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"reverseGearNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Reverse Gear",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"cruiseDisabledNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Cruise is Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Cruise is off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"noTargetNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"No Close Lead",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"No close lead car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# permanent alerts to display on small UI upper box
|
||||
"steerUnavailablePermanent": Alert(
|
||||
"STEER FAULT",
|
||||
"RESTART THE CAR",
|
||||
"STEER FAULT: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
"brakeUnavailablePermanent": Alert(
|
||||
"BRAKE FAULT",
|
||||
"RESTART THE CAR",
|
||||
"BRAKE FAULT: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
"lowSpeedLockoutPermanent": Alert(
|
||||
"CRUISE FAULT",
|
||||
"RESTART THE CAR",
|
||||
"CRUISE FAULT: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
}
|
||||
|
@ -445,7 +445,8 @@ class AlertManager(object):
|
|||
enabled=enabled)
|
||||
|
||||
self.activealerts.append(added_alert)
|
||||
self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True)
|
||||
# sort by priority first and then by start_time
|
||||
self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True)
|
||||
|
||||
# TODO: cycle through alerts?
|
||||
def process_alerts(self, cur_time):
|
||||
|
|
|
@ -2,6 +2,20 @@ from common.numpy_fast import clip
|
|||
from cereal import car
|
||||
|
||||
|
||||
class MPC_COST_LAT:
|
||||
PATH = 1.0
|
||||
LANE = 3.0
|
||||
HEADING = 1.0
|
||||
STEER_RATE = 1.0
|
||||
|
||||
|
||||
class MPC_COST_LONG:
|
||||
TTC = 5.0
|
||||
DISTANCE = 0.1
|
||||
ACCELERATION = 10.0
|
||||
JERK = 20.0
|
||||
|
||||
|
||||
class EventTypes:
|
||||
ENABLE = 'enable'
|
||||
PRE_ENABLE = 'preEnable'
|
||||
|
|
|
@ -1,7 +1,7 @@
|
|||
import math
|
||||
import numpy as np
|
||||
|
||||
from selfdrive.controls.lib.pid import PIController
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from common.numpy_fast import interp
|
||||
from common.realtime import sec_since_boot
|
||||
|
@ -34,7 +34,7 @@ class LatControl(object):
|
|||
|
||||
def setup_mpc(self, steer_rate_cost):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
self.libmpc.init(steer_rate_cost)
|
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
|
||||
|
||||
self.mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
self.cur_state = libmpc_py.ffi.new("state_t *")
|
||||
|
@ -74,7 +74,12 @@ class LatControl(object):
|
|||
l_poly, r_poly, p_poly,
|
||||
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
|
||||
|
||||
# reset to current steer angle if not active or overriding
|
||||
if active:
|
||||
delta_desired = self.mpc_solution[0].delta[1]
|
||||
else:
|
||||
delta_desired = math.radians(angle_steers - angle_offset) / VM.CP.steerRatio
|
||||
|
||||
self.cur_state[0].delta = delta_desired
|
||||
|
||||
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.steerRatio) + angle_offset)
|
||||
|
@ -85,7 +90,7 @@ class LatControl(object):
|
|||
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
|
||||
t = sec_since_boot()
|
||||
if nans:
|
||||
self.libmpc.init(VM.CP.steerRateCost)
|
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
|
||||
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
|
||||
|
||||
if t > self.last_cloudlog_t + 5.0:
|
||||
|
@ -108,4 +113,4 @@ class LatControl(object):
|
|||
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego)
|
||||
|
||||
self.sat_flag = self.pid.saturated
|
||||
return output_steer
|
||||
return output_steer, float(self.angle_steers_des)
|
||||
|
|
|
@ -4,7 +4,6 @@
|
|||
#define deg2rad(d) (d/180.0*PI)
|
||||
|
||||
const int controlHorizon = 50;
|
||||
const double samplingTime = 0.05; // 20 Hz
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
|
@ -1,12 +1,11 @@
|
|||
import os
|
||||
import sys
|
||||
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_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
|
||||
subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef("""
|
||||
|
@ -21,7 +20,7 @@ typedef struct {
|
|||
double delta[20];
|
||||
} log_t;
|
||||
|
||||
void init(double steer_rate_cost);
|
||||
void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
|
||||
int 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);
|
||||
|
|
|
@ -28,9 +28,10 @@ typedef struct {
|
|||
double delta[N];
|
||||
} log_t;
|
||||
|
||||
void init(double steerRateCost){
|
||||
void init(double pathCost, double laneCost, double headingCost, double steerRateCost){
|
||||
acado_initializeSolver();
|
||||
int i;
|
||||
const int STEP_MULTIPLIER = 3;
|
||||
|
||||
/* Initialize the states and controls. */
|
||||
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
|
||||
|
@ -46,18 +47,18 @@ void init(double steerRateCost){
|
|||
for (i = 0; i < N; i++) {
|
||||
int f = 1;
|
||||
if (i > 4){
|
||||
f = 3;
|
||||
f = STEP_MULTIPLIER;
|
||||
}
|
||||
acadoVariables.W[25 * i + 0] = 1.0 * f;
|
||||
acadoVariables.W[25 * i + 6] = 1.0 * f;
|
||||
acadoVariables.W[25 * i + 12] = 1.0 * f;
|
||||
acadoVariables.W[25 * i + 18] = 1.0 * f;
|
||||
acadoVariables.W[25 * i + 0] = pathCost * f;
|
||||
acadoVariables.W[25 * i + 6] = laneCost * f;
|
||||
acadoVariables.W[25 * i + 12] = laneCost * f;
|
||||
acadoVariables.W[25 * i + 18] = headingCost * f;
|
||||
acadoVariables.W[25 * i + 24] = steerRateCost * f;
|
||||
}
|
||||
acadoVariables.WN[0] = 1.0;
|
||||
acadoVariables.WN[5] = 1.0;
|
||||
acadoVariables.WN[10] = 1.0;
|
||||
acadoVariables.WN[15] = 1.0;
|
||||
acadoVariables.WN[0] = pathCost * STEP_MULTIPLIER;
|
||||
acadoVariables.WN[5] = laneCost * STEP_MULTIPLIER;
|
||||
acadoVariables.WN[10] = laneCost * STEP_MULTIPLIER;
|
||||
acadoVariables.WN[15] = headingCost * STEP_MULTIPLIER;
|
||||
}
|
||||
|
||||
int run_mpc(state_t * x0, log_t * solution,
|
||||
|
|
|
@ -1,7 +1,6 @@
|
|||
#include <acado_code_generation.hpp>
|
||||
|
||||
const int controlHorizon = 50;
|
||||
const double samplingTime = 0.2;
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
|
@ -1,11 +1,10 @@
|
|||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
|
||||
from cffi import FFI
|
||||
|
||||
mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
|
||||
subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
|
||||
subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
|
||||
|
||||
|
||||
def _get_libmpc(mpc_id):
|
||||
|
@ -28,7 +27,7 @@ def _get_libmpc(mpc_id):
|
|||
double a_l[20];
|
||||
} log_t;
|
||||
|
||||
void init();
|
||||
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
|
||||
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
|
||||
int run_mpc(state_t * x0, log_t * solution,
|
||||
double l);
|
||||
|
|
|
@ -31,9 +31,10 @@ typedef struct {
|
|||
double a_l[N];
|
||||
} log_t;
|
||||
|
||||
void init(){
|
||||
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
|
||||
acado_initializeSolver();
|
||||
int i;
|
||||
const int STEP_MULTIPLIER = 3;
|
||||
|
||||
/* Initialize the states and controls. */
|
||||
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
|
||||
|
@ -50,16 +51,16 @@ void init(){
|
|||
for (i = 0; i < N; i++) {
|
||||
int f = 1;
|
||||
if (i > 4){
|
||||
f = 3;
|
||||
f = STEP_MULTIPLIER;
|
||||
}
|
||||
acadoVariables.W[16 * i + 0] = 5.0 * f; // exponential cost for danger zone
|
||||
acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance
|
||||
acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration
|
||||
acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk
|
||||
acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
|
||||
acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance
|
||||
acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration
|
||||
acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk
|
||||
}
|
||||
acadoVariables.WN[0] = 5.0; // exponential cost for danger zone
|
||||
acadoVariables.WN[4] = 0.1; // desired distance
|
||||
acadoVariables.WN[8] = 10.0; // acceleration
|
||||
acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone
|
||||
acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance
|
||||
acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -12,7 +12,7 @@ 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.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET
|
||||
from selfdrive.controls.lib.pathplanner import PathPlanner
|
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.speed_smoother import speed_smoother
|
||||
|
@ -34,8 +34,8 @@ _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
|||
|
||||
# need fast accel at very low speed for stop and go
|
||||
# make sure these accelerations are smaller than mpc limits
|
||||
_A_CRUISE_MAX_V = [1., 1., .8, .5, .3]
|
||||
_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .3]
|
||||
_A_CRUISE_MAX_V = [1.1, 1.1, .8, .5, .3]
|
||||
_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 1.2, .7, .3]
|
||||
_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
|
||||
|
||||
# Lookup table for turns
|
||||
|
@ -172,7 +172,8 @@ class LongitudinalMpc(object):
|
|||
|
||||
def setup_mpc(self):
|
||||
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
|
||||
self.libmpc.init()
|
||||
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
|
||||
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
|
||||
self.mpc_solution = ffi.new("log_t *")
|
||||
self.cur_state = ffi.new("state_t *")
|
||||
|
@ -245,7 +246,8 @@ class LongitudinalMpc(object):
|
|||
cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
|
||||
self.mpc_id, backwards, crashing, nans))
|
||||
|
||||
self.libmpc.init()
|
||||
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
|
||||
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
self.cur_state[0].v_ego = CS.vEgo
|
||||
self.cur_state[0].a_ego = 0.0
|
||||
self.v_mpc = CS.vEgo
|
||||
|
|
|
@ -213,7 +213,7 @@ class Cluster(object):
|
|||
lead.fcw = self.is_potential_fcw()
|
||||
|
||||
def __str__(self):
|
||||
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aRel, self.dPath)
|
||||
ret = "x: %4.1f y: %4.1f v: %4.1f a: %4.1f d: %4.2f" % (self.dRel, self.yRel, self.vRel, self.aLeadK, self.dPath)
|
||||
if self.stationary:
|
||||
ret += " stationary"
|
||||
if self.vision:
|
||||
|
|
|
@ -260,12 +260,13 @@ def radard_thread(gctx=None):
|
|||
|
||||
for cnt, ids in enumerate(tracks.keys()):
|
||||
if DEBUG:
|
||||
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
|
||||
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \
|
||||
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
|
||||
tracks[ids].dPath, tracks[ids].vLat,
|
||||
tracks[ids].vLead, tracks[ids].vLeadK,
|
||||
tracks[ids].aLeadK,
|
||||
tracks[ids].stationary)
|
||||
tracks[ids].stationary,
|
||||
tracks[ids].measured)
|
||||
dat.liveTracks[cnt].trackId = ids
|
||||
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
|
||||
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
|
||||
|
|
|
@ -1,6 +1,7 @@
|
|||
"""Install exception handler for process crash."""
|
||||
import os
|
||||
import sys
|
||||
from selfdrive.version import version, dirty
|
||||
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
|
@ -16,9 +17,8 @@ if os.getenv("NOLOG") or os.getenv("NOCRASH"):
|
|||
else:
|
||||
from raven import Client
|
||||
from raven.transport.http import HTTPTransport
|
||||
|
||||
client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924',
|
||||
install_sys_hook=False, transport=HTTPTransport)
|
||||
install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty})
|
||||
|
||||
def capture_exception(*args, **kwargs):
|
||||
client.captureException(*args, **kwargs)
|
||||
|
|
|
@ -4,10 +4,15 @@ import argparse
|
|||
import zmq
|
||||
import json
|
||||
from hexdump import hexdump
|
||||
from threading import Thread
|
||||
|
||||
from cereal import log
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
|
||||
def run_server(socketio):
|
||||
socketio.run(app, host='0.0.0.0', port=4000)
|
||||
|
||||
if __name__ == "__main__":
|
||||
context = zmq.Context()
|
||||
poller = zmq.Poller()
|
||||
|
@ -17,33 +22,63 @@ if __name__ == "__main__":
|
|||
parser.add_argument('--raw', action='store_true')
|
||||
parser.add_argument('--json', action='store_true')
|
||||
parser.add_argument('--dump-json', action='store_true')
|
||||
parser.add_argument('--no-print', action='store_true')
|
||||
parser.add_argument('--proxy', action='store_true', help='republish on localhost')
|
||||
parser.add_argument('--map', action='store_true')
|
||||
parser.add_argument('--addr', default='127.0.0.1')
|
||||
parser.add_argument("socket", type=str, nargs='*', help="socket name")
|
||||
args = parser.parse_args()
|
||||
|
||||
republish_socks = {}
|
||||
|
||||
for m in args.socket if len(args.socket) > 0 else service_list:
|
||||
if m in service_list:
|
||||
messaging.sub_sock(context, service_list[m].port, poller, addr=args.addr)
|
||||
port = service_list[m].port
|
||||
elif m.isdigit():
|
||||
messaging.sub_sock(context, int(m), poller, addr=args.addr)
|
||||
port = int(m)
|
||||
else:
|
||||
print("service not found")
|
||||
exit(-1)
|
||||
sock = messaging.sub_sock(context, port, poller, addr=args.addr)
|
||||
if args.proxy:
|
||||
republish_socks[sock] = messaging.pub_sock(context, port)
|
||||
|
||||
if args.map:
|
||||
from flask.ext.socketio import SocketIO
|
||||
from flask import Flask
|
||||
app = Flask(__name__)
|
||||
socketio = SocketIO(app, async_mode='threading')
|
||||
server_thread = Thread(target=run_server, args=(socketio,))
|
||||
server_thread.daemon = True
|
||||
server_thread.start()
|
||||
print 'server running'
|
||||
|
||||
while 1:
|
||||
polld = poller.poll(timeout=1000)
|
||||
for sock, mode in polld:
|
||||
if mode != zmq.POLLIN:
|
||||
continue
|
||||
msg = sock.recv()
|
||||
evt = log.Event.from_bytes(msg)
|
||||
if sock in republish_socks:
|
||||
republish_socks[sock].send(msg)
|
||||
if args.map and evt.which() == 'liveLocation':
|
||||
print 'send loc'
|
||||
socketio.emit('location', {
|
||||
'lat': evt.liveLocation.lat,
|
||||
'lon': evt.liveLocation.lon,
|
||||
'alt': evt.liveLocation.alt,
|
||||
})
|
||||
if not args.no_print:
|
||||
if args.pipe:
|
||||
sys.stdout.write(sock.recv())
|
||||
sys.stdout.write(msg)
|
||||
sys.stdout.flush()
|
||||
elif args.raw:
|
||||
hexdump(sock.recv())
|
||||
hexdump(msg)
|
||||
elif args.json:
|
||||
print(json.loads(sock.recv()))
|
||||
print(json.loads(msg))
|
||||
elif args.dump_json:
|
||||
print json.dumps(messaging.recv_one(sock).to_dict())
|
||||
print json.dumps(evt.to_dict())
|
||||
else:
|
||||
print messaging.recv_one(sock)
|
||||
print evt
|
||||
|
||||
|
|
|
@ -0,0 +1,140 @@
|
|||
def GET_FIELD_U(w, nb, pos):
|
||||
return (((w) >> (pos)) & ((1 << (nb)) - 1))
|
||||
|
||||
|
||||
def twos_complement(v, nb):
|
||||
sign = v >> (nb - 1)
|
||||
value = v
|
||||
if sign != 0:
|
||||
value = value - (1 << nb)
|
||||
return value
|
||||
|
||||
|
||||
def GET_FIELD_S(w, nb, pos):
|
||||
v = GET_FIELD_U(w, nb, pos)
|
||||
return twos_complement(v, nb)
|
||||
|
||||
|
||||
def extract_uint8(v, b):
|
||||
return (v >> (8 * (3 - b))) & 255
|
||||
|
||||
def extract_int8(v, b):
|
||||
value = extract_uint8(v, b)
|
||||
if value > 127:
|
||||
value -= 256
|
||||
return value
|
||||
|
||||
class EphemerisData:
|
||||
'''container for parsing a AID_EPH message
|
||||
Thanks to Sylvain Munaut <tnt@246tNt.com>
|
||||
http://cgit.osmocom.org/cgit/osmocom-lcs/tree/gps.c
|
||||
on of this parser
|
||||
|
||||
See IS-GPS-200F.pdf Table 20-III for the field meanings, scaling factors and
|
||||
field widths
|
||||
'''
|
||||
|
||||
def __init__(self, svId, subframes):
|
||||
from math import pow
|
||||
self.svId = svId
|
||||
week_no = GET_FIELD_U(subframes[1][2+0], 10, 20)
|
||||
# code_on_l2 = GET_FIELD_U(subframes[1][0], 2, 12)
|
||||
# sv_ura = GET_FIELD_U(subframes[1][0], 4, 8)
|
||||
# sv_health = GET_FIELD_U(subframes[1][0], 6, 2)
|
||||
# l2_p_flag = GET_FIELD_U(subframes[1][1], 1, 23)
|
||||
t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6)
|
||||
iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U(
|
||||
subframes[1][2+5], 8, 22)
|
||||
|
||||
t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6)
|
||||
a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22)
|
||||
a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6)
|
||||
a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8)
|
||||
|
||||
c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6)
|
||||
delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14)
|
||||
m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[2][2+2], 24, 6)
|
||||
c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14)
|
||||
e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6)
|
||||
c_us = GET_FIELD_S(subframes[2][2+5], 16, 14)
|
||||
a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[2][2+6], 24, 6)
|
||||
t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14)
|
||||
# fit_flag = GET_FIELD_U(subframes[2][7], 1, 7)
|
||||
|
||||
c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14)
|
||||
omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[3][2+1], 24, 6)
|
||||
c_is = GET_FIELD_S(subframes[3][2+2], 16, 14)
|
||||
i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U(
|
||||
subframes[3][2+3], 24, 6)
|
||||
c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14)
|
||||
w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6)
|
||||
omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6)
|
||||
idot = GET_FIELD_S(subframes[3][2+7], 14, 8)
|
||||
|
||||
self._rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6)
|
||||
self._rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6)
|
||||
self._rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6)
|
||||
self._rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14)
|
||||
self.aodo = GET_FIELD_U(subframes[2][2+7], 5, 8)
|
||||
|
||||
# Definition of Pi used in the GPS coordinate system
|
||||
gpsPi = 3.1415926535898
|
||||
|
||||
# now form variables in radians, meters and seconds etc
|
||||
self.Tgd = t_gd * pow(2, -31)
|
||||
self.A = pow(a_powhalf * pow(2, -19), 2.0)
|
||||
self.cic = c_ic * pow(2, -29)
|
||||
self.cis = c_is * pow(2, -29)
|
||||
self.crc = c_rc * pow(2, -5)
|
||||
self.crs = c_rs * pow(2, -5)
|
||||
self.cuc = c_uc * pow(2, -29)
|
||||
self.cus = c_us * pow(2, -29)
|
||||
self.deltaN = delta_n * pow(2, -43) * gpsPi
|
||||
self.ecc = e * pow(2, -33)
|
||||
self.i0 = i_0 * pow(2, -31) * gpsPi
|
||||
self.idot = idot * pow(2, -43) * gpsPi
|
||||
self.M0 = m_0 * pow(2, -31) * gpsPi
|
||||
self.omega = w * pow(2, -31) * gpsPi
|
||||
self.omega_dot = omega_dot * pow(2, -43) * gpsPi
|
||||
self.omega0 = omega_0 * pow(2, -31) * gpsPi
|
||||
self.toe = t_oe * pow(2, 4)
|
||||
|
||||
# clock correction information
|
||||
self.toc = t_oc * pow(2, 4)
|
||||
self.gpsWeek = week_no
|
||||
self.af0 = a_f0 * pow(2, -31)
|
||||
self.af1 = a_f1 * pow(2, -43)
|
||||
self.af2 = a_f2 * pow(2, -55)
|
||||
|
||||
iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22)
|
||||
iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22)
|
||||
self.valid = (iode1 == iode2) and (iode1 == (iodc & 0xff))
|
||||
self.iode = iode1
|
||||
|
||||
if GET_FIELD_U(subframes[4][2+0], 2, 28) != 1:
|
||||
raise RuntimeError("subframe 4 not of type 1")
|
||||
if GET_FIELD_U(subframes[5][2+0], 2, 28) != 1:
|
||||
raise RuntimeError("subframe 5 not of type 1")
|
||||
print 'page :', GET_FIELD_U(subframes[4][2+0], 6, 22)
|
||||
if GET_FIELD_U(subframes[4][2+0], 6, 22) == 56:
|
||||
a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30)
|
||||
a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27)
|
||||
a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24)
|
||||
a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24)
|
||||
b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11)
|
||||
b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14)
|
||||
b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16)
|
||||
b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16)
|
||||
|
||||
self.ionoAlpha = [a0, a1, a2, a3]
|
||||
self.ionoBeta = [b0, b1, b2, b3]
|
||||
self.ionoCoeffsValid = True
|
||||
else:
|
||||
self.ionoAlpha = []
|
||||
self.ionoBeta = []
|
||||
self.ionoCoeffsValid = False
|
||||
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
#!/usr/bin/env python
|
||||
import zmq
|
||||
from copy import copy
|
||||
from selfdrive import messaging
|
||||
from selfdrive.services import service_list
|
||||
from cereal import log
|
||||
|
||||
from common.transformations.coordinates import geodetic2ecef
|
||||
|
||||
def main(gctx=None):
|
||||
context = zmq.Context()
|
||||
poller = zmq.Poller()
|
||||
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, poller)
|
||||
gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
|
||||
app_sock = messaging.sub_sock(context, service_list['applanixLocation'].port, poller)
|
||||
loc_sock = messaging.pub_sock(context, service_list['liveLocation'].port)
|
||||
|
||||
last_ext, last_gps, last_app = -1, -1, -1
|
||||
# 5 sec
|
||||
max_gap = 5*1e9
|
||||
preferred_type = None
|
||||
|
||||
while 1:
|
||||
for sock, event in poller.poll(500):
|
||||
if sock is app_sock:
|
||||
msg = messaging.recv_one(sock)
|
||||
last_app = msg.logMonoTime
|
||||
this_type = 'app'
|
||||
if sock is gps_sock:
|
||||
msg = messaging.recv_one(sock)
|
||||
gps_pkt = msg.gpsLocation
|
||||
last_gps = msg.logMonoTime
|
||||
this_type = 'gps'
|
||||
if sock is gps_ext_sock:
|
||||
msg = messaging.recv_one(sock)
|
||||
gps_pkt = msg.gpsLocationExternal
|
||||
last_ext = msg.logMonoTime
|
||||
this_type = 'ext'
|
||||
|
||||
last = max(last_gps, last_ext, last_app)
|
||||
|
||||
if last_app > last - max_gap:
|
||||
new_preferred_type = 'app'
|
||||
elif last_ext > last - max_gap:
|
||||
new_preferred_type = 'ext'
|
||||
else:
|
||||
new_preferred_type = 'gps'
|
||||
|
||||
if preferred_type != new_preferred_type:
|
||||
print "switching from %s to %s" % (preferred_type, new_preferred_type)
|
||||
preferred_type = new_preferred_type
|
||||
|
||||
if this_type == preferred_type:
|
||||
new_msg = messaging.new_message()
|
||||
if this_type == 'app':
|
||||
# straight proxy the applanix
|
||||
new_msg.init('liveLocation')
|
||||
new_msg.liveLocation = copy(msg.applanixLocation)
|
||||
else:
|
||||
new_msg.logMonoTime = msg.logMonoTime
|
||||
new_msg.init('liveLocation')
|
||||
pkt = new_msg.liveLocation
|
||||
pkt.lat = gps_pkt.latitude
|
||||
pkt.lon = gps_pkt.longitude
|
||||
pkt.alt = gps_pkt.altitude
|
||||
pkt.speed = gps_pkt.speed
|
||||
pkt.heading = gps_pkt.bearing
|
||||
pkt.positionECEF = [float(x) for x in geodetic2ecef([pkt.lat, pkt.lon, pkt.alt])]
|
||||
pkt.source = log.LiveLocationData.SensorSource.dummy
|
||||
loc_sock.send(new_msg.to_bytes())
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
|
@ -0,0 +1,995 @@
|
|||
#!/usr/bin/env python
|
||||
'''
|
||||
UBlox binary protocol handling
|
||||
|
||||
Copyright Andrew Tridgell, October 2012
|
||||
Released under GNU GPL version 3 or later
|
||||
|
||||
WARNING: This code has originally intended for
|
||||
ublox version 7, it has been adapted to work
|
||||
for ublox version 8, not all functions may work.
|
||||
'''
|
||||
|
||||
import struct
|
||||
import time, os
|
||||
|
||||
# protocol constants
|
||||
PREAMBLE1 = 0xb5
|
||||
PREAMBLE2 = 0x62
|
||||
|
||||
# message classes
|
||||
CLASS_NAV = 0x01
|
||||
CLASS_RXM = 0x02
|
||||
CLASS_INF = 0x04
|
||||
CLASS_ACK = 0x05
|
||||
CLASS_CFG = 0x06
|
||||
CLASS_MON = 0x0A
|
||||
CLASS_AID = 0x0B
|
||||
CLASS_TIM = 0x0D
|
||||
CLASS_ESF = 0x10
|
||||
|
||||
# ACK messages
|
||||
MSG_ACK_NACK = 0x00
|
||||
MSG_ACK_ACK = 0x01
|
||||
|
||||
# NAV messages
|
||||
MSG_NAV_POSECEF = 0x1
|
||||
MSG_NAV_POSLLH = 0x2
|
||||
MSG_NAV_STATUS = 0x3
|
||||
MSG_NAV_DOP = 0x4
|
||||
MSG_NAV_SOL = 0x6
|
||||
MSG_NAV_PVT = 0x7
|
||||
MSG_NAV_POSUTM = 0x8
|
||||
MSG_NAV_VELNED = 0x12
|
||||
MSG_NAV_VELECEF = 0x11
|
||||
MSG_NAV_TIMEGPS = 0x20
|
||||
MSG_NAV_TIMEUTC = 0x21
|
||||
MSG_NAV_CLOCK = 0x22
|
||||
MSG_NAV_SVINFO = 0x30
|
||||
MSG_NAV_AOPSTATUS = 0x60
|
||||
MSG_NAV_DGPS = 0x31
|
||||
MSG_NAV_DOP = 0x04
|
||||
MSG_NAV_EKFSTATUS = 0x40
|
||||
MSG_NAV_SBAS = 0x32
|
||||
MSG_NAV_SOL = 0x06
|
||||
|
||||
# RXM messages
|
||||
MSG_RXM_RAW = 0x15
|
||||
MSG_RXM_SFRB = 0x11
|
||||
MSG_RXM_SFRBX = 0x13
|
||||
MSG_RXM_SVSI = 0x20
|
||||
MSG_RXM_EPH = 0x31
|
||||
MSG_RXM_ALM = 0x30
|
||||
MSG_RXM_PMREQ = 0x41
|
||||
|
||||
# AID messages
|
||||
MSG_AID_ALM = 0x30
|
||||
MSG_AID_EPH = 0x31
|
||||
MSG_AID_ALPSRV = 0x32
|
||||
MSG_AID_AOP = 0x33
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_ALP = 0x50
|
||||
MSG_AID_DATA = 0x10
|
||||
MSG_AID_HUI = 0x02
|
||||
MSG_AID_INI = 0x01
|
||||
MSG_AID_REQ = 0x00
|
||||
|
||||
# CFG messages
|
||||
MSG_CFG_PRT = 0x00
|
||||
MSG_CFG_ANT = 0x13
|
||||
MSG_CFG_DAT = 0x06
|
||||
MSG_CFG_EKF = 0x12
|
||||
MSG_CFG_ESFGWT = 0x29
|
||||
MSG_CFG_CFG = 0x09
|
||||
MSG_CFG_USB = 0x1b
|
||||
MSG_CFG_RATE = 0x08
|
||||
MSG_CFG_SET_RATE = 0x01
|
||||
MSG_CFG_NAV5 = 0x24
|
||||
MSG_CFG_FXN = 0x0E
|
||||
MSG_CFG_INF = 0x02
|
||||
MSG_CFG_ITFM = 0x39
|
||||
MSG_CFG_MSG = 0x01
|
||||
MSG_CFG_NAVX5 = 0x23
|
||||
MSG_CFG_NMEA = 0x17
|
||||
MSG_CFG_NVS = 0x22
|
||||
MSG_CFG_PM2 = 0x3B
|
||||
MSG_CFG_PM = 0x32
|
||||
MSG_CFG_RINV = 0x34
|
||||
MSG_CFG_RST = 0x04
|
||||
MSG_CFG_RXM = 0x11
|
||||
MSG_CFG_SBAS = 0x16
|
||||
MSG_CFG_TMODE2 = 0x3D
|
||||
MSG_CFG_TMODE = 0x1D
|
||||
MSG_CFG_TPS = 0x31
|
||||
MSG_CFG_TP = 0x07
|
||||
MSG_CFG_GNSS = 0x3E
|
||||
MSG_CFG_ODO = 0x1E
|
||||
|
||||
# ESF messages
|
||||
MSG_ESF_MEAS = 0x02
|
||||
MSG_ESF_STATUS = 0x10
|
||||
|
||||
# INF messages
|
||||
MSG_INF_DEBUG = 0x04
|
||||
MSG_INF_ERROR = 0x00
|
||||
MSG_INF_NOTICE = 0x02
|
||||
MSG_INF_TEST = 0x03
|
||||
MSG_INF_WARNING = 0x01
|
||||
|
||||
# MON messages
|
||||
MSG_MON_SCHD = 0x01
|
||||
MSG_MON_HW = 0x09
|
||||
MSG_MON_HW2 = 0x0B
|
||||
MSG_MON_IO = 0x02
|
||||
MSG_MON_MSGPP = 0x06
|
||||
MSG_MON_RXBUF = 0x07
|
||||
MSG_MON_RXR = 0x21
|
||||
MSG_MON_TXBUF = 0x08
|
||||
MSG_MON_VER = 0x04
|
||||
|
||||
# TIM messages
|
||||
MSG_TIM_TP = 0x01
|
||||
MSG_TIM_TM2 = 0x03
|
||||
MSG_TIM_SVIN = 0x04
|
||||
MSG_TIM_VRFY = 0x06
|
||||
|
||||
# port IDs
|
||||
PORT_DDC = 0
|
||||
PORT_SERIAL1 = 1
|
||||
PORT_SERIAL2 = 2
|
||||
PORT_USB = 3
|
||||
PORT_SPI = 4
|
||||
|
||||
# dynamic models
|
||||
DYNAMIC_MODEL_PORTABLE = 0
|
||||
DYNAMIC_MODEL_STATIONARY = 2
|
||||
DYNAMIC_MODEL_PEDESTRIAN = 3
|
||||
DYNAMIC_MODEL_AUTOMOTIVE = 4
|
||||
DYNAMIC_MODEL_SEA = 5
|
||||
DYNAMIC_MODEL_AIRBORNE1G = 6
|
||||
DYNAMIC_MODEL_AIRBORNE2G = 7
|
||||
DYNAMIC_MODEL_AIRBORNE4G = 8
|
||||
|
||||
#reset items
|
||||
RESET_HOT = 0
|
||||
RESET_WARM = 1
|
||||
RESET_COLD = 0xFFFF
|
||||
|
||||
RESET_HW = 0
|
||||
RESET_SW = 1
|
||||
RESET_SW_GPS = 2
|
||||
RESET_HW_GRACEFUL = 4
|
||||
RESET_GPS_STOP = 8
|
||||
RESET_GPS_START = 9
|
||||
|
||||
|
||||
class UBloxError(Exception):
|
||||
'''Ublox error class'''
|
||||
|
||||
def __init__(self, msg):
|
||||
Exception.__init__(self, msg)
|
||||
self.message = msg
|
||||
|
||||
|
||||
class UBloxAttrDict(dict):
|
||||
'''allow dictionary members as attributes'''
|
||||
|
||||
def __init__(self):
|
||||
dict.__init__(self)
|
||||
|
||||
def __getattr__(self, name):
|
||||
try:
|
||||
return self.__getitem__(name)
|
||||
except KeyError:
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
if self.__dict__.has_key(name):
|
||||
# allow set on normal attributes
|
||||
dict.__setattr__(self, name, value)
|
||||
else:
|
||||
self.__setitem__(name, value)
|
||||
|
||||
|
||||
def ArrayParse(field):
|
||||
'''parse an array descriptor'''
|
||||
arridx = field.find('[')
|
||||
if arridx == -1:
|
||||
return (field, -1)
|
||||
alen = int(field[arridx + 1:-1])
|
||||
fieldname = field[:arridx]
|
||||
return (fieldname, alen)
|
||||
|
||||
|
||||
class UBloxDescriptor:
|
||||
'''class used to describe the layout of a UBlox message'''
|
||||
|
||||
def __init__(self,
|
||||
name,
|
||||
msg_format,
|
||||
fields=[],
|
||||
count_field=None,
|
||||
format2=None,
|
||||
fields2=None):
|
||||
self.name = name
|
||||
self.msg_format = msg_format
|
||||
self.fields = fields
|
||||
self.count_field = count_field
|
||||
self.format2 = format2
|
||||
self.fields2 = fields2
|
||||
|
||||
def unpack(self, msg):
|
||||
'''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg'''
|
||||
msg._fields = {}
|
||||
|
||||
# unpack main message blocks. A comm
|
||||
formats = self.msg_format.split(',')
|
||||
buf = msg._buf[6:-2]
|
||||
count = 0
|
||||
msg._recs = []
|
||||
fields = self.fields[:]
|
||||
|
||||
for fmt in formats:
|
||||
size1 = struct.calcsize(fmt)
|
||||
if size1 > len(buf):
|
||||
raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf)))
|
||||
f1 = list(struct.unpack(fmt, buf[:size1]))
|
||||
i = 0
|
||||
while i < len(f1):
|
||||
field = fields.pop(0)
|
||||
(fieldname, alen) = ArrayParse(field)
|
||||
if alen == -1:
|
||||
msg._fields[fieldname] = f1[i]
|
||||
if self.count_field == fieldname:
|
||||
count = int(f1[i])
|
||||
i += 1
|
||||
else:
|
||||
msg._fields[fieldname] = [0] * alen
|
||||
for a in range(alen):
|
||||
msg._fields[fieldname][a] = f1[i]
|
||||
i += 1
|
||||
buf = buf[size1:]
|
||||
if len(buf) == 0:
|
||||
break
|
||||
|
||||
if self.count_field == '_remaining':
|
||||
count = len(buf) / struct.calcsize(self.format2)
|
||||
|
||||
if count == 0:
|
||||
msg._unpacked = True
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
return
|
||||
|
||||
size2 = struct.calcsize(self.format2)
|
||||
for c in range(count):
|
||||
r = UBloxAttrDict()
|
||||
if size2 > len(buf):
|
||||
raise UBloxError("INVALID_SIZE=%u, " % len(buf))
|
||||
f2 = list(struct.unpack(self.format2, buf[:size2]))
|
||||
for i in range(len(self.fields2)):
|
||||
r[self.fields2[i]] = f2[i]
|
||||
buf = buf[size2:]
|
||||
msg._recs.append(r)
|
||||
if len(buf) != 0:
|
||||
raise UBloxError("EXTRA_BYTES=%u" % len(buf))
|
||||
msg._unpacked = True
|
||||
|
||||
def pack(self, msg, msg_class=None, msg_id=None):
|
||||
'''pack a UBloxMessage from the .fields and ._recs attributes in msg'''
|
||||
f1 = []
|
||||
if msg_class is None:
|
||||
msg_class = msg.msg_class()
|
||||
if msg_id is None:
|
||||
msg_id = msg.msg_id()
|
||||
msg._buf = ''
|
||||
|
||||
fields = self.fields[:]
|
||||
for f in fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if not fieldname in msg._fields:
|
||||
break
|
||||
if alen == -1:
|
||||
f1.append(msg._fields[fieldname])
|
||||
else:
|
||||
for a in range(alen):
|
||||
f1.append(msg._fields[fieldname][a])
|
||||
try:
|
||||
# try full length message
|
||||
fmt = self.msg_format.replace(',', '')
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
except Exception:
|
||||
# try without optional part
|
||||
fmt = self.msg_format.split(',')[0]
|
||||
msg._buf = struct.pack(fmt, *tuple(f1))
|
||||
|
||||
length = len(msg._buf)
|
||||
if msg._recs:
|
||||
length += len(msg._recs) * struct.calcsize(self.format2)
|
||||
header = struct.pack('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
|
||||
msg._buf = header + msg._buf
|
||||
|
||||
for r in msg._recs:
|
||||
f2 = []
|
||||
for f in self.fields2:
|
||||
f2.append(r[f])
|
||||
msg._buf += struct.pack(self.format2, *tuple(f2))
|
||||
msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
|
||||
|
||||
def format(self, msg):
|
||||
'''return a formatted string for a message'''
|
||||
if not msg._unpacked:
|
||||
self.unpack(msg)
|
||||
ret = self.name + ': '
|
||||
for f in self.fields:
|
||||
(fieldname, alen) = ArrayParse(f)
|
||||
if not fieldname in msg._fields:
|
||||
continue
|
||||
v = msg._fields[fieldname]
|
||||
if isinstance(v, list):
|
||||
ret += '%s=[' % fieldname
|
||||
for a in range(alen):
|
||||
ret += '%s, ' % v[a]
|
||||
ret = ret[:-2] + '], '
|
||||
elif isinstance(v, str):
|
||||
ret += '%s="%s", ' % (f, v.rstrip(' \0'))
|
||||
else:
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
for r in msg._recs:
|
||||
ret += '[ '
|
||||
for f in self.fields2:
|
||||
v = r[f]
|
||||
ret += '%s=%s, ' % (f, v)
|
||||
ret = ret[:-2] + ' ], '
|
||||
return ret[:-2]
|
||||
|
||||
|
||||
# list of supported message types.
|
||||
msg_types = {
|
||||
(CLASS_ACK, MSG_ACK_ACK):
|
||||
UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_ACK, MSG_ACK_NACK):
|
||||
UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
|
||||
(CLASS_CFG, MSG_CFG_USB):
|
||||
UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
|
||||
'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
|
||||
'vendorString', 'productString', 'serialNumber'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_PRT):
|
||||
UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
|
||||
'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
|
||||
'reserved4', 'reserved5'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_CFG):
|
||||
UBloxDescriptor('CFG_CFG', '<III,B',
|
||||
['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
|
||||
(CLASS_CFG, MSG_CFG_RXM):
|
||||
UBloxDescriptor('CFG_RXM', '<BB',
|
||||
['reserved1', 'lpMode']),
|
||||
(CLASS_CFG, MSG_CFG_RST):
|
||||
UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
|
||||
(CLASS_CFG, MSG_CFG_SBAS):
|
||||
UBloxDescriptor('CFG_SBAS', '<BBBBI',
|
||||
['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
|
||||
(CLASS_CFG, MSG_CFG_GNSS):
|
||||
UBloxDescriptor('CFG_GNSS', '<BBBB',
|
||||
['msgVer', 'numTrkChHw', 'numTrkChUse',
|
||||
'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
|
||||
['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
|
||||
(CLASS_CFG, MSG_CFG_RATE):
|
||||
UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
|
||||
(CLASS_CFG, MSG_CFG_MSG):
|
||||
UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
|
||||
(CLASS_NAV, MSG_NAV_POSLLH):
|
||||
UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
|
||||
['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELNED):
|
||||
UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
|
||||
'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_DOP):
|
||||
UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
|
||||
['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
|
||||
(CLASS_NAV, MSG_NAV_STATUS):
|
||||
UBloxDescriptor('NAV_STATUS', '<IBBBBII',
|
||||
['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
|
||||
(CLASS_NAV, MSG_NAV_SOL):
|
||||
UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
|
||||
'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
|
||||
'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_PVT):
|
||||
UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
|
||||
'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
|
||||
'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
|
||||
'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
|
||||
'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSUTM):
|
||||
UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
|
||||
['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
|
||||
(CLASS_NAV, MSG_NAV_SBAS):
|
||||
UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
|
||||
'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
|
||||
'reserved03'
|
||||
], 'cnt', 'BBBBBBhHh', [
|
||||
'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_POSECEF):
|
||||
UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
|
||||
(CLASS_NAV, MSG_NAV_VELECEF):
|
||||
UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
|
||||
'sAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEGPS):
|
||||
UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
|
||||
['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
|
||||
(CLASS_NAV, MSG_NAV_TIMEUTC):
|
||||
UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
|
||||
'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
|
||||
]),
|
||||
(CLASS_NAV, MSG_NAV_CLOCK):
|
||||
UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
|
||||
(CLASS_NAV, MSG_NAV_DGPS):
|
||||
UBloxDescriptor('NAV_DGPS', '<IihhBBH',
|
||||
['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
|
||||
'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
|
||||
(CLASS_NAV, MSG_NAV_SVINFO):
|
||||
UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
|
||||
'reserved2'], 'numCh', '<BBBBBbhi',
|
||||
['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
|
||||
(CLASS_RXM, MSG_RXM_SVSI):
|
||||
UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
|
||||
'<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
|
||||
(CLASS_RXM, MSG_RXM_EPH):
|
||||
UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_EPH):
|
||||
UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
|
||||
['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
|
||||
(CLASS_AID, MSG_AID_HUI):
|
||||
UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
|
||||
['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
|
||||
'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
|
||||
'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
|
||||
(CLASS_AID, MSG_AID_AOP):
|
||||
UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
|
||||
['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
|
||||
'optional1[48]']),
|
||||
(CLASS_RXM, MSG_RXM_RAW):
|
||||
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
|
||||
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
|
||||
], 'numMeas', '<ddfBBBBHBBBBBB', [
|
||||
'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'reserved2', 'freqId', 'locktime', 'cno',
|
||||
'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
|
||||
]),
|
||||
(CLASS_RXM, MSG_RXM_SFRB):
|
||||
UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
|
||||
(CLASS_RXM, MSG_RXM_SFRBX):
|
||||
UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
|
||||
'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
|
||||
(CLASS_AID, MSG_AID_ALM):
|
||||
UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
|
||||
(CLASS_RXM, MSG_RXM_ALM):
|
||||
UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
|
||||
(CLASS_CFG, MSG_CFG_ODO):
|
||||
UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
|
||||
'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
|
||||
'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAV5):
|
||||
UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
|
||||
'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
|
||||
'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
|
||||
'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_CFG, MSG_CFG_NAVX5):
|
||||
UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
|
||||
'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
|
||||
'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
|
||||
'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
|
||||
'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW):
|
||||
UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
|
||||
'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
|
||||
'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
|
||||
'pullL'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_HW2):
|
||||
UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
|
||||
'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
|
||||
'reserved2[8]', 'postStatus', 'reserved3[4]'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_SCHD):
|
||||
UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
|
||||
'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
|
||||
'ptlSly'
|
||||
]),
|
||||
(CLASS_MON, MSG_MON_VER):
|
||||
UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
|
||||
'_remaining', '30s', ['extension']),
|
||||
(CLASS_TIM, MSG_TIM_TP):
|
||||
UBloxDescriptor('TIM_TP', '<IIiHBB',
|
||||
['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
|
||||
(CLASS_TIM, MSG_TIM_TM2):
|
||||
UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
|
||||
'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
|
||||
'accEst'
|
||||
]),
|
||||
(CLASS_TIM, MSG_TIM_SVIN):
|
||||
UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
|
||||
'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
|
||||
]),
|
||||
(CLASS_INF, MSG_INF_ERROR):
|
||||
UBloxDescriptor('INF_ERR', '<18s', ['str']),
|
||||
(CLASS_INF, MSG_INF_DEBUG):
|
||||
UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
|
||||
}
|
||||
|
||||
|
||||
class UBloxMessage:
|
||||
'''UBlox message class - holds a UBX binary message'''
|
||||
|
||||
def __init__(self):
|
||||
self._buf = ""
|
||||
self._fields = {}
|
||||
self._recs = []
|
||||
self._unpacked = False
|
||||
self.debug_level = 1
|
||||
|
||||
def __str__(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def as_dict(self):
|
||||
'''format a message as a string'''
|
||||
if not self.valid():
|
||||
return 'UBloxMessage(INVALID)'
|
||||
type = self.msg_type()
|
||||
if type in msg_types:
|
||||
return msg_types[type].format(self)
|
||||
return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
|
||||
|
||||
def __getattr__(self, name):
|
||||
'''allow access to message fields'''
|
||||
try:
|
||||
return self._fields[name]
|
||||
except KeyError:
|
||||
if name == 'recs':
|
||||
return self._recs
|
||||
raise AttributeError(name)
|
||||
|
||||
def __setattr__(self, name, value):
|
||||
'''allow access to message fields'''
|
||||
if name.startswith('_'):
|
||||
self.__dict__[name] = value
|
||||
else:
|
||||
self._fields[name] = value
|
||||
|
||||
def have_field(self, name):
|
||||
'''return True if a message contains the given field'''
|
||||
return name in self._fields
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def unpack(self):
|
||||
'''unpack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
msg_types[type].unpack(self)
|
||||
return self._fields, self._recs
|
||||
|
||||
def pack(self):
|
||||
'''pack a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
raise UBloxError('Unknown message %s' % str(type))
|
||||
msg_types[type].pack(self)
|
||||
|
||||
def name(self):
|
||||
'''return the short string name for a message'''
|
||||
if not self.valid():
|
||||
raise UBloxError('INVALID MESSAGE')
|
||||
type = self.msg_type()
|
||||
if not type in msg_types:
|
||||
raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf)))
|
||||
return msg_types[type].name
|
||||
|
||||
def msg_class(self):
|
||||
'''return the message class'''
|
||||
return ord(self._buf[2])
|
||||
|
||||
def msg_id(self):
|
||||
'''return the message id within the class'''
|
||||
return ord(self._buf[3])
|
||||
|
||||
def msg_type(self):
|
||||
'''return the message type tuple (class, id)'''
|
||||
return (self.msg_class(), self.msg_id())
|
||||
|
||||
def msg_length(self):
|
||||
'''return the payload length'''
|
||||
(payload_length, ) = struct.unpack('<H', self._buf[4:6])
|
||||
return payload_length
|
||||
|
||||
def valid_so_far(self):
|
||||
'''check if the message is valid so far'''
|
||||
if len(self._buf) > 0 and ord(self._buf[0]) != PREAMBLE1:
|
||||
return False
|
||||
if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2:
|
||||
self.debug(1, "bad pre2")
|
||||
return False
|
||||
if self.needed_bytes() == 0 and not self.valid():
|
||||
if len(self._buf) > 8:
|
||||
self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf),
|
||||
self.needed_bytes()))
|
||||
else:
|
||||
self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes()))
|
||||
return False
|
||||
return True
|
||||
|
||||
def add(self, bytes):
|
||||
'''add some bytes to a message'''
|
||||
self._buf += bytes
|
||||
while not self.valid_so_far() and len(self._buf) > 0:
|
||||
'''handle corrupted streams'''
|
||||
self._buf = self._buf[1:]
|
||||
if self.needed_bytes() < 0:
|
||||
self._buf = ""
|
||||
|
||||
def checksum(self, data=None):
|
||||
'''return a checksum tuple for a message'''
|
||||
if data is None:
|
||||
data = self._buf[2:-2]
|
||||
#cs = 0
|
||||
ck_a = 0
|
||||
ck_b = 0
|
||||
for i in data:
|
||||
ck_a = (ck_a + ord(i)) & 0xFF
|
||||
ck_b = (ck_b + ck_a) & 0xFF
|
||||
return (ck_a, ck_b)
|
||||
|
||||
def valid_checksum(self):
|
||||
'''check if the checksum is OK'''
|
||||
(ck_a, ck_b) = self.checksum()
|
||||
#d = self._buf[2:-2]
|
||||
(ck_a2, ck_b2) = struct.unpack('<BB', self._buf[-2:])
|
||||
return ck_a == ck_a2 and ck_b == ck_b2
|
||||
|
||||
def needed_bytes(self):
|
||||
'''return number of bytes still needed'''
|
||||
if len(self._buf) < 6:
|
||||
return 8 - len(self._buf)
|
||||
return self.msg_length() + 8 - len(self._buf)
|
||||
|
||||
def valid(self):
|
||||
'''check if a message is valid'''
|
||||
return len(self._buf) >= 8 and self.needed_bytes() == 0 and self.valid_checksum()
|
||||
|
||||
|
||||
class UBlox:
|
||||
'''main UBlox control class.
|
||||
|
||||
port can be a file (for reading only) or a serial device
|
||||
'''
|
||||
|
||||
def __init__(self, port, baudrate=115200, timeout=0, panda=False, grey=False):
|
||||
|
||||
self.serial_device = port
|
||||
self.baudrate = baudrate
|
||||
self.use_sendrecv = False
|
||||
self.read_only = False
|
||||
self.debug_level = 0
|
||||
|
||||
if panda:
|
||||
from panda import Panda, PandaSerial
|
||||
|
||||
self.panda = Panda()
|
||||
|
||||
# resetting U-Blox module
|
||||
self.panda.set_esp_power(0)
|
||||
time.sleep(0.1)
|
||||
self.panda.set_esp_power(1)
|
||||
time.sleep(0.5)
|
||||
|
||||
# can't set above 9600 now...
|
||||
self.baudrate = 9600
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
|
||||
self.baudrate = 460800
|
||||
print "upping baud:",self.baudrate
|
||||
self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate)
|
||||
time.sleep(0.1)
|
||||
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
elif grey:
|
||||
import zmq
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
|
||||
class BoarddSerial(object):
|
||||
def __init__(self):
|
||||
context = zmq.Context()
|
||||
self.ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port)
|
||||
self.buf = ""
|
||||
|
||||
def read(self, n):
|
||||
for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n):
|
||||
self.buf += msg.ubloxRaw
|
||||
ret = self.buf[:n]
|
||||
self.buf = self.buf[n:]
|
||||
return ret
|
||||
|
||||
|
||||
def write(self, dat):
|
||||
pass
|
||||
|
||||
self.dev = BoarddSerial()
|
||||
else:
|
||||
if self.serial_device.startswith("tcp:"):
|
||||
import socket
|
||||
a = self.serial_device.split(':')
|
||||
destination_addr = (a[1], int(a[2]))
|
||||
self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.dev.connect(destination_addr)
|
||||
self.dev.setblocking(1)
|
||||
self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
|
||||
self.use_sendrecv = True
|
||||
elif os.path.isfile(self.serial_device):
|
||||
self.read_only = True
|
||||
self.dev = open(self.serial_device, mode='rb')
|
||||
else:
|
||||
import serial
|
||||
self.dev = serial.Serial(
|
||||
self.serial_device,
|
||||
baudrate=self.baudrate,
|
||||
dsrdtr=False,
|
||||
rtscts=False,
|
||||
xonxoff=False,
|
||||
timeout=timeout)
|
||||
|
||||
self.logfile = None
|
||||
self.log = None
|
||||
self.preferred_dynamic_model = None
|
||||
self.preferred_usePPP = None
|
||||
self.preferred_dgps_timeout = None
|
||||
|
||||
def close(self):
|
||||
'''close the device'''
|
||||
self.dev.close()
|
||||
self.dev = None
|
||||
|
||||
def set_debug(self, debug_level):
|
||||
'''set debug level'''
|
||||
self.debug_level = debug_level
|
||||
|
||||
def debug(self, level, msg):
|
||||
'''write a debug message'''
|
||||
if self.debug_level >= level:
|
||||
print(msg)
|
||||
|
||||
def set_logfile(self, logfile, append=False):
|
||||
'''setup logging to a file'''
|
||||
if self.log is not None:
|
||||
self.log.close()
|
||||
self.log = None
|
||||
self.logfile = logfile
|
||||
if self.logfile is not None:
|
||||
if append:
|
||||
mode = 'ab'
|
||||
else:
|
||||
mode = 'wb'
|
||||
self.log = open(self.logfile, mode=mode)
|
||||
|
||||
def set_preferred_dynamic_model(self, model):
|
||||
'''set the preferred dynamic model for receiver'''
|
||||
self.preferred_dynamic_model = model
|
||||
if model is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_dgps_timeout(self, timeout):
|
||||
'''set the preferred DGPS timeout for receiver'''
|
||||
self.preferred_dgps_timeout = timeout
|
||||
if timeout is not None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
|
||||
def set_preferred_usePPP(self, usePPP):
|
||||
'''set the preferred usePPP setting for the receiver'''
|
||||
if usePPP is None:
|
||||
self.preferred_usePPP = None
|
||||
return
|
||||
self.preferred_usePPP = int(usePPP)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def nmea_checksum(self, msg):
|
||||
d = msg[1:]
|
||||
cs = 0
|
||||
for i in d:
|
||||
cs ^= ord(i)
|
||||
return cs
|
||||
|
||||
def write(self, buf):
|
||||
'''write some bytes'''
|
||||
if not self.read_only:
|
||||
if self.use_sendrecv:
|
||||
return self.dev.send(buf)
|
||||
return self.dev.write(buf)
|
||||
|
||||
def read(self, n):
|
||||
'''read some bytes'''
|
||||
if self.use_sendrecv:
|
||||
import socket
|
||||
try:
|
||||
return self.dev.recv(n)
|
||||
except socket.error:
|
||||
return ''
|
||||
return self.dev.read(n)
|
||||
|
||||
def send_nmea(self, msg):
|
||||
if not self.read_only:
|
||||
s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n"
|
||||
self.write(s)
|
||||
|
||||
def set_binary(self):
|
||||
'''put a UBlox into binary mode using a NMEA string'''
|
||||
if not self.read_only:
|
||||
print("try set binary at %u" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate)
|
||||
self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate)
|
||||
|
||||
def disable_nmea(self):
|
||||
''' stop sending all types of nmea messages '''
|
||||
self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0")
|
||||
self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0")
|
||||
self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0")
|
||||
|
||||
def seek_percent(self, pct):
|
||||
'''seek to the given percentage of a file'''
|
||||
self.dev.seek(0, 2)
|
||||
filesize = self.dev.tell()
|
||||
self.dev.seek(pct * 0.01 * filesize)
|
||||
|
||||
def special_handling(self, msg):
|
||||
'''handle automatic configuration changes'''
|
||||
if msg.name() == 'CFG_NAV5':
|
||||
msg.unpack()
|
||||
sendit = False
|
||||
pollit = False
|
||||
if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model:
|
||||
msg.dynModel = self.preferred_dynamic_model
|
||||
sendit = True
|
||||
pollit = True
|
||||
if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout:
|
||||
msg.dgpsTimeOut = self.preferred_dgps_timeout
|
||||
self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut)
|
||||
sendit = True
|
||||
# we don't re-poll for this one, as some receivers refuse to set it
|
||||
if sendit:
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
if pollit:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAV5)
|
||||
if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None:
|
||||
msg.unpack()
|
||||
if msg.usePPP != self.preferred_usePPP:
|
||||
msg.usePPP = self.preferred_usePPP
|
||||
msg.mask = 1 << 13
|
||||
msg.pack()
|
||||
self.send(msg)
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5)
|
||||
|
||||
def receive_message(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message'''
|
||||
msg = UBloxMessage()
|
||||
while True:
|
||||
n = msg.needed_bytes()
|
||||
b = self.read(n)
|
||||
if not b:
|
||||
if ignore_eof:
|
||||
time.sleep(0.01)
|
||||
continue
|
||||
if len(msg._buf) > 0:
|
||||
self.debug(1, "dropping %d bytes" % len(msg._buf))
|
||||
return None
|
||||
msg.add(b)
|
||||
if self.log is not None:
|
||||
self.log.write(b)
|
||||
self.log.flush()
|
||||
if msg.valid():
|
||||
self.special_handling(msg)
|
||||
return msg
|
||||
|
||||
def receive_message_noerror(self, ignore_eof=False):
|
||||
'''blocking receive of one ublox message, ignoring errors'''
|
||||
try:
|
||||
return self.receive_message(ignore_eof=ignore_eof)
|
||||
except UBloxError as e:
|
||||
print(e)
|
||||
return None
|
||||
except OSError as e:
|
||||
# Occasionally we get hit with 'resource temporarily unavailable'
|
||||
# messages here on the serial device, catch them too.
|
||||
print(e)
|
||||
return None
|
||||
|
||||
def send(self, msg):
|
||||
'''send a preformatted ublox message'''
|
||||
if not msg.valid():
|
||||
self.debug(1, "invalid send")
|
||||
return
|
||||
if not self.read_only:
|
||||
self.write(msg._buf)
|
||||
|
||||
def send_message(self, msg_class, msg_id, payload):
|
||||
'''send a ublox message with class, id and payload'''
|
||||
msg = UBloxMessage()
|
||||
msg._buf = struct.pack('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
|
||||
msg._buf += payload
|
||||
(ck_a, ck_b) = msg.checksum(msg._buf[2:])
|
||||
msg._buf += struct.pack('<BB', ck_a, ck_b)
|
||||
self.send(msg)
|
||||
|
||||
def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
|
||||
'''configure the solution rate in milliseconds'''
|
||||
payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
|
||||
|
||||
def configure_message_rate(self, msg_class, msg_id, rate):
|
||||
'''configure the message rate for a given message'''
|
||||
payload = struct.pack('<BBB', msg_class, msg_id, rate)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
|
||||
|
||||
def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
|
||||
'''configure a IO port'''
|
||||
if baudrate is None:
|
||||
baudrate = self.baudrate
|
||||
payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
|
||||
outMask, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
|
||||
|
||||
def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
|
||||
'''configure configuration load/save'''
|
||||
payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
|
||||
|
||||
def configure_poll(self, msg_class, msg_id, payload=''):
|
||||
'''poll a configuration message'''
|
||||
self.send_message(msg_class, msg_id, payload)
|
||||
|
||||
def configure_poll_port(self, portID=None):
|
||||
'''poll a port configuration'''
|
||||
if portID is None:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
|
||||
else:
|
||||
self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
|
||||
|
||||
def configure_min_max_sats(self, min_sats=4, max_sats=32):
|
||||
'''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
|
||||
payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
|
||||
|
||||
def module_reset(self, set, mode):
|
||||
''' Reset the module for hot/warm/cold start'''
|
||||
payload = struct.pack('<HBB', set, mode, 0)
|
||||
self.send_message(CLASS_CFG, MSG_CFG_RST, payload)
|
|
@ -0,0 +1,286 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import serial
|
||||
import ublox
|
||||
import time
|
||||
import datetime
|
||||
import struct
|
||||
import sys
|
||||
from cereal import log
|
||||
from common import realtime
|
||||
import zmq
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
from ephemeris import EphemerisData, GET_FIELD_U
|
||||
|
||||
panda = os.getenv("PANDA") is not None # panda directly connected
|
||||
grey = not (os.getenv("EVAL") is not None) # panda through boardd
|
||||
debug = os.getenv("DEBUG") is not None # debug prints
|
||||
print_dB = os.getenv("PRINT_DB") is not None # print antenna dB
|
||||
|
||||
timeout = 1
|
||||
dyn_model = 4 # auto model
|
||||
baudrate = 460800
|
||||
ports = ["/dev/ttyACM0","/dev/ttyACM1"]
|
||||
rate = 100 # send new data every 100ms
|
||||
|
||||
# which SV IDs we have seen and when we got iono
|
||||
svid_seen = {}
|
||||
svid_ephemeris = {}
|
||||
iono_seen = 0
|
||||
|
||||
def configure_ublox(dev):
|
||||
# configure ports and solution parameters and rate
|
||||
# TODO: configure constellations and channels to allow for 10Hz and high precision
|
||||
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
|
||||
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
|
||||
|
||||
if panda:
|
||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
|
||||
else:
|
||||
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
|
||||
|
||||
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
|
||||
dev.configure_poll_port()
|
||||
dev.configure_poll_port(ublox.PORT_SERIAL1)
|
||||
dev.configure_poll_port(ublox.PORT_SERIAL2)
|
||||
dev.configure_poll_port(ublox.PORT_USB)
|
||||
dev.configure_solution_rate(rate_ms=rate)
|
||||
|
||||
# Configure solution
|
||||
payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
|
||||
payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
|
||||
3, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
|
||||
#payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
|
||||
# 0, 0, 0, 0, 0, 0,
|
||||
# 0, 0, 0, 0, 0, 0,
|
||||
# 0, 0, 0, 0, 0, 0,
|
||||
# 0, 0, 0, 0, 0, 0,
|
||||
# 0, 0, 0, 0)
|
||||
#dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
|
||||
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
|
||||
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
|
||||
|
||||
# Configure RAW and PVT messages to be sent every solution cycle
|
||||
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
|
||||
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
|
||||
|
||||
|
||||
|
||||
def int_to_bool_list(num):
|
||||
# for parsing bool bytes
|
||||
return [bool(num & (1<<n)) for n in range(8)]
|
||||
|
||||
|
||||
def gen_ephemeris(ephem_data):
|
||||
ephem = {'ephemeris':
|
||||
{'svId': ephem_data.svId,
|
||||
|
||||
'toc': ephem_data.toc,
|
||||
'gpsWeek': ephem_data.gpsWeek,
|
||||
|
||||
'af0': ephem_data.af0,
|
||||
'af1': ephem_data.af1,
|
||||
'af2': ephem_data.af2,
|
||||
|
||||
'iode': ephem_data.iode,
|
||||
'crs': ephem_data.crs,
|
||||
'deltaN': ephem_data.deltaN,
|
||||
'm0': ephem_data.M0,
|
||||
|
||||
'cuc': ephem_data.cuc,
|
||||
'ecc': ephem_data.ecc,
|
||||
'cus': ephem_data.cus,
|
||||
'a': ephem_data.A,
|
||||
|
||||
'toe': ephem_data.toe,
|
||||
'cic': ephem_data.cic,
|
||||
'omega0': ephem_data.omega0,
|
||||
'cis': ephem_data.cis,
|
||||
|
||||
'i0': ephem_data.i0,
|
||||
'crc': ephem_data.crc,
|
||||
'omega': ephem_data.omega,
|
||||
'omegaDot': ephem_data.omega_dot,
|
||||
|
||||
'iDot': ephem_data.idot,
|
||||
|
||||
'tgd': ephem_data.Tgd,
|
||||
|
||||
'ionoCoeffsValid': ephem_data.ionoCoeffsValid,
|
||||
'ionoAlpha': ephem_data.ionoAlpha,
|
||||
'ionoBeta': ephem_data.ionoBeta}}
|
||||
return log.Event.new_message(ubloxGnss=ephem)
|
||||
|
||||
|
||||
def gen_solution(msg):
|
||||
msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
|
||||
timestamp = int(((datetime.datetime(msg_data['year'],
|
||||
msg_data['month'],
|
||||
msg_data['day'],
|
||||
msg_data['hour'],
|
||||
msg_data['min'],
|
||||
msg_data['sec'])
|
||||
- datetime.datetime(1970,1,1)).total_seconds())*1e+03
|
||||
+ msg_data['nano']*1e-06)
|
||||
gps_fix = {'bearing': msg_data['headMot']*1e-05, # heading of motion in degrees
|
||||
'altitude': msg_data['height']*1e-03, # altitude above ellipsoid
|
||||
'latitude': msg_data['lat']*1e-07, # latitude in degrees
|
||||
'longitude': msg_data['lon']*1e-07, # longitude in degrees
|
||||
'speed': msg_data['gSpeed']*1e-03, # ground speed in meters
|
||||
'accuracy': msg_data['hAcc']*1e-03, # horizontal accuracy (1 sigma?)
|
||||
'timestamp': timestamp, # UTC time in ms since start of UTC stime
|
||||
'vNED': [msg_data['velN']*1e-03,
|
||||
msg_data['velE']*1e-03,
|
||||
msg_data['velD']*1e-03], # velocity in NED frame in m/s
|
||||
'speedAccuracy': msg_data['sAcc']*1e-03, # speed accuracy in m/s
|
||||
'verticalAccuracy': msg_data['vAcc']*1e-03, # vertical accuracy in meters
|
||||
'bearingAccuracy': msg_data['headAcc']*1e-05, # heading accuracy in degrees
|
||||
'source': 'ublox'}
|
||||
return log.Event.new_message(gpsLocationExternal=gps_fix)
|
||||
|
||||
def gen_nav_data(msg, nav_frame_buffer):
|
||||
# TODO this stuff needs to be parsed and published.
|
||||
# refer to https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_%28UBX-13003221%29.pdf
|
||||
# section 9.1
|
||||
msg_meta_data, measurements = msg.unpack()
|
||||
|
||||
# parse GPS ephem
|
||||
gnssId = msg_meta_data['gnssId']
|
||||
if gnssId == 0:
|
||||
svId = msg_meta_data['svid']
|
||||
subframeId = GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
|
||||
words = []
|
||||
for m in measurements:
|
||||
words.append(m['dwrd'])
|
||||
|
||||
# parse from
|
||||
if subframeId == 1:
|
||||
nav_frame_buffer[gnssId][svId] = {}
|
||||
nav_frame_buffer[gnssId][svId][subframeId] = words
|
||||
elif subframeId-1 in nav_frame_buffer[gnssId][svId]:
|
||||
nav_frame_buffer[gnssId][svId][subframeId] = words
|
||||
if len(nav_frame_buffer[gnssId][svId]) == 5:
|
||||
ephem_data = EphemerisData(svId, nav_frame_buffer[gnssId][svId])
|
||||
return gen_ephemeris(ephem_data)
|
||||
|
||||
|
||||
|
||||
|
||||
def gen_raw(msg):
|
||||
# meta data is in first part of tuple
|
||||
# list of measurements is in second part
|
||||
msg_meta_data, measurements = msg.unpack()
|
||||
measurements_parsed = []
|
||||
for m in measurements:
|
||||
trackingStatus_bools = int_to_bool_list(m['trkStat'])
|
||||
trackingStatus = {'pseudorangeValid': trackingStatus_bools[0],
|
||||
'carrierPhaseValid': trackingStatus_bools[1],
|
||||
'halfCycleValid': trackingStatus_bools[2],
|
||||
'halfCycleSubtracted': trackingStatus_bools[3]}
|
||||
measurements_parsed.append({
|
||||
'svId': m['svId'],
|
||||
'pseudorange': m['prMes'],
|
||||
'carrierCycles': m['cpMes'],
|
||||
'doppler': m['doMes'],
|
||||
'gnssId': m['gnssId'],
|
||||
'glonassFrequencyIndex': m['freqId'],
|
||||
'locktime': m['locktime'],
|
||||
'cno': m['cno'],
|
||||
'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
|
||||
'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
|
||||
'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
|
||||
'trackingStatus': trackingStatus})
|
||||
if print_dB:
|
||||
cnos = {}
|
||||
for meas in measurements_parsed:
|
||||
cnos[meas['svId']] = meas['cno']
|
||||
print 'Carrier to noise ratio for each sat: \n', cnos, '\n'
|
||||
receiverStatus_bools = int_to_bool_list(msg_meta_data['recStat'])
|
||||
receiverStatus = {'leapSecValid': receiverStatus_bools[0],
|
||||
'clkReset': receiverStatus_bools[2]}
|
||||
raw_meas = {'measurementReport': {'rcvTow': msg_meta_data['rcvTow'],
|
||||
'gpsWeek': msg_meta_data['week'],
|
||||
'leapSeconds': msg_meta_data['leapS'],
|
||||
'receiverStatus': receiverStatus,
|
||||
'numMeas': msg_meta_data['numMeas'],
|
||||
'measurements': measurements_parsed}}
|
||||
return log.Event.new_message(ubloxGnss=raw_meas)
|
||||
|
||||
def init_reader():
|
||||
port_counter = 0
|
||||
while True:
|
||||
try:
|
||||
dev = ublox.UBlox(ports[port_counter], baudrate=baudrate, timeout=timeout, panda=panda, grey=grey)
|
||||
configure_ublox(dev)
|
||||
return dev
|
||||
except serial.serialutil.SerialException as e:
|
||||
print(e)
|
||||
port_counter = (port_counter + 1)%len(ports)
|
||||
time.sleep(2)
|
||||
|
||||
def handle_msg(dev, msg, nav_frame_buffer):
|
||||
try:
|
||||
if debug:
|
||||
print(str(msg))
|
||||
sys.stdout.flush()
|
||||
if msg.name() == 'NAV_PVT':
|
||||
sol = gen_solution(msg)
|
||||
sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
gpsLocationExternal.send(sol.to_bytes())
|
||||
elif msg.name() == 'RXM_RAW':
|
||||
raw = gen_raw(msg)
|
||||
raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(raw.to_bytes())
|
||||
elif msg.name() == 'RXM_SFRBX':
|
||||
nav = gen_nav_data(msg, nav_frame_buffer)
|
||||
if nav is not None:
|
||||
nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
|
||||
ubloxGnss.send(nav.to_bytes())
|
||||
|
||||
else:
|
||||
print "UNKNNOWN MESSAGE:", msg.name()
|
||||
except ublox.UBloxError as e:
|
||||
print(e)
|
||||
|
||||
#if dev is not None and dev.dev is not None:
|
||||
# dev.close()
|
||||
|
||||
def main(gctx=None):
|
||||
global gpsLocationExternal, ubloxGnss
|
||||
nav_frame_buffer = {}
|
||||
nav_frame_buffer[0] = {}
|
||||
for i in xrange(1,33):
|
||||
nav_frame_buffer[0][i] = {}
|
||||
|
||||
|
||||
context = zmq.Context()
|
||||
gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
|
||||
ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
|
||||
|
||||
dev = init_reader()
|
||||
while True:
|
||||
try:
|
||||
msg = dev.receive_message()
|
||||
except serial.serialutil.SerialException as e:
|
||||
print(e)
|
||||
dev.close()
|
||||
dev = init_reader()
|
||||
if msg is not None:
|
||||
handle_msg(dev, msg, nav_frame_buffer)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
Binary file not shown.
|
@ -53,7 +53,6 @@ os.environ['BASEDIR'] = BASEDIR
|
|||
|
||||
import zmq
|
||||
from setproctitle import setproctitle
|
||||
from smbus2 import SMBus
|
||||
|
||||
from common.params import Params
|
||||
from common.realtime import sec_since_boot
|
||||
|
@ -63,7 +62,7 @@ from selfdrive.swaglog import cloudlog
|
|||
import selfdrive.messaging as messaging
|
||||
from selfdrive.thermal import read_thermal
|
||||
from selfdrive.registration import register
|
||||
from selfdrive.version import version
|
||||
from selfdrive.version import version, dirty
|
||||
import selfdrive.crash as crash
|
||||
|
||||
from selfdrive.loggerd.config import ROOT
|
||||
|
@ -75,6 +74,8 @@ managed_processes = {
|
|||
"uploader": "selfdrive.loggerd.uploader",
|
||||
"controlsd": "selfdrive.controls.controlsd",
|
||||
"radard": "selfdrive.controls.radard",
|
||||
"ubloxd": "selfdrive.locationd.ubloxd",
|
||||
"locationd_dummy": "selfdrive.locationd.locationd_dummy",
|
||||
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
|
||||
"logmessaged": "selfdrive.logmessaged",
|
||||
"tombstoned": "selfdrive.tombstoned",
|
||||
|
@ -107,6 +108,8 @@ persistent_processes = [
|
|||
'uploader',
|
||||
'ui',
|
||||
'gpsd',
|
||||
'ubloxd',
|
||||
'locationd_dummy',
|
||||
'updated',
|
||||
]
|
||||
|
||||
|
@ -231,28 +234,22 @@ def cleanup_all_processes(signal, frame):
|
|||
|
||||
# ****************** run loop ******************
|
||||
|
||||
def manager_init():
|
||||
def manager_init(should_register=True):
|
||||
global gctx
|
||||
|
||||
if should_register:
|
||||
reg_res = register()
|
||||
if reg_res:
|
||||
dongle_id, dongle_secret = reg_res
|
||||
else:
|
||||
raise Exception("server registration failed")
|
||||
else:
|
||||
dongle_id = "c"*16
|
||||
|
||||
# set dongle id
|
||||
cloudlog.info("dongle id is " + dongle_id)
|
||||
os.environ['DONGLE_ID'] = dongle_id
|
||||
|
||||
if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
|
||||
upstream = "origin/master"
|
||||
else:
|
||||
if 'chffrplus' in version:
|
||||
upstream = "origin/release"
|
||||
else:
|
||||
upstream = "origin/release2"
|
||||
|
||||
dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0
|
||||
cloudlog.info("dirty is %d" % dirty)
|
||||
if not dirty:
|
||||
os.environ['CLEAN'] = '1'
|
||||
|
@ -286,6 +283,7 @@ def setup_eon_fan():
|
|||
|
||||
os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch")
|
||||
|
||||
from smbus2 import SMBus
|
||||
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
|
||||
|
@ -300,6 +298,7 @@ def set_eon_fan(val):
|
|||
if not EON:
|
||||
return
|
||||
|
||||
from smbus2 import SMBus
|
||||
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)
|
||||
|
@ -391,7 +390,6 @@ def manager_thread():
|
|||
|
||||
params = Params()
|
||||
|
||||
passive = params.get("Passive") == "1"
|
||||
passive_starter = LocationStarter()
|
||||
|
||||
started_ts = None
|
||||
|
@ -400,6 +398,7 @@ def manager_thread():
|
|||
fan_speed = 0
|
||||
ignition_seen = False
|
||||
battery_was_high = False
|
||||
panda_seen = False
|
||||
|
||||
health_sock.RCVTIMEO = 1500
|
||||
|
||||
|
@ -455,13 +454,20 @@ def manager_thread():
|
|||
ignition = td is not None and td.health.started
|
||||
ignition_seen = ignition_seen or ignition
|
||||
|
||||
# add voltage check for ignition
|
||||
if not ignition_seen and td is not None and td.health.voltage > 13500:
|
||||
ignition = True
|
||||
|
||||
do_uninstall = params.get("DoUninstall") == "1"
|
||||
accepted_terms = params.get("HasAcceptedTerms") == "1"
|
||||
|
||||
should_start = ignition
|
||||
|
||||
# start on gps in passive mode
|
||||
if passive and not ignition_seen:
|
||||
# have we seen a panda?
|
||||
panda_seen = panda_seen or td is not None
|
||||
|
||||
# start on gps if we have no connection to a panda
|
||||
if not panda_seen:
|
||||
should_start = should_start or passive_starter.update(started_ts, location)
|
||||
|
||||
# with 2% left, we killall, otherwise the phone will take a long time to boot
|
||||
|
@ -509,6 +515,7 @@ def manager_thread():
|
|||
running=running.keys(),
|
||||
count=count,
|
||||
health=(td.to_dict() if td else None),
|
||||
location=(location.to_dict() if location else None),
|
||||
thermal=msg.to_dict())
|
||||
|
||||
if do_uninstall:
|
||||
|
@ -641,7 +648,12 @@ def main():
|
|||
if params.get("IsUploadVideoOverCellularEnabled") is None:
|
||||
params.put("IsUploadVideoOverCellularEnabled", "1")
|
||||
|
||||
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
|
||||
# is this chffrplus?
|
||||
if os.getenv("PASSIVE") is not None:
|
||||
params.put("Passive", str(int(os.getenv("PASSIVE"))))
|
||||
|
||||
if params.get("Passive") is None:
|
||||
raise Exception("Passive must be set to continue")
|
||||
|
||||
# put something on screen while we set things up
|
||||
if os.getenv("PREPAREONLY") is not None:
|
||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -55,13 +55,22 @@ gpsPlannerPoints: [8043, true]
|
|||
gpsPlannerPlan: [8044, true]
|
||||
applanixRaw: [8046, true]
|
||||
orbLocation: [8047, true]
|
||||
trafficSigns: [8048, true]
|
||||
trafficEvents: [8048, true]
|
||||
liveLocationTiming: [8049, true]
|
||||
orbslamCorrection: [8050, true]
|
||||
liveLocationCorrected: [8051, true]
|
||||
orbObservation: [8052, true]
|
||||
applanixLocation: [8053, true]
|
||||
liveLocationKalman: [8054, true]
|
||||
uiNavigationEvent: [8055, true]
|
||||
orbOdometry: [8057, true]
|
||||
orbFeatures: [8058, true]
|
||||
orbKeyFrame: [8059, true]
|
||||
uiLayoutState: [8060, true]
|
||||
|
||||
testModel: [8040, false]
|
||||
testLiveLocation: [8045, false]
|
||||
testJoystick: [8056, false]
|
||||
|
||||
|
||||
# manager -- base process to manage starting and stopping of all others
|
||||
|
|
|
@ -170,7 +170,7 @@ class Plant(object):
|
|||
fcw = True
|
||||
|
||||
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
|
||||
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
|
||||
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
|
||||
else:
|
||||
brake = 0.0
|
||||
|
||||
|
|
|
@ -7,7 +7,7 @@ import datetime
|
|||
from raven import Client
|
||||
from raven.transport.http import HTTPTransport
|
||||
|
||||
from selfdrive.version import version
|
||||
from selfdrive.version import version, dirty
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
def get_tombstones():
|
||||
|
@ -69,7 +69,7 @@ def main(gctx):
|
|||
initial_tombstones = set(get_tombstones())
|
||||
|
||||
client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615',
|
||||
install_sys_hook=False, transport=HTTPTransport)
|
||||
install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty})
|
||||
|
||||
while True:
|
||||
now_tombstones = set(get_tombstones())
|
||||
|
|
|
@ -44,13 +44,28 @@
|
|||
#define STATUS_ALERT 4
|
||||
#define STATUS_MAX 5
|
||||
|
||||
#define ALERTSIZE_NONE 0
|
||||
#define ALERTSIZE_SMALL 1
|
||||
#define ALERTSIZE_MID 2
|
||||
#define ALERTSIZE_FULL 3
|
||||
|
||||
#define UI_BUF_COUNT 4
|
||||
|
||||
|
||||
const int box_x = 330;
|
||||
const int box_y = 30;
|
||||
const int box_width = 1560;
|
||||
const int box_height = 1020;
|
||||
const int vwp_w = 1920;
|
||||
const int vwp_h = 1080;
|
||||
const int nav_w = 0;
|
||||
const int nav_ww= 0;
|
||||
const int sbr_w = 300;
|
||||
const int bdr_s = 30;
|
||||
const int box_x = sbr_w+bdr_s;
|
||||
const int box_y = bdr_s;
|
||||
const int box_w = vwp_w-sbr_w-(bdr_s*2);
|
||||
const int box_h = vwp_h-(bdr_s*2);
|
||||
const int viz_x = box_x+nav_w;
|
||||
const int viz_y = box_y;
|
||||
const int viz_w = box_w-nav_w;
|
||||
const int viz_h = box_h;
|
||||
const int viz_header_h = 420;
|
||||
|
||||
const uint8_t bg_colors[][4] = {
|
||||
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
|
||||
|
@ -61,11 +76,18 @@ const uint8_t bg_colors[][4] = {
|
|||
};
|
||||
|
||||
const uint8_t alert_colors[][4] = {
|
||||
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0x80},
|
||||
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0x80},
|
||||
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0x80},
|
||||
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0x80},
|
||||
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0x80},
|
||||
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xf1},
|
||||
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xf1},
|
||||
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xf1},
|
||||
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xf1},
|
||||
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xf1},
|
||||
};
|
||||
|
||||
const int alert_sizes[] = {
|
||||
[ALERTSIZE_NONE] = 0,
|
||||
[ALERTSIZE_SMALL] = 241,
|
||||
[ALERTSIZE_MID] = 390,
|
||||
[ALERTSIZE_FULL] = vwp_h,
|
||||
};
|
||||
|
||||
typedef struct UIScene {
|
||||
|
@ -91,6 +113,15 @@ typedef struct UIScene {
|
|||
float curvature;
|
||||
int engaged;
|
||||
|
||||
bool uilayout_sidebarcollapsed;
|
||||
bool uilayout_mapenabled;
|
||||
// responsive sizes for sidebar
|
||||
int ui_viz_rx;
|
||||
int ui_viz_rw;
|
||||
// responsize offset for frame + projections
|
||||
int ui_frame_offset;
|
||||
int ui_world_offset;
|
||||
|
||||
int lead_status;
|
||||
float lead_d_rel, lead_y_rel, lead_v_rel;
|
||||
|
||||
|
@ -110,7 +141,6 @@ typedef struct UIScene {
|
|||
int cal_status;
|
||||
int cal_perc;
|
||||
|
||||
|
||||
// Used to show gps planner status
|
||||
bool gps_planner_active;
|
||||
|
||||
|
@ -130,6 +160,8 @@ typedef struct UIState {
|
|||
int font_courbd;
|
||||
int font_sans_regular;
|
||||
int font_sans_semibold;
|
||||
int font_sans_bold;
|
||||
int img_wheel;
|
||||
|
||||
zsock_t *thermal_sock;
|
||||
void *thermal_sock_raw;
|
||||
|
@ -146,6 +178,9 @@ typedef struct UIState {
|
|||
zsock_t *plus_sock;
|
||||
void *plus_sock_raw;
|
||||
|
||||
zsock_t *uilayout_sock;
|
||||
void *uilayout_sock_raw;
|
||||
|
||||
int plus_state;
|
||||
|
||||
// vision state
|
||||
|
@ -185,6 +220,7 @@ typedef struct UIState {
|
|||
int status;
|
||||
bool is_metric;
|
||||
bool passive;
|
||||
int alert_size;
|
||||
|
||||
float light_sensor;
|
||||
} UIState;
|
||||
|
@ -273,7 +309,7 @@ static const mat4 device_transform = {{
|
|||
|
||||
// frame from 4/3 to box size with a 2x zoon
|
||||
static const mat4 frame_transform = {{
|
||||
2*(4./3.)/((float)box_width/box_height), 0.0, 0.0, 0.0,
|
||||
2*(4./3.)/((float)(box_w+sbr_w-(bdr_s*2))/viz_h), 0.0, 0.0, 0.0,
|
||||
0.0, 2.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0,
|
||||
|
@ -299,6 +335,10 @@ static void ui_init(UIState *s) {
|
|||
assert(s->live100_sock);
|
||||
s->live100_sock_raw = zsock_resolve(s->live100_sock);
|
||||
|
||||
s->uilayout_sock = zsock_new_sub(">tcp://127.0.0.1:8060", "");
|
||||
assert(s->uilayout_sock);
|
||||
s->uilayout_sock_raw = zsock_resolve(s->uilayout_sock);
|
||||
|
||||
s->livecalibration_sock = zsock_new_sub(">tcp://127.0.0.1:8019", "");
|
||||
assert(s->livecalibration_sock);
|
||||
s->livecalibration_sock_raw = zsock_resolve(s->livecalibration_sock);
|
||||
|
@ -334,6 +374,11 @@ static void ui_init(UIState *s) {
|
|||
assert(s->font_sans_regular >= 0);
|
||||
s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf");
|
||||
assert(s->font_sans_semibold >= 0);
|
||||
s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/OpenSans-Bold.ttf");
|
||||
assert(s->font_sans_bold >= 0);
|
||||
|
||||
assert(s->img_wheel >= 0);
|
||||
s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1);
|
||||
|
||||
// init gl
|
||||
s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader);
|
||||
|
@ -627,7 +672,7 @@ static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, s
|
|||
}
|
||||
|
||||
static void draw_path(UIState *s, const float *points, float off,
|
||||
NVGcolor color) {
|
||||
NVGcolor color, int width) {
|
||||
const UIScene *scene = &s->scene;
|
||||
|
||||
nvgSave(s->vg);
|
||||
|
@ -643,7 +688,7 @@ static void draw_path(UIState *s, const float *points, float off,
|
|||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgStrokeColor(s->vg, color);
|
||||
nvgStrokeWidth(s->vg, 5);
|
||||
nvgStrokeWidth(s->vg, width);
|
||||
bool started = false;
|
||||
|
||||
for (int i=0; i<50; i++) {
|
||||
|
@ -673,22 +718,21 @@ static void draw_path(UIState *s, const float *points, float off,
|
|||
}
|
||||
|
||||
static void draw_model_path(UIState *s, const PathData path, NVGcolor color) {
|
||||
draw_path(s, path.points, 0.0, color, 4*path.prob);
|
||||
float var = min(path.std, 0.7);
|
||||
draw_path(s, path.points, 0.0, color);
|
||||
color.a /= 4;
|
||||
draw_path(s, path.points, -var, color);
|
||||
draw_path(s, path.points, var, color);
|
||||
draw_path(s, path.points, -var, color, 2);
|
||||
draw_path(s, path.points, var, color, 2);
|
||||
}
|
||||
|
||||
static void draw_steering(UIState *s, float curvature) {
|
||||
|
||||
float points[50];
|
||||
for (int i = 0; i < 50; i++) {
|
||||
float y_actual = i * tan(asin(clamp(i * curvature, -0.999, 0.999)) / 2.);
|
||||
points[i] = y_actual;
|
||||
}
|
||||
|
||||
draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128));
|
||||
draw_path(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5);
|
||||
}
|
||||
|
||||
static void draw_frame(UIState *s) {
|
||||
|
@ -709,10 +753,10 @@ static void draw_frame(UIState *s) {
|
|||
} else {
|
||||
out_mat = matmul(device_transform, frame_transform);
|
||||
|
||||
x1 = 0.0;
|
||||
x2 = 1.0;
|
||||
y1 = 0.0;
|
||||
y2 = 1.0;
|
||||
x1 = 1.0;
|
||||
x2 = 0.0;
|
||||
y1 = 1.0;
|
||||
y2 = 0.0;
|
||||
}
|
||||
|
||||
const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
|
||||
|
@ -779,27 +823,101 @@ static void ui_draw_rounded_rect(
|
|||
nvgStroke(c);
|
||||
}
|
||||
|
||||
// Draw all world space objects.
|
||||
static void ui_draw_world(UIState *s) {
|
||||
static void fill_lane(UIState *s, const float *l_points, const float *r_points, float off,
|
||||
NVGcolor color) {
|
||||
const UIScene *scene = &s->scene;
|
||||
if (!scene->world_objects_visible) {
|
||||
return;
|
||||
|
||||
nvgSave(s->vg);
|
||||
|
||||
// path coords are worked out in rgb-box space
|
||||
nvgTranslate(s->vg, 240.0f, 0.0);
|
||||
|
||||
// zoom 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);
|
||||
bool started = false;
|
||||
|
||||
int gradient_start_x = 0;
|
||||
int gradient_start_y = 0;
|
||||
int gradient_end_x = 0;
|
||||
int gradient_end_y = 0;
|
||||
|
||||
// left side of lane
|
||||
for (int i=0; i<50; i++) {
|
||||
float px = (float)i;
|
||||
float py = l_points[i] + off;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//draw_steering(s, scene->curvature);
|
||||
if (!started) {
|
||||
nvgMoveTo(s->vg, x, y);
|
||||
gradient_start_x = x;
|
||||
gradient_start_y = y;
|
||||
started = true;
|
||||
} else {
|
||||
nvgLineTo(s->vg, x, y);
|
||||
}
|
||||
}
|
||||
|
||||
// right side of lane
|
||||
for (int i=50; i>0; i--) {
|
||||
float px = (float)i;
|
||||
float py = r_points[i] + off;
|
||||
|
||||
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);
|
||||
gradient_end_y = y;
|
||||
gradient_end_x = x;
|
||||
}
|
||||
}
|
||||
|
||||
NVGpaint bg = nvgLinearGradient(s->vg, gradient_start_x, gradient_start_y, gradient_end_x, gradient_end_y,
|
||||
nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.left_lane.prob), nvgRGBAf(0.6, 0.6, 0.6, 1.0*scene->model.right_lane.prob));
|
||||
|
||||
nvgFillPaint(s->vg, bg);
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgRestore(s->vg);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_lanes(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) {
|
||||
int left_lane_color = (int)(255 * scene->model.left_lane.prob);
|
||||
int right_lane_color = (int)(255 * scene->model.right_lane.prob);
|
||||
// draw left lane edge
|
||||
draw_model_path(
|
||||
s, scene->model.left_lane,
|
||||
nvgRGBA(left_lane_color, left_lane_color, left_lane_color, 128));
|
||||
nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob));
|
||||
|
||||
// draw right lane edge
|
||||
draw_model_path(
|
||||
s, scene->model.right_lane,
|
||||
nvgRGBA(right_lane_color, right_lane_color, right_lane_color, 128));
|
||||
nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob));
|
||||
|
||||
// draw paths
|
||||
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255));
|
||||
// draw projected path line
|
||||
draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255), 5);
|
||||
|
||||
// draw MPC only if engaged
|
||||
if (scene->engaged) {
|
||||
|
@ -808,110 +926,12 @@ static void ui_draw_world(UIState *s) {
|
|||
}
|
||||
}
|
||||
|
||||
static void ui_draw_vision(UIState *s) {
|
||||
static void ui_draw_vision_topbar(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
|
||||
glClearColor(0.0, 0.0, 0.0, 0.0);
|
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
||||
|
||||
// hack for eon ui
|
||||
glEnable(GL_SCISSOR_TEST);
|
||||
glScissor(box_x, s->fb_h-(box_y+box_height), box_width, box_height);
|
||||
glViewport(box_x, s->fb_h-(box_y+box_height), box_width, box_height);
|
||||
draw_frame(s);
|
||||
glViewport(0, 0, s->fb_w, s->fb_h);
|
||||
glDisable(GL_SCISSOR_TEST);
|
||||
|
||||
// nvg drawings
|
||||
glEnable(GL_BLEND);
|
||||
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
||||
// glEnable(GL_CULL_FACE);
|
||||
|
||||
glClear(GL_STENCIL_BUFFER_BIT);
|
||||
|
||||
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
|
||||
|
||||
nvgSave(s->vg);
|
||||
|
||||
// hack for eon ui
|
||||
const int inner_height = box_width*9/16;
|
||||
nvgScissor(s->vg, box_x, box_y, box_width, box_height);
|
||||
nvgTranslate(s->vg, box_x, box_y + (box_height-inner_height)/2.0);
|
||||
nvgScale(s->vg, (float)box_width / s->fb_w, (float)inner_height / s->fb_h);
|
||||
|
||||
if (!scene->frontview) {
|
||||
// ui_draw_transformed_box(s, 0xFF00FF00);
|
||||
ui_draw_world(s);
|
||||
|
||||
if (scene->lead_status) {
|
||||
draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30,
|
||||
nvgRGBA(255, 0, 0, 128));
|
||||
}
|
||||
|
||||
const float label_size = 65.0f;
|
||||
|
||||
nvgFontFace(s->vg, "courbd");
|
||||
|
||||
if (scene->awareness_status > 0) {
|
||||
nvgBeginPath(s->vg);
|
||||
int bar_height = scene->awareness_status * 700;
|
||||
nvgRect(s->vg, 100, 300 + (700 - bar_height), 50, bar_height);
|
||||
nvgFillColor(s->vg, nvgRGBA(255 * (1 - scene->awareness_status),
|
||||
255 * scene->awareness_status, 0, 128));
|
||||
nvgFill(s->vg);
|
||||
}
|
||||
|
||||
// Draw calibration progress (if needed)
|
||||
if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
|
||||
int rec_width = 1120;
|
||||
int x_pos = 500;
|
||||
nvgBeginPath(s->vg);
|
||||
nvgStrokeWidth(s->vg, 14);
|
||||
nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
|
||||
nvgStroke(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFontSize(s->vg, 40*2.5);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
|
||||
char calib_status_str[64];
|
||||
snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc);
|
||||
|
||||
nvgText(s->vg, x_pos, 1010, calib_status_str, NULL);
|
||||
if (s->is_metric) {
|
||||
nvgText(s->vg, x_pos + 120, 1110, "Drive above 72 km/h", NULL);
|
||||
} else {
|
||||
nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL);
|
||||
}
|
||||
} else if (scene->gps_planner_active) {
|
||||
int rec_width = 1120;
|
||||
int x_pos = 500;
|
||||
nvgBeginPath(s->vg);
|
||||
nvgStrokeWidth(s->vg, 14);
|
||||
nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
|
||||
nvgStroke(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFontSize(s->vg, 40*2.5);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
|
||||
nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL);
|
||||
}
|
||||
}
|
||||
|
||||
nvgRestore(s->vg);
|
||||
|
||||
|
||||
if (!ui_alert_active(s) && !scene->frontview) {
|
||||
// draw top bar
|
||||
|
||||
const int bar_x = box_x;
|
||||
const int bar_y = box_y;
|
||||
const int bar_width = box_width;
|
||||
const int bar_width = box_w;
|
||||
const int bar_height = 250 - box_y;
|
||||
|
||||
assert(s->status < ARRAYSIZE(bg_colors));
|
||||
|
@ -925,7 +945,7 @@ static void ui_draw_vision(UIState *s) {
|
|||
const int message_y = box_y;
|
||||
const int message_height = bar_height;
|
||||
const int message_width = 800;
|
||||
const int message_x = box_x + box_width / 2 - message_width / 2;
|
||||
const int message_x = box_x + box_w / 2 - message_width / 2;
|
||||
|
||||
// message background
|
||||
nvgBeginPath(s->vg);
|
||||
|
@ -995,10 +1015,10 @@ static void ui_draw_vision(UIState *s) {
|
|||
(int)(scene->v_cruise + 0.5));
|
||||
} else {
|
||||
/* Convert KPH to MPH. Using an approximated mph to kph
|
||||
conversion factor of 1.609 because this is what the Honda
|
||||
conversion factor of 1.60642 because this is what the Honda
|
||||
hud seems to be using */
|
||||
snprintf(speed_str, sizeof(speed_str), "%3d mph",
|
||||
(int)(scene->v_cruise * 0.621504 + 0.5));
|
||||
(int)(scene->v_cruise * 0.6225 + 0.5));
|
||||
}
|
||||
nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL);
|
||||
} else {
|
||||
|
@ -1037,11 +1057,304 @@ static void ui_draw_vision(UIState *s) {
|
|||
nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL);
|
||||
}
|
||||
|
||||
nvgEndFrame(s->vg);
|
||||
static void ui_draw_calibration_status(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
|
||||
glDisable(GL_BLEND);
|
||||
// glDisable(GL_CULL_FACE);
|
||||
int viz_calib_w = 1120;
|
||||
int viz_calib_x = (ui_viz_rx + ((ui_viz_rw/2) - (viz_calib_w/2)));
|
||||
int viz_calib_y = 760;
|
||||
int viz_calib_h = 250;
|
||||
int viz_calibtext_x = (ui_viz_rx + (ui_viz_rw/2));
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgStrokeWidth(s->vg, 10);
|
||||
nvgRoundedRect(s->vg, viz_calib_x, viz_calib_y, viz_calib_w, viz_calib_h, 20);
|
||||
nvgStroke(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFontSize(s->vg, 40*2.5);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
|
||||
char calib_status_str[64];
|
||||
snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc);
|
||||
|
||||
nvgText(s->vg, viz_calibtext_x, viz_calib_y+100, calib_status_str, NULL);
|
||||
if (s->is_metric) {
|
||||
nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 72 km/h", NULL);
|
||||
} else {
|
||||
nvgText(s->vg, viz_calibtext_x, viz_calib_y+200, "Drive above 45 mph", NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_draw_gpsplanner_status(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
|
||||
int rec_width = 1120;
|
||||
int x_pos = 500;
|
||||
nvgBeginPath(s->vg);
|
||||
nvgStrokeWidth(s->vg, 14);
|
||||
nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20);
|
||||
nvgStroke(s->vg);
|
||||
nvgFillColor(s->vg, nvgRGBA(0,0,0,180));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFontSize(s->vg, 40*2.5);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220));
|
||||
nvgText(s->vg, x_pos, 1010, "GPS planner active", NULL);
|
||||
}
|
||||
|
||||
|
||||
// Draw all world space objects.
|
||||
static void ui_draw_world(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
if (!scene->world_objects_visible) {
|
||||
return;
|
||||
}
|
||||
|
||||
//draw_steering(s, scene->curvature);
|
||||
ui_draw_vision_lanes(s);
|
||||
|
||||
}
|
||||
|
||||
static void ui_draw_vision_maxspeed(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
float maxspeed = s->scene.v_cruise;
|
||||
|
||||
const int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2));
|
||||
const int viz_maxspeed_y = (viz_y + (bdr_s*1.5));
|
||||
const int viz_maxspeed_w = 180;
|
||||
const int viz_maxspeed_h = 202;
|
||||
char maxspeed_str[32];
|
||||
bool is_cruise_set = (maxspeed != 0 && maxspeed != 255);
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20);
|
||||
nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
|
||||
nvgStrokeWidth(s->vg, 6);
|
||||
nvgStroke(s->vg);
|
||||
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgFontSize(s->vg, 26*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 148, "MAX", NULL);
|
||||
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFontSize(s->vg, 52*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
if (is_cruise_set) {
|
||||
if (s->is_metric) {
|
||||
snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed + 0.5));
|
||||
} else {
|
||||
snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed * 0.6225 + 0.5));
|
||||
}
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, maxspeed_str, NULL);
|
||||
} else {
|
||||
nvgFontSize(s->vg, 42*2.5);
|
||||
nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, "N/A", NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_draw_vision_speed(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
float speed = s->scene.v_ego;
|
||||
|
||||
const int viz_speed_w = 280;
|
||||
const int viz_speed_x = ui_viz_rx+((ui_viz_rw/2)-(viz_speed_w/2));
|
||||
char speed_str[32];
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRect(s->vg, viz_speed_x, viz_y, viz_speed_w, viz_header_h);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
|
||||
if (s->is_metric) {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5));
|
||||
} else {
|
||||
snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2374144 + 0.5));
|
||||
}
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 96*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 240, speed_str, NULL);
|
||||
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgFontSize(s->vg, 36*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
|
||||
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_wheel(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
const int ui_viz_rx = scene->ui_viz_rx;
|
||||
const int ui_viz_rw = scene->ui_viz_rw;
|
||||
const int viz_event_w = 220;
|
||||
const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
|
||||
const int viz_event_y = (viz_y + (bdr_s*1.5));
|
||||
const int viz_event_h = (viz_header_h - (bdr_s*1.5));
|
||||
// draw steering wheel
|
||||
const int viz_wheel_size = 96;
|
||||
const int viz_wheel_x = viz_event_x + (viz_event_w-viz_wheel_size);
|
||||
const int viz_wheel_y = viz_event_y + (viz_wheel_size/2);
|
||||
const int img_wheel_size = viz_wheel_size*1.5;
|
||||
const int img_wheel_x = viz_wheel_x-(img_wheel_size/2);
|
||||
const int img_wheel_y = viz_wheel_y-25;
|
||||
float img_wheel_alpha = 0.5f;
|
||||
bool is_engaged = (s->status == STATUS_ENGAGED);
|
||||
bool is_warning = (s->status == STATUS_WARNING);
|
||||
if (is_engaged || is_warning) {
|
||||
nvgBeginPath(s->vg);
|
||||
nvgCircle(s->vg, viz_wheel_x, (viz_wheel_y + (bdr_s*1.5)), viz_wheel_size);
|
||||
if (is_engaged) {
|
||||
nvgFillColor(s->vg, nvgRGBA(23, 134, 68, 255));
|
||||
} else if (is_warning) {
|
||||
nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 255));
|
||||
}
|
||||
nvgFill(s->vg);
|
||||
img_wheel_alpha = 1.0f;
|
||||
}
|
||||
nvgBeginPath(s->vg);
|
||||
NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y,
|
||||
img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha);
|
||||
nvgRect(s->vg, img_wheel_x, img_wheel_y, img_wheel_size, img_wheel_size);
|
||||
nvgFillPaint(s->vg, imgPaint);
|
||||
nvgFill(s->vg);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_header(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
NVGpaint gradient = nvgLinearGradient(s->vg, ui_viz_rx,
|
||||
(viz_y+(viz_header_h-(viz_header_h/2.5))),
|
||||
ui_viz_rx, viz_y+viz_header_h,
|
||||
nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0));
|
||||
nvgFillPaint(s->vg, gradient);
|
||||
nvgRect(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_header_h);
|
||||
nvgFill(s->vg);
|
||||
|
||||
ui_draw_vision_maxspeed(s);
|
||||
ui_draw_vision_speed(s);
|
||||
ui_draw_vision_wheel(s);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_alert(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
|
||||
bool mapEnabled = s->scene.uilayout_mapenabled;
|
||||
bool longAlert1 = strlen(scene->alert_text1) > 15;
|
||||
|
||||
const uint8_t *color = alert_colors[s->status];
|
||||
const int alr_s = alert_sizes[s->alert_size];
|
||||
const int alr_x = ui_viz_rx-(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)-bdr_s;
|
||||
const int alr_w = ui_viz_rw+(mapEnabled?(hasSidebar?nav_w:(nav_ww+(bdr_s*2))):0)+(bdr_s*2);
|
||||
const int alr_h = alr_s+(s->alert_size==ALERTSIZE_NONE?0:bdr_s);
|
||||
const int alr_y = vwp_h-alr_h;
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h);
|
||||
nvgFillColor(s->vg, nvgRGBA(color[0],color[1],color[2],color[3]));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h,
|
||||
nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35));
|
||||
nvgFillPaint(s->vg, gradient);
|
||||
nvgRect(s->vg, alr_x, alr_y, alr_w, alr_h);
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
|
||||
if (s->alert_size == ALERTSIZE_SMALL) {
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFontSize(s->vg, 40*2.5);
|
||||
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, scene->alert_text1, NULL);
|
||||
} else if (s->alert_size == ALERTSIZE_MID) {
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 48*2.5);
|
||||
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, scene->alert_text1, NULL);
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgFontSize(s->vg, 36*2.5);
|
||||
nvgText(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, scene->alert_text2, NULL);
|
||||
} else if (s->alert_size == ALERTSIZE_FULL) {
|
||||
nvgFontSize(s->vg, (longAlert1?72:96)*2.5);
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
|
||||
nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, scene->alert_text1, NULL);
|
||||
nvgFontSize(s->vg, 48*2.5);
|
||||
nvgFontFace(s->vg, "sans-regular");
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
|
||||
nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, scene->alert_text2, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_draw_vision(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
int ui_viz_rx = scene->ui_viz_rx;
|
||||
int ui_viz_rw = scene->ui_viz_rw;
|
||||
|
||||
glClearColor(0.0, 0.0, 0.0, 0.0);
|
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
||||
|
||||
glEnable(GL_SCISSOR_TEST);
|
||||
const int viewport_rw = (box_w+sbr_w-(bdr_s*2));
|
||||
glViewport(ui_viz_rx + scene->ui_frame_offset, s->fb_h-(viz_y+viz_h), viewport_rw - scene->ui_frame_offset, viz_h);
|
||||
glScissor(ui_viz_rx, s->fb_h-(viz_y+viz_h), ui_viz_rw, viz_h);
|
||||
draw_frame(s);
|
||||
glViewport(0, 0, s->fb_w, s->fb_h);
|
||||
glDisable(GL_SCISSOR_TEST);
|
||||
|
||||
glEnable(GL_BLEND);
|
||||
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
||||
glClear(GL_STENCIL_BUFFER_BIT);
|
||||
|
||||
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
|
||||
nvgSave(s->vg);
|
||||
|
||||
// hack for eon
|
||||
const int inner_height = viewport_rw*9/16;
|
||||
nvgScissor(s->vg, ui_viz_rx, viz_y, ui_viz_rw, viz_h);
|
||||
nvgTranslate(s->vg, ui_viz_rx + scene->ui_world_offset, viz_y + (viz_h-inner_height)/2.0);
|
||||
nvgScale(s->vg, (float)viewport_rw / s->fb_w, (float)inner_height / s->fb_h);
|
||||
|
||||
if (!scene->frontview) {
|
||||
ui_draw_world(s);
|
||||
|
||||
if (scene->lead_status) {
|
||||
draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30,
|
||||
nvgRGBA(255, 0, 0, 128));
|
||||
}
|
||||
}
|
||||
|
||||
nvgRestore(s->vg);
|
||||
|
||||
// draw vision elements
|
||||
ui_draw_vision_header(s);
|
||||
ui_draw_vision_alert(s);
|
||||
|
||||
// Draw calibration progress (if needed)
|
||||
if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
|
||||
ui_draw_calibration_status(s);
|
||||
}
|
||||
|
||||
nvgEndFrame(s->vg);
|
||||
glDisable(GL_BLEND);
|
||||
}
|
||||
|
||||
|
||||
static void ui_draw_alerts(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
|
@ -1057,7 +1370,7 @@ static void ui_draw_alerts(UIState *s) {
|
|||
}
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgRect(s->vg, box_x, box_y, box_width, box_height);
|
||||
nvgRect(s->vg, box_x, box_y, box_w, box_h);
|
||||
nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3]));
|
||||
nvgFill(s->vg);
|
||||
|
||||
|
@ -1070,7 +1383,7 @@ static void ui_draw_alerts(UIState *s) {
|
|||
}
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
|
||||
nvgTextBox(s->vg, box_x + 50, box_y + 287, box_width - 50, alert_text1_upper, NULL);
|
||||
nvgTextBox(s->vg, box_x + 50, box_y + 287, box_w - 50, alert_text1_upper, NULL);
|
||||
|
||||
|
||||
if (strlen(scene->alert_text2) > 0) {
|
||||
|
@ -1079,7 +1392,7 @@ static void ui_draw_alerts(UIState *s) {
|
|||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
nvgFontSize(s->vg, 44.0*2.5);
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
|
||||
nvgTextBox(s->vg, box_x + 50, box_y + box_height - 250, box_width - 50, scene->alert_text2, NULL);
|
||||
nvgTextBox(s->vg, box_x + 50, box_y + box_h - 250, box_w - 50, scene->alert_text2, NULL);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1138,10 +1451,10 @@ static void ui_draw(UIState *s) {
|
|||
|
||||
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
|
||||
|
||||
if (s->vision_connected) {
|
||||
if (s->vision_connected && !s->scene.uilayout_sidebarcollapsed) {
|
||||
ui_draw_aside(s);
|
||||
}
|
||||
ui_draw_alerts(s);
|
||||
// ui_draw_alerts(s);
|
||||
|
||||
nvgEndFrame(s->vg);
|
||||
glDisable(GL_BLEND);
|
||||
|
@ -1235,12 +1548,18 @@ static void ui_update(UIState *s) {
|
|||
|
||||
assert(glGetError() == GL_NO_ERROR);
|
||||
|
||||
// Default UI Measurements (Assumes sidebar visible)
|
||||
s->scene.ui_viz_rx = box_x;
|
||||
s->scene.ui_viz_rw = box_w;
|
||||
s->scene.ui_frame_offset = -(sbr_w - 4*bdr_s);
|
||||
s->scene.ui_world_offset = -(sbr_w - 6*bdr_s);
|
||||
|
||||
s->vision_connect_firstrun = false;
|
||||
}
|
||||
|
||||
// poll for events
|
||||
while (true) {
|
||||
zmq_pollitem_t polls[8] = {{0}};
|
||||
zmq_pollitem_t polls[9] = {{0}};
|
||||
polls[0].socket = s->live100_sock_raw;
|
||||
polls[0].events = ZMQ_POLLIN;
|
||||
polls[1].socket = s->livecalibration_sock_raw;
|
||||
|
@ -1253,15 +1572,16 @@ static void ui_update(UIState *s) {
|
|||
polls[4].events = ZMQ_POLLIN;
|
||||
polls[5].socket = s->thermal_sock_raw;
|
||||
polls[5].events = ZMQ_POLLIN;
|
||||
|
||||
polls[6].socket = s->plus_sock_raw;
|
||||
polls[6].socket = s->uilayout_sock_raw;
|
||||
polls[6].events = ZMQ_POLLIN;
|
||||
polls[7].socket = s->plus_sock_raw;
|
||||
polls[7].events = ZMQ_POLLIN;
|
||||
|
||||
int num_polls = 7;
|
||||
int num_polls = 8;
|
||||
if (s->vision_connected) {
|
||||
assert(s->ipc_fd >= 0);
|
||||
polls[7].fd = s->ipc_fd;
|
||||
polls[7].events = ZMQ_POLLIN;
|
||||
polls[8].fd = s->ipc_fd;
|
||||
polls[8].events = ZMQ_POLLIN;
|
||||
num_polls++;
|
||||
}
|
||||
|
||||
|
@ -1275,12 +1595,12 @@ static void ui_update(UIState *s) {
|
|||
}
|
||||
|
||||
if (polls[0].revents || polls[1].revents || polls[2].revents ||
|
||||
polls[3].revents || polls[4].revents) {
|
||||
polls[3].revents || polls[4].revents || polls[6].revents || polls[7].revents) {
|
||||
// awake on any (old) activity
|
||||
set_awake(s, true);
|
||||
}
|
||||
|
||||
if (s->vision_connected && polls[7].revents) {
|
||||
if (s->vision_connected && polls[8].revents) {
|
||||
// vision ipc event
|
||||
VisionPacket rp;
|
||||
err = vipc_recv(s->ipc_fd, &rp);
|
||||
|
@ -1329,7 +1649,7 @@ static void ui_update(UIState *s) {
|
|||
} else {
|
||||
assert(false);
|
||||
}
|
||||
} else if (polls[6].revents) {
|
||||
} else if (polls[7].revents) {
|
||||
// plus socket
|
||||
|
||||
zmq_msg_t msg;
|
||||
|
@ -1347,7 +1667,7 @@ static void ui_update(UIState *s) {
|
|||
} else {
|
||||
// zmq messages
|
||||
void* which = NULL;
|
||||
for (int i=0; i<6; i++) {
|
||||
for (int i=0; i<num_polls - 1; i++) {
|
||||
if (polls[i].revents) {
|
||||
which = polls[i].socket;
|
||||
break;
|
||||
|
@ -1401,6 +1721,15 @@ static void ui_update(UIState *s) {
|
|||
s->scene.alert_ts = eventd.logMonoTime;
|
||||
|
||||
s->scene.alert_size = datad.alertSize;
|
||||
if (datad.alertSize == cereal_Live100Data_AlertSize_none) {
|
||||
s->alert_size = ALERTSIZE_NONE;
|
||||
} else if (datad.alertSize == cereal_Live100Data_AlertSize_small) {
|
||||
s->alert_size = ALERTSIZE_SMALL;
|
||||
} else if (datad.alertSize == cereal_Live100Data_AlertSize_mid) {
|
||||
s->alert_size = ALERTSIZE_MID;
|
||||
} else if (datad.alertSize == cereal_Live100Data_AlertSize_full) {
|
||||
s->alert_size = ALERTSIZE_FULL;
|
||||
}
|
||||
|
||||
if (datad.alertStatus == cereal_Live100Data_AlertStatus_userPrompt) {
|
||||
update_status(s, STATUS_WARNING);
|
||||
|
@ -1474,12 +1803,26 @@ static void ui_update(UIState *s) {
|
|||
}
|
||||
|
||||
s->scene.started_ts = datad.startedTs;
|
||||
} else if (eventd.which == cereal_Event_uiLayoutState) {
|
||||
struct cereal_UiLayoutState datad;
|
||||
cereal_read_UiLayoutState(&datad, eventd.uiLayoutState);
|
||||
s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed;
|
||||
s->scene.uilayout_mapenabled = datad.mapEnabled;
|
||||
|
||||
bool hasSidebar = !s->scene.uilayout_sidebarcollapsed;
|
||||
bool mapEnabled = s->scene.uilayout_mapenabled;
|
||||
if (mapEnabled) {
|
||||
s->scene.ui_viz_rx = hasSidebar ? viz_x : (viz_x-(bdr_s*2));
|
||||
s->scene.ui_viz_rw = hasSidebar ? viz_w : (viz_w+(bdr_s*2));
|
||||
} else {
|
||||
s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2);
|
||||
s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2));
|
||||
s->scene.ui_frame_offset = hasSidebar ? -(sbr_w - 4*bdr_s) : -(bdr_s*2);
|
||||
s->scene.ui_world_offset = hasSidebar ? -(sbr_w - 6*bdr_s) : -(bdr_s*2);
|
||||
}
|
||||
}
|
||||
|
||||
capn_free(&ctx);
|
||||
|
||||
zmq_msg_close(&msg);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1676,6 +2019,7 @@ int main() {
|
|||
|
||||
if (EON) {
|
||||
// light sensor is only exposed on EONs
|
||||
|
||||
float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B;
|
||||
if (clipped_light_sensor > 255) clipped_light_sensor = 255;
|
||||
smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99;
|
||||
|
|
|
@ -8,6 +8,7 @@ import subprocess
|
|||
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.version import dirty
|
||||
|
||||
def main(gctx=None):
|
||||
while True:
|
||||
|
@ -17,7 +18,13 @@ def main(gctx=None):
|
|||
time.sleep(60)
|
||||
continue
|
||||
|
||||
# If there are modifications we want to full history
|
||||
# otherwise only store head to save space
|
||||
if dirty:
|
||||
r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--unshallow"])
|
||||
else:
|
||||
r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"])
|
||||
|
||||
cloudlog.info("git fetch: %r", r)
|
||||
if r:
|
||||
time.sleep(60)
|
||||
|
|
|
@ -1,3 +1,17 @@
|
|||
import os
|
||||
import subprocess
|
||||
with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf:
|
||||
version = _versionf.read().split('"')[1]
|
||||
|
||||
try:
|
||||
if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
|
||||
upstream = "origin/master"
|
||||
else:
|
||||
if 'chffrplus' in version:
|
||||
upstream = "origin/release"
|
||||
else:
|
||||
upstream = "origin/release2"
|
||||
|
||||
dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0
|
||||
except subprocess.CalledProcessError:
|
||||
dirty = True
|
||||
|
|
|
@ -1,3 +1,3 @@
|
|||
visiond runs the openpilot vision pipeline. Everything running between the camera hardware and model outputs lives here.
|
||||
visiond runs the openpilot/chffrplus vision pipeline. Everything running between the camera hardware and model outputs lives here.
|
||||
|
||||
Contact us if you'd like features added or support for your platform.
|
||||
|
|
Binary file not shown.
Loading…
Reference in New Issue