mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.3.6 release

Browse Source 
old-commit-hash: 99cb610b12334893cf3087f974d33f0e233f38e8
This commit is contained in:
Vehicle Researcher
2017-08-09 17:32:45 -07:00
parent 07238569c7
commit ee8459a55f
29 changed files with 613 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
RELEASES.md
View File

@@ -1,3 +1,11 @@
Version 0.3.6 (2017-08-08)
==========================
* Fix alpha CR-V support
* Improved GPS
* Fix display of target speed not always matching HUD
* Increased acceleration after stop
* Mitigated some vehicles driving too close to the right line
Version 0.3.5 (2017-07-30)
==========================
* Fix bug where new devices would not begin calibration

2
cereal/build_from_src.mk
View File

@@ -3,7 +3,7 @@ SRCS := log.capnp car.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
UNAME_M := $(shell uname -m)
UNAME_M ?= $(shell uname -m)
# only generate C++ for docker tests
ifneq ($(OPTEST),1)

26
cereal/car.capnp
View File

@@ -180,23 +180,29 @@ struct CarControl {
struct CarParams {
carName @0: Text;
radarName @1: Text;
carFingerprint @11: Text;
carFingerprint @2: Text;
enableSteer @2: Bool;
enableGas @3: Bool;
enableBrake @4: Bool;
enableCruise @5: Bool;
enableSteer @3: Bool;
enableGas @4: Bool;
enableBrake @5: Bool;
enableCruise @6: Bool;
# things about the car in the manual
wheelBase @6: Float32; # in meters
steerRatio @7: Float32;
m @7: Float32; # [kg] running weight
l @8: Float32; # [m] wheelbase
sR @9: Float32; # [] steering ratio
aF @10: Float32; # [m] GC distance to front axle
aR @11: Float32; # [m] GC distance to rear axle
chi @12: Float32; # [] rear steering ratio wrt front steering (usually 0)
# things we can derive
slipFactor @8: Float32;
j @13: Float32; # [kg*m2] body rot inertia
cF @14: Float32; # [N/rad] front tire coeff of stiff
cR @15: Float32; # [N/rad] rear tire coeff of stiff
# Kp and Ki for the lateral control
steerKp @9: Float32;
steerKi @10: Float32;
steerKp @16: Float32;
steerKi @17: Float32;
# TODO: Kp and Ki for long control, perhaps not needed?
}

207
cereal/log.capnp
View File

@@ -26,6 +26,8 @@ struct InitData {
deviceType @3 :DeviceType;
version @4 :Text;
gitCommit @10 :Text;
gitBranch @11 :Text;
androidBuildInfo @5 :AndroidBuildInfo;
androidSensors @6 :List(AndroidSensor);
@@ -326,14 +328,15 @@ struct Live100Data {
vTargetLead @3 :Float32;
upAccelCmd @4 :Float32;
uiAccelCmd @5 :Float32;
yActual @6 :Float32;
yActualDEPRECATED @6 :Float32;
yDes @7 :Float32;
upSteer @8 :Float32;
uiSteer @9 :Float32;
aTargetMin @10 :Float32;
aTargetMax @11 :Float32;
jerkFactor @12 :Float32;
angleSteers @13 :Float32; # Steering angle in degrees.
angleSteers @13 :Float32; # Steering angle in degrees.
angleSteersDes @29 :Float32;
hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32;
@@ -701,10 +704,66 @@ struct QcomGnss {
union {
measurementReport @1 :MeasurementReport;
clockReport @2 :ClockReport;
drMeasurementReport @3 :DrMeasurementReport;
}
enum MeasurementSource @0xd71a12b6faada7ee {
gps @0;
glonass @1;
beidou @2;
}
enum SVObservationState @0xe81e829a0d6c83e9 {
idle @0;
search @1;
searchVerify @2;
bitEdge @3;
trackVerify @4;
track @5;
restart @6;
dpo @7;
glo10msBe @8;
glo10msAt @9;
}
struct MeasurementStatus @0xe501010e1bcae83b {
subMillisecondIsValid @0 :Bool;
subBitTimeIsKnown @1 :Bool;
satelliteTimeIsKnown @2 :Bool;
bitEdgeConfirmedFromSignal @3 :Bool;
measuredVelocity @4 :Bool;
fineOrCoarseVelocity @5 :Bool;
lockPointValid @6 :Bool;
lockPointPositive @7 :Bool;
lastUpdateFromDifference @8 :Bool;
lastUpdateFromVelocityDifference @9 :Bool;
strongIndicationOfCrossCorelation @10 :Bool;
tentativeMeasurement @11 :Bool;
measurementNotUsable @12 :Bool;
sirCheckIsNeeded @13 :Bool;
probationMode @14 :Bool;
glonassMeanderBitEdgeValid @15 :Bool;
glonassTimeMarkValid @16 :Bool;
gpsRoundRobinRxDiversity @17 :Bool;
gpsRxDiversity @18 :Bool;
gpsLowBandwidthRxDiversityCombined @19 :Bool;
gpsHighBandwidthNu4 @20 :Bool;
gpsHighBandwidthNu8 @21 :Bool;
gpsHighBandwidthUniform @22 :Bool;
gpsMultipathIndicator @23 :Bool;
imdJammingIndicator @24 :Bool;
lteB13TxJammingIndicator @25 :Bool;
freshMeasurementIndicator @26 :Bool;
multipathEstimateIsValid @27 :Bool;
directionIsValid @28 :Bool;
}
struct MeasurementReport {
source @0 :Source;
source @0 :MeasurementSource;
fCount @1 :UInt32;
@@ -720,11 +779,6 @@ struct QcomGnss {
sv @10 :List(SV);
enum Source {
gps @0;
glonass @1;
}
struct SV {
svId @0 :UInt8;
observationState @2 :SVObservationState;
@@ -736,7 +790,7 @@ struct QcomGnss {
filterStages @7 :UInt8;
carrierNoise @8 :UInt16;
latency @9 :Int16;
predetectIntegration @10 :UInt8;
predetectInterval @10 :UInt8;
postdetections @11 :UInt16;
unfilteredMeasurementIntegral @12 :UInt32;
@@ -753,55 +807,6 @@ struct QcomGnss {
fineSpeed @23 :Float32;
fineSpeedUncertainty @24 :Float32;
cycleSlipCount @25 :UInt8;
struct MeasurementStatus {
subMillisecondIsValid @0 :Bool;
subBitTimeIsKnown @1 :Bool;
satelliteTimeIsKnown @2 :Bool;
bitEdgeConfirmedFromSignal @3 :Bool;
measuredVelocity @4 :Bool;
fineOrCoarseVelocity @5 :Bool;
lockPointValid @6 :Bool;
lockPointPositive @7 :Bool;
lastUpdateFromDifference @8 :Bool;
lastUpdateFromVelocityDifference @9 :Bool;
strongIndicationOfCrossCorelation @10 :Bool;
tentativeMeasurement @11 :Bool;
measurementNotUsable @12 :Bool;
sirCheckIsNeeded @13 :Bool;
probationMode @14 :Bool;
glonassMeanderBitEdgeValid @15 :Bool;
glonassTimeMarkValid @16 :Bool;
gpsRoundRobinRxDiversity @17 :Bool;
gpsRxDiversity @18 :Bool;
gpsLowBandwidthRxDiversityCombined @19 :Bool;
gpsHighBandwidthNu4 @20 :Bool;
gpsHighBandwidthNu8 @21 :Bool;
gpsHighBandwidthUniform @22 :Bool;
gpsMultipathIndicator @23 :Bool;
imdJammingIndicator @24 :Bool;
lteB13TxJammingIndicator @25 :Bool;
freshMeasurementIndicator @26 :Bool;
multipathEstimateIsValid @27 :Bool;
directionIsValid @28 :Bool;
}
enum SVObservationState {
idle @0;
search @1;
searchVerify @2;
bitEdge @3;
trackVerify @4;
track @5;
restart @6;
dpo @7;
glo10msBe @8;
glo10msAt @9;
}
}
}
@@ -810,8 +815,8 @@ struct QcomGnss {
hasFCount @0 :Bool;
fCount @1 :UInt32;
hasGpsWeekNumber @2 :Bool;
gpsWeekNumber @3 :UInt16;
hasGpsWeek @2 :Bool;
gpsWeek @3 :UInt16;
hasGpsMilliseconds @4 :Bool;
gpsMilliseconds @5 :UInt32;
gpsTimeBias @6 :Float32;
@@ -866,6 +871,88 @@ struct QcomGnss {
lpmRtcCount @49 :UInt32;
clockResets @50 :UInt32;
}
struct DrMeasurementReport {
reason @0 :UInt8;
seqNum @1 :UInt8;
seqMax @2 :UInt8;
rfLoss @3 :UInt16;
systemRtcValid @4 :Bool;
fCount @5 :UInt32;
clockResets @6 :UInt32;
systemRtcTime @7 :UInt64;
gpsLeapSeconds @8 :UInt8;
gpsLeapSecondsUncertainty @9 :UInt8;
gpsToGlonassTimeBiasMilliseconds @10 :Float32;
gpsToGlonassTimeBiasMillisecondsUncertainty @11 :Float32;
gpsWeek @12 :UInt16;
gpsMilliseconds @13 :UInt32;
gpsTimeBiasMs @14 :UInt32;
gpsClockTimeUncertaintyMs @15 :UInt32;
gpsClockSource @16 :UInt8;
glonassClockSource @17 :UInt8;
glonassYear @18 :UInt8;
glonassDay @19 :UInt16;
glonassMilliseconds @20 :UInt32;
glonassTimeBias @21 :Float32;
glonassClockTimeUncertainty @22 :Float32;
clockFrequencyBias @23 :Float32;
clockFrequencyUncertainty @24 :Float32;
frequencySource @25 :UInt8;
source @26 :MeasurementSource;
sv @27 :List(SV);
struct SV {
svId @0 :UInt8;
glonassFrequencyIndex @1 :Int8;
observationState @2 :SVObservationState;
observations @3 :UInt8;
goodObservations @4 :UInt8;
filterStages @5 :UInt8;
predetectInterval @6 :UInt8;
cycleSlipCount @7 :UInt8;
postdetections @8 :UInt16;
measurementStatus @9 :MeasurementStatus;
carrierNoise @10 :UInt16;
rfLoss @11 :UInt16;
latency @12 :Int16;
filteredMeasurementFraction @13 :Float32;
filteredMeasurementIntegral @14 :UInt32;
filteredTimeUncertainty @15 :Float32;
filteredSpeed @16 :Float32;
filteredSpeedUncertainty @17 :Float32;
unfilteredMeasurementFraction @18 :Float32;
unfilteredMeasurementIntegral @19 :UInt32;
unfilteredTimeUncertainty @20 :Float32;
unfilteredSpeed @21 :Float32;
unfilteredSpeedUncertainty @22 :Float32;
multipathEstimate @23 :UInt32;
azimuth @24 :Float32;
elevation @25 :Float32;
dopplerAcceleration @26 :Float32;
fineSpeed @27 :Float32;
fineSpeedUncertainty @28 :Float32;
carrierPhase @29 :Float64;
fCount @30 :UInt32;
parityErrorCount @31 :UInt16;
goodParity @32 :Bool;
}
}
}
struct LidarPts {

2
common/params.py
View File

@@ -57,7 +57,7 @@ keys = {
"IsMetric": TxType.PERSISTANT,
"IsRearViewMirror": TxType.PERSISTANT,
# written: visiond
# read: visiond
# read: visiond, controlsd
"CalibrationParams": TxType.PERSISTANT,
# written: visiond
# read: visiond, ui

2
opendbc

Submodule opendbc updated: b77861eb00...008089045e

2
panda

Submodule panda updated: 5409c51041...ff8a4bd4e8

115
selfdrive/car/honda/carstate.py
View File

@@ -21,6 +21,7 @@ def get_can_parser(CP):
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("WHEEL_SPEED_RR", 0x1d0, 0),
("STEER_ANGLE", 0x14a, 0),
("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x191, 0),
@@ -33,13 +34,13 @@ def get_can_parser(CP):
("SEATBELT_DRIVER_LAMP", 0x305, 1),
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
("BRAKE_PRESSED", 0x17c, 0),
("BRAKE_SWITCH", 0x17c, 0),
("CAR_GAS", 0x130, 0),
("CRUISE_BUTTONS", 0x296, 0),
("ESP_DISABLED", 0x1a4, 1),
("HUD_LEAD", 0x30c, 0),
("USER_BRAKE", 0x1a4, 0),
("STEER_STATUS", 0x18f, 5),
("WHEEL_SPEED_RR", 0x1d0, 0),
("BRAKE_ERROR_1", 0x1b0, 1),
("BRAKE_ERROR_2", 0x1b0, 1),
("GEAR_SHIFTER", 0x191, 0),
@@ -74,6 +75,7 @@ def get_can_parser(CP):
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("WHEEL_SPEED_RR", 0x1d0, 0),
("STEER_ANGLE", 0x156, 0),
("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x1a3, 0),
@@ -86,13 +88,13 @@ def get_can_parser(CP):
("SEATBELT_DRIVER_LAMP", 0x305, 1),
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
("BRAKE_PRESSED", 0x17c, 0),
("BRAKE_SWITCH", 0x17c, 0),
("CAR_GAS", 0x130, 0),
("CRUISE_BUTTONS", 0x1a6, 0),
("ESP_DISABLED", 0x1a4, 1),
("HUD_LEAD", 0x30c, 0),
("USER_BRAKE", 0x1a4, 0),
("STEER_STATUS", 0x18f, 5),
("WHEEL_SPEED_RR", 0x1d0, 0),
("BRAKE_ERROR_1", 0x1b0, 1),
("BRAKE_ERROR_2", 0x1b0, 1),
("GEAR_SHIFTER", 0x1a3, 0),
@@ -125,6 +127,7 @@ def get_can_parser(CP):
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("WHEEL_SPEED_RR", 0x1d0, 0),
("STEER_ANGLE", 0x156, 0),
#("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x191, 0),
@@ -137,6 +140,7 @@ def get_can_parser(CP):
("SEATBELT_DRIVER_LAMP", 0x305, 1),
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
("BRAKE_PRESSED", 0x17c, 0),
("BRAKE_SWITCH", 0x17c, 0),
#("CAR_GAS", 0x130, 0),
("PEDAL_GAS", 0x17C, 0),
("CRUISE_BUTTONS", 0x1a6, 0),
@@ -144,58 +148,6 @@ def get_can_parser(CP):
("HUD_LEAD", 0x30c, 0),
("USER_BRAKE", 0x1a4, 0),
#("STEER_STATUS", 0x18f, 5),
("WHEEL_SPEED_RR", 0x1d0, 0),
("BRAKE_ERROR_1", 0x1b0, 1),
("BRAKE_ERROR_2", 0x1b0, 1),
("GEAR_SHIFTER", 0x191, 0),
("MAIN_ON", 0x1a6, 0),
("ACC_STATUS", 0x17c, 0),
("PEDAL_GAS", 0x17c, 0),
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
(0x158, 100),
(0x17c, 100),
#(0x1a3, 50),
(0x1a4, 50),
(0x1a6, 50),
(0x1b0, 50),
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x405, 3),
]
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
dbc_f = 'honda_crv_touring_2016_can.dbc'
signals = [
("XMISSION_SPEED", 0x158, 0),
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("STEER_ANGLE", 0x156, 0),
("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x191, 0),
("WHEELS_MOVING", 0x1b0, 1),
("DOOR_OPEN_FL", 0x405, 1),
("DOOR_OPEN_FR", 0x405, 1),
("DOOR_OPEN_RL", 0x405, 1),
("DOOR_OPEN_RR", 0x405, 1),
("CRUISE_SPEED_PCM", 0x324, 0),
("SEATBELT_DRIVER_LAMP", 0x305, 1),
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
("BRAKE_PRESSED", 0x17c, 0),
#("CAR_GAS", 0x130, 0),
("CRUISE_BUTTONS", 0x1a6, 0),
("ESP_DISABLED", 0x1a4, 1),
("HUD_LEAD", 0x30c, 0),
("USER_BRAKE", 0x1a4, 0),
("STEER_STATUS", 0x18f, 5),
("WHEEL_SPEED_RR", 0x1d0, 0),
("BRAKE_ERROR_1", 0x1b0, 1),
("BRAKE_ERROR_2", 0x1b0, 1),
("GEAR_SHIFTER", 0x191, 0),
@@ -213,7 +165,58 @@ def get_can_parser(CP):
(0x158, 100),
(0x17c, 100),
(0x191, 100),
(0x1a3, 50),
(0x1a4, 50),
(0x1a6, 50),
(0x1b0, 50),
(0x1d0, 50),
(0x305, 10),
(0x324, 10),
(0x405, 3),
]
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
dbc_f = 'honda_crv_touring_2016_can.dbc'
signals = [
("XMISSION_SPEED", 0x158, 0),
("WHEEL_SPEED_FL", 0x1d0, 0),
("WHEEL_SPEED_FR", 0x1d0, 0),
("WHEEL_SPEED_RL", 0x1d0, 0),
("WHEEL_SPEED_RR", 0x1d0, 0),
("STEER_ANGLE", 0x156, 0),
("STEER_TORQUE_SENSOR", 0x18f, 0),
("GEAR", 0x191, 0),
("WHEELS_MOVING", 0x1b0, 1),
("DOOR_OPEN_FL", 0x405, 1),
("DOOR_OPEN_FR", 0x405, 1),
("DOOR_OPEN_RL", 0x405, 1),
("DOOR_OPEN_RR", 0x405, 1),
("CRUISE_SPEED_PCM", 0x324, 0),
("SEATBELT_DRIVER_LAMP", 0x305, 1),
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
("BRAKE_PRESSED", 0x17c, 0),
("BRAKE_SWITCH", 0x17c, 0),
#("CAR_GAS", 0x130, 0),
("CRUISE_BUTTONS", 0x1a6, 0),
("ESP_DISABLED", 0x1a4, 1),
("HUD_LEAD", 0x30c, 0),
("USER_BRAKE", 0x1a4, 0),
("STEER_STATUS", 0x18f, 5),
("BRAKE_ERROR_1", 0x1b0, 1),
("BRAKE_ERROR_2", 0x1b0, 1),
("GEAR_SHIFTER", 0x191, 0),
("MAIN_ON", 0x1a6, 0),
("ACC_STATUS", 0x17c, 0),
("PEDAL_GAS", 0x17c, 0),
("CRUISE_SETTING", 0x1a6, 0),
("LEFT_BLINKER", 0x294, 0),
("RIGHT_BLINKER", 0x294, 0),
("COUNTER", 0x324, 0),
("ENGINE_RPM", 0x17C, 0)
]
checks = [
(0x156, 100),
(0x158, 100),
(0x17c, 100),
(0x191, 100),
(0x1a4, 50),
(0x1a6, 50),
(0x1b0, 50),
@@ -379,7 +382,7 @@ class CarState(object):
else:
self.car_gas = cp.vl[0x130]['CAR_GAS']
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or cp.vl[0x17C]['BRAKE_SWITCH']
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']

73
selfdrive/car/honda/interface.py
View File

@@ -59,8 +59,8 @@ class CarInterface(object):
@staticmethod
def get_params(candidate, fingerprint):
# pedal
brake_only = 0x201 not in fingerprint
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
@@ -70,32 +70,63 @@ class CarInterface(object):
ret.enableSteer = True
ret.enableBrake = True
ret.enableGas = not brake_only
ret.enableCruise = brake_only
#ret.enableCruise = False
# TODO: those parameters should be platform dependent
ret.wheelBase = 2.67
ret.slipFactor = 0.0014
# pedal
ret.enableGas = 0x201 in fingerprint
ret.enableCruise = not ret.enableGas
# FIXME: hardcoding honda civic 2016 touring wight so it can be used to
# scale unknown params for other cars
m_civic = 2923./2.205 + std_cargo
l_civic = 2.70
aF_civic = l_civic * 0.4
aR_civic = l_civic - aF_civic
j_civic = 2500
cF_civic = 85400
cR_civic = 90000
if candidate == "HONDA CIVIC 2016 TOURING":
ret.steerRatio = 13.0
ret.steerKp, ret.steerKi = 6.0, 1.4
ret.m = m_civic
ret.l = l_civic
ret.aF = aF_civic
ret.sR = 13.0
ret.steerKp, ret.steerKi = 1.0, 0.24
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
ret.steerRatio = 15.3
if not brake_only:
# assuming if we have an interceptor we also have a torque mod
ret.steerKp, ret.steerKi = 3.0, 0.7
else:
ret.steerKp, ret.steerKi = 6.0, 1.4
ret.m = 3095./2.205 + std_cargo
ret.l = 2.67
ret.aF = ret.l * 0.37
ret.sR = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
# FIXME: using dongleId isn't great, better to identify the car than the Neo
is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee'
ret.steerKp, ret.steerKi = [0.5, 0.12] if is_fw_modified else [1.0, 0.24]
elif candidate == "HONDA ACCORD 2016 TOURING":
ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 6.0, 1.4
ret.m = 3580./2.205 + std_cargo
ret.l = 2.74
ret.aF = ret.l * 0.38
ret.sR = 15.3
ret.steerKp, ret.steerKi = 1.0, 0.24
elif candidate == "HONDA CR-V 2016 TOURING":
ret.steerRatio = 15.3
ret.steerKp, ret.steerKi = 3.0, 0.7
ret.m = 3572./2.205 + std_cargo
ret.l = 2.62
ret.aF = ret.l * 0.41
ret.sR = 15.3
ret.steerKp, ret.steerKi = 0.5, 0.12
else:
raise ValueError("unsupported car %s" % candidate)
ret.aR = ret.l - ret.aF
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position... all cars will have approximately similar dyn behaviors
ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic)
ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic)
# no rear steering, at least on the listed cars above
ret.chi = 0.
return ret
@@ -278,7 +309,7 @@ class CarInterface(object):
"chimeRepeated": (BP.MUTE, CM.REPEATED),
"chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
pcm_accel = int(np.clip(c.cruiseControl.accelOverride/1.4,0,1)*0xc6)
pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
c.gas, c.brake, c.steeringTorque, \

14
selfdrive/common/cereal.mk
View File

@@ -1,3 +1,14 @@
UNAME_M ?= $(shell uname -m)
ifeq ($(UNAME_M),x86_64)
CEREAL_CFLAGS = -I$(ONE)/external/capnp/include
CEREAL_CXXFLAGS = $(CEREAL_CFLAGS)
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(ONE)/external/capnp/lib \
-l:libcapnp.a -l:libkj.a -l:libcapnp_c.a
endif
else
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
@@ -5,6 +16,9 @@ ifeq ($(CEREAL_LIBS),)
-L$(PHONELIBS)/capnp-c/aarch64/lib/ \
-l:libcapn.a -l:libcapnp.a -l:libkj.a
endif
endif
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o
log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++

3
selfdrive/common/params.c
View File

@@ -2,7 +2,10 @@
#include "selfdrive/common/util.h"
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif // _GNU_SOURCE
#include <sys/file.h>
#include <unistd.h>
#include <stdlib.h>

2
selfdrive/common/params.h
View File

@@ -7,6 +7,8 @@
extern "C" {
#endif
#define PARAMS_PATH "/data/params"
int write_db_value(const char* params_path, const char* key, const char* value,
size_t value_size);

2
selfdrive/common/version.h
View File

@@ -1 +1 @@
#define OPENPILOT_VERSION "0.3.5"
#define OPENPILOT_VERSION "0.3.6"

14
selfdrive/config.py
View File

@@ -1,14 +1,16 @@
import numpy as np
class Conversions:
MPH_TO_MS = 1.609/3.6
MS_TO_MPH = 3.6/1.609
KPH_TO_MS = 1./3.6
MPH_TO_KPH = 1.609344
KPH_TO_MPH = 1. / MPH_TO_KPH
MS_TO_KPH = 3.6
MPH_TO_KPH = 1.609
KPH_TO_MPH = 1./1.609
KNOTS_TO_MS = 1/1.9438
KPH_TO_MS = 1. / MS_TO_KPH
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
MS_TO_KNOTS = 1.9438
KNOTS_TO_MS = 1. / MS_TO_KNOTS
DEG_TO_RAD = np.pi/180.
RAD_TO_DEG = 1. / DEG_TO_RAD
# Car decode decimal minutes into decimal degrees, can work with numpy arrays as input
@staticmethod

30
selfdrive/controls/controlsd.py
View File

@@ -1,29 +1,29 @@
#!/usr/bin/env python
import os
import zmq
import selfdrive.messaging as messaging
import json
from copy import copy
from cereal import car, log
import zmq
from cereal import car, log
from common.numpy_fast import clip
from common.fingerprints import fingerprint
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
from common.params import Params
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car import get_car
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.adaptivecruise import A_ACC_MAX
V_CRUISE_MAX = 144
V_CRUISE_MIN = 8
@@ -71,6 +71,7 @@ class Controls(object):
self.AM = AlertManager()
self.LoC = LongControl()
self.LaC = LatControl()
self.VM = VehicleModel(self.CP)
# write CarParams
params = Params()
@@ -88,6 +89,13 @@ class Controls(object):
# learned angle offset
self.angle_offset = 0
calibration_params = params.get("CalibrationParams")
if calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.angle_offset = calibration_params["angle_offset"]
except (ValueError, KeyError):
pass
# rear view camera state
self.rear_view_toggle = False
@@ -276,7 +284,7 @@ class Controls(object):
# *** steering PID loop ***
final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle,
self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.CP)
self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM)
self.prof.checkpoint("PID")
@@ -345,8 +353,10 @@ class Controls(object):
self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0)
#CC.cruiseControl.accelOverride = float(AC.a_pcm)
# TODO: fix this
self.CC.cruiseControl.accelOverride = float(1.0)
# TODO: parametrize 0.714 in interface?
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX))
self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
self.CC.hudControl.speedVisible = self.enabled
@@ -407,8 +417,8 @@ class Controls(object):
dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd)
# lateral control state
dat.live100.yActual = float(self.LaC.y_actual)
dat.live100.yDes = float(self.LaC.y_des)
dat.live100.angleSteersDes = float(self.LaC.angle_steers_des)
dat.live100.upSteer = float(self.LaC.Up_steer)
dat.live100.uiSteer = float(self.LaC.Ui_steer)

21
selfdrive/controls/lib/adaptivecruise.py
View File

@@ -3,6 +3,8 @@ import numpy as np
from common.numpy_fast import clip, interp
import selfdrive.messaging as messaging
# TODO: we compute a_pcm but we don't use it, as accelOverride is hardcoded in controlsd
# lookup tables VS speed to determine min and max accels in cruise
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
@@ -25,7 +27,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP):
deg_to_rad = np.pi / 180. # from can reading to rad
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.steerRatio * CP.wheelBase)
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
a_target[1] = min(a_target[1], a_x_allowed)
@@ -109,12 +111,15 @@ _A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
# speeds
_A_CORR_BY_SPEED_BP = [0., 5., 20.]
# max acceleration allowed in acc, which happens in restart
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
a_coast_min = -1.0 # never coast faster then -1m/s^2
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
# regardless v_target
if v_ref > min(v_coast, v_target):
# for smooth coast we can be agrressive and target a point where car would actually crash
# for smooth coast we can be aggressive and target a point where car would actually crash
v_offset_coast = 0.
d_offset_coast = d_des/2. - 4.
@@ -127,9 +132,8 @@ def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_c
else:
a_max = a_coast_min
else:
# same as cruise accel, but add a small correction based on lead acceleration at low speeds
# when lead car accelerates faster, we can do the same, and vice versa
# same as cruise accel, plus add a small correction based on relative lead speed
# if the lead car is faster, we can accelerate more, if the car is slower, then we can reduce acceleration
a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
* clip(-v_rel / 4., -.5, 1)
return a_max
@@ -154,7 +158,7 @@ def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
v_target, v_coast, a_target, a_pcm):
#*** compute max accel ***
# v_rel is now your velocity in lead car frame
v_rel = -v_rel # this simplifiess things when thinking in d_rel-v_rel diagram
v_rel *= -1 # this simplifies things when thinking in d_rel-v_rel diagram
v_rel_pid = v_pid - v_lead
@@ -227,8 +231,9 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
#*** set accel limits as cruise accel/decel limits ***
a_target = calc_cruise_accel_limits(v_ego)
# Always 1 for now.
a_pcm = 1
# start with 1
a_pcm = 1.
#*** limit max accel in sharp turns
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP)

