From 490307fe50f59dbb12cfbb16cb850f59bfbff521 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Wed, 22 Nov 2017 04:30:24 -0800 Subject: [PATCH] openpilot v0.3.9 release old-commit-hash: 5627d0d7fd3fe0eb673b21788a9ce0ca442df7b1 --- README.md | 22 +- RELEASES.md | 14 +- apk/com.baseui.apk | 4 +- cereal/car.capnp | 14 +- cereal/log.capnp | 19 +- common/dbc.py | 2 +- common/fingerprints.py | 3 + common/params.py | 1 + opendbc | 2 +- panda | 2 +- selfdrive/car/__init__.py | 2 +- selfdrive/car/honda/carcontroller.py | 26 +- selfdrive/car/honda/carstate.py | 23 +- selfdrive/car/honda/interface.py | 96 ++++- selfdrive/car/toyota/carcontroller.py | 71 ++-- selfdrive/car/toyota/carstate.py | 15 +- selfdrive/car/toyota/interface.py | 42 +- selfdrive/car/toyota/toyotacan.py | 18 +- selfdrive/car/toyota/values.py | 8 + selfdrive/common/version.h | 2 +- selfdrive/config.py | 2 + selfdrive/controls/controlsd.py | 50 +-- selfdrive/controls/lib/adaptivecruise.py | 294 -------------- selfdrive/controls/lib/alertmanager.py | 6 +- selfdrive/controls/lib/drive_helpers.py | 4 +- selfdrive/controls/lib/fcw.py | 66 ---- selfdrive/controls/lib/latcontrol.py | 27 +- selfdrive/controls/lib/longcontrol.py | 74 ++-- .../controls/lib/longitudinal_mpc/Makefile | 94 +++++ .../controls/lib/longitudinal_mpc/__init__.py | 0 .../lib/longitudinal_mpc/generator.cpp | 98 +++++ .../lib/longitudinal_mpc/libmpc_py.py | 43 ++ selfdrive/controls/lib/longitudinal_mpc/mpc.c | 118 ++++++ .../mpc_export/acado_auxiliary_functions.c | 3 + .../mpc_export/acado_auxiliary_functions.h | 3 + .../mpc_export/acado_common.h | 3 + .../mpc_export/acado_integrator.c | 3 + .../mpc_export/acado_qpoases_interface.cpp | 3 + .../mpc_export/acado_qpoases_interface.hpp | 3 + .../mpc_export/acado_solver.c | 3 + selfdrive/controls/lib/pid.py | 22 +- selfdrive/controls/lib/planner.py | 371 ++++++++++++++++-- selfdrive/controls/lib/speed_smoother.py | 88 +++++ selfdrive/controls/radard.py | 8 +- selfdrive/loggerd/loggerd | 2 +- selfdrive/manager.py | 2 + selfdrive/radar/toyota/interface.py | 25 +- selfdrive/sensord/gpsd | 4 +- selfdrive/sensord/sensord | 2 +- selfdrive/service_list.yaml | 1 + selfdrive/test/plant/maneuver.py | 2 +- selfdrive/test/plant/maneuverplots.py | 14 +- selfdrive/test/plant/plant.py | 3 +- .../test/tests/plant/test_longitudinal.py | 12 + selfdrive/visiond/visiond | 4 +- 55 files changed, 1189 insertions(+), 654 deletions(-) create mode 100644 selfdrive/car/toyota/values.py delete mode 100644 selfdrive/controls/lib/adaptivecruise.py delete mode 100644 selfdrive/controls/lib/fcw.py create mode 100644 selfdrive/controls/lib/longitudinal_mpc/Makefile create mode 100644 selfdrive/controls/lib/longitudinal_mpc/__init__.py create mode 100644 selfdrive/controls/lib/longitudinal_mpc/generator.cpp create mode 100644 selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc.c create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp create mode 100644 selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c create mode 100644 selfdrive/controls/lib/speed_smoother.py diff --git a/README.md b/README.md index f7c682c864..f9a91e8f03 100644 --- a/README.md +++ b/README.md @@ -30,21 +30,25 @@ Supported Cars - Can only be enabled above 25 mph - Toyota RAV-4 2016+ with TSS-P (alpha!) - - Can only be enabled above 20 mph + - By default it uses stock Toyota ACC for longitudinal control + - openpilot longitudinal control available after unplugging the Driving Support ECU and can be enabled above 20 mph + +- Toyota Prius 2017 (alpha!) + - By default it uses stock Toyota ACC for longitudinal control + - openpilot longitudinal control available after unplugging the Driving Support ECU In Progress Cars ------ +- Probably all TSS-P Toyota with Steering Assist. + - 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option. + - Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported. -- Toyota Prius 2017 - -- Probably all TSS-P Toyota - -Community Ported Cars +Community WIP Cars ------ -- Chevy Volt 2016-2018 Premier with Driver Confidence II +- [Chevy Volt 2016-2018 Premier with Driver Confidence II](https://github.com/commaai/openpilot/pull/104) -- Classic Tesla Model S (pre-AP) +- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145) Directory structure ------ @@ -98,7 +102,7 @@ User Data / chffr Account / Crash Reporting By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone. -It's open source software, so you are free to disable it if you wish. +It's open source software, so you are free to disable it if you wish. It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. It does not log the user facing camera or the microphone. diff --git a/RELEASES.md b/RELEASES.md index 3839e464aa..e4d234a06a 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Version 0.3.9 (2017-11-21) +========================== + * Add alpha support for 2017 Toyota Prius + * Improved longitudinal control using model predictive control + * Enable Forward Collision Warning + * Acura ILX now maintains openpilot engaged at standstill when brakes are applied + Version 0.3.8.2 (2017-10-30) ========================== * Add alpha support for 2017 Toyota RAV4 @@ -17,9 +24,9 @@ Version 0.3.7 (2017-09-30) * Fixed sporadic longitudinal pulsing in Civic * Cleanups to vehicle interface -Version 0.3.6.1 (2017-08-15) -============================ - * Mitigate low speed steering oscillations on some vehicles +Version 0.3.6.1 (2017-08-15) +============================ + * Mitigate low speed steering oscillations on some vehicles * Include board steering check for CR-V Version 0.3.6 (2017-08-08) @@ -139,4 +146,3 @@ Version 0.1 (2016-11-29) * Lane keep assist is working * Support for Acura ILX 2016 with AcuraWatch Plus * Support for Honda Civic 2016 Touring Edition - diff --git a/apk/com.baseui.apk b/apk/com.baseui.apk index 4cc33ce663..204b1987d1 100644 --- a/apk/com.baseui.apk +++ b/apk/com.baseui.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:85daa6764a264b9639ee3693fd577259ec6e73c83ae9355f5a62ae12049fb832 -size 6238486 +oid sha256:2617aa99a01fd0994e53b34a32699ac75186ac8a40c2e3021e4ce3ca2ba03de2 +size 6335294 diff --git a/cereal/car.capnp b/cereal/car.capnp index 35828f08c9..1580aa437a 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -77,6 +77,7 @@ struct CarState { # brake pedal, 0.0-1.0 brake @5 :Float32; # this is user pedal only brakePressed @6 :Bool; # this is user pedal only + brakeLights @19 :Bool; # steering wheel steeringAngle @7 :Float32; # deg @@ -92,6 +93,8 @@ struct CarState { # button presses buttonEvents @11 :List(ButtonEvent); + leftBlinker @20 :Bool; + rightBlinker @21 :Bool; # which packets this state came from canMonoTimes @12: List(UInt64); @@ -109,6 +112,7 @@ struct CarState { speed @1 :Float32; available @2 :Bool; speedOffset @3 :Float32; + standstill @4 :Bool; } enum GearShifter { @@ -291,10 +295,16 @@ struct CarParams { steerKi @17 :Float32; steerKf @26 :Float32; + # Kp and Ki for the longitudinal control + longitudinalKpBP @37 :List(Float32); + longitudinalKpV @38 :List(Float32); + longitudinalKiBP @39 :List(Float32); + longitudinalKiV @40 :List(Float32); + steerLimitAlert @30 :Bool; vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake - - # TODO: Kp and Ki for long control, perhaps not needed? + stoppingControl @35 :Bool; # Does the car allows full control even at lows speeds when stopping + startAccel @36 :Float32; # Required acceleraton to overcome creep braking } diff --git a/cereal/log.capnp b/cereal/log.capnp index 6193d2c3ee..fd550c2de9 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -337,12 +337,15 @@ struct Live100Data { vTargetLead @3 :Float32; upAccelCmd @4 :Float32; uiAccelCmd @5 :Float32; + ufAccelCmd @33 :Float32; yActualDEPRECATED @6 :Float32; - yDes @7 :Float32; + yDesDEPRECATED @7 :Float32; upSteer @8 :Float32; uiSteer @9 :Float32; - aTargetMin @10 :Float32; - aTargetMax @11 :Float32; + ufSteer @34 :Float32; + aTargetMinDEPRECATED @10 :Float32; + aTargetMaxDEPRECATED @11 :Float32; + aTarget @35 :Float32; jerkFactor @12 :Float32; angleSteers @13 :Float32; # Steering angle in degrees. angleSteersDes @29 :Float32; @@ -350,6 +353,7 @@ struct Live100Data { cumLagMs @15 :Float32; enabled @19 :Bool; + active @36 :Bool; steerOverride @20 :Bool; vCruise @22 :Float32; @@ -388,7 +392,7 @@ struct ModelData { leftLane @2 :PathData; rightLane @3 :PathData; lead @4 :LeadData; - leadNew @6 :List(Float32); + freePath @6 :List(Float32); settings @5 :ModelSettings; @@ -472,10 +476,13 @@ struct Plan { # longitudinal longitudinalValid @2 :Bool; + vCruise @16 :Float32; + aCruise @17 :Float32; vTarget @3 :Float32; vTargetFuture @14 :Float32; - aTargetMin @4 :Float32; - aTargetMax @5 :Float32; + aTargetMinDEPRECATED @4 :Float32; + aTargetMaxDEPRECATED @5 :Float32; + aTarget @18 :Float32; jerkFactor @6 :Float32; hasLead @7 :Bool; fcw @8 :Bool; diff --git a/common/dbc.py b/common/dbc.py index 6b93cf410c..8865ae0f58 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -143,7 +143,7 @@ class dbc(object): msg = self.msgs.get(x[0]) if msg is None: if x[0] not in self._warned_addresses: - print("WARNING: Unknown message address {}".format(x[0])) + #print("WARNING: Unknown message address {}".format(x[0])) self._warned_addresses.add(x[0]) return None, None diff --git a/common/fingerprints.py b/common/fingerprints.py index 101f6cb5d4..9881115b69 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -19,6 +19,9 @@ _FINGERPRINTS = { "TOYOTA RAV4 2017": { 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 }, + "TOYOTA PRIUS 2017": { + 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }, } # support additional internal only fingerprints diff --git a/common/params.py b/common/params.py index 7e2ea7900a..a13095731f 100755 --- a/common/params.py +++ b/common/params.py @@ -56,6 +56,7 @@ keys = { # read: ui, controls "IsMetric": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT, + "IsFcwEnabled": TxType.PERSISTANT, # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTANT, diff --git a/opendbc b/opendbc index 835a9739d6..c8eeedce17 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85 +Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21 diff --git a/panda b/panda index 849f68879d..3cab372975 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383 +Subproject commit 3cab37297566962fd6e48a674db3e1f6de8fa4da diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index f27e41ed16..516ed7c0c0 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -85,4 +85,4 @@ def get_car(logcan, sendcan=None, timeout=None): interface_cls = interfaces[candidate] params = interface_cls.get_params(candidate, fingerprints) - return interface_cls(params, logcan, sendcan), params + return interface_cls(params, sendcan), params diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b22ad701ab..287deccb58 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -18,7 +18,7 @@ CAMERA_MSGS = [0xe4, 0x194] def actuator_hystereses(brake, braking, brake_steady, v_ego, civic): # hyst params... TODO: move these to VehicleParams - brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value + brake_hyst_on = 0.02 # to activate brakes exceed this value brake_hyst_off = 0.005 # to deactivate brakes below this value brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value @@ -36,14 +36,8 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic): brake_steady = brake + brake_hyst_gap brake = brake_steady - if not civic: - brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived - brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds - # offset the brake command for threshold in the brake system. no brake torque perceived below it - brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v) - brake_offset = brake_on_offset - brake_hyst_on - if brake > 0.0: - brake += brake_offset + if not civic and brake > 0.0: + brake += 0.15 return brake, braking, brake_steady @@ -69,6 +63,7 @@ HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5", "lanes", "beep", "X8", "chime", "acc_alert"]) + class CarController(object): def __init__(self, enable_camera=True): self.braking = False @@ -95,8 +90,7 @@ class CarController(object): pcm_cancel_cmd = True # *** rate limit after the enable check *** - brake = rate_limit(brake, self.brake_last, -2., 1./100) - self.brake_last = brake + self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100) # vehicle hud display, wait for one update from 10Hz 0x304 msg #TODO: use enum!! @@ -142,17 +136,9 @@ class CarController(object): # steer torque is converted back to CAN reference (positive when steering right) apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1)) - apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1)) + apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) - # no gas if you are hitting the brake or the user is - if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed): - apply_gas = 0 - - # no computer brake if the gas is being pressed - if CS.car_gas > 0 and apply_brake != 0: - apply_brake = 0 - # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 if CS.steer_not_allowed: apply_steer = 0 diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 8903afc89b..6ade91d580 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -2,7 +2,6 @@ import os import time from cereal import car from common.numpy_fast import interp -from common.realtime import sec_since_boot import selfdrive.messaging as messaging from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV @@ -275,7 +274,7 @@ def get_can_parser(CP): return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0) class CarState(object): - def __init__(self, CP, logcan): + def __init__(self, CP): self.acura = False self.civic = False self.accord = False @@ -294,9 +293,6 @@ class CarState(object): self.brake_only = CP.enableCruise self.CP = CP - # initialize can parser - self.cp = get_can_parser(CP) - self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 @@ -315,14 +311,13 @@ class CarState(object): self.v_ego_C = np.matrix([1.0, 0.0]) self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) self.v_ego_R = 1e3 + self.v_ego = 0.0 # import control # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # self.v_ego_K = np.transpose(K) self.v_ego_K = np.matrix([[0.12287673], [0.29666309]]) - def update(self, can_pub_main=None): - cp = self.cp - cp.update(int(sec_since_boot() * 1e9), False) + def update(self, cp): # copy can_valid self.can_valid = cp.can_valid @@ -368,6 +363,9 @@ class CarState(object): # blend in transmission speed at low speed, since it has more low speed accuracy self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v) speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel + + if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_x = np.matrix([[speed], [0.0]]) self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed) self.v_ego_raw = speed @@ -449,13 +447,15 @@ class CarState(object): else: self.car_gas = cp.vl[0x130]['CAR_GAS'] self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200 + self.steer_torque_driver = cp.vl[0x18F]['STEER_TORQUE_SENSOR'] # brake switch has shown some single time step noise, so only considered when # switch is on for at least 2 consecutive CAN samples + self.brake_switch = cp.vl[0x17C]['BRAKE_SWITCH'] self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \ - (cp.vl[0x17C]['BRAKE_SWITCH'] and self.brake_switch_prev and \ + (self.brake_switch and self.brake_switch_prev and \ cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts) - self.brake_switch_prev = cp.vl[0x17C]['BRAKE_SWITCH'] + self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH'] self.user_brake = cp.vl[0x1A4]['USER_BRAKE'] @@ -472,7 +472,6 @@ if __name__ == '__main__': import time from selfdrive.services import service_list context = zmq.Context() - logcan = messaging.sub_sock(context, service_list['can'].port) class CarParams(object): def __init__(self): @@ -480,7 +479,7 @@ if __name__ == '__main__': self.enableGas = 0 self.enableCruise = 0 CP = CarParams() - CS = CarState(CP, logcan) + CS = CarState(CP) while 1: CS.update() diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e4356676b1..90eca2b38d 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -2,23 +2,34 @@ import os import time import numpy as np -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging -from selfdrive.car.honda.carstate import CarState +from selfdrive.car.honda.carstate import CarState, get_can_parser from selfdrive.car.honda.carcontroller import CAMERA_MSGS from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH +from selfdrive.controls.lib.planner import A_ACC_MAX try: from .carcontroller import CarController except ImportError: CarController = None -def get_compute_gb(): + +def compute_gb_honda(accel, speed): + creep_brake = 0.0 + creep_speed = 2.3 + creep_brake_value = 0.15 + if speed < creep_speed: + creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + return float(accel) / 4.8 - creep_brake + + +def get_compute_gb_acura(): # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0] # where -1.0 is max brake and 1.0 is max gas # see debug/dump_accel_from_fiber.py to see how those parameters were generated @@ -45,8 +56,8 @@ def get_compute_gb(): def leakyrelu(x, alpha): return np.maximum(x, alpha * x) - def _compute_gb(accel, speed): - #linearly extrap below v1 using v1 and v2 data + def _compute_gb_acura(accel, speed): + # linearly extrap below v1 using v1 and v2 data v1 = 5. v2 = 10. dat = np.array([accel, speed]) @@ -60,12 +71,11 @@ def get_compute_gb(): m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 return float(m4) - return _compute_gb + return _compute_gb_acura class CarInterface(object): - def __init__(self, CP, logcan, sendcan=None): - self.logcan = logcan + def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 @@ -75,8 +85,10 @@ class CarInterface(object): self.brake_pressed_prev = False self.can_invalid_count = 0 + self.cp = get_can_parser(CP) + # *** init the major players *** - self.CS = CarState(CP, self.logcan) + self.CS = CarState(CP) # sending if read only is False if sendcan is not None: @@ -87,6 +99,25 @@ class CarInterface(object): # self.accord_msg = [] raise NotImplementedError + if not self.CS.civic: + self.compute_gb = get_compute_gb_acura() + else: + self.compute_gb = compute_gb_honda + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + eA = a_ego - a_target + valuesA = [1.0, 0.1] + bpA = [0.0, 0.5] + + eV = v_ego - v_target + valuesV = [1.0, 0.1] + bpV = [0.0, 0.5] + + # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant + # unless aTargetMax is very high and then we scale with it; this help in quicker restart + return float(max(0.714, a_target / A_ACC_MAX)) * min(interp(eA, bpA, valuesA), interp(eV, bpV, valuesV)) + @staticmethod def get_params(candidate, fingerprint): @@ -130,6 +161,11 @@ class CarInterface(object): # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [3.6, 2.4, 1.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.54, 0.36] elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS": stop_and_go = False ret.m = 3095./2.205 + std_cargo @@ -139,6 +175,11 @@ class CarInterface(object): # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] elif candidate == "HONDA ACCORD 2016 TOURING": stop_and_go = False ret.m = 3580./2.205 + std_cargo @@ -146,6 +187,11 @@ class CarInterface(object): ret.aF = ret.l * 0.38 ret.sR = 15.3 ret.steerKp, ret.steerKi = 0.8, 0.24 + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] elif candidate == "HONDA CR-V 2016 TOURING": stop_and_go = False ret.m = 3572./2.205 + std_cargo @@ -153,6 +199,11 @@ class CarInterface(object): ret.aF = ret.l * 0.41 ret.sR = 15.3 ret.steerKp, ret.steerKi = 0.8, 0.24 + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] else: raise ValueError("unsupported car %s" % candidate) @@ -188,19 +239,20 @@ class CarInterface(object): ret.longPidDeadzoneBP = [0.] ret.longPidDeadzoneV = [0.] + ret.stoppingControl = True ret.steerLimitAlert = True + ret.startAccel = 0.5 return ret - compute_gb = staticmethod(get_compute_gb()) - # returns a car.CarState def update(self, c): # ******************* do can recv ******************* - can_pub_main = [] canMonoTimes = [] - self.CS.update(can_pub_main) + self.cp.update(int(sec_since_boot() * 1e9), False) + + self.CS.update(self.cp) # create message ret = car.CarState.new_message() @@ -225,6 +277,10 @@ class CarInterface(object): # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 + # FIXME: read sendcan for brakelights + brakelights_threshold = 0.02 if self.CS.civic else 0.1 + ret.brakeLights = bool(self.CS.brake_switch or + c.actuators.brake > brakelights_threshold) # steering wheel ret.steeringAngle = self.CS.angle_steers @@ -233,7 +289,7 @@ class CarInterface(object): # gear shifter lever ret.gearShifter = self.CS.gear_shifter - ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR'] + ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state @@ -241,9 +297,12 @@ class CarInterface(object): ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset + ret.cruiseState.standstill = False # TODO: button presses buttonEvents = [] + ret.leftBlinker = bool(self.CS.left_blinker_on) + ret.rightBlinker = bool(self.CS.right_blinker_on) if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() @@ -332,16 +391,16 @@ class CarInterface(object): (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed: if ret.gasPressed: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert - if self.CP.enableCruise and not ret.cruiseState.enabled and \ - (c.actuators.brake <= 0. or ret.vEgo < 0.3): + if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) + if not self.CS.civic and ret.vEgo < 0.001: + events.append(create_event('manualRestart', [ET.WARNING])) cur_time = sec_since_boot() enable_pressed = False @@ -380,14 +439,11 @@ class CarInterface(object): self.brake_pressed_prev = ret.brakePressed # cast to reader so it can't be modified - #print ret return ret.as_reader() # pass in a car.CarControl # to be called @ 100hz def apply(self, c): - #print c - if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH else: diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 115a2599d7..097bf361d7 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,9 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ create_steer_command, create_ui_command, \ - create_ipas_steer_command, create_accel_command + create_ipas_steer_command, create_accel_command, \ + create_fcw_command +from selfdrive.car.toyota.values import CAR, ECU ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value @@ -14,22 +16,12 @@ ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) STEER_MAX = 1500 STEER_DELTA_UP = 10 # 1.5s time to peak torque -STEER_DELTA_DOWN = 45 # lower than 50 otherwise the Rav4 faults (Prius seems ok though) +STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_ERROR_MAX = 500 # max delta between torque cmd and torque motor STEER_IPAS_MAX = 340 STEER_IPAS_DELTA_MAX = 3 -class CAR: - PRIUS = "TOYOTA PRIUS 2017" - RAV4 = "TOYOTA RAV4 2017" - -class ECU: - CAM = 0 # camera - DSU = 1 # driving support unit - APGS = 2 # advanced parking guidance system - - TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345, 0x363, 0x364, 0x365, 0x370, 0x371, 0x372, 0x373, 0x374, 0x375, 0x380, 0x381, 0x382, @@ -75,7 +67,6 @@ STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), - 0x411: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x20\x00\x00\x10\x00\x80\x00'), 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), 0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'), } @@ -107,18 +98,23 @@ def accel_hysteresis(accel, accel_steady, enabled): def process_hud_alert(hud_alert, audible_alert): # initialize to no alert - hud1 = 0 - hud2 = 0 - if hud_alert in ['steerRequired', 'fcw']: - if audible_alert == 'chimeRepeated': - hud1 = 3 - else: - hud1 = 1 - if audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']: + steer = 0 + fcw = 0 + sound1 = 0 + sound2 = 0 + + if hud_alert == 'fcw': + fcw = 1 + elif hud_alert == 'steerRequired': + steer = 1 + + if audible_alert == 'chimeRepeated': + sound1 = 1 + elif audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']: # TODO: find a way to send single chimes - hud2 = 1 + sound2 = 1 - return hud1, hud2 + return steer, fcw, sound1, sound2 class CarController(object): @@ -130,6 +126,8 @@ class CarController(object): self.accel_steady = 0. self.car_fingerprint = car_fingerprint self.alert_active = False + self.last_standstill = False + self.standstill_req = False self.fake_ecus = set() if enable_camera: self.fake_ecus.add(ECU.CAM) @@ -140,7 +138,7 @@ class CarController(object): pcm_cancel_cmd, hud_alert, audible_alert): # *** compute control surfaces *** - tt = sec_since_boot() + ts = sec_since_boot() # steer torque is converted back to CAN reference (positive when steering right) apply_accel = actuators.gas - actuators.brake @@ -170,8 +168,16 @@ class CarController(object): if not enabled or CS.steer_error: apply_steer = 0 + # on entering standstill, send standstill request + if CS.standstill and not self.last_standstill: + self.standstill_req = True + if CS.pcm_acc_status != 8: + # pcm entered standstill or it's disabled + self.standstill_req = False + self.last_steer = apply_steer self.last_accel = apply_accel + self.last_standstill = CS.standstill can_sends = [] @@ -190,9 +196,9 @@ class CarController(object): # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): if ECU.DSU in self.fake_ecus: - can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd)) + can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd, self.standstill_req)) else: - can_sends.append(create_accel_command(0, pcm_cancel_cmd)) + can_sends.append(create_accel_command(0, pcm_cancel_cmd, False)) if frame % 10 == 0 and ECU.CAM in self.fake_ecus: for addr in TARGET_IDS: @@ -201,24 +207,25 @@ class CarController(object): # ui mesg is at 100Hz but we send asap if: # - there is something to display # - there is something to stop displaying - hud1, hud2 = process_hud_alert(hud_alert, audible_alert) - if ((hud1 or hud2) and not self.alert_active) or \ - (not (hud1 or hud2) and self.alert_active): + alert_out = process_hud_alert(hud_alert, audible_alert) + steer, fcw, sound1, sound2 = alert_out + + if (any(alert_out) and not self.alert_active) or \ + (not any(alert_out) and self.alert_active): send_ui = True self.alert_active = not self.alert_active else: send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: - can_sends.append(create_ui_command(hud1, hud2)) + can_sends.append(create_ui_command(steer, sound1, sound2)) + can_sends.append(create_fcw_command(fcw)) #*** static msgs *** for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems(): if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: - # send msg! # special cases - if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame / 5) % 7) + 1) << 5 vl = chr(cnt) + vl diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index dcfc3fa633..2b948ac493 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,8 +1,7 @@ import os import time -from common.realtime import sec_since_boot import selfdrive.messaging as messaging -from selfdrive.car.toyota.carcontroller import CAR +from selfdrive.car.toyota.values import CAR from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV import numpy as np @@ -85,6 +84,7 @@ def get_can_parser(CP): ("STEER_TORQUE_EPS", 608, 0), ("TURN_SIGNALS", 1556, 3), # 3 is no blinkers ("LKA_STATE", 610, 0), + ("BRAKE_LIGHTS_ACC", 951, 0), ] checks += [ @@ -100,14 +100,13 @@ def get_can_parser(CP): class CarState(object): - def __init__(self, CP, logcan): + def __init__(self, CP): self.CP = CP self.left_blinker_on = 0 self.right_blinker_on = 0 # initialize can parser - self.cp = get_can_parser(CP) self.car_fingerprint = CP.carFingerprint # vEgo kalman filter @@ -117,14 +116,13 @@ class CarState(object): self.v_ego_C = np.matrix([1.0, 0.0]) self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) self.v_ego_R = 1e3 + self.v_ego = 0.0 # import control # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # self.v_ego_K = np.transpose(K) self.v_ego_K = np.matrix([[0.12287673], [0.29666309]]) - def update(self): - cp = self.cp - cp.update(int(sec_since_boot() * 1e9), False) + def update(self, cp): # copy can_valid self.can_valid = cp.can_valid @@ -160,6 +158,8 @@ class CarState(object): cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS # Kalman filter + if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_x = np.matrix([[self.v_wheel], [0.0]]) self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel) self.v_ego_raw = self.v_wheel self.v_ego = float(self.v_ego_x[0]) @@ -185,3 +185,4 @@ class CarState(object): self.car_gas = self.pedal_gas self.gas_pressed = not cp.vl[466]['GAS_RELEASED'] self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2 + self.brake_lights = bool(cp.vl[951]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4ef103aa2d..3d8e4e170d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,19 +1,19 @@ #!/usr/bin/env python import os import time +from common.realtime import sec_since_boot import common.numpy_fast as np from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.carstate import CarState, CAR -from selfdrive.car.toyota.carcontroller import CarController, ECU, check_ecu_msgs +from selfdrive.car.toyota.carstate import CarState, get_can_parser +from selfdrive.car.toyota.values import CAR, ECU +from selfdrive.car.toyota.carcontroller import CarController, check_ecu_msgs from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event - class CarInterface(object): - def __init__(self, CP, logcan, sendcan=None): - self.logcan = logcan + def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 @@ -23,13 +23,23 @@ class CarInterface(object): self.cruise_enabled_prev = False # *** init the major players *** - self.CS = CarState(CP, self.logcan) + self.CS = CarState(CP) + + self.cp = get_can_parser(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 3.0 + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + @staticmethod def get_params(candidate, fingerprint): @@ -108,13 +118,15 @@ class CarInterface(object): ret.enableGas = True ret.steerLimitAlert = False + ret.stoppingControl = False + ret.startAccel = 0.0 - return ret + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [3.6, 2.4, 1.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.54, 0.36] - @staticmethod - def compute_gb(accel, speed): - # toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through. - return accel + return ret # returns a car.CarState def update(self, c): @@ -122,7 +134,9 @@ class CarInterface(object): can_pub_main = [] canMonoTimes = [] - self.CS.update() + self.cp.update(int(sec_since_boot() * 1e9), False) + + self.CS.update(self.cp) # create message ret = car.CarState.new_message() @@ -147,6 +161,7 @@ class CarInterface(object): # brake pedal ret.brake = self.CS.user_brake ret.brakePressed = self.CS.brake_pressed != 0 + ret.brakeLights = self.CS.brake_lights # steering wheel ret.steeringAngle = self.CS.angle_steers @@ -160,6 +175,7 @@ class CarInterface(object): ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. + ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 # TODO: button presses buttonEvents = [] @@ -177,6 +193,8 @@ class CarInterface(object): buttonEvents.append(be) ret.buttonEvents = buttonEvents + ret.leftBlinker = bool(self.CS.left_blinker_on) + ret.rightBlinker = bool(self.CS.right_blinker_on) # events events = [] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index cfba3ce5af..812032f8dc 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -68,16 +68,24 @@ def create_steer_command(steer, raw_cnt): return make_can_msg(0x2e4, msg, 0, True) -def create_accel_command(accel, pcm_cancel): +def create_accel_command(accel, pcm_cancel, standstill_req): # TODO: find the exact canceling bit - state = 0xc0 + pcm_cancel # this allows automatic restart from hold without driver cmd + state = 0x40 if standstill_req else 0xC0 + state += pcm_cancel # this allows automatic restart from hold without driver cmd msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00) return make_can_msg(0x343, msg, 0, True) +def create_fcw_command(fcw): -def create_ui_command(hud1, hud2): - msg = struct.pack('!BBBBBBBB', 0x54, 0x04 + hud1 + (hud2 << 4), 0x0C, - 0x00, 0x00, 0x2C, 0x38, 0x02) + msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00) + + return make_can_msg(0x411, msg, 0, False) + + +def create_ui_command(steer, sound1, sound2): + + msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00, + sound1, 0x2C, 0x38, 0x02) return make_can_msg(0x412, msg, 0, False) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py new file mode 100644 index 0000000000..06373b3b18 --- /dev/null +++ b/selfdrive/car/toyota/values.py @@ -0,0 +1,8 @@ +class CAR: + PRIUS = "TOYOTA PRIUS 2017" + RAV4 = "TOYOTA RAV4 2017" + +class ECU: + CAM = 0 # camera + DSU = 1 # driving support unit + APGS = 2 # advanced parking guidance system diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index fad8964840..777dde97d1 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.3.8.2-openpilot" +#define COMMA_VERSION "0.3.9-openpilot" diff --git a/selfdrive/config.py b/selfdrive/config.py index fc08f6292f..3488a43816 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -21,12 +21,14 @@ class Conversions: # Car button codes +# TODO: this is Honda specific, move to honda/interface.py class CruiseButtons: RES_ACCEL = 4 DECEL_SET = 3 CANCEL = 2 MAIN = 1 +RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car # Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21) class ImageParams: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 218845a73c..4a33a22c81 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,15 +1,13 @@ #!/usr/bin/env python -import os import json from copy import copy import zmq -from cereal import car, log +from cereal import car from common.numpy_fast import clip 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 @@ -22,7 +20,7 @@ from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEE 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 @@ -31,7 +29,6 @@ V_CRUISE_ENABLE_MIN = 40 AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car -AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted class Calibration: @@ -103,9 +100,9 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): return CS, events, cal_status, overtemp, free_space -def calc_plan(CS, events, PL, LoC): +def calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status): # plan runs always, independently of the state - plan_packet = PL.update(CS, LoC) + plan_packet = PL.update(CS, LoC, v_cruise_kph, awareness_status < -0.) plan = plan_packet.plan plan_ts = plan_packet.logMonoTime @@ -114,7 +111,7 @@ def calc_plan(CS, events, PL, LoC): # disable if lead isn't close when system is active and brake is pressed to avoid # unexpected vehicle accelerations - if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED: + if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED: events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) return plan, plan_ts @@ -265,24 +262,18 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s for e in get_events(events, [ET.WARNING]): AM.add(e, enabled) - # if user is not responsive to awareness alerts, then start a smooth deceleration - if awareness_status < -0.: - plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL) - plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax) - # *** angle offset learning *** if rk.frame % 5 == 2 and plan.lateralValid: # *** run this at 20hz again *** angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, - PL.PP.c_poly, PL.PP.c_prob, LaC.y_des, + PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, CS.steeringPressed) # *** gas/brake PID loop *** - actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, - v_cruise_kph, plan.vTarget, - [plan.aTargetMin, plan.aTargetMax], - plan.jerkFactor, CP) + actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, + v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, + CP, PL.lead_1) # *** steering PID loop *** actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle, @@ -323,11 +314,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, # brake discount removes a sharp nonlinearity brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0)) CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.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 - CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX)) + CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = isEnabled(state) @@ -356,6 +343,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, # if controls is enabled dat.live100.enabled = isEnabled(state) + dat.live100.active = isActive(state) # car state dat.live100.vEgo = CS.vEgo @@ -372,17 +360,17 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, dat.live100.vCruise = float(v_cruise_kph) dat.live100.upAccelCmd = float(LoC.pid.p) dat.live100.uiAccelCmd = float(LoC.pid.i) + dat.live100.ufAccelCmd = float(LoC.pid.f) # lateral control state - dat.live100.yDes = float(LaC.y_des) dat.live100.angleSteersDes = float(LaC.angle_steers_des) dat.live100.upSteer = float(LaC.pid.p) dat.live100.uiSteer = float(LaC.pid.i) + dat.live100.ufSteer = float(LaC.pid.f) # processed radar state, should add a_pcm? dat.live100.vTargetLead = float(plan.vTarget) - dat.live100.aTargetMin = float(plan.aTargetMin) - dat.live100.aTargetMax = float(plan.aTargetMax) + dat.live100.aTarget = float(plan.aTarget) dat.live100.jerkFactor = float(plan.jerkFactor) # log learned angle offset @@ -459,8 +447,10 @@ def controlsd_thread(gctx, rate=100): if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput - PL = Planner(CP) - LoC = LongControl(CI.compute_gb) + fcw_enabled = params.get("IsFcwEnabled") == "1" + + PL = Planner(CP, fcw_enabled) + LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() @@ -481,7 +471,7 @@ def controlsd_thread(gctx, rate=100): rear_view_allowed = params.get("IsRearViewMirror") == "1" # 0.0 - 1.0 - awareness_status = 0. + awareness_status = 1. rk = Ratekeeper(rate, print_delay_threshold=2./1000) @@ -507,7 +497,7 @@ def controlsd_thread(gctx, rate=100): prof.checkpoint("Sample") # define plan - plan, plan_ts = calc_plan(CS, events, PL, LoC) + plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status) prof.checkpoint("Plan") if not passive: diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py deleted file mode 100644 index 8d3ceec674..0000000000 --- a/selfdrive/controls/lib/adaptivecruise.py +++ /dev/null @@ -1,294 +0,0 @@ -import math -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.] - -# need fast accel at very low speed for stop and go -_A_CRUISE_MAX_V = [1., 1., .8, .5, .30] -_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] - -def calc_cruise_accel_limits(v_ego): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) - return np.vstack([a_cruise_min, a_cruise_max]) - -_A_TOTAL_MAX_V = [1.5, 1.9, 3.2] -_A_TOTAL_MAX_BP = [0., 20., 40.] - -def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP): - #*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration - # this should avoid accelerating when losing the target in turns - 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.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) - a_pcm = min(a_pcm, a_x_allowed) - return a_target, a_pcm - -def process_a_lead(a_lead): - # soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead - a_lead_threshold = 0.5 - a_lead = min(a_lead + a_lead_threshold, 0) - return a_lead - -def calc_desired_distance(v_lead): - #*** compute desired distance *** - t_gap = 1.7 # good to be far away - d_offset = 4 # distance when at zero speed - return d_offset + v_lead * t_gap - - -#linear slope -_L_SLOPE_V = [0.40, 0.10] -_L_SLOPE_BP = [0., 40] - -# parabola slope -_P_SLOPE_V = [1.0, 0.25] -_P_SLOPE_BP = [0., 40] - -def calc_desired_speed(d_lead, d_des, v_lead, a_lead): - #*** compute desired speed *** - # the desired speed curve is divided in 4 portions: - # 1-constant - # 2-linear to regain distance - # 3-linear to shorten distance - # 4-parabolic (constant decel) - - max_runaway_speed = -2. # no slower than 2m/s over the lead - - # interpolate the lookups to find the slopes for a give lead speed - l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V) - p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V) - - # this is where parabola and linear curves are tangents - x_linear_to_parabola = p_slope / l_slope**2 - - # parabola offset to have the parabola being tangent to the linear curve - x_parabola_offset = p_slope / (2 * l_slope**2) - - if d_lead < d_des: - # calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des - v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des) - # calculate v_rel_des on one third of the linear slope - v_rel_des_2 = (d_lead - d_des) * l_slope / 3. - # take the min of the 2 above - v_rel_des = min(v_rel_des_1, v_rel_des_2) - v_rel_des = max(v_rel_des, max_runaway_speed) - elif d_lead < d_des + x_linear_to_parabola: - v_rel_des = (d_lead - d_des) * l_slope - v_rel_des = max(v_rel_des, max_runaway_speed) - else: - v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope) - - # compute desired speed - v_target = v_rel_des + v_lead - - # compute v_coast: above this speed we want to coast - t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region - v_coast_shift = max(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0 - v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line - v_coast = min(v_coast, v_target) - - return v_target, v_coast - -def calc_critical_decel(d_lead, v_rel, d_offset, v_offset): - # this function computes the required decel to avoid crashing, given safety offsets - a_critical = - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5) - return a_critical - - -# maximum acceleration adjustment -_A_CORR_BY_SPEED_V = [0.4, 0.4, 0] -# speeds -_A_CORR_BY_SPEED_BP = [0., 2., 10.] - -# 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 aggressive and target a point where car would actually crash - v_offset_coast = 0. - d_offset_coast = d_des/2. - 4. - - # acceleration value to smoothly coast until we hit v_target - if d_lead > d_offset_coast + 0.1: - a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast) - # if lead is decelerating, then offset the coast decel - a_coast += a_lead_contr - a_max = max(a_coast, a_coast_min) - else: - a_max = a_coast_min - else: - # 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 - -# arbitrary limits to avoid too high accel being computed -_A_SAT = [-10., 5.] - -# do not consider a_lead at 0m/s, fully consider it at 10m/s -_A_LEAD_LOW_SPEED_V = [0., 1.] - -# speed break points -_A_LEAD_LOW_SPEED_BP = [0., 10.] - -# add a small offset to the desired decel, just for safety margin -_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3] - -# speed bp: different offset based on the likelyhood that lead decels abruptly -_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.] - - -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 *= -1 # this simplifies things when thinking in d_rel-v_rel diagram - - v_rel_pid = v_pid - v_lead - - # this is how much lead accel we consider in assigning the desired decel - a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP, - _A_LEAD_LOW_SPEED_V) * 0.8 - - # first call of calc_positive_accel_limit is used to shape v_pid - a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid, - v_rel_pid, v_coast, v_target, - a_lead_contr, a_target[1]) - # second call of calc_positive_accel_limit is used to limit the pcm throttle - # control (only useful when we don't control throttle directly) - a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, - v_rel, v_coast, v_target, - a_lead_contr, a_pcm) - - #*** compute max decel *** - v_offset = 1. # assume the car is 1m/s slower - d_offset = 1. # assume the distance is 1m lower - if v_target - v_ego > 0.5: - pass # acc target speed is above vehicle speed, so we can use the cruise limits - elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions - # compute needed accel to get to 1m distance with -1m/s rel speed - decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V) - - critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset) - a_target[0] = min(decel_offset + critical_decel + a_lead_contr, - a_target[0]) - else: - a_target[0] = _A_SAT[0] - # a_min can't be higher than a_max - a_target[0] = min(a_target[0], a_target[1]) - # final check on limits - a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1]) - a_target = a_target.tolist() - return a_target, a_pcm - -def calc_jerk_factor(d_lead, v_rel): - # we don't have an explicit jerk limit, so this function calculates a factor - # that is used by the PID controller to scale the gains. Not the cleanest solution - # but we need this for the demo. - # TODO: Calculate Kp and Ki directly in this function. - - # the higher is the decel required to avoid a crash, the higher is the PI factor scaling - d_offset = 0.5 - v_offset = 2. - a_offset = 1. - jerk_factor_max = 1.0 # can't increase Kp and Ki more than double. - if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions - jerk_factor = jerk_factor_max - else: - a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset) - # increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2 - jerk_factor = max(a_critical - a_offset, 0.)/5. - jerk_factor = min(jerk_factor, jerk_factor_max) - return jerk_factor - - - -MAX_SPEED_POSSIBLE = 55. - -def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP): - # drive limits - # TODO: Make lims function of speed (more aggressive at low speed). - a_lim = [-3., 1.5] - - #*** set target speed pretty high, as lead hasn't been considered yet - v_target_lead = MAX_SPEED_POSSIBLE - - #*** set accel limits as cruise accel/decel limits *** - a_target = calc_cruise_accel_limits(v_ego) - - # 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) - jerk_factor = 0. - - if l1 is not None and l1.status: - #*** process noisy a_lead signal from radar processing *** - a_lead_p = process_a_lead(l1.aLeadK) - - #*** compute desired distance *** - d_des = calc_desired_distance(l1.vLead) - - #*** compute desired speed *** - v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p) - - if l2 is not None and l2.status: - #*** process noisy a_lead signal from radar processing *** - a_lead_p2 = process_a_lead(l2.aLeadK) - - #*** compute desired distance *** - d_des2 = calc_desired_distance(l2.vLead) - - #*** compute desired speed *** - v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2) - - # listen to lead that makes you go slower - if v_target_lead2 < v_target_lead: - l1 = l2 - d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2 - - # l1 is the main lead now - - #*** compute accel limits *** - a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead, - l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm) - - # we can now limit a_target to a_lim - a_target = np.clip(a_target1, a_lim[0], a_lim[1]) - a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist() - - #*** compute max factor *** - jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel) - - # force coasting decel if driver hasn't been controlling car in a while - return v_target_lead, a_target, a_pcm, jerk_factor - - -class AdaptiveCruise(object): - def __init__(self): - self.l1, self.l2 = None, None - def update(self, v_ego, angle_steers, v_pid, CP, l20): - if l20 is not None: - self.l1 = l20.live20.leadOne - self.l2 = l20.live20.leadTwo - - self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \ - compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP) - self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index a585958796..30422b20ee 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -59,9 +59,9 @@ class AlertManager(object): PT.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "", - "", - PT.LOW, None, None, .1, .1, .1), + "Brake", + "Risk of Collision", + PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "Take Control", diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 699893134e..5c1ba67953 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -33,7 +33,7 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override): +def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight # while being in the middle of the lane min_offset = -5. # deg @@ -42,7 +42,7 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_d min_learn_speed = 1. # learn less at low speed or when turning - alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.5*abs(y_des)) + alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.2 * abs(angle_steers)) # only learn if lateral control is active and if driver is not overriding: if lateral_control and not steer_override: diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py deleted file mode 100644 index 15b6f245f4..0000000000 --- a/selfdrive/controls/lib/fcw.py +++ /dev/null @@ -1,66 +0,0 @@ -import numpy as np -from common.realtime import sec_since_boot - -#Time to collisions greater than 5s are iognored -MAX_TTC = 5. - -def calc_ttc(l1): - # if l1 is None, return max ttc immediately - if not l1: - return MAX_TTC - # this function returns the time to collision (ttc), assuming that - # ARel will stay constant TODO: review this assumptions - # change sign to rel quantities as it's going to be easier for calculations - vRel = -l1.vRel - aRel = -l1.aRel - - # assuming that closing gap ARel comes from lead vehicle decel, - # then limit ARel so that v_lead will get to zero in no sooner than t_decel. - # This helps underweighting ARel when v_lead is close to zero. - t_decel = 2. - aRel = np.minimum(aRel, l1.vLead/t_decel) - - # delta of the quadratic equation to solve for ttc - delta = vRel**2 + 2 * l1.dRel * aRel - - # assign an arbitrary high ttc value if there is no solution to ttc - if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1): - ttc = MAX_TTC - else: - ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC) - return ttc - -class ForwardCollisionWarning(object): - def __init__(self, dt): - self.last_active = 0. - self.violation_time = 0. - self.active = False - self.dt = dt # time step - - def process(self, CS, AC): - # send an fcw alert if the violation time > violation_thrs - violation_thrs = 0.3 # fcw turns on after a continuous violation for this time - fcw_t_delta = 5. # no more than one fcw alert within this time - a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw - a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw - ttc_thrs = 2.5 # ttc threshold for fcw - v_fcw_min = 5. # no fcw below 5m/s - steer_angle_th = 40. # deg, no fcw above this steer angle - cur_time = sec_since_boot() - - ttc = calc_ttc(AC.l1) - a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off - - # increase violation time if we want to decelerate quite fast - if AC.l1 and ( \ - (CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \ - and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \ - and AC.l1.fcw): - self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs) - else: - self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0) - - # fire FCW - self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta) - if self.active: - self.last_active = cur_time diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 5b86dc9063..c8439a79a0 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -2,11 +2,14 @@ import math from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.lateral_mpc import libmpc_py from common.numpy_fast import clip, interp - +from common.realtime import sec_since_boot # 100ms is a rule of thumb estimation of lag from image processing to actuator command ACTUATORS_DELAY = 0.1 +_DT = 0.01 # 100Hz +_DT_MPC = 0.05 # 20Hz + def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio): states[0].x = v_ego * ACTUATORS_DELAY @@ -23,8 +26,6 @@ class LatControl(object): self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) self.setup_mpc() - self.y_des = -1 # Legacy - def setup_mpc(self): self.libmpc = libmpc_py.libmpc self.libmpc.init() @@ -38,15 +39,20 @@ class LatControl(object): self.cur_state[0].delta = 0.0 self.last_mpc_ts = 0.0 - self.angle_steers_des = 0 + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_prev = 0.0 + self.angle_steers_des_time = 0.0 def reset(self): self.pid.reset() def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL): + cur_time = sec_since_boot() self.mpc_updated = False - if self.last_mpc_ts + 0.001 < PL.last_md_ts: + if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts + self.angle_steers_des_prev = self.angle_steers_des_mpc curvature_factor = VM.curvature_factor(v_ego) @@ -65,16 +71,19 @@ class LatControl(object): delta_desired = self.mpc_solution[0].delta[1] self.cur_state[0].delta = delta_desired - self.angle_steers_des = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset) + self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset) + self.angle_steers_des_time = cur_time self.mpc_updated = True if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() else: - steer_max = get_steer_max(VM.CP, v_ego) - self.pid.pos_limit = steer_max - self.pid.neg_limit = -steer_max + dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) + steers_max = get_steer_max(VM.CP, v_ego) + self.pid.pos_limit = steers_max + self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel) output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 3924069c2e..f54bf25c06 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,7 +4,8 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.pid import PIController STOPPING_EGO_SPEED = 0.5 -STOPPING_TARGET_SPEED = 0.3 +MIN_CAN_SPEED = 0.3 +STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 STARTING_TARGET_SPEED = 0.5 BRAKE_THRESHOLD_TO_PID = 0.2 @@ -18,12 +19,16 @@ class LongCtrlState: starting = 'starting' # starting and releasing brakes in open loop before giving back to PID + def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, - output_gb, brake_pressed): + output_gb, brake_pressed, cruise_standstill): + + stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ + (v_ego < STOPPING_EGO_SPEED and \ + ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or + brake_pressed)) - stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \ - (((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or - (brake_pressed)) + starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill if not active: long_control_state = LongCtrlState.off @@ -38,7 +43,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, long_control_state = LongCtrlState.stopping elif long_control_state == LongCtrlState.stopping: - if (v_target > STARTING_TARGET_SPEED): + if starting_condition: long_control_state = LongCtrlState.starting elif long_control_state == LongCtrlState.starting: @@ -50,15 +55,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, return long_control_state -_KP_BP = [0., 5., 35.] -_KP_V = [1.2, 0.8, 0.5] - -_KI_BP = [0., 35.] -_KI_V = [0.18, 0.12] - stopping_brake_rate = 0.2 # brake_travel/s while trying to stop starting_brake_rate = 0.8 # brake_travel/s while releasing on restart -starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary _MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints @@ -66,10 +64,10 @@ _MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this class LongControl(object): - def __init__(self, compute_gb): + def __init__(self, CP, compute_gb): self.long_control_state = LongCtrlState.off # initialized to off - self.pid = PIController((_KP_BP, _KP_V), - (_KI_BP, _KI_V), + self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), + (CP.longitudinalKiBP, CP.longitudinalKiV), rate=100.0, sat_limit=0.8, convert=compute_gb) @@ -80,27 +78,18 @@ class LongControl(object): self.pid.reset() self.v_pid = v_pid - def update(self, active, v_ego, brake_pressed, standstill, v_cruise, v_target_lead, a_target, - jerk_factor, CP): - + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1): # actuation limits gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) - overshoot_allowance = 2.0 # overshoot allowed when changing accel sign - output_gb = self.last_output_gb - - # limit max target speed based on cruise setting - v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS) rate = 100.0 - max_speed_delta_up = a_target[1] * 1.0 / rate - max_speed_delta_down = a_target[0] * 1.0 / rate - - self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\ - v_target, self.v_pid, output_gb, brake_pressed) + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego, + v_target_future, self.v_pid, output_gb, + brake_pressed, cruise_standstill) - v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 # *** long control behavior based on state if self.long_control_state == LongCtrlState.off: @@ -110,29 +99,15 @@ class LongControl(object): # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: - #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego - if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)): - self.v_pid = max(v_target, v_ego + overshoot_allowance) - elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)): - self.v_pid = min(v_target, v_ego - overshoot_allowance) - - # move v_pid no faster than allowed accel limits - if (v_target > self.v_pid + max_speed_delta_up): - self.v_pid += max_speed_delta_up - elif (v_target < self.v_pid + max_speed_delta_down): - self.v_pid += max_speed_delta_down - else: - self.v_pid = v_target - - # to avoid too much wind up on acceleration, limit positive speed error - if CP.enableGas: - max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V) - self.v_pid = min(self.v_pid, v_ego + max_speed_error) + prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 + self.v_pid = max(v_target, MIN_CAN_SPEED) self.pid.pos_limit = gas_max self.pid.neg_limit = - brake_max deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor, deadzone=deadzone) + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) + if prevent_overshoot: + output_gb = min(output_gb, 0.0) # intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: @@ -150,7 +125,6 @@ class LongControl(object): output_gb += starting_brake_rate / rate self.v_pid = v_ego self.pid.reset() - self.pid.i = starting_Ui self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile new file mode 100644 index 0000000000..409715cf17 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/Makefile @@ -0,0 +1,94 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../../phonelibs + +UNAME_M := $(shell uname -m) + +CFLAGS = -O3 -fPIC -I. +CXXFLAGS = -O3 -fPIC -I. + +QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC + +ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado + +ifeq ($(UNAME_M),aarch64) +ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +else +ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +endif + +OBJS = \ + qp/Bounds.o \ + qp/Constraints.o \ + qp/CyclingManager.o \ + qp/Indexlist.o \ + qp/MessageHandling.o \ + qp/QProblem.o \ + qp/QProblemB.o \ + qp/SubjectTo.o \ + qp/Utils.o \ + qp/EXTRAS/SolutionAnalysis.o \ + mpc_export/acado_qpoases_interface.o \ + mpc_export/acado_integrator.o \ + mpc_export/acado_solver.o \ + mpc_export/acado_auxiliary_functions.o \ + mpc.o + +DEPS := $(OBJS:.o=.d) + +.PHONY: all +all: libcommampc1.so libcommampc2.so + +libcommampc1.so: $(OBJS) + $(CXX) -shared -o '$@' $^ -lm + +libcommampc2.so: libcommampc1.so + cp libcommampc1.so libcommampc2.so + +qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp + @echo "[ CXX ] $@" + mkdir -p qp + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp + @echo "[ CXX ] $@" + mkdir -p qp/EXTRAS + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +%.o: %.cpp + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +%.o: %.c + @echo "[ CC ] $@" + $(CC) $(CFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +generator: generator.cpp + $(CXX) -Wall -std=c++11 \ + generator.cpp \ + -o generator \ + $(ACADO_FLAGS) \ + $(ACADO_LIBS) + +.PHONY: generate +generate: generator + ./generator + +.PHONY: clean +clean: + rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS) + +-include $(DEPS) diff --git a/selfdrive/controls/lib/longitudinal_mpc/__init__.py b/selfdrive/controls/lib/longitudinal_mpc/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp new file mode 100644 index 0000000000..6bf14e6c60 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -0,0 +1,98 @@ +#include + +const int controlHorizon = 50; +const double samplingTime = 0.2; + +using namespace std; + +#define G 9.81 +#define TR 1.8 + +#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) +#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) + +int main( ) +{ + USING_NAMESPACE_ACADO + + + DifferentialEquation f; + + DifferentialState x_ego, v_ego, a_ego; + DifferentialState x_l, v_l, a_l; + + OnlineData lambda; + + Control j_ego; + + auto desired = 4.0 + RW(v_ego, v_l); + auto d_l = x_l - x_ego; + + // Equations of motion + f << dot(x_ego) == v_ego; + f << dot(v_ego) == a_ego; + f << dot(a_ego) == j_ego; + + f << dot(x_l) == v_l; + f << dot(v_l) == a_l; + f << dot(a_l) == -lambda * a_l; + + // Running cost + Function h; + h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + h << (d_l - desired) / (0.1 * v_ego + 0.5); + h << a_ego * (1.0 + v_ego / 10.0); + h << j_ego * (1.0 + v_ego / 10.0); + + DMatrix Q(4,4); + Q(0,0) = 5.0; + Q(1,1) = 0.1; + Q(2,2) = 10.0; + Q(3,3) = 20.0; + + // Terminal cost + Function hN; + hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + hN << (d_l - desired) / (0.1 * v_ego + 0.5); + hN << a_ego * (1.0 + v_ego / 10.0); + + DMatrix QN(3,3); + QN(0,0) = 5.0; + QN(1,1) = 0.1; + QN(2,2) = 10.0; + + // Setup Optimal Control Problem + const double tStart = 0.0; + const double tEnd = samplingTime * controlHorizon; + + OCP ocp( tStart, tEnd, controlHorizon ); + ocp.subjectTo(f); + + ocp.minimizeLSQ(Q, h); + ocp.minimizeLSQEndTerm(QN, hN); + + ocp.subjectTo( 0.0 <= v_ego); + ocp.setNOD(1); + + OCPexport mpc(ocp); + mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); + mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); + mpc.set( INTEGRATOR_TYPE, INT_RK4 ); + mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); + mpc.set( MAX_NUM_QP_ITERATIONS, 500); + + mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); + mpc.set( QP_SOLVER, QP_QPOASES ); + mpc.set( HOTSTART_QP, YES ); + mpc.set( GENERATE_TEST_FILE, NO); + mpc.set( GENERATE_MAKE_FILE, NO ); + mpc.set( GENERATE_MATLAB_INTERFACE, NO ); + mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); + + if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN) + exit( EXIT_FAILURE ); + + mpc.printDimensionsQP( ); + + return EXIT_SUCCESS; +} diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py new file mode 100644 index 0000000000..600224b33e --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -0,0 +1,43 @@ +import os +import subprocess + +from cffi import FFI + +mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) +subprocess.check_output(["make", "-j4"], cwd=mpc_dir) + + +def _get_libmpc(mpc_id): + libmpc_fn = os.path.join(mpc_dir, "libcommampc%d.so" % mpc_id) + + ffi = FFI() + ffi.cdef(""" + typedef struct { + double x_ego, v_ego, a_ego, x_l, v_l, a_l; + } state_t; + + + typedef struct { + double x_ego[50]; + double v_ego[50]; + double a_ego[50]; + double j_ego[50]; + double x_l[50]; + double v_l[50]; + double a_l[50]; + } log_t; + + void init(); + void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + int run_mpc(state_t * x0, log_t * solution, + double l); + """) + + return (ffi, ffi.dlopen(libmpc_fn)) + + +mpcs = [_get_libmpc(1), _get_libmpc(2)] + + +def get_libmpc(mpc_id): + return mpcs[mpc_id - 1] diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/mpc.c new file mode 100644 index 0000000000..c153ae725d --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc.c @@ -0,0 +1,118 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include + +#define NX ACADO_NX /* Number of differential state variables. */ +#define NXA ACADO_NXA /* Number of algebraic variables. */ +#define NU ACADO_NU /* Number of control inputs. */ +#define NOD ACADO_NOD /* Number of online data values. */ + +#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ +#define NYN ACADO_NYN /* Number of measurements/references on node N. */ + +#define N ACADO_N /* Number of intervals in the horizon. */ + +ACADOvariables acadoVariables; +ACADOworkspace acadoWorkspace; + +typedef struct { + double x_ego, v_ego, a_ego, x_l, v_l, a_l; +} state_t; + + +typedef struct { + double x_ego[N]; + double v_ego[N]; + double a_ego[N]; + double j_ego[N]; + double x_l[N]; + double v_l[N]; + double a_l[N]; +} log_t; + +void init(){ + acado_initializeSolver(); + int i; + + /* Initialize the states and controls. */ + for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; +} + +void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){ + int i; + double x_ego = 0.0; + double a_ego = 0.0; + + if (v_ego > v_l){ + a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l; + } + double dt = 0.2; + + for (i = 0; i < N + 1; ++i){ + acadoVariables.x[i*NX] = x_ego; + acadoVariables.x[i*NX+1] = v_ego; + acadoVariables.x[i*NX+2] = a_ego; + + acadoVariables.x[i*NX+3] = x_l; + acadoVariables.x[i*NX+4] = v_l; + acadoVariables.x[i*NX+5] = a_l; + + x_ego += v_ego * dt; + v_ego += a_ego * dt; + + x_l += v_l * dt; + v_l += a_l * dt; + a_l += -l * a_l * dt; + + if (v_ego <= 0.0) { + v_ego = 0.0; + a_ego = 0.0; + } + } + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; +} + +int run_mpc(state_t * x0, log_t * solution, double l){ + int i; + + for (i = 0; i <= NOD * N; i+= NOD){ + acadoVariables.od[i] = l; + } + + acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; + acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; + acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; + acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l; + acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l; + acadoVariables.x[5] = acadoVariables.x0[5] = x0->a_l; + + acado_preparationStep(); + acado_feedbackStep(); + + for (i = 0; i <= N; i++){ + solution->x_ego[i] = acadoVariables.x[i*NX]; + solution->v_ego[i] = acadoVariables.x[i*NX+1]; + solution->a_ego[i] = acadoVariables.x[i*NX+2]; + solution->x_l[i] = acadoVariables.x[i*NX+3]; + solution->v_l[i] = acadoVariables.x[i*NX+4]; + solution->a_l[i] = acadoVariables.x[i*NX+5]; + + solution->j_ego[i] = acadoVariables.u[i]; + } + + // Dont shift states here. Current solution is closer to next timestep than if + // we shift by 0.2 seconds. + + return acado_getNWSR(); +} diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c new file mode 100644 index 0000000000..174ed75004 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357 +size 4906 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h new file mode 100644 index 0000000000..ac98266ff4 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270 +size 3428 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h new file mode 100644 index 0000000000..ceb7513b28 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6db0b40b4f066266c4382512de1752cd8fd2260bcd58355f1291e5db272b7ec1 +size 8655 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c new file mode 100644 index 0000000000..123b863624 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fbe29a0acd4c2ec8fea880b159440ac68b8d0b6ab7437c95c6f84443db13abef +size 33237 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp new file mode 100644 index 0000000000..ba72d3c925 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac20ce29802179e0d9b91f2a43673e6a0a41c2d1abcecadd54daca7a08befda8 +size 1948 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp new file mode 100644 index 0000000000..c9591d2e9e --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e1ccfb2ae276bc3bc787e2bfc68861de08017e8ff97411fe5ceab65ae9997f18 +size 1821 diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c new file mode 100644 index 0000000000..2145dbe0db --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5d9df24f3df44e3fbd8d4d9695c8f3f452e391179f01ba8c52d28473e1bbd6a3 +size 303043 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 4b71525510..40ae836b9e 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -12,7 +12,7 @@ def apply_deadzone(error, deadzone): return error class PIController(object): - def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self._k_p = k_p # proportional gain self._k_i = k_i # integrale gain self.k_f = k_f # feedforward gain @@ -24,7 +24,6 @@ class PIController(object): self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.sat_limit = sat_limit - self.jerk_factor = 0.0 self.convert = convert self.reset() @@ -36,7 +35,7 @@ class PIController(object): else: kp = interp(self.speed, self._k_p[0], self._k_p[1]) - return (1.0 + self.jerk_factor) * kp + return kp @property def k_i(self): @@ -45,7 +44,7 @@ class PIController(object): else: ki = interp(self.speed, self._k_i[0], self._k_i[1]) - return (1.0 + self.jerk_factor) * ki + return ki def _check_saturation(self, control, override, error): saturated = (control < self.neg_limit) or (control > self.pos_limit) @@ -62,34 +61,35 @@ class PIController(object): def reset(self): self.p = 0.0 self.i = 0.0 + self.f = 0.0 self.sat_count = 0.0 self.saturated = False self.control = 0 - def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0., deadzone=0.): + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): self.speed = speed - self.jerk_factor = jerk_factor error = float(apply_deadzone(setpoint - measurement, deadzone)) self.p = error * self.k_p - f = feedforward * self.k_f + self.f = feedforward * self.k_f if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: i = self.i + error * self.k_i * self.i_rate - control = self.p + f + i + control = self.p + self.f + i if self.convert is not None: control = self.convert(control, speed=self.speed) # Update when changing i will move the control away from the limits # or when i will move towards the sign of the error - if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ - (error <= 0 and (control >= self.neg_limit or i > 0.0)): + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: self.i = i - control = self.p + f + self.i + control = self.p + self.f + self.i if self.convert is not None: control = self.convert(control, speed=self.speed) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 9cc657c138..67052ed400 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,24 +1,230 @@ #!/usr/bin/env python import zmq + +import numpy as np +import math + from common.realtime import sec_since_boot +from common.params import Params +from common.numpy_fast import interp import selfdrive.messaging as messaging +from selfdrive.swaglog import cloudlog +from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.pathplanner import PathPlanner -from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise -from selfdrive.controls.lib.fcw import ForwardCollisionWarning +from selfdrive.controls.lib.longitudinal_mpc import libmpc_py +from selfdrive.controls.lib.speed_smoother import speed_smoother +from selfdrive.controls.lib.longcontrol import LongCtrlState + +_DT = 0.01 # 100Hz +_DT_MPC = 0.2 # 5Hz +MAX_SPEED_ERROR = 2.0 +AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted +_DEBUG = False +_LEAD_ACCEL_TAU = 1.5 + +# lookup tables VS speed to determine min and max accels in cruise +# make sure these accelerations are smaller than mpc limits +_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] + +# need fast accel at very low speed for stop and go +# make sure these accelerations are smaller than mpc limits +_A_CRUISE_MAX_V = [1., 1., .8, .5, .3] +_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .3] +_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] + +# Lookup table for turns +_A_TOTAL_MAX_V = [1.5, 1.9, 3.2] +_A_TOTAL_MAX_BP = [0., 20., 40.] + +# max acceleration allowed in acc, which happens in restart +A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) + + +def calc_cruise_accel_limits(v_ego, following): + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + + if following: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + return np.vstack([a_cruise_min, a_cruise_max]) + + +def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): + """ + This function returns a limited long acceleration allowed, depending on the existing lateral acceleration + this should avoid accelerating when losing the target in turns + """ + 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.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) + return a_target + + +class FCWChecker(object): + def __init__(self): + self.fcw_count = 0 + self.last_fcw_a = 0.0 + self.v_lead_max = 0.0 + self.lead_seen_t = 0.0 + self.last_fcw_time = 0.0 + + def reset_lead(self, cur_time): + self.v_lead_max = 0.0 + self.lead_seen_t = cur_time + + def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers): + min_a_mpc = min(list(mpc_solution[0].a_ego)[1:]) + self.v_lead_max = max(self.v_lead_max, v_lead) + + if (fcw_lead > 0.99 + and v_ego > 5.0 + and min_a_mpc < -4.0 + and self.v_lead_max > 2.5 + and v_ego > v_lead + and self.lead_seen_t < cur_time - 2.0 + and abs(y_lead) < 1.0 + and abs(vlat_lead) < 0.3 + and not blinkers): + self.fcw_count += 1 + if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time: + self.last_fcw_time = cur_time + self.last_fcw_a = min_a_mpc + return True + else: + self.fcw_count = 0 + + return False + + +class LongitudinalMpc(object): + def __init__(self, mpc_id, live_longitudinal_mpc): + self.live_longitudinal_mpc = live_longitudinal_mpc + self.mpc_id = mpc_id + + self.setup_mpc() + self.v_mpc = 0.0 + self.v_mpc_future = 0.0 + self.a_mpc = 0.0 + self.v_cruise = 0.0 + self.prev_lead_status = False + self.prev_lead_x = 0.0 + self.new_lead = False + + self.last_cloudlog_t = 0.0 + + def send_mpc_solution(self, qp_iterations, calculation_time): + qp_iterations = max(0, qp_iterations) + dat = messaging.new_message() + dat.init('liveLongitudinalMpc') + dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) + dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) + dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) + dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) + dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) + dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l) + dat.liveLongitudinalMpc.aLeadTau = self.l + dat.liveLongitudinalMpc.qpIterations = qp_iterations + dat.liveLongitudinalMpc.mpcId = self.mpc_id + dat.liveLongitudinalMpc.calculationTime = calculation_time + self.live_longitudinal_mpc.send(dat.to_bytes()) + + def setup_mpc(self): + ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) + self.libmpc.init() + + self.mpc_solution = ffi.new("log_t *") + self.cur_state = ffi.new("state_t *") + self.cur_state[0].v_ego = 0 + self.cur_state[0].a_ego = 0 + self.l = _LEAD_ACCEL_TAU + + def set_cur_state(self, v, a): + self.cur_state[0].v_ego = v + self.cur_state[0].a_ego = a + + def update(self, CS, lead, v_cruise_setpoint): + # Setup current mpc state + self.cur_state[0].x_ego = 0.0 + + if lead is not None and lead.status: + x_lead = lead.dRel + v_lead = max(0.0, lead.vLead) + a_lead = lead.aLeadK + + if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): + v_lead = 0.0 + a_lead = 0.0 + + # Learn if constant acceleration + if abs(a_lead) < 0.5: + self.l = _LEAD_ACCEL_TAU + else: + self.l *= 0.9 + + l = max(self.l, -a_lead / (v_lead + 0.01)) + self.new_lead = False + if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: + self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, l) + self.new_lead = True + + self.prev_lead_status = True + self.prev_lead_x = x_lead + self.cur_state[0].x_l = x_lead + self.cur_state[0].v_l = v_lead + self.cur_state[0].a_l = a_lead + else: + self.prev_lead_status = False + # Fake a fast lead car, so mpc keeps running + self.cur_state[0].x_l = 50.0 + self.cur_state[0].v_l = CS.vEgo + 10.0 + self.cur_state[0].a_l = 0.0 + l = _LEAD_ACCEL_TAU + + # Calculate mpc + t = sec_since_boot() + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l) + duration = int((sec_since_boot() - t) * 1e9) + self.send_mpc_solution(n_its, duration) -_DT = 0.01 # 100Hz + # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed + self.v_mpc = self.mpc_solution[0].v_ego[1] + self.a_mpc = self.mpc_solution[0].a_ego[1] + self.v_mpc_future = self.mpc_solution[0].v_ego[10] + + # Reset if NaN or goes through lead car + dls = np.array(list(self.mpc_solution[0].x_l)[1:]) - np.array(list(self.mpc_solution[0].x_ego)[1:]) + crashing = min(dls) < -50.0 + nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego))) + backwards = min(list(self.mpc_solution[0].v_ego)[1:]) < -0.01 + + if ((backwards or crashing) and self.prev_lead_status) or nans: + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % ( + self.mpc_id, backwards, crashing, nans)) + + self.libmpc.init() + self.cur_state[0].v_ego = CS.vEgo + self.cur_state[0].a_ego = 0.0 + self.prev_lead_status = False class Planner(object): - def __init__(self, CP): + def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.model = messaging.sub_sock(context, service_list['model'].port) - self.plan = messaging.pub_sock(context, service_list['plan'].port) + self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port) self.last_md_ts = 0 self.last_l20_ts = 0 @@ -29,36 +235,141 @@ class Planner(object): self.radar_errors = [] self.PP = PathPlanner() - self.AC = AdaptiveCruise() - self.FCW = ForwardCollisionWarning(_DT) + self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) + self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) + + self.v_acc_start = 0.0 + self.a_acc_start = 0.0 + self.acc_start_time = sec_since_boot() + self.v_acc = 0.0 + self.v_acc_sol = 0.0 + self.v_acc_future = 0.0 + self.a_acc = 0.0 + self.a_acc_sol = 0.0 + self.v_cruise = 0.0 + self.a_cruise = 0.0 + + self.lead_1 = None + self.lead_2 = None + + self.longitudinalPlanSource = 'cruise' + self.fcw = False + self.fcw_checker = FCWChecker() + self.fcw_enabled = fcw_enabled + + def choose_solution(self, v_cruise_setpoint): + solutions = {'cruise': self.v_cruise} + if self.mpc1.prev_lead_status: + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status: + solutions['mpc2'] = self.mpc2.v_mpc + + slowest = min(solutions, key=solutions.get) + + if _DEBUG: + print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol + print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise + print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise + + self.longitudinalPlanSource = slowest + + # Choose lowest of MPC and cruise + if slowest == 'mpc1': + self.v_acc = self.mpc1.v_mpc + self.a_acc = self.mpc1.a_mpc + elif slowest == 'mpc2': + self.v_acc = self.mpc2.v_mpc + self.a_acc = self.mpc2.a_mpc + elif slowest == 'cruise': + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise + + self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) # this runs whenever we get a packet that can change the plan - def update(self, CS, LoC): + def update(self, CS, LoC, v_cruise_kph, user_distracted): cur_time = sec_since_boot() + v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = messaging.recv_sock(self.model) if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False - if cur_time - self.last_model > 0.5: - self.model_dead = True - l20 = messaging.recv_sock(self.live20) + self.PP.update(CS.vEgo, md) + + l20 = messaging.recv_sock(self.live20) if md is None else None if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) - if cur_time - self.last_l20 > 0.5: - self.radar_dead = True - self.PP.update(CS.vEgo, md) + self.v_acc_start = self.v_acc_sol + self.a_acc_start = self.a_acc_sol + self.acc_start_time = cur_time - # LoC.v_pid -> CS.vEgo - # TODO: is this change okay? - self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) + self.lead_1 = l20.live20.leadOne + self.lead_2 = l20.live20.leadTwo + enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) + following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 + + # Calculate speed for normal cruise control + if enabled: + + accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) + # TODO: make a separate lookup for jerk tuning + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] + accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) + if user_distracted: + # if user is not responsive to awareness alerts, then start a smooth deceleration + accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) + accel_limits[0] = min(accel_limits[0], accel_limits[1]) + + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, + v_cruise_setpoint, + accel_limits[1], accel_limits[0], + jerk_limits[1], + jerk_limits[0], + _DT_MPC) + else: + starting = LoC.long_control_state == LongCtrlState.starting + self.v_cruise = CS.vEgo + self.a_cruise = self.CP.startAccel if starting else CS.aEgo + self.v_acc_start = CS.vEgo + self.a_acc_start = self.CP.startAccel if starting else CS.aEgo + self.v_acc = CS.vEgo + self.a_acc = self.CP.startAccel if starting else CS.aEgo + self.v_acc_sol = CS.vEgo + self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo + + self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) + self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) + + self.mpc1.update(CS, self.lead_1, v_cruise_setpoint) + self.mpc2.update(CS, self.lead_2, v_cruise_setpoint) + + self.choose_solution(v_cruise_setpoint) + + # determine fcw + if self.mpc1.new_lead: + self.fcw_checker.reset_lead(cur_time) + + blinkers = CS.leftBlinker or CS.rightBlinker + self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, + self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat, + self.lead_1.fcw, blinkers) \ + and not CS.brakePressed + if self.fcw: + cloudlog.info("FCW triggered") + + if cur_time - self.last_model > 0.5: + self.model_dead = True + + if cur_time - self.last_l20 > 0.5: + self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') @@ -71,8 +382,12 @@ class Planner(object): if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - plan_send.plan.events = events + # Interpolation of trajectory + dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) + self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 + plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts @@ -83,15 +398,17 @@ class Planner(object): # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead - plan_send.plan.vTarget = float(self.AC.v_target_lead) - plan_send.plan.aTargetMin = float(self.AC.a_target[0]) - plan_send.plan.aTargetMax = float(self.AC.a_target[1]) - plan_send.plan.jerkFactor = float(self.AC.jerk_factor) - plan_send.plan.hasLead = self.AC.has_lead - - # compute risk of collision events: fcw - self.FCW.process(CS, self.AC) - plan_send.plan.fcw = bool(self.FCW.active) + plan_send.plan.vCruise = self.v_cruise + plan_send.plan.aCruise = self.a_cruise + plan_send.plan.vTarget = self.v_acc_sol + plan_send.plan.aTarget = self.a_acc_sol + plan_send.plan.vTargetFuture = self.v_acc_future + plan_send.plan.hasLead = self.mpc1.prev_lead_status + plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource + + # Send out fcw + fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) + plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py new file mode 100644 index 0000000000..50d3687376 --- /dev/null +++ b/selfdrive/controls/lib/speed_smoother.py @@ -0,0 +1,88 @@ +import numpy as np + + +def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin): + + tDelta = 0. + if aEgo > aMax: + tDelta = (aMax - aEgo) / jMin + elif aEgo < aMin: + tDelta = (aMin - aEgo) / jMax + + return tDelta + + +def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): + + dV = vT - vEgo + + tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin) + + if (ts <= tDelta): + if (aEgo < aMin): + vEgo += ts * aEgo + 0.5 * ts**2 * jMax + aEgo += ts * jMax + return vEgo, aEgo + elif (aEgo > aMax): + vEgo += ts * aEgo + 0.5 * ts**2 * jMin + aEgo += ts * jMin + return vEgo, aEgo + + if aEgo > aMax: + dV -= 0.5 * (aMax**2 - aEgo**2) / jMin + vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin + aEgo += tDelta * jMin + elif aEgo < aMin: + dV -= 0.5 * (aMin**2 - aEgo**2) / jMax + vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax + aEgo += tDelta * jMax + + ts -= tDelta + + jLim = jMin if aEgo >= 0 else jMax + # if we reduce the accel to zero immediately, how much delta speed we generate? + dv_min_shift = - 0.5 * aEgo**2 / jLim + + # flip signs so we can consider only one case + flipped = False + if dV < dv_min_shift: + flipped = True + dV *= -1 + vEgo *= -1 + aEgo *= -1 + aMax = -aMin + jMaxcopy = -jMin + jMin = -jMax + jMax = jMaxcopy + + # small addition needed to avoid numerical issues with sqrt of ~zero + aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin)) + + if aPeak > aMax: + aPeak = aMax + t1 = (aPeak - aEgo) / jMax + vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin + if vChange < aPeak * ts: + t2 = t1 + vChange / aPeak + else: + t2 = t1 + ts + else: + t1 = (aPeak - aEgo) / jMax + t2 = t1 + t3 = t2 - aPeak / jMin + + dt1 = min(ts, t1) + dt2 = max(min(ts, t2) - t1, 0.) + dt3 = max(min(ts, t3) - t2, 0.) + + if ts > t3: + vEgo += dV + aEgo = 0. + else: + vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin + aEgo += jMax * dt1 + dt3 * jMin + + vEgo *= -1 if flipped else 1 + aEgo *= -1 if flipped else 1 + + return float(vEgo), float(aEgo) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ddfb7766de..1e2eed8cd0 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -79,8 +79,9 @@ def radard_thread(gctx=None): tsv = 1./rate v_len = 20 # how many speed data points to remember for t alignment with rdr data - enabled = 0 + active = 0 steer_angle = 0. + steer_override = False tracks = defaultdict(dict) @@ -104,9 +105,10 @@ def radard_thread(gctx=None): # receive the live100s l100 = messaging.recv_sock(live100) if l100 is not None: - enabled = l100.live100.enabled + active = l100.live100.active v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers + steer_override = l100.live100.steerOverride v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1) v_ego_array = v_ego_array[:, 1:] @@ -137,7 +139,7 @@ def radard_thread(gctx=None): del ar_pts[VISION_POINT] # *** compute the likely path_y *** - if enabled: # use path from model path_poly + if active and not steer_override: # 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, VM, angle_offset=0)[0] diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index e1464b0cf5..e4dc7a4125 100755 --- a/selfdrive/loggerd/loggerd +++ b/selfdrive/loggerd/loggerd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:a1adb2870715bfac1640fc3afa1b7a274a24ff3aa98b3a8bbbab622039a7868a +oid sha256:b7988b5c9f39ef6650f25e1a8181d4eb2cfa1ded0a89d871440f121f0234f02e size 1412368 diff --git a/selfdrive/manager.py b/selfdrive/manager.py index e1b785ed52..ef83e88e56 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -581,6 +581,8 @@ def main(): params.put("IsMetric", "0") if params.get("IsRearViewMirror") is None: params.put("IsRearViewMirror", "1") + if params.get("IsFcwEnabled") is None: + params.put("IsFcwEnabled", "1") params.put("Passive", "1" if os.getenv("PASSIVE") else "0") diff --git a/selfdrive/radar/toyota/interface.py b/selfdrive/radar/toyota/interface.py index a16a972f97..6b0554673e 100755 --- a/selfdrive/radar/toyota/interface.py +++ b/selfdrive/radar/toyota/interface.py @@ -8,16 +8,18 @@ import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging + +RADAR_MSGS = range(0x210, 0x220) + def _create_radard_can_parser(): dbc_f = 'toyota_prius_2017_adas.dbc' - radar_messages = range(0x210, 0x220) - msg_n = len(radar_messages) - msg_last = radar_messages[-1] + msg_n = len(RADAR_MSGS) + msg_last = RADAR_MSGS[-1] signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n + ['REL_SPEED'] * msg_n + ['VALID'] * msg_n, - radar_messages * 5, + RADAR_MSGS * 5, [255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n) - checks = zip(radar_messages, [20]*msg_n) + checks = zip(RADAR_MSGS, [20]*msg_n) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) @@ -25,6 +27,7 @@ class RadarInterface(object): def __init__(self): # radar self.pts = {} + self.ptsValid = {key: False for key in RADAR_MSGS} self.track_id = 0 self.delay = 0.0 # Delay of radar @@ -55,7 +58,17 @@ class RadarInterface(object): #print "NEW TRACKS" for ii in updated_messages: cpt = self.rcp.vl[ii] - if cpt['LONG_DIST'] < 255 and cpt['VALID']: + + # a point needs one valid measurement before being considered + #if cpt['NEW_TRACK'] or cpt['LONG_DIST'] >= 255: + # self.ptsValid[ii] = False # reset validity + # TODO: find better way to eliminate both false positive and false negative + if cpt['VALID'] and cpt['LONG_DIST'] < 255: + self.ptsValid[ii] = True + else: + self.ptsValid[ii] = False + + if self.ptsValid[ii]: #print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1)) if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index db4e0b6b0c..29753684dc 100755 --- a/selfdrive/sensord/gpsd +++ b/selfdrive/sensord/gpsd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c1db908ac63c3036eb0e4758c2b3af19524df2c34f011de055cdbef16b655c0d -size 981608 +oid sha256:15804c73a05556fcbb976a266e66d4c22ec6f7dd4a98aeff89f15437252bdb3c +size 981400 diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index bff6774b14..b89644ed6f 100755 --- a/selfdrive/sensord/sensord +++ b/selfdrive/sensord/sensord @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:056e3f9de2d89dfee1a7092615c1958959f7d01a0e3c660798b19b084dce8819 +oid sha256:51c5ed132e3984edfc2fbfd856169ea3d999cada499ef90ceba0122a35f857bb size 972296 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index b2d3cc0768..e87f8ebac4 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -45,6 +45,7 @@ gpsLocationExternal: [8032, true] ubloxGnss: [8033, true] clocks: [8034, true] liveMpc: [8035, true] +liveLongitudinalMpc: [8036, true] testModel: [8040, false] diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 8117b8b193..5a408c9913 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -64,7 +64,7 @@ class Maneuver(object): v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid, cruise_speed=last_live100.vCruise, jerk_factor=last_live100.jerkFactor, - a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax) + a_target=last_live100.aTarget) print "maneuver end" diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/plant/maneuverplots.py index 4e163103b7..365a137238 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/plant/maneuverplots.py @@ -29,8 +29,7 @@ class ManeuverPlot(object): self.cruise_speed_array = [] self.jerk_factor_array = [] - self.a_target_min_array = [] - self.a_target_max_array = [] + self.a_target_array = [] self.v_target_array = [] @@ -38,8 +37,7 @@ class ManeuverPlot(object): def add_data(self, time, gas, brake, steer_torque, distance, speed, acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, - v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target_min, - a_target_max): + v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target): self.time_array.append(time) self.gas_array.append(gas) self.brake_array.append(brake) @@ -56,8 +54,7 @@ class ManeuverPlot(object): self.pid_speed_array.append(pid_speed) self.cruise_speed_array.append(cruise_speed) self.jerk_factor_array.append(jerk_factor) - self.a_target_min_array.append(a_target_min) - self.a_target_max_array.append(a_target_max) + self.a_target_array.append(a_target) def write_plot(self, path, maneuver_name): @@ -90,12 +87,11 @@ class ManeuverPlot(object): plt.figure(plt_num) plt.plot( np.array(self.time_array), np.array(self.acceleration_array), 'g', - np.array(self.time_array), np.array(self.a_target_min_array), 'k--', - np.array(self.time_array), np.array(self.a_target_max_array), 'k--', + np.array(self.time_array), np.array(self.a_target_array), 'k--', ) plt.xlabel('Time [s]') plt.ylabel('Acceleration [m/s^2]') - plt.legend(['accel', 'max', 'min'], loc=0) + plt.legend(['ego-plant', 'target'], loc=0) plt.grid() pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 5b52db24cb..d2208aeba1 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -253,7 +253,8 @@ class Plant(object): Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes()) # ******** publish a fake model going straight and fake calibration ******** - if publish_model: + # note that this is worst case for MPC, since model will delay long mpc by one time step + if publish_model and self.rk.frame % 5 == 0: md = messaging.new_message() cal = messaging.new_message() md.init('model') diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py index 886b767b95..9ffb247f8f 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -156,6 +156,18 @@ maneuvers = [ (CB.RES_ACCEL, 1.4), (0.0, 1.5), (CB.RES_ACCEL, 1.6), (0.0, 1.7)] ), + Maneuver( + "stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops", + duration=45., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=20., + speed_lead_values=[10., 0., 0., 10., 0., 0.] , + speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), Maneuver( "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", duration=30., diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 40d609a142..3c211308fd 100755 --- a/selfdrive/visiond/visiond +++ b/selfdrive/visiond/visiond @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9818be4dff22fef5dbcf5b6fce468c15c34594f7e3ae45b8be0cb6164694919a -size 13330512 +oid sha256:9fe67ea99d1cc0e003fb68cb9c71bbc6ff0b0124b043bcb58ee4df43bbdb7547 +size 13334208