spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.3.9 release

pull/158/head
Vehicle Researcher 2017-11-22 04:30:24 -08:00
parent 2cfdbefde8
commit 5627d0d7fd
55 changed files with 4415 additions and 648 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
README.md
View File

@ -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.

14
RELEASES.md
View File

@ -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

BIN
apk/com.baseui.apk
View File

Binary file not shown.

14
cereal/car.capnp
View File

@ -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
}

19
cereal/log.capnp
View File

@ -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;

2
common/dbc.py
View File

@ -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

3
common/fingerprints.py
View File

@ -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

1
common/params.py
View File

@ -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,

2
opendbc

@ -1 +1 @@
Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85
Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21

2
panda

@ -1 +1 @@
Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383
Subproject commit 3cab37297566962fd6e48a674db3e1f6de8fa4da

2
selfdrive/car/__init__.py
View File

@ -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

26
selfdrive/car/honda/carcontroller.py
View File

@ -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

23
selfdrive/car/honda/carstate.py
View File

@ -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()

96
selfdrive/car/honda/interface.py
View File

@ -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:

73
selfdrive/car/toyota/carcontroller.py
View File

@ -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']:
# TODO: find a way to send single chimes
hud2 = 1
steer = 0
fcw = 0
sound1 = 0
sound2 = 0
return hud1, hud2
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
sound2 = 1
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

15
selfdrive/car/toyota/carstate.py
View File

@ -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)

42
selfdrive/car/toyota/interface.py
View File

@ -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,21 +118,25 @@ class CarInterface(object):
ret.enableGas = True
ret.steerLimitAlert = False
ret.stoppingControl = False
ret.startAccel = 0.0
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
return ret
@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
# returns a car.CarState
def update(self, c):
# ******************* do can recv *******************
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 = []

18
selfdrive/car/toyota/toyotacan.py
View File

@ -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)

8
selfdrive/car/toyota/values.py 100644
View File

@ -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

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.3.8.2-openpilot"
#define COMMA_VERSION "0.3.9-openpilot"

2
selfdrive/config.py
View File

@ -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:

50
selfdrive/controls/controlsd.py
View File

@ -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:

294
selfdrive/controls/lib/adaptivecruise.py
View File

@ -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

6
selfdrive/controls/lib/alertmanager.py
View File

@ -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",

4
selfdrive/controls/lib/drive_helpers.py
View File

@ -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:

66
selfdrive/controls/lib/fcw.py
View File

@ -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

27
selfdrive/controls/lib/latcontrol.py
View File

@ -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)

76
selfdrive/controls/lib/longcontrol.py
View File

@ -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):
stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \
(((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or
(brake_pressed))
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
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))
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_future, self.v_pid, output_gb,
brake_pressed, cruise_standstill)
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
v_target, self.v_pid, output_gb, brake_pressed)
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)

94
selfdrive/controls/lib/longitudinal_mpc/Makefile 100644
View File

@ -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)

0
selfdrive/controls/lib/longitudinal_mpc/__init__.py 100644
View File

98
selfdrive/controls/lib/longitudinal_mpc/generator.cpp 100644
View File

@ -0,0 +1,98 @@
#include <acado_code_generation.hpp>
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;
}

43
selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py 100644
View File

@ -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]

118
selfdrive/controls/lib/longitudinal_mpc/mpc.c 100644
View File

@ -0,0 +1,118 @@
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include <stdio.h>
#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();
}

212
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.c 100644
View File

