diff --git a/RELEASES.md b/RELEASES.md index e816b0d796..a0c91d0f47 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -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 diff --git a/cereal/build_from_src.mk b/cereal/build_from_src.mk index 37eb8242c6..3a580592b1 100644 --- a/cereal/build_from_src.mk +++ b/cereal/build_from_src.mk @@ -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) diff --git a/cereal/car.capnp b/cereal/car.capnp index c08c5d08f8..c52a5d12af 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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? } diff --git a/cereal/log.capnp b/cereal/log.capnp index fc3dac779a..183ca904d8 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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 { diff --git a/common/params.py b/common/params.py index b889d7193a..8e8a11edc2 100755 --- a/common/params.py +++ b/common/params.py @@ -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 diff --git a/opendbc b/opendbc index b77861eb00..008089045e 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b77861eb00d45e25af501f78bbed155d2df06159 +Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba diff --git a/panda b/panda index 5409c51041..ff8a4bd4e8 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5409c51041cfe8f139650a4e0decf4f6d863eb07 +Subproject commit ff8a4bd4e844c2b0a0dd7efb321b31781d8034c8 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index c4cf058c40..b63220b70d 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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,7 +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), @@ -161,7 +164,7 @@ def get_can_parser(CP): (0x156, 100), (0x158, 100), (0x17c, 100), - #(0x1a3, 50), + (0x191, 100), (0x1a4, 50), (0x1a6, 50), (0x1b0, 50), @@ -177,6 +180,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), @@ -189,13 +193,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", 0x191, 0), @@ -213,7 +217,6 @@ def get_can_parser(CP): (0x158, 100), (0x17c, 100), (0x191, 100), - (0x1a3, 50), (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'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index a6e006537c..38357d0d8a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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, \ diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk index 8f3a3ad6c4..f4603dc32d 100644 --- a/selfdrive/common/cereal.mk +++ b/selfdrive/common/cereal.mk @@ -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++ diff --git a/selfdrive/common/params.c b/selfdrive/common/params.c index 980fede3ca..7cefc7b06b 100644 --- a/selfdrive/common/params.c +++ b/selfdrive/common/params.c @@ -2,7 +2,10 @@ #include "selfdrive/common/util.h" +#ifndef _GNU_SOURCE #define _GNU_SOURCE +#endif // _GNU_SOURCE + #include #include #include diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index ca7bd08a33..05ddb1dd79 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -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); diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 686ee31c24..29df39c7a3 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define OPENPILOT_VERSION "0.3.5" +#define OPENPILOT_VERSION "0.3.6" diff --git a/selfdrive/config.py b/selfdrive/config.py index 440da22746..7d923b8098 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9d447954b8..abd7afa228 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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) diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py index 3b3207e7c6..1fcf6cad68 100644 --- a/selfdrive/controls/lib/adaptivecruise.py +++ b/selfdrive/controls/lib/adaptivecruise.py @@ -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) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index dcf336b99d..b8a9ddc1f7 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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. diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 4c217cab03..720d7947f8 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py new file mode 100755 index 0000000000..3c676fbeab --- /dev/null +++ b/selfdrive/controls/lib/vehicle_model.py @@ -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) + diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 936b81294e..00ee9182df 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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(): diff --git a/selfdrive/debug/getframes/Makefile b/selfdrive/debug/getframes/Makefile new file mode 100644 index 0000000000..932673e208 --- /dev/null +++ b/selfdrive/debug/getframes/Makefile @@ -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 diff --git a/selfdrive/debug/getframes/__init__.py b/selfdrive/debug/getframes/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py new file mode 100755 index 0000000000..5c63b8986d --- /dev/null +++ b/selfdrive/debug/getframes/getframes.py @@ -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] diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index de1345e897..c3bf23ddb9 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index cb5c27b891..119e0d68a4 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -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 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 292e72623b..1e28cf22c9 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 464c318ecc..51a0e9409c 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -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 diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 0219f037da..fc5c16a942 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -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); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 9d74122bb6..23e6505bf2 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