4
selfdrive/controls/lib/drive_helpers.py
View File

@@ -5,8 +5,8 @@ def rate_limit(new_value, last_value, dw_step, up_step):
def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
min_offset = -1. # deg
max_offset = 1. # deg
min_offset = -5. # deg
max_offset = 5. # deg
alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
min_learn_speed = 1.

43
selfdrive/controls/lib/latcontrol.py
View File

@@ -1,12 +1,7 @@
import math
import numpy as np
from common.numpy_fast import clip, interp
def calc_curvature(v_ego, angle_steers, CP, angle_offset=0):
deg_to_rad = np.pi/180.
angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad
curvature = angle_steers_rad/(CP.steerRatio * CP.wheelBase * (1. + CP.slipFactor * v_ego**2))
return curvature
from selfdrive.config import Conversions as CV
_K_CURV_V = [1., 0.6]
_K_CURV_BP = [0., 0.002]
@@ -26,27 +21,33 @@ def calc_d_lookahead(v_ego, d_poly):
k_curv = interp(curv, _K_CURV_BP, _K_CURV_V)
# sqrt on speed is needed to keep, for a given curvature, the y_offset
# proportional to speed. Indeed, y_offset is prop to d_lookahead^2
# sqrt on speed is needed to keep, for a given curvature, the y_des
# proportional to speed. Indeed, y_des is prop to d_lookahead^2
# 36m at 25m/s
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv
return d_lookahead
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, CP, angle_offset):
#*** this function return teh lateral offset given the steering angle, speed and the lookahead distance
curvature = calc_curvature(v_ego, angle_steers, CP, angle_offset)
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset):
#*** this function returns the lateral offset given the steering angle, speed and the lookahead distance
sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD
curvature = VM.calc_curvature(sa, v_ego)
# clip is to avoid arcsin NaNs due to too sharp turns
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
return y_actual, curvature
def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset):
# inverse of the above function
curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead
steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset
return steer_des, curvature
def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max,
steer_override, sat_count, enabled, Kp, Ki, rate):
sat_count_rate = 1./rate
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
error_steer = y_des - y_actual
error_steer = sa_des - sa_actual
Ui_unwind_speed = 0.3/rate #.3 per second
Up_steer = error_steer*Kp
@@ -109,7 +110,7 @@ class LatControl(object):
def reset(self):
self.Ui_steer = 0.
def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP):
def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM):
rate = 100
steer_max = 1.0
@@ -117,16 +118,16 @@ class LatControl(object):
# how far we look ahead is function of speed and desired path
d_lookahead = calc_d_lookahead(v_ego, d_poly)
# calculate actual offset at the lookahead point
self.y_actual, _ = calc_lookahead_offset(v_ego, angle_steers,
d_lookahead, CP, angle_offset)
# desired lookahead offset
self.y_des = np.polyval(d_poly, d_lookahead)
# calculate actual offset at the lookahead point
self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des,
d_lookahead, VM, angle_offset)
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
steer_override, self.sat_count, enabled, CP.steerKp, CP.steerKi, rate)
v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max,
steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate)
final_steer = clip(output_steer, -steer_max, steer_max)
return final_steer, sat_flag