@ -0,0 +1,212 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#include "acado_auxiliary_functions.h"
#include <stdio.h>
real_t* acado_getVariablesX( )
{
return acadoVariables.x;
}
real_t* acado_getVariablesU( )
{
return acadoVariables.u;
}
#if ACADO_NY > 0
real_t* acado_getVariablesY( )
{
return acadoVariables.y;
}
#endif
#if ACADO_NYN > 0
real_t* acado_getVariablesYN( )
{
return acadoVariables.yN;
}
#endif
real_t* acado_getVariablesX0( )
{
#if ACADO_INITIAL_VALUE_FIXED
return acadoVariables.x0;
#else
return 0;
#endif
}
/** Print differential variables. */
void acado_printDifferentialVariables( )
{
int i, j;
printf("\nDifferential variables:\n[\n");
for (i = 0; i < ACADO_N + 1; ++i)
{
for (j = 0; j < ACADO_NX; ++j)
printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
printf("\n");
}
printf("]\n\n");
}
/** Print control variables. */
void acado_printControlVariables( )
{
int i, j;
printf("\nControl variables:\n[\n");
for (i = 0; i < ACADO_N; ++i)
{
for (j = 0; j < ACADO_NU; ++j)
printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
printf("\n");
}
printf("]\n\n");
}
/** Print ACADO code generation notice. */
void acado_printHeader( )
{
printf(
"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n"
"Milan Vukov and Rien Quirynen, KU Leuven.\n"
);
printf(
"Developed within the Optimization in Engineering Center (OPTEC) under\n"
"supervision of Moritz Diehl. All rights reserved.\n\n"
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
"General Public License 3 in the hope that it will be useful,\n"
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
"GNU Lesser General Public License for more details.\n\n"
);
}
#if !(defined _DSPACE)
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
void acado_tic( acado_timer* t )
{
QueryPerformanceFrequency(&t->freq);
QueryPerformanceCounter(&t->tic);
}
real_t acado_toc( acado_timer* t )
{
QueryPerformanceCounter(&t->toc);
return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
}
#elif (defined __APPLE__)
void acado_tic( acado_timer* t )
{
/* read current clock cycles */
t->tic = mach_absolute_time();
}
real_t acado_toc( acado_timer* t )
{
uint64_t duration; /* elapsed time in clock cycles*/
t->toc = mach_absolute_time();
duration = t->toc - t->tic;
/*conversion from clock cycles to nanoseconds*/
mach_timebase_info(&(t->tinfo));
duration *= t->tinfo.numer;
duration /= t->tinfo.denom;
return (real_t)duration / 1e9;
}
#else
#if __STDC_VERSION__ >= 199901L
/* C99 mode */
/* read current time */
void acado_tic( acado_timer* t )
{
gettimeofday(&t->tic, 0);
}
/* return time passed since last call to tic on this timer */
real_t acado_toc( acado_timer* t )
{
struct timeval temp;
gettimeofday(&t->toc, 0);
if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
}
else
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
}
return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
}
#else
/* ANSI */
/* read current time */
void acado_tic( acado_timer* t )
{
clock_gettime(CLOCK_MONOTONIC, &t->tic);
}
/* return time passed since last call to tic on this timer */
real_t acado_toc( acado_timer* t )
{
struct timespec temp;
clock_gettime(CLOCK_MONOTONIC, &t->toc);
if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
}
else
{
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
}
return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
}
#endif /* __STDC_VERSION__ >= 199901L */
#endif /* (defined _WIN32 || _WIN64) */
#endif

138
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_auxiliary_functions.h 100644
View File

