From b816310c5045142f72e571badedcbbb145fdb790 Mon Sep 17 00:00:00 2001 From: Gernby Date: Fri, 11 Jan 2019 17:31:48 -0600 Subject: [PATCH 1/3] added resonant parameters --- selfdrive/car/toyota/interface.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4f3b53236acb18..b75c7bfda687d9 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -75,7 +75,12 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay - + + ret.steerReactance = 1.0 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 @@ -85,7 +90,9 @@ def get_params(candidate, fingerprint): ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis - ret.steerActuatorDelay = 0.25 + ret.steerActuatorDelay = 0.15 + ret.steerReactance = 0.7 + ret.eonToFront = -1.0 elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file From 3f7abe394a395b085b176c174bc6cc4f357e7406 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Sat, 12 Jan 2019 09:33:39 -0600 Subject: [PATCH 2/3] added default parameters --- selfdrive/car/ford/interface.py | 4 ++++ selfdrive/car/gm/interface.py | 5 +++++ selfdrive/car/hyundai/interface.py | 5 +++++ selfdrive/car/mock/interface.py | 4 ++++ selfdrive/car/toyota/interface.py | 9 ++++----- 5 files changed, 22 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 633ce4b8ae0458..a7660ec2f57b17 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -77,6 +77,10 @@ def get_params(candidate, fingerprint): ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 f = 1.2 tireStiffnessFront_civic *= f diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 9ec1b9e5d0e12c..f1caa148a704a1 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -64,6 +64,11 @@ def get_params(candidate, fingerprint): ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) ret.openpilotLongitudinalControl = ret.enableCamera + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + std_cargo = 136 if candidate == CAR.VOLT: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c525c0185e939b..2020634dacc451 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -69,6 +69,11 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + ret.steerActuatorDelay = 0.1 # Default delay tire_stiffness_factor = 1. diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index b7d60a4257bfee..160d9b469b5f8e 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -74,6 +74,10 @@ def get_params(candidate, fingerprint): ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.] ret.steerActuatorDelay = 0. + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 return ret diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b75c7bfda687d9..0947317854ccfd 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -75,12 +75,12 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay - - ret.steerReactance = 1.0 + + ret.steerReactance = 0.7 ret.steerInductance = 1.0 ret.steerResistance = 1.0 ret.eonToFront = 0.5 - + if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 @@ -90,8 +90,7 @@ def get_params(candidate, fingerprint): ret.steerKpV, ret.steerKiV = [[0.4], [0.01]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis - ret.steerActuatorDelay = 0.15 - ret.steerReactance = 0.7 + ret.steerActuatorDelay = 0.25 ret.eonToFront = -1.0 elif candidate in [CAR.RAV4, CAR.RAV4H]: From 497bc739da60a38d942735fcb1bc54d3a92e0b69 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Mon, 14 Jan 2019 21:15:11 -0600 Subject: [PATCH 3/3] added independent dashboard server --- launch_chffrplus.sh | 10 +- launch_openpilot.sh | 2 +- selfdrive/dashboard.py | 322 +++++++++++++++++++++++++++++++++++++++++ 3 files changed, 328 insertions(+), 6 deletions(-) create mode 100644 selfdrive/dashboard.py diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 41f8a607cf9ca5..b882395c795643 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -6,11 +6,11 @@ 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 + #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 diff --git a/launch_openpilot.sh b/launch_openpilot.sh index 1525e1715f99c2..ac491e769f7df0 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,5 +1,5 @@ #!/usr/bin/bash -export PASSIVE="0" +export PASSIVE="1" exec ./launch_chffrplus.sh diff --git a/selfdrive/dashboard.py b/selfdrive/dashboard.py new file mode 100644 index 00000000000000..38ea6fda987713 --- /dev/null +++ b/selfdrive/dashboard.py @@ -0,0 +1,322 @@ +#!/usr/bin/env python +#import gc +import zmq +import time +import numpy as np +from influxdb import InfluxDBClient, SeriesHelper +#import numpy.matlib +#import importlib +#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_helpers import calc_lookahead_offset +from selfdrive.controls.lib.pathplanner import PathPlanner +#from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \ +# RDR_TO_LDR, NO_FUSION_SCORE +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 set_realtime_priority, Ratekeeper +#from common.kalman.ekf import EKF, SimpleSensor + + +def dashboard_thread(rate=100): + set_realtime_priority(4) + + USER = '' + PASSWORD = '' + DBNAME = 'carDB' + #influx = InfluxDBClient('192.168.1.61', 8086, USER, PASSWORD, DBNAME) + influx = InfluxDBClient('192.168.43.2', 8086, USER, PASSWORD, DBNAME) + influxLineString = "" + + context = zmq.Context() + + poller = zmq.Poller() + ipaddress = "127.0.0.1" + #ipaddress = "192.168.43.1" + #ipaddress = "192.168.1.33" + model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller) + live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=True, poller=poller) + live20 = messaging.sub_sock(context, service_list['live20'].port, addr=ipaddress, conflate=True, poller=poller) + carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller) + can = messaging.sub_sock(context, service_list['can'].port, addr=ipaddress, conflate=True, poller=poller) + frame = messaging.sub_sock(context, service_list['frame'].port, addr=ipaddress, conflate=True, poller=poller) + sensorEvents = messaging.sub_sock(context, service_list['sensorEvents'].port, addr=ipaddress, conflate=True, poller=poller) + carControl = messaging.sub_sock(context, service_list['carControl'].port, addr=ipaddress, conflate=True, poller=poller) + health = messaging.sub_sock(context, service_list['health'].port, addr=ipaddress, conflate=True, poller=poller) + sendcan = messaging.sub_sock(context, service_list['sendcan'].port, addr=ipaddress, conflate=True, poller=poller) + androidLog = messaging.sub_sock(context, service_list['androidLog'].port, addr=ipaddress, conflate=True, poller=poller) + + _model = None + _live100 = None + _live20 = None + _carState = None + _can = None + _frame = None + _sensorEvents = None + _carControl = None + _health = None + _sendcan = None + _androidLog = None + + frame_count = 0 + + rk = Ratekeeper(rate, print_delay_threshold=np.inf) + while 1: + sample_str = "" + + for socket, event in poller.poll(0): + if socket is live100: + _live100 = messaging.recv_one(socket) + + if sample_str != "": + sample_str += "," + sample_str += ("angleSteersDes=%f,vEgo=%f,steerOverride=%f,vPid=%f,upSteer=%f,uiSteer=%f,ufSteer=%f,cumLagMs=%f,vCruise=%f" % + (_live100.live100.angleSteersDes, _live100.live100.vEgo, _live100.live100.steerOverride, _live100.live100.vPid, + _live100.live100.upSteer, _live100.live100.uiSteer, _live100.live100.ufSteer, _live100.live100.cumLagMs, _live100.live100.vCruise)) + + '''print(_live100) + live100 = ( + vEgo = 0, + aEgoDEPRECATED = 0, + vPid = 0.3, + vTargetLead = 0, + upAccelCmd = 0, + uiAccelCmd = 0, + yActualDEPRECATED = 0, + yDesDEPRECATED = 0, + upSteer = 0, + uiSteer = 0, + aTargetMinDEPRECATED = 0, + aTargetMaxDEPRECATED = 0, + jerkFactor = 0, + angleSteers = -1, + hudLeadDEPRECATED = 0, + cumLagMs = -0.57132614, + canMonoTimeDEPRECATED = 0, + l20MonoTimeDEPRECATED = 0, + mdMonoTimeDEPRECATED = 0, + enabled = false, + steerOverride = false, + canMonoTimes = [], + vCruise = 255, + rearViewCam = false, + alertText1 = "", + alertText2 = "", + awarenessStatus = 0, + angleOffset = -0.74244022, + planMonoTime = 10166703166901, + angleSteersDes = -1, + longControlState = off, + state = disabled, + vEgoRaw = 0, + ufAccelCmd = 0, + ufSteer = 0, + aTarget = 0, + active = false, + curvature = -0.00038641863, + alertStatus = normal, + alertSize = none, + gpsPlannerActive = false, + engageable = false, + alertBlinkingRate = 0, + driverMonitoringOn = false, + alertType = "", + alertSound = "" ) )''' + elif socket is model: + _model = messaging.recv_one(socket) + '''model = ( + frameId = 19786, + path = ( + points = [-0.002040863, 0.0048789978, 0.0024032593, -0.029251099, -0.050567627, -0.071716309, -0.10424805, -0.14196777, -0.18005371, -0.20825195, -0.2277832, -0.26391602, -0.31420898, -0.38085938, -0.43212891, -0.47900391, -0.51318359, -0.56494141, -0.62646484, -0.68212891, -0.73632812, -0.74951172, -0.82519531, -0.89648438, -0.97265625, -1.0615234, -1.1464844, -1.2412109, -1.3369141, -1.4462891, -1.5488281, -1.6445312, -1.7460938, -1.8544922, -1.9658203, -2.0820312, -2.2089844, -2.3320312, -2.484375, -2.6152344, -2.7265625, -2.8554688, -2.984375, -3.1425781, -3.2636719, -3.4160156, -3.5566406, -3.6835938, -3.8222656, -3.9746094], + prob = 1, + std = -0.66490114 ), + leftLane = ( + points = [1.7112548, 1.7149169, 1.720166, 1.7105224, 1.704602, 1.7070434, 1.7025878, 1.6870239, 1.6792724, 1.6579101, 1.6454589, 1.633374, 1.6216552, 1.6204345, 1.5994384, 1.5890625, 1.5706298, 1.5534179, 1.539746, 1.5231445, 1.5058105, 1.4767578, 1.4601562, 1.4442871, 1.4245117, 1.407666, 1.3986328, 1.3683593, 1.340039, 1.3173339, 1.2873046, 1.2677734, 1.234082, 1.199414, 1.1696289, 1.1339843, 1.1095703, 1.0788085, 1.0495117, 1.0299804, 0.98798829, 0.95527345, 0.925, 0.89277345, 0.86982423, 0.81464845, 0.77656251, 0.75117189, 0.71308595, 0.66328126], + prob = 0.11716748, + std = -3.1076281 ), + rightLane = ( + points = [-2.0839355, -2.0990722, -2.1112792, -2.1142089, -2.1159179, -2.1269042, -2.1374023, -2.1483886, -2.1569335, -2.1713378, -2.1752441, -2.1828125, -2.1942871, -2.2208984, -2.2377441, -2.2616699, -2.3014648, -2.3073242, -2.3302734, -2.3463867, -2.3625, -2.37666, -2.3898437, -2.4123046, -2.4396484, -2.4586914, -2.4953125, -2.5304687, -2.5529296, -2.5724609, -2.6032226, -2.6344726, -2.6637695, -2.6911132, -2.727246, -2.758496, -2.7921875, -2.8205078, -2.8458984, -2.8791015, -2.9230468, -2.9416015, -2.9894531, -3.0148437, -3.0392578, -3.0695312, -3.1144531, -3.1388671, -3.1652343, -3.1916015], + prob = 0.095686942, + std = -2.2504346 ), + lead = (dist = 13.424072, prob = 0.37279058, std = 16.329063), + settings = ( + bigBoxX = 0, + bigBoxY = 0, + bigBoxWidth = 0, + bigBoxHeight = 0, + inputTransform = [1.25, 0, 374, 0, 1.25, 337.75, 0, 0, 1] ) ) )''' + elif socket is live20: + _live20 = messaging.recv_one(socket) + '''live20 = ( + angleOffsetDEPRECATED = 0, + calStatusDEPRECATED = 0, + leadOne = ( + dRel = 0, + yRel = 0, + vRel = 0, + aRel = 0, + vLead = 0, + aLeadDEPRECATED = 0, + dPath = 0, + vLat = 0, + vLeadK = 0, + aLeadK = 0, + fcw = false, + status = false, + aLeadTau = 0 ), + cumLagMs = 7678.7031, + mdMonoTime = 10166684697265, + ftMonoTimeDEPRECATED = 0, + calCycleDEPRECATED = 0, + calPercDEPRECATED = 0, + canMonoTimes = [], + l100MonoTime = 10166705048151, + radarErrors = [] ) )''' + elif socket is carState: + _carState = messaging.recv_one(socket) + + if sample_str != "": + sample_str += "," + sample_str += ("steeringTorque=%f,steeringRate=%f,yawRate=%f" % (_carState.carState.steeringTorque, _carState.carState.steeringRate, _carState.carState.yawRate)) + + '''carState = ( + vEgo = 0, + wheelSpeeds = (fl = 0, fr = 0, rl = 0, rr = 0), + gas = 0, + gasPressed = false, + brake = 0, + brakePressed = false, + steeringAngle = -1, + steeringTorque = 84, + steeringPressed = false, + cruiseState = ( + enabled = false, + speed = 0, + available = true, + speedOffset = -0.3, + standstill = false ), + buttonEvents = [], + canMonoTimes = [], + events = [ + ( name = wrongGear, + enable = false, + noEntry = true, + warning = false, + userDisable = false, + softDisable = true, + immediateDisable = false, + preEnable = false, + permanent = false ), + gearShifter = park, + steeringRate = 0, + aEgo = 0, + vEgoRaw = 0, + standstill = true, + brakeLights = false, + leftBlinker = false, + rightBlinker = false, + yawRate = -0, + genericToggle = false, + doorOpen = false, + seatbeltUnlatched = true ) )''' + elif socket is can: + _can = messaging.recv_one(socket) + '''can = [ + ( address = 513, + busTime = 42188, + dat = "", + src = 1 ),''' + elif socket is frame: + _frame = messaging.recv_one(socket) + '''frame = ( + frameId = 14948, + encodeId = 14947, + timestampEof = 10728391665000, + frameLength = 5419, + integLines = 601, + globalGain = 509, + frameType = unknown, + timestampSof = 0, + transform = [1, 0, 0, 0, 1, 0, 0, 0, 1], + lensPos = 281, + lensSag = -0.59991455, + lensErr = 54, + lensTruePos = 273.13062 ) )''' + elif socket is sensorEvents: + _sensorEvents = messaging.recv_one(socket) + '''sensorEvents = [ + ( version = 104, + sensor = 2, + type = 2, + timestamp = 10551787945172, + magnetic = ( + v = [-40.446472, 92.475891, 17.285156], + status = 2 ), + source = android, + uncalibratedDEPRECATED = false ), + ( version = 104, + sensor = 5, + type = 16, + timestamp = 10551844372174, + source = android, + uncalibratedDEPRECATED = false, + gyroUncalibrated = ( + v = [-0.00062561035, -0.0029144287, -0.039916992, 1.5258789e-05, -0.0028686523, -0.039474487], + status = 0 ) ) ] )''' + elif socket is carControl: + _carControl = messaging.recv_one(socket) + '''carControl = ( + enabled = false, + gasDEPRECATED = 0, + brakeDEPRECATED = 0, + steeringTorqueDEPRECATED = 0, + cruiseControl = ( + cancel = false, + override = true, + speedOverride = 0, + accelOverride = 0.714 ), + hudControl = ( + speedVisible = false, + setSpeed = 70.833336, + lanesVisible = false, + leadVisible = false, + visualAlert = none, + audibleAlert = none ), + actuators = (gas = 0, brake = -0, steer = 0, steerAngle = -2.7103169), + active = false ) )''' + elif socket is _health: + _health = messaging.recv_one(socket) + print(_health) + elif socket is _sendcan: + _sendcan = messaging.recv_one(socket) + print(_sendcan) + elif socket is _androidLog: + _androidLog = messaging.recv_one(socket) + print(_androidLog) + + if sample_str != "": + influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % int(time.time() * 1000000000)) + frame_count += 1 + + if frame_count >= 20: + headers = { 'Content-type': 'application/octet-stream', 'Accept': 'text/plain' } + try: + return = influx.request("write",'POST', {'db':DBNAME}, influxLineString.encode('utf-8'), 204, headers) + print(return) + print ('%d %d' % (frame_count, len(influxLineString))) + except: + continue + frame_count = 0 + influxLineString = "" + + rk.keep_time() + +def main(rate=100): + dashboard_thread(rate) + +if __name__ == "__main__": + main()