87
selfdrive/controls/lib/vehicle_model.py Executable file
View File

@@ -0,0 +1,87 @@
#!/usr/bin/env python
import numpy as np
from numpy.linalg import inv
from selfdrive.car.honda.interface import CarInterface
## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
# Xdot = A*X + B*U
# where X = [v, r], with v and r lateral speed and rotational speed, respectively
# and U is the steering angle (controller input)
#
# A depends on longitudinal speed, u, and vehicle parameters CP
def create_dyn_state_matrices(u, CP):
A = np.zeros((2,2))
B = np.zeros((2,1))
A[0,0] = - (CP.cF + CP.cR)/(CP.m*u)
A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u
A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u)
A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u)
B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR
B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR
return A, B
def kin_ss_sol(sa, u, CP):
# kinematic solution, useful when speed ~ 0
K = np.zeros((2,1))
K[0,0] = CP.aR / CP.sR / CP.l * u
K[1,0] = 1. / CP.sR / CP.l * u
return K * sa
def dyn_ss_sol(sa, u, CP):
# Dynamic solution, useful when speed > 0
A, B = create_dyn_state_matrices(u, CP)
return - np.matmul(inv(A), B) * sa
def calc_slip_factor(CP):
# the slip factor is a measure of how the curvature changes with speed
# it's positive for Oversteering vehicle, negative (usual case) otherwise
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
class VehicleModel(object):
def __init__(self, CP, init_state=np.asarray([[0.],[0.]])):
self.dt = 0.1
lookahead = 2. # s
self.steps = int(lookahead / self.dt)
self.update_state(init_state)
self.state_pred = np.zeros((self.steps, self.state.shape[0]))
self.CP = CP
def update_state(self, state):
self.state = state
def steady_state_sol(self, sa, u):
# if the speed is too small we can't use the dynamic model
# (tire slip is undefined), we then use the kinematic model
if u > 0.1:
return dyn_ss_sol(sa, u, self.CP)
else:
return kin_ss_sol(sa, u, self.CP)
def calc_curvature(self, sa, u):
# this formula can be derived from state equations in steady state conditions
sf = calc_slip_factor(self.CP)
return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l
def get_steer_from_curvature(self, curv, u):
# this function is the exact inverse of calc_curvature, returning steer angle given curvature
sf = calc_slip_factor(self.CP)
return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv
def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self.CP)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
if __name__ == '__main__':
# load car params
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
print CP
VM = VehicleModel(CP)
print VM.steady_state_sol(.1, 0.15)