@ -0,0 +1,138 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef ACADO_AUXILIARY_FUNCTIONS_H
#define ACADO_AUXILIARY_FUNCTIONS_H
#include "acado_common.h"
#ifndef __MATLAB__
#ifdef __cplusplus
extern "C"
{
#endif /* __cplusplus */
#endif /* __MATLAB__ */
/** Get pointer to the matrix with differential variables. */
real_t* acado_getVariablesX( );
/** Get pointer to the matrix with control variables. */
real_t* acado_getVariablesU( );
#if ACADO_NY > 0
/** Get pointer to the matrix with references/measurements. */
real_t* acado_getVariablesY( );
#endif
#if ACADO_NYN > 0
/** Get pointer to the vector with references/measurement on the last node. */
real_t* acado_getVariablesYN( );
#endif
/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
real_t* acado_getVariablesX0( );
/** Print differential variables. */
void acado_printDifferentialVariables( );
/** Print control variables. */
void acado_printControlVariables( );
/** Print ACADO code generation notice. */
void acado_printHeader( );
/*
* A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for
* providing us with the following timing routines.
*/
#if !(defined _DSPACE)
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
/* Use Windows QueryPerformanceCounter for timing. */
#include <Windows.h>
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
LARGE_INTEGER tic;
LARGE_INTEGER toc;
LARGE_INTEGER freq;
} acado_timer;
#elif (defined __APPLE__)
#include "unistd.h"
#include <mach/mach_time.h>
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
uint64_t tic;
uint64_t toc;
mach_timebase_info_data_t tinfo;
} acado_timer;
#else
/* Use POSIX clock_gettime() for timing on non-Windows machines. */
#include <time.h>
#if __STDC_VERSION__ >= 199901L
/* C99 mode of operation. */
#include <sys/stat.h>
#include <sys/time.h>
typedef struct acado_timer_
{
struct timeval tic;
struct timeval toc;
} acado_timer;
#else
/* ANSI C */
/** A structure for keeping internal timer data. */
typedef struct acado_timer_
{
struct timespec tic;
struct timespec toc;
} acado_timer;
#endif /* __STDC_VERSION__ >= 199901L */
#endif /* (defined _WIN32 || defined _WIN64) */
/** A function for measurement of the current time. */
void acado_tic( acado_timer* t );
/** A function which returns the elapsed time. */
real_t acado_toc( acado_timer* t );
#endif
#ifndef __MATLAB__
#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */
#endif /* __MATLAB__ */
#endif /* ACADO_AUXILIARY_FUNCTIONS_H */

349
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h 100644
View File

