openpilot v0.4.3 release
Vehicle Researcher
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 {
|
||||
|
|
|
|||
133
cereal/log.capnp
133
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;
|
||||
|
|
@ -576,12 +579,14 @@ struct LiveLocationData {
|
|||
source @14 :SensorSource;
|
||||
# if we are fixing a location in the past
|
||||
fixMonoTime @15 :UInt64;
|
||||
|
||||
|
||||
gpsWeek @16 :Int32;
|
||||
timeOfWeek @17 :Float64;
|
||||
|
||||
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: {
|
||||
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.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
|
||||
if can_gear_shifter == 0x1:
|
||||
return "park"
|
||||
elif can_gear_shifter == 0x2:
|
||||
return "reverse"
|
||||
|
||||
# TODO: Use VAL from DBC to parse this field
|
||||
if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY):
|
||||
if can_gear_shifter == 0x3:
|
||||
if can_gear_shifter == 0x1:
|
||||
return "park"
|
||||
elif can_gear_shifter == 0x2:
|
||||
return "reverse"
|
||||
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 = []
|
||||
|
|
@ -106,7 +138,7 @@ def create_radar_commands(v_ego, car_fingerprint, idx):
|
|||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
elif car_fingerprint == CAR.PILOT:
|
||||
msg_0x301 = "\x00\x00\x56\x02\x58\x00\x00"
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
|
||||
commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
|
||||
return 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]
|
||||
|
|
@ -203,7 +215,7 @@ class CarInterface(object):
|
|||
ret.longitudinalKpBP = [0., 5., 35.]
|
||||
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.18, 0.12]
|
||||
ret.longitudinalKiV = [0.18, 0.12]
|
||||
elif candidate == CAR.ODYSSEY:
|
||||
stop_and_go = False
|
||||
ret.mass = 4354./2.205 + std_cargo
|
||||
|
|
@ -431,9 +443,12 @@ 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.:
|
||||
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
||||
# 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_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
|
||||
|
|
@ -146,11 +150,10 @@ class CarInterface(object):
|
|||
|
||||
ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM)
|
||||
ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU)
|
||||
ret.enableApgs = False # not check_ecu_msgs(fingerprint, candidate, ECU.APGS)
|
||||
ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS)
|
||||
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,8 +273,8 @@ 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,
|
||||
CS.steeringPressed, plan.dPoly, angle_offset, VM, PL)
|
||||
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
|
||||
if LaC.sat_flag and CP.steerLimitAlert:
|
||||
|
|
@ -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")
|
||||
|
||||
|
|
|
|||
|
|
@ -15,14 +15,14 @@ AlertSize = log.Live100Data.AlertSize
|
|||
AlertStatus = log.Live100Data.AlertStatus
|
||||
|
||||
class Alert(object):
|
||||
def __init__(self,
|
||||
def __init__(self,
|
||||
alert_text_1,
|
||||
alert_text_2,
|
||||
alert_status,
|
||||
alert_size,
|
||||
alert_priority,
|
||||
visual_alert,
|
||||
audible_alert,
|
||||
audible_alert,
|
||||
duration_sound,
|
||||
duration_hud_alert,
|
||||
duration_text):
|
||||
|
|
@ -34,7 +34,7 @@ class Alert(object):
|
|||
self.alert_priority = alert_priority
|
||||
self.visual_alert = visual_alert if visual_alert is not None else "none"
|
||||
self.audible_alert = audible_alert if audible_alert is not None else "none"
|
||||
|
||||
|
||||
self.duration_sound = duration_sound
|
||||
self.duration_hud_alert = duration_hud_alert
|
||||
self.duration_text = duration_text
|
||||
|
|
@ -71,356 +71,356 @@ class AlertManager(object):
|
|||
Priority.MID, None, "beepSingle", .2, 0., 0.),
|
||||
|
||||
"fcw": Alert(
|
||||
"Brake!",
|
||||
"Risk of Collision",
|
||||
"Brake!",
|
||||
"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",
|
||||
"Ethical Dilemma Detected",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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),
|
||||
|
||||
# Non-entry only alerts
|
||||
"wrongCarModeNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Main Switch Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Main Switch Off",
|
||||
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,
|
||||
"Comma Unavailable",
|
||||
"Data needed for calibration. Upload drive, try again",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
|
||||
|
||||
"outOfSpaceNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Out of Space",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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",
|
||||
"System Overheated",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"System Overheated",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"wrongGear": Alert(
|
||||
"Take Control Immediately",
|
||||
"Gear not D",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gear not D",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"calibrationInvalid": Alert(
|
||||
"Take Control Immediately",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
"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",
|
||||
"Door Open",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Door Open",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"seatbeltNotLatched": Alert(
|
||||
"Take Control Immediately",
|
||||
"Seatbelt Unlatched",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"espDisabled": Alert(
|
||||
"Take Control Immediately",
|
||||
"ESP Off",
|
||||
"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",
|
||||
"Radar Error: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"radarFault": Alert(
|
||||
"Take Control Immediately",
|
||||
"Radar Error: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"modelCommIssue": Alert(
|
||||
"Take Control Immediately",
|
||||
"Model Error: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Model Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"controlsFailed": Alert(
|
||||
"Take Control Immediately",
|
||||
"Controls Failed",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Failed",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"controlsMismatch": Alert(
|
||||
"Take Control Immediately",
|
||||
"Controls Mismatch",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Mismatch",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"commIssue": Alert(
|
||||
"Take Control Immediately",
|
||||
"CAN Error: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"CAN Error: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"steerUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"Steer Fault: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Steer Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"brakeUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"Brake Fault: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Brake Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"gasUnavailable": Alert(
|
||||
"Take Control Immediately",
|
||||
"Gas Fault: Restart the Car",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gas Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"reverseGear": Alert(
|
||||
"Take Control Immediately",
|
||||
"Reverse Gear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Reverse Gear",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"cruiseDisabled": Alert(
|
||||
"Take Control Immediately",
|
||||
"Cruise Is Off",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Is Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
# 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,
|
||||
"Comma Unavailable",
|
||||
"System overheated",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"wrongGearNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Gear not D",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Gear not D",
|
||||
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,
|
||||
"Comma Unavailable",
|
||||
"Calibration Invalid: Reposition EON and Recalibrate",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"calibrationInProgressNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"doorOpenNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Door Open",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Door open",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"seatbeltNotLatchedNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Seatbelt Unlatched",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Seatbelt unlatched",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
|
||||
"espDisabledNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"ESP Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"ESP Off",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"radarCommIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"radarFaultNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Radar Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"modelCommIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Model Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Model Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"controlsFailedNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Controls Failed",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Controls Failed",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"commIssueNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"CAN Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"CAN Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"steerUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Steer Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Steer Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"brakeUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Brake Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Brake Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"gasUnavailableNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Gas Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Gas Error: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"reverseGearNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Reverse Gear",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"Reverse Gear",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"cruiseDisabledNoEntry": Alert(
|
||||
"Comma Unavailable",
|
||||
"Cruise is Off",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
"Comma Unavailable",
|
||||
"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,
|
||||
"Comma Unavailable",
|
||||
"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,13 +445,14 @@ 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):
|
||||
|
||||
# first get rid of all the expired alerts
|
||||
self.activealerts = [a for a in self.activealerts if a.start_time +
|
||||
self.activealerts = [a for a in self.activealerts if a.start_time +
|
||||
max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
|
||||
|
||||
ca = self.activealerts[0] if self.alertPresent() else None
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
delta_desired = self.mpc_solution[0].delta[1]
|
||||
# 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
|
||||
if args.pipe:
|
||||
sys.stdout.write(sock.recv())
|
||||
sys.stdout.flush()
|
||||
elif args.raw:
|
||||
hexdump(sock.recv())
|
||||
elif args.json:
|
||||
print(json.loads(sock.recv()))
|
||||
elif args.dump_json:
|
||||
print json.dumps(messaging.recv_one(sock).to_dict())
|
||||
else:
|
||||
print messaging.recv_one(sock)
|
||||
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(msg)
|
||||
sys.stdout.flush()
|
||||
elif args.raw:
|
||||
hexdump(msg)
|
||||
elif args.json:
|
||||
print(json.loads(msg))
|
||||
elif args.dump_json:
|
||||
print json.dumps(evt.to_dict())
|
||||
else:
|
||||
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
|
||||
|
||||
reg_res = register()
|
||||
if reg_res:
|
||||
dongle_id, dongle_secret = reg_res
|
||||
if should_register:
|
||||
reg_res = register()
|
||||
if reg_res:
|
||||
dongle_id, dongle_secret = reg_res
|
||||
else:
|
||||
raise Exception("server registration failed")
|
||||
else:
|
||||
raise Exception("server registration failed")
|
||||
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())
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
|
||||
r = subprocess.call(["nice", "-n", "19", "git", "fetch", "--depth=1"])
|
||||
# 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