8
selfdrive/controls/radard.py
View File

@@ -4,18 +4,15 @@ import zmq
import numpy as np
import numpy.matlib
from collections import defaultdict
from fastcluster import linkage_vector
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.swaglog import cloudlog
from cereal import car
from common.params import Params
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.kalman.ekf import EKF, SimpleSensor
@@ -51,6 +48,7 @@ def radard_thread(gctx=None):
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
@@ -142,7 +140,7 @@ def radard_thread(gctx=None):
if enabled: # use path from model path_poly
path_y = np.polyval(PP.d_poly, path_x)
else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, CP, angle_offset=0)[0]
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():

26
selfdrive/debug/getframes/Makefile Normal file
View File

@@ -0,0 +1,26 @@
CC = clang
CXX = clang++
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
.PHONY: all
all: libvisionipc.so
visionipc.o: ../../common/visionipc.c ../../common/visionipc.h
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
-I../.. -I../../.. \
-c -o '$@' ../../common/visionipc.c
libvisionipc.so: visionipc.o
$(CC) -shared -fPIC -o '$@' visionipc.o
.PHONY: clean
clean:
rm visionipc.o libvisionipc.so

0
selfdrive/debug/getframes/__init__.py Normal file
View File

93
selfdrive/debug/getframes/getframes.py Executable file
View File