@ -0,0 +1,349 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef ACADO_COMMON_H
#define ACADO_COMMON_H
#include <math.h>
#include <string.h>
#ifndef __MATLAB__
#ifdef __cplusplus
extern "C"
{
#endif /* __cplusplus */
#endif /* __MATLAB__ */
/** \defgroup ACADO ACADO CGT generated module. */
/** @{ */
/** qpOASES QP solver indicator. */
#define ACADO_QPOASES 0
#define ACADO_QPOASES3 1
/** FORCES QP solver indicator.*/
#define ACADO_FORCES 2
/** qpDUNES QP solver indicator.*/
#define ACADO_QPDUNES 3
/** HPMPC QP solver indicator. */
#define ACADO_HPMPC 4
#define ACADO_GENERIC 5
/** Indicator for determining the QP solver used by the ACADO solver code. */
#define ACADO_QP_SOLVER ACADO_QPOASES
#include "acado_qpoases_interface.hpp"
/*
* Common definitions
*/
/** User defined block based condensing. */
#define ACADO_BLOCK_CONDENSING 0
/** Compute covariance matrix of the last state estimate. */
#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
/** Flag indicating whether constraint values are hard-coded or not. */
#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
/** Indicator for fixed initial state. */
#define ACADO_INITIAL_STATE_FIXED 1
/** Number of control/estimation intervals. */
#define ACADO_N 50
/** Number of online data values. */
#define ACADO_NOD 1
/** Number of path constraints. */
#define ACADO_NPAC 0
/** Number of control variables. */
#define ACADO_NU 1
/** Number of differential variables. */
#define ACADO_NX 6
/** Number of algebraic variables. */
#define ACADO_NXA 0
/** Number of differential derivative variables. */
#define ACADO_NXD 0
/** Number of references/measurements per node on the first N nodes. */
#define ACADO_NY 4
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 3
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 56
/** Number of integration steps per shooting interval. */
#define ACADO_RK_NIS 1
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
#define ACADO_USE_ARRIVAL_COST 0
/** Indicator for usage of non-hard-coded linear terms in the objective. */
#define ACADO_USE_LINEAR_TERMS 0
/** Indicator for type of fixed weighting matrices. */
#define ACADO_WEIGHTING_MATRICES_TYPE 0
/*
* Globally used structure definitions
*/
/** The structure containing the user data.
*
* Via this structure the user "communicates" with the solver code.
*/
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 51 x 6 (row major format)
*
* Matrix containing 51 differential variable vectors.
*/
real_t x[ 306 ];
/** Column vector of size: 50
*
* Matrix containing 50 control variable vectors.
*/
real_t u[ 50 ];
/** Column vector of size: 51
*
* Matrix containing 51 online data vectors.
*/
real_t od[ 51 ];
/** Column vector of size: 200
*
* Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes.
*/
real_t y[ 200 ];
/** Column vector of size: 3
*
* Reference/measurement vector for the 51. node.
*/
real_t yN[ 3 ];
/** Column vector of size: 6
*
* Current state feedback vector.
*/
real_t x0[ 6 ];
} ACADOvariables;
/** Private workspace used by the auto-generated code.
*
* Data members of this structure are private to the solver.
* In other words, the user code should not modify values of this
* structure.
*/
typedef struct ACADOworkspace_
{
real_t rk_ttt;
/** Row vector of size: 50 */
real_t rk_xxx[ 50 ];
/** Matrix of size: 4 x 48 (row major format) */
real_t rk_kkk[ 192 ];
/** Row vector of size: 50 */
real_t state[ 50 ];
/** Column vector of size: 300 */
real_t d[ 300 ];
/** Column vector of size: 200 */
real_t Dy[ 200 ];
/** Column vector of size: 3 */
real_t DyN[ 3 ];
/** Matrix of size: 300 x 6 (row major format) */
real_t evGx[ 1800 ];
/** Column vector of size: 300 */
real_t evGu[ 300 ];
/** Column vector of size: 32 */
real_t objAuxVar[ 32 ];
/** Row vector of size: 8 */
real_t objValueIn[ 8 ];
/** Row vector of size: 32 */
real_t objValueOut[ 32 ];
/** Matrix of size: 300 x 6 (row major format) */
real_t Q1[ 1800 ];
/** Matrix of size: 300 x 4 (row major format) */
real_t Q2[ 1200 ];
/** Column vector of size: 50 */
real_t R1[ 50 ];
/** Matrix of size: 50 x 4 (row major format) */
real_t R2[ 200 ];
/** Column vector of size: 300 */
real_t S1[ 300 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t QN1[ 36 ];
/** Matrix of size: 6 x 3 (row major format) */
real_t QN2[ 18 ];
/** Column vector of size: 6 */
real_t Dx0[ 6 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t T[ 36 ];
/** Column vector of size: 7650 */
real_t E[ 7650 ];
/** Column vector of size: 7650 */
real_t QE[ 7650 ];
/** Matrix of size: 300 x 6 (row major format) */
real_t QGx[ 1800 ];
/** Column vector of size: 300 */
real_t Qd[ 300 ];
/** Column vector of size: 306 */
real_t QDy[ 306 ];
/** Matrix of size: 50 x 6 (row major format) */
real_t H10[ 300 ];
/** Matrix of size: 56 x 56 (row major format) */
real_t H[ 3136 ];
/** Matrix of size: 50 x 56 (row major format) */
real_t A[ 2800 ];
/** Column vector of size: 56 */
real_t g[ 56 ];
/** Column vector of size: 56 */
real_t lb[ 56 ];
/** Column vector of size: 56 */
real_t ub[ 56 ];
/** Column vector of size: 50 */
real_t lbA[ 50 ];
/** Column vector of size: 50 */
real_t ubA[ 50 ];
/** Column vector of size: 56 */
real_t x[ 56 ];
/** Column vector of size: 106 */
real_t y[ 106 ];
} ACADOworkspace;
/*
* Forward function declarations.
*/
/** Performs the integration and sensitivity propagation for one shooting interval.
*
* \param rk_eta Working array to pass the input values and return the results.
* \param resetIntegrator The internal memory of the integrator can be reset.
*
* \return Status code of the integrator.
*/
int acado_integrate( real_t* const rk_eta, int resetIntegrator );
/** Export of an ACADO symbolic function.
*
* \param in Input to the exported function.
* \param out Output of the exported function.
*/
void acado_rhs_forw(const real_t* in, real_t* out);
/** Preparation step of the RTI scheme.
*
* \return Status of the integration module. =0: OK, otherwise the error code.
*/
int acado_preparationStep( );
/** Feedback/estimation step of the RTI scheme.
*
* \return Status code of the qpOASES QP solver.
*/
int acado_feedbackStep( );
/** Solver initialization. Must be called once before any other function call.
*
* \return =0: OK, otherwise an error code of a QP solver.
*/
int acado_initializeSolver( );
/** Initialize shooting nodes by a forward simulation starting from the first node.
*/
void acado_initializeNodesByForwardSimulation( );
/** Shift differential variables vector by one interval.
*
* \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation.
* \param xEnd Value for the x vector on the last node. If =0 the old value is used.
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
*/
void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
/** Shift controls vector by one interval.
*
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
*/
void acado_shiftControls( real_t* const uEnd );
/** Get the KKT tolerance of the current iterate.
*
* \return The KKT tolerance value.
*/
real_t acado_getKKT( );
/** Calculate the objective value.
*
* \return Value of the objective function.
*/
real_t acado_getObjective( );
/*
* Extern declarations.
*/
extern ACADOworkspace acadoWorkspace;
extern ACADOvariables acadoVariables;
/** @} */
#ifndef __MATLAB__
#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */
#endif /* __MATLAB__ */
#endif /* ACADO_COMMON_H */

383
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c 100644
View File

@ -0,0 +1,383 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#include "acado_common.h"
void acado_rhs_forw(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* u = in + 48;
const real_t* od = in + 49;
/* Compute outputs: */
out[0] = xd[1];
out[1] = xd[2];
out[2] = u[0];
out[3] = xd[4];
out[4] = xd[5];
out[5] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[5]);
out[6] = xd[12];
out[7] = xd[13];
out[8] = xd[14];
out[9] = xd[15];
out[10] = xd[16];
out[11] = xd[17];
out[12] = xd[18];
out[13] = xd[19];
out[14] = xd[20];
out[15] = xd[21];
out[16] = xd[22];
out[17] = xd[23];
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = (real_t)(0.0000000000000000e+00);
out[20] = (real_t)(0.0000000000000000e+00);
out[21] = (real_t)(0.0000000000000000e+00);
out[22] = (real_t)(0.0000000000000000e+00);
out[23] = (real_t)(0.0000000000000000e+00);
out[24] = xd[30];
out[25] = xd[31];
out[26] = xd[32];
out[27] = xd[33];
out[28] = xd[34];
out[29] = xd[35];
out[30] = xd[36];
out[31] = xd[37];
out[32] = xd[38];
out[33] = xd[39];
out[34] = xd[40];
out[35] = xd[41];
out[36] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[36]);
out[37] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[37]);
out[38] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[38]);
out[39] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[39]);
out[40] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[40]);
out[41] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[41]);
out[42] = xd[43];
out[43] = xd[44];
out[44] = (real_t)(1.0000000000000000e+00);
out[45] = xd[46];
out[46] = xd[47];
out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]);
}
/* Fixed step size:0.2 */
int acado_integrate( real_t* const rk_eta, int resetIntegrator )
{
int error;
int run1;
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[6] = 1.0000000000000000e+00;
rk_eta[7] = 0.0000000000000000e+00;
rk_eta[8] = 0.0000000000000000e+00;
rk_eta[9] = 0.0000000000000000e+00;
rk_eta[10] = 0.0000000000000000e+00;
rk_eta[11] = 0.0000000000000000e+00;
rk_eta[12] = 0.0000000000000000e+00;
rk_eta[13] = 1.0000000000000000e+00;
rk_eta[14] = 0.0000000000000000e+00;
rk_eta[15] = 0.0000000000000000e+00;
rk_eta[16] = 0.0000000000000000e+00;
rk_eta[17] = 0.0000000000000000e+00;
rk_eta[18] = 0.0000000000000000e+00;
rk_eta[19] = 0.0000000000000000e+00;
rk_eta[20] = 1.0000000000000000e+00;
rk_eta[21] = 0.0000000000000000e+00;
rk_eta[22] = 0.0000000000000000e+00;
rk_eta[23] = 0.0000000000000000e+00;
rk_eta[24] = 0.0000000000000000e+00;
rk_eta[25] = 0.0000000000000000e+00;
rk_eta[26] = 0.0000000000000000e+00;
rk_eta[27] = 1.0000000000000000e+00;
rk_eta[28] = 0.0000000000000000e+00;
rk_eta[29] = 0.0000000000000000e+00;
rk_eta[30] = 0.0000000000000000e+00;
rk_eta[31] = 0.0000000000000000e+00;
rk_eta[32] = 0.0000000000000000e+00;
rk_eta[33] = 0.0000000000000000e+00;
rk_eta[34] = 1.0000000000000000e+00;
rk_eta[35] = 0.0000000000000000e+00;
rk_eta[36] = 0.0000000000000000e+00;
rk_eta[37] = 0.0000000000000000e+00;
rk_eta[38] = 0.0000000000000000e+00;
rk_eta[39] = 0.0000000000000000e+00;
rk_eta[40] = 0.0000000000000000e+00;
rk_eta[41] = 1.0000000000000000e+00;
rk_eta[42] = 0.0000000000000000e+00;
rk_eta[43] = 0.0000000000000000e+00;
rk_eta[44] = 0.0000000000000000e+00;
rk_eta[45] = 0.0000000000000000e+00;
rk_eta[46] = 0.0000000000000000e+00;
rk_eta[47] = 0.0000000000000000e+00;
acadoWorkspace.rk_xxx[48] = rk_eta[48];
acadoWorkspace.rk_xxx[49] = rk_eta[49];
for (run1 = 0; run1 < 1; ++run1)
{
acadoWorkspace.rk_xxx[0] = + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[24] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[25] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[26] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[27] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[28] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[29] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[30] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[31] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[32] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[33] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[34] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[35] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[36] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[37] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[38] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[39] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[40] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[41] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[42] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[43] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[44] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[45] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[46] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[47] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[72] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[73] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[74] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[75] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[76] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[77] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[78] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[79] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[80] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[81] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[82] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[83] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[84] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[85] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[86] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[87] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[88] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[89] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[90] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[91] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[92] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[93] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[94] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[95] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) );
rk_eta[0] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[144];
rk_eta[1] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[145];
rk_eta[2] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[146];
rk_eta[3] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[147];
rk_eta[4] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[148];
rk_eta[5] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[149];
rk_eta[6] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[150];
rk_eta[7] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[151];
rk_eta[8] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[152];
rk_eta[9] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[153];
rk_eta[10] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[154];
rk_eta[11] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[155];
rk_eta[12] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[156];
rk_eta[13] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[157];
rk_eta[14] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[158];
rk_eta[15] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[159];
rk_eta[16] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[160];
rk_eta[17] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[161];
rk_eta[18] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[162];
rk_eta[19] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[163];
rk_eta[20] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[164];
rk_eta[21] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[165];
rk_eta[22] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[166];
rk_eta[23] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[167];
rk_eta[24] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[168];
rk_eta[25] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[169];
rk_eta[26] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[170];
rk_eta[27] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[171];
rk_eta[28] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[172];
rk_eta[29] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[173];
rk_eta[30] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[174];
rk_eta[31] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[175];
rk_eta[32] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[176];
rk_eta[33] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[177];
rk_eta[34] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[178];
rk_eta[35] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[179];
rk_eta[36] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[180];
rk_eta[37] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[181];
rk_eta[38] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[182];
rk_eta[39] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[183];
rk_eta[40] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[184];
rk_eta[41] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[185];
rk_eta[42] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[186];
rk_eta[43] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[187];
rk_eta[44] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[188];
rk_eta[45] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[189];
rk_eta[46] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[190];
rk_eta[47] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[191];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
error = 0;
return error;
}

