openpilot v0.3.8.2 tweaks
parent
187a70f760
commit
7dabcdace8
79
SAFETY.md
79
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.
|
||||
|
|
|
@ -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?
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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",
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -213,22 +213,22 @@ tmpQ2[0] = + tmpFx[0];
|
|||
tmpQ2[1] = + tmpFx[4];
|
||||
tmpQ2[2] = + tmpFx[8];
|
||||
tmpQ2[3] = + tmpFx[12];
|
||||
tmpQ2[4] = + tmpFx[16]*(real_t)5.0000000000000000e-01;
|
||||
tmpQ2[4] = + tmpFx[16]*(real_t)2.0000000000000000e+00;
|
||||
tmpQ2[5] = + tmpFx[1];
|
||||
tmpQ2[6] = + tmpFx[5];
|
||||
tmpQ2[7] = + tmpFx[9];
|
||||
tmpQ2[8] = + tmpFx[13];
|
||||
tmpQ2[9] = + tmpFx[17]*(real_t)5.0000000000000000e-01;
|
||||
tmpQ2[9] = + tmpFx[17]*(real_t)2.0000000000000000e+00;
|
||||
tmpQ2[10] = + tmpFx[2];
|
||||
tmpQ2[11] = + tmpFx[6];
|
||||
tmpQ2[12] = + tmpFx[10];
|
||||
tmpQ2[13] = + tmpFx[14];
|
||||
tmpQ2[14] = + tmpFx[18]*(real_t)5.0000000000000000e-01;
|
||||
tmpQ2[14] = + tmpFx[18]*(real_t)2.0000000000000000e+00;
|
||||
tmpQ2[15] = + tmpFx[3];
|
||||
tmpQ2[16] = + tmpFx[7];
|
||||
tmpQ2[17] = + tmpFx[11];
|
||||
tmpQ2[18] = + tmpFx[15];
|
||||
tmpQ2[19] = + tmpFx[19]*(real_t)5.0000000000000000e-01;
|
||||
tmpQ2[19] = + tmpFx[19]*(real_t)2.0000000000000000e+00;
|
||||
tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16];
|
||||
tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17];
|
||||
tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18];
|
||||
|
@ -253,7 +253,7 @@ tmpR2[0] = + tmpFu[0];
|
|||
tmpR2[1] = + tmpFu[1];
|
||||
tmpR2[2] = + tmpFu[2];
|
||||
tmpR2[3] = + tmpFu[3];
|
||||
tmpR2[4] = + tmpFu[4]*(real_t)5.0000000000000000e-01;
|
||||
tmpR2[4] = + tmpFu[4]*(real_t)2.0000000000000000e+00;
|
||||
tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4];
|
||||
}
|
||||
|
||||
|
@ -1965,7 +1965,7 @@ tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5];
|
|||
tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1];
|
||||
tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2];
|
||||
tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3];
|
||||
tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)5.0000000000000000e-01;
|
||||
tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]*(real_t)2.0000000000000000e+00;
|
||||
objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4];
|
||||
}
|
||||
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
Loading…
Reference in New Issue