@@ -0,0 +1,93 @@
#!/usr/bin/env python
import os
import time
import subprocess
from cffi import FFI
import ctypes
import numpy as np
gf_dir = os.path.dirname(os.path.abspath(__file__))
subprocess.check_call(["make"], cwd=gf_dir)
ffi = FFI()
ffi.cdef("""
typedef enum VisionStreamType {
VISION_STREAM_UI_BACK,
VISION_STREAM_UI_FRONT,
VISION_STREAM_YUV,
VISION_STREAM_MAX,
} VisionStreamType;
typedef struct VisionUIInfo {
int big_box_x, big_box_y;
int big_box_width, big_box_height;
int transformed_width, transformed_height;
int front_box_x, front_box_y;
int front_box_width, front_box_height;
} VisionUIInfo;
typedef struct VisionStreamBufs {
VisionStreamType type;
int width, height, stride;
size_t buf_len;
union {
VisionUIInfo ui_info;
} buf_info;
} VisionStreamBufs;
typedef struct VisionBuf {
int fd;
size_t len;
void* addr;
} VisionBuf;
typedef struct VisionBufExtra {
uint32_t frame_id; // only for yuv
} VisionBufExtra;
typedef struct VisionStream {
int ipc_fd;
int last_idx;
int num_bufs;
VisionStreamBufs bufs_info;
VisionBuf *bufs;
} VisionStream;
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra);
void visionstream_destroy(VisionStream *s);
"""
)
clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so"))
def getframes():
s = ffi.new("VisionStream*")
buf_info = ffi.new("VisionStreamBufs*")
err = clib.visionstream_init(s, clib.VISION_STREAM_UI_BACK, True, buf_info)
assert err == 0
w = buf_info.width
h = buf_info.height
assert buf_info.stride == w*3
assert buf_info.buf_len == w*h*3
while True:
buf = clib.visionstream_get(s, ffi.NULL)
pbuf = ffi.buffer(buf.addr, buf.len)
yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3))
if __name__ == "__main__":
for buf in getframes():
print buf.shape, buf[101, 101]