70
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp 100644
View File

@ -0,0 +1,70 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
extern "C"
{
#include "acado_common.h"
}
#include "INCLUDE/QProblem.hpp"
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
static int acado_nWSR;
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
static SolutionAnalysis acado_sa;
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(56, 50);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
qp.getPrimalSolution( acadoWorkspace.x );
qp.getDualSolution( acadoWorkspace.y );
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
if (retVal != SUCCESSFUL_RETURN)
return (int)retVal;
retVal = acado_sa.getHessianInverse( &qp,var );
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
return (int)retVal;
}
int acado_getNWSR( void )
{
return acado_nWSR;
}
const char* acado_getErrorString( int error )
{
return MessageHandling::getErrorString( error );
}

65
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp 100644
View File

@ -0,0 +1,65 @@
/*
* This file was auto-generated using the ACADO Toolkit.
*
* While ACADO Toolkit is free software released under the terms of
* the GNU Lesser General Public License (LGPL), the generated code
* as such remains the property of the user who used ACADO Toolkit
* to generate this code. In particular, user dependent data of the code
* do not inherit the GNU LGPL license. On the other hand, parts of the
* generated code that are a direct copy of source code from the
* ACADO Toolkit or the software tools it is based on, remain, as derived
* work, automatically covered by the LGPL license.
*
* ACADO Toolkit is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
*
*/
#ifndef QPOASES_HEADER
#define QPOASES_HEADER
#ifdef PC_DEBUG
#include <stdio.h>
#endif /* PC_DEBUG */
#include <math.h>
#ifdef __cplusplus
#define EXTERNC extern "C"
#else
#define EXTERNC
#endif
/*
* A set of options for qpOASES
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 56
/** Maximum number of constraints. */
#define QPOASES_NCMAX 50
/** Maximum number of working set recalculations. */
#define QPOASES_NWSRMAX 500
/** Print level for qpOASES. */
#define QPOASES_PRINTLEVEL PL_NONE
/** The value of EPS */
#define QPOASES_EPS 2.221e-16
/** Internally used floating point type */
typedef double real_t;
/*
* Forward function declarations
*/
/** A function that calls the QP solver */
EXTERNC int acado_solve( void );
/** Get the number of active set changes */
EXTERNC int acado_getNWSR( void );
/** Get the error string. */
const char* acado_getErrorString( int error );
#endif /* QPOASES_HEADER */

