From f054ff0b087e27f76d9920481bc371c965f40ff1 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Fri, 3 Nov 2017 02:24:15 -0700 Subject: [PATCH] openpilot v0.3.8.2 tweaks old-commit-hash: 7dabcdace8107b9acf12f71c64db7185947dd258 --- SAFETY.md | 79 ++++++++++++++----- cereal/car.capnp | 6 ++ selfdrive/car/honda/interface.py | 3 + selfdrive/car/toyota/interface.py | 10 +-- selfdrive/controls/lib/alertmanager.py | 6 +- .../controls/lib/lateral_mpc/generator.cpp | 2 +- .../lib/lateral_mpc/mpc_export/acado_solver.c | 2 +- selfdrive/controls/lib/longcontrol.py | 3 +- selfdrive/controls/lib/pid.py | 13 ++- 9 files changed, 90 insertions(+), 34 deletions(-) diff --git a/SAFETY.md b/SAFETY.md index 72297d8d7c..125705ad04 100644 --- a/SAFETY.md +++ b/SAFETY.md @@ -1,36 +1,73 @@ openpilot Safety ====== -openpilot is an Adaptive Cruise Control and Lane Keeping Assist System. Like -other ACC and LKAS systems, openpilot requires the driver to be alert and to pay -attention at all times. We repeat, **driver alertness is necessary, but not +openpilot is an Adaptive Cruise Control (ACC) and Lane Keeping Assist (LKA) system. +Like other ACC and LKA systems, openpilot requires the driver to be alert and to +pay attention at all times. We repeat, **driver alertness is necessary, but not sufficient, for openpilot to be used safely**. Even with an attentive driver, we must make further efforts for the system to be safe. We have designed openpilot with two other safety considerations. -1. The vehicle must always be controllable by the driver. +1. The driver must always be capable to immediately retake manual control of the vehicle, + by stepping on either pedal or by pressing the cancel button. 2. The vehicle must not alter its trajectory too quickly for the driver to safely - react. + react. This means that while the system is engaged, the actuators are constrained + to operate within reasonable limits. -To address these, we came up with two safety principles. +Following are details of the car specific safety implementations: -1. Enforced disengagements. Step on either pedal or press the cancel button to - retake manual control of the car immediately. - - These are hard enforced by the board, and soft enforced by the software. The - green led on the board signifies if the board is allowing control messages. - - Honda CAN uses both a counter and a checksum to ensure integrity and prevent - replay of the same message. +Honda/Acura +------ + + - While the system is engaged, gas, brake and steer limits are subject to the same limits used by + the stock system. + + - Without an interceptor, the gas is controlled by the Powertrain Control Module (PCM). + The PCM limits acceleration to what is reasonable for a cruise control system. With an + interceptor, the gas is clipped to 60%. -2. Actuation limits. While the system is engaged, the actuators are constrained - to operate within reasonable limits; the same limits used by the stock system on - the Honda. - - Without an interceptor, the gas is controlled by the PCM. The PCM limits - acceleration to what is reasonable for a cruise control system. With an - interceptor, the gas is clipped to 60% in longcontrol.py - The brake is controlled by the 0x1FA CAN message. This message allows full braking, although the board and the software clip it to 1/4th of the max. This is around .3g of braking. - - Steering is controlled by the 0xE4 CAN message. The EPS controller in the - car limits the torque to a very small amount, so regardless of the message, - the controller cannot jerk the wheel. + + - Steering is controlled by the 0xE4 CAN message. The Electronic Power Steering (EPS) + controller in the car limits the torque to a very small amount, so regardless of the + message, the controller cannot jerk the wheel. + + - Brake and gas pedal pressed signals are contained in the 0x17C CAN message. A rising edge of + either signal triggers a disengagement, which is enforced by the board and in software. The + green led on the board signifies if the board is allowing control messages. + + - Honda CAN uses both a counter and a checksum to ensure integrity and prevent + replay of the same message. + +Toyota +------ + + - While the system is engaged, gas, brake and steer limits are subject to the same limits used by + the stock system. + + - With the stock Driving Support Unit (DSU) enabled, the acceleration is controlled + by the stock system and is subject to the stock adaptive cruise control limits. Without the + stock DSU connected, the acceleration command is controlled by the 0x343 CAN message and its + value is limited by the board and the software to between .3g of deceleration and .15g of + acceleration. The acceleration command is ignored by the Engine Control Module (ECM) while the + cruise control system is disengaged. + + - Steering torque is controlled through the 0x2E4 CAN message and it's limited by the board and in + software to a value of -1500 and 1500. In addition, the vehicle EPS unit will not respond to + commands outside these limits. A steering torque rate limit is enforced by the board and in + software so that the commanded steering torque must rise from 0 to max value no faster than + 1.5s. Commanded steering torque is limited by the board and in software to be no more than 500 + units above the actual EPS generated motor torque to ensure limited differences between + commanded and actual torques. + + - Brake and gas pedal pressed signals are contained in the 0x224 and 0x1D2 CAN messages, + respectively. A rising edge of either signal triggers a disengagement, which is enforced by the + board and in software. Additionally, the cruise control system disengages on the rising edge of + the brake pedal pressed signal. + + - The cruise control system state is contained in the 0x1D2 message. No control messages are + allowed if the cruise control system is not active. This is enforced by the software and the + board. The green led on the board signifies if the board is allowing control messages. diff --git a/cereal/car.capnp b/cereal/car.capnp index bafc77f80d..35828f08c9 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -262,6 +262,9 @@ struct CarParams { brakeMaxBP @24 :List(Float32); brakeMaxV @25 :List(Float32); + longPidDeadzoneBP @33 :List(Float32); + longPidDeadzoneV @34 :List(Float32); + enum SafetyModels { # does NOT match board setting noOutput @0; @@ -290,5 +293,8 @@ struct CarParams { 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? } diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 397c905988..e4356676b1 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -185,6 +185,9 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed + ret.longPidDeadzoneBP = [0.] + ret.longPidDeadzoneV = [0.] + ret.steerLimitAlert = True return ret diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b7af7d05ec..4ef103aa2d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -65,11 +65,11 @@ class CarInterface(object): ret.l = 2.70 ret.aF = ret.l * 0.44 ret.sR = 14.5 #Rav4 2017, TODO: find exact value for Prius - if candidate == CAR.PRIUS: - ret.steerKp, ret.steerKi = 0.2, 0.01 - elif candidate == CAR.RAV4: # rav4 control seem to be ok with integrators - ret.steerKp, ret.steerKi = 0.2, 0.05 - ret.steerKf = 0.00007818594 # full torque for 10 deg at 80mph + ret.steerKp, ret.steerKi = 0.6, 0.05 + ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 + + ret.longPidDeadzoneBP = [0., 9.] + ret.longPidDeadzoneV = [0., .15] # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 30422b20ee..a585958796 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( - "Brake", - "Risk of Collision", - PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), + "", + "", + PT.LOW, None, None, .1, .1, .1), "steerSaturated": Alert( "Take Control", diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 350a783ee5..36d8aea56b 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -79,7 +79,7 @@ int main( ) Q(3,3) = 1.0; - Q(4,4) = 0.5; + Q(4,4) = 2.0; // Terminal cost Function hN; diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c index 321b8a540a..47ea64b9d8 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f6f985c451e36d05b11e35ba61e229e28d49bcfac8874146bb500f6b0736e409 +oid sha256:f01adf6d07d2ca818d2df6a79b1c10a4806dd6a1a525438950329ff0c2791437 size 194874 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 536bd0c693..3924069c2e 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -131,7 +131,8 @@ class LongControl(object): self.pid.pos_limit = gas_max self.pid.neg_limit = - brake_max - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor) + 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) # intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 8b53e46a85..4b71525510 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -2,6 +2,15 @@ import numpy as np from common.numpy_fast import clip, interp import numbers +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + 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): self._k_p = k_p # proportional gain @@ -57,11 +66,11 @@ class PIController(object): 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.): + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0., deadzone=0.): self.speed = speed self.jerk_factor = jerk_factor - error = float(setpoint - measurement) + error = float(apply_deadzone(setpoint - measurement, deadzone)) self.p = error * self.k_p f = feedforward * self.k_f