4
selfdrive/loggerd/loggerd
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:71026624dc5524417a9f569d35655deb790289631c641453c835046a50ba2a2a
size 1365152
oid sha256:51d01404e86f6e1f1d2ecea67bfea72c8e568f03b4f839b4480b40c27993e685
size 1367088

13
selfdrive/messaging.py
View File

@@ -13,8 +13,10 @@ def pub_sock(context, port, addr="*"):
sock.bind("tcp://%s:%d" % (addr, port))
return sock
def sub_sock(context, port, poller=None, addr="127.0.0.1"):
def sub_sock(context, port, poller=None, addr="127.0.0.1", conflate=False):
sock = context.socket(zmq.SUB)
if conflate:
sock.setsockopt(zmq.CONFLATE, 1)
sock.connect("tcp://%s:%d" % (addr, port))
sock.setsockopt(zmq.SUBSCRIBE, "")
if poller is not None:
@@ -50,3 +52,12 @@ def recv_sock(sock, wait=False):
if dat is not None:
dat = log.Event.from_bytes(dat)
return dat
def recv_one(sock):
return log.Event.from_bytes(sock.recv())
def recv_one_or_none(sock):
try:
return log.Event.from_bytes(sock.recv(zmq.NOBLOCK))
except zmq.error.Again:
return None