2036
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c 100644
View File

File diff suppressed because one or more lines are too long

22
selfdrive/controls/lib/pid.py
View File

@ -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)

371
selfdrive/controls/lib/planner.py
View File

@ -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 = 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)
# 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)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
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
self.PP.update(CS.vEgo, md)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
# **** 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
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
# compute risk of collision events: fcw
self.FCW.process(CS, self.AC)
plan_send.plan.fcw = bool(self.FCW.active)
# 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

88
selfdrive/controls/lib/speed_smoother.py 100644
View File

@ -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)

8
selfdrive/controls/radard.py
View File

@ -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]

BIN
selfdrive/loggerd/loggerd
View File

Binary file not shown.

2
selfdrive/manager.py
View File

@ -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")

25
selfdrive/radar/toyota/interface.py
View File

@ -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()

BIN
selfdrive/sensord/gpsd
View File

Binary file not shown.

BIN
selfdrive/sensord/sensord
View File

Binary file not shown.

1
selfdrive/service_list.yaml
View File

@ -45,6 +45,7 @@ gpsLocationExternal: [8032, true]
ubloxGnss: [8033, true]
clocks: [8034, true]
liveMpc: [8035, true]
liveLongitudinalMpc: [8036, true]
testModel: [8040, false]

2
selfdrive/test/plant/maneuver.py
View File

@ -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"

14
selfdrive/test/plant/maneuverplots.py
View File

@ -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)

3
selfdrive/test/plant/plant.py
View File

@ -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')

12
selfdrive/test/tests/plant/test_longitudinal.py
View File

@ -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.,

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.