4
selfdrive/sensord/sensord
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:58e476760c0f87f13b68b75d64860fb5c57b5509b4174d31f466074e0423e7eb
size 918928
oid sha256:fc4b862553d2ec9d63740b554997e5069808c531d9d722b4ce2cf83a2f4429ed
size 935360

32
selfdrive/test/plant/plant.py
View File

@@ -17,31 +17,15 @@ from selfdrive.car.honda.carstate import get_can_parser
from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from cereal import car
from common.dbc import dbc
acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc"))
def fake_car_params():
ret = car.CarParams.new_message()
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
CP = CarInterface.get_params("ACURA ILX 2016 ACURAWATCH PLUS", {0x201})
# largely copied from honda
ret.carName = "honda"
ret.radarName = "nidec"
ret.carFingerprint = "ACURA ILX 2016 ACURAWATCH PLUS"
ret.enableSteer = True
ret.enableBrake = True
ret.enableGas = True
ret.enableCruise = False
ret.wheelBase = 2.67
ret.steerRatio = 15.3
ret.slipFactor = 0.0014
ret.steerKp, ret.steerKi = 12.0, 1.0
return ret
def car_plant(pos, speed, grade, gas, brake):
# vehicle parameters
@@ -111,6 +95,7 @@ class Plant(object):
Plant.logcan = messaging.pub_sock(context, service_list['can'].port)
Plant.sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
Plant.model = messaging.pub_sock(context, service_list['model'].port)
Plant.cal = messaging.pub_sock(context, service_list['liveCalibration'].port)
Plant.live100 = messaging.sub_sock(context, service_list['live100'].port)
Plant.messaging_initialized = True
@@ -158,7 +143,7 @@ class Plant(object):
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
cp2 = get_can_parser(fake_car_params())
cp2 = get_can_parser(CP)
sgs = cp2._sgs
msgs = cp2._msgs
cks_msgs = cp2.msgs_ck
@@ -263,17 +248,22 @@ class Plant(object):
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
# ******** publish a fake model going straight ********
# ******** publish a fake model going straight and fake calibration ********
if publish_model:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0]*50
x.prob = 1.0
x.std = 1.0
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
self.speed = speed

6
selfdrive/ui/ui.c
View File

@@ -753,9 +753,11 @@ static void ui_draw_vision(UIState *s) {
snprintf(speed_str, sizeof(speed_str), "%3d KPH",
(int)(scene->v_cruise + 0.5));
} else {
// Convert KPH to MPH.
/* Convert KPH to MPH. Using an approximated mph to kph
conversion factor of 1.609 because this is what the Honda
hud seems to be using */
snprintf(speed_str, sizeof(speed_str), "%3d MPH",
(int)(scene->v_cruise * 0.621371 + 0.5));
(int)(scene->v_cruise * 0.621504 + 0.5));
}
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE);
nvgText(s->vg, 480, 95, speed_str, NULL);

4
selfdrive/visiond/visiond
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2441337ea200d9750160c1e83a57da0431af2972801e4f2acb26dc110f46be77
size 16399784
oid sha256:e2dc36a454c0447387826dd0de64ee47e170012df0ec041189ae77d64aa6de3f
size 16407976