retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.4.0.1 release

Vehicle Researcher 2017-12-23 17:15:27 -08:00
parent c7b5fb9116
commit a77c0a1098
73 changed files with 10303 additions and 2840 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
README.md
View File

@ -3,7 +3,7 @@ Welcome to openpilot
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent.
Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas and Acuras. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas, Acuras and Toyotas. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
@ -29,13 +29,18 @@ Supported Cars
- Honda CR-V Touring 2015-2016
- Can only be enabled above 25 mph
- Toyota RAV-4 2016+ with TSS-P (alpha!)
- Toyota RAV-4 2016+ non-hybrid with TSS-P
- By default it uses stock Toyota ACC for longitudinal control
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) and can be enabled above 20 mph
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) 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](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29)
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29)
- Lateral control needs improvements
- Toyota RAV-4 2017 hybrid (alpha!)
- By default it uses stock Toyota ACC for longitudinal control
- openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can do stop and go
In Progress Cars
------
@ -50,6 +55,12 @@ Community WIP Cars
- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
- [Honda Odyssey 2018 with Honda Sensing](https://github.com/commaai/openpilot/pull/155)
- [Honda Pilot 2017 with Honda Sensing](https://github.com/commaai/openpilot/pull/161)
- [Acura RDX 2018 with AcuraWatch Plus](https://github.com/commaai/openpilot/pull/162)
Directory structure
------

9
RELEASES.md
View File

@ -1,3 +1,12 @@
Version 0.4.0.1 (2017-12-21)
==========================
* New UI to match chffrplus
* Improved lateral control tuning to fix oscillations on Civic
* Add alpha support for 2017 Toyota Rav4 Hybrid
* Reduced CPU usage
* Removed unnecessary utilization of fan at max speed
* Minor bug fixes
Version 0.3.9 (2017-11-21)
==========================
* Add alpha support for 2017 Toyota Prius

BIN
apk/ai.comma.plus.black.apk 100644
View File

Binary file not shown.

BIN
apk/ai.comma.plus.frame.apk 100644
View File

Binary file not shown.

BIN
apk/ai.comma.plus.offroad.apk 100644
View File

Binary file not shown.

BIN
apk/com.baseui.apk
View File

Binary file not shown.

11
cereal/car.capnp
View File

@ -12,12 +12,13 @@ $Java.outerClassname("Car");
struct CarEvent @0x9b1657f34caf3ad3 {
name @0 :EventName;
enable @1 :Bool;
preEnable @7 :Bool;
noEntry @2 :Bool;
warning @3 :Bool;
userDisable @4 :Bool;
softDisable @5 :Bool;
immediateDisable @6 :Bool;
preEnable @7 :Bool;
permanent @8 :Bool;
enum EventName @0xbaa8c5d505f727de {
# TODO: copy from error list
@ -64,9 +65,10 @@ struct CarState {
events @13 :List(CarEvent);
# car speed
vEgo @1 :Float32; # best estimate of speed
aEgo @16 :Float32; # best estimate of acceleration
vEgoRaw @17 :Float32; # unfiltered speed
vEgo @1 :Float32; # best estimate of speed
aEgo @16 :Float32; # best estimate of acceleration
vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors
yawRate @22 :Float32; # best estimate of yaw rate
standstill @18 :Bool;
wheelSpeeds @2 :WheelSpeeds;
@ -309,4 +311,5 @@ struct CarParams {
directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping
startAccel @35 :Float32; # Required acceleraton to overcome creep braking
steerRateCost @40 :Float32; # Lateral MPC cost on steering rate
}

41
cereal/log.capnp
View File

@ -28,6 +28,7 @@ struct InitData {
version @4 :Text;
gitCommit @10 :Text;
gitBranch @11 :Text;
gitRemote @13 :Text;
androidBuildInfo @5 :AndroidBuildInfo;
androidSensors @6 :List(AndroidSensor);
@ -154,6 +155,8 @@ struct SensorEventData {
pressure @9 :SensorVec;
magneticUncalibrated @11 :SensorVec;
gyroUncalibrated @12 :SensorVec;
proximity @13: Float32;
light @14: Float32;
}
source @8 :SensorSource;
@ -242,8 +245,11 @@ struct ThermalData {
freeSpace @7 :Float32;
batteryPercent @8 :Int16;
batteryStatus @9 :Text;
usbOnline @12 :Bool;
fanSpeed @10 :UInt16;
started @11 :Bool;
startedTs @13 :UInt64;
}
struct HealthData {
@ -349,6 +355,7 @@ struct Live100Data {
jerkFactor @12 :Float32;
angleSteers @13 :Float32; # Steering angle in degrees.
angleSteersDes @29 :Float32;
curvature @37 :Float32; # path curvature from vehicle model
hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32;
@ -361,6 +368,8 @@ struct Live100Data {
rearViewCam @23 :Bool;
alertText1 @24 :Text;
alertText2 @25 :Text;
alertStatus @38 :AlertStatus;
alertSize @39 :AlertSize;
awarenessStatus @26 :Float32;
angleOffset @27 :Float32;
@ -378,6 +387,20 @@ struct Live100Data {
stopping @2;
starting @3;
}
enum AlertStatus {
normal @0; # low priority alert for user's convenience
userPrompt @1; # mid piority alert that might require user intervention
critical @2; # high priority alert that needs immediate user intervention
}
enum AlertSize {
none @0; # don't display the alert
small @1; # small box
mid @2; # mid screen
full @3; # full screen
}
}
struct LiveEventData {
@ -600,6 +623,23 @@ struct NavUpdate {
}
}
struct NavStatus {
isNavigating @0 :Bool;
currentAddress @1 :Address;
struct Address {
title @0 :Text;
lat @1 :Float64;
lng @2 :Float64;
house @3 :Text;
address @4 :Text;
street @5 :Text;
city @6 :Text;
state @7 :Text;
country @8 :Text;
}
}
struct CellInfo {
timestamp @0 :UInt64;
repr @1 :Text; # android toString() for now
@ -1300,5 +1340,6 @@ struct Event {
clocks @35 :Clocks;
liveMpc @36 :LiveMpcData;
liveLongitudinalMpc @37 :LiveLongitudinalMpcData;
navStatus @38 :NavStatus;
}
}

2
common/basedir.py
View File

@ -1,4 +1,4 @@
import os
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))

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 RAV4 2017 HYBRID": {
36L: 8, 37L: 8, 170L: 8, 180L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 713L: 8, 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, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 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, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1232L: 8, 1235L: 8, 1264L: 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, 1596L: 8, 1597L: 8, 1600L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 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
},

6
common/kalman/ekf.py
View File

@ -35,7 +35,7 @@ class SensorReading:
# A generic sensor class that does no pre-processing of data
class SimpleSensor:
# obs_model can be
# a full obesrvation model matrix, or
# a full observation model matrix, or
# an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
# covar can be
# a full covariance matrix
@ -129,7 +129,7 @@ class EKF:
print "covar:\n",self.covar
def update_scalar(self, reading):
# like update but knowing that measurment is a scalar
# like update but knowing that measurement is a scalar
# this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
# innovation = reading.data - np.matmul(reading.obs_model, self.state)
@ -188,7 +188,7 @@ class EKF:
Current implementations calculate A and J as functions of state. Control input
can be added trivially by adding a control parameter to predict() and calc_tranfer_update(),
and using it during calcualtion of A and J
and using it during calculation of A and J
"""

3
common/params.py
View File

@ -52,12 +52,14 @@ keys = {
"Version": TxType.PERSISTANT,
"GitCommit": TxType.PERSISTANT,
"GitBranch": TxType.PERSISTANT,
"GitRemote": TxType.PERSISTANT,
# written: baseui
# read: ui, controls
"IsMetric": TxType.PERSISTANT,
"IsRearViewMirror": TxType.PERSISTANT,
"IsFcwEnabled": TxType.PERSISTANT,
"HasAcceptedTerms": TxType.PERSISTANT,
"IsUploadVideoOverCellularEnabled": TxType.PERSISTANT,
# written: visiond
# read: visiond, controlsd
"CalibrationParams": TxType.PERSISTANT,
@ -69,6 +71,7 @@ keys = {
"CarParams": TxType.CLEAR_ON_CAR_START,
"Passive": TxType.PERSISTANT,
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
}
def fsync_dir(path):

49
common/profiler.py
View File

@ -1,32 +1,47 @@
from common.realtime import sec_since_boot
import time
class Profiler(object):
def __init__(self, enabled=False):
self.enabled = enabled
self.cp = []
self.start_time = sec_since_boot()
self.cp = {}
self.cp_ignored = []
self.iter = 0
self.start_time = time.clock()
self.last_time = self.start_time
def checkpoint(self, name):
if not self.enabled:
return
tt = sec_since_boot()
self.cp.append((name, tt - self.last_time))
self.last_time = tt
self.tot = 0.
def reset(self, enabled=False):
self.enabled = enabled
self.cp = []
self.start_time = sec_since_boot()
self.cp = {}
self.cp_ignored = []
self.iter = 0
self.start_time = time.clock()
self.last_time = self.start_time
def checkpoint(self, name, ignore=False):
# ignore flag needed when benchmarking threads with ratekeeper
if not self.enabled:
return
tt = time.clock()
if name not in self.cp:
self.cp[name] = 0.
if ignore:
self.cp_ignored.append(name)
self.cp[name] += tt - self.last_time
if not ignore:
self.tot += tt - self.last_time
self.last_time = tt
def display(self):
if not self.enabled:
return
self.iter += 1
print "******* Profiling *******"
tot = 0.0
for n, ms in self.cp:
print "%30s: %7.2f" % (n, ms*1000.0)
tot += ms
print " TOTAL: %7.2f" % (tot*1000.0)
for n in self.cp:
ms = self.cp[n]
if n in self.cp_ignored:
print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED"
else:
print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100)
print "Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot)

39
launch_openpilot.sh 100755
View File

@ -0,0 +1,39 @@
#!/usr/bin/bash
function launch {
# apply update
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
git reset --hard @{u} &&
git clean -xdf &&
exec "${BASH_SOURCE[0]}"
fi
# check if NEOS update is required
while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do
curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater
sleep 10
done
# no cpu rationing for now
echo 0-3 > /dev/cpuset/background/cpus
echo 0-3 > /dev/cpuset/system-background/cpus
echo 0-3 > /dev/cpuset/foreground/boost/cpus
echo 0-3 > /dev/cpuset/foreground/cpus
echo 0-3 > /dev/cpuset/android/cpus
# wait for network
(cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$!
until ping -W 1 -c 1 8.8.8.8; do sleep 1; done
kill $spin_pid
export PYTHONPATH="$PWD"
# start manager
cd selfdrive
./manager.py
# if broken, keep on screen error
while true; do sleep 1; done
}
launch

BIN
selfdrive/assets/OpenSans-Regular.ttf 100644
View File

Binary file not shown.

BIN
selfdrive/assets/OpenSans-SemiBold.ttf 100644
View File

Binary file not shown.

6
selfdrive/boardd/boardd.cc
View File

@ -130,7 +130,11 @@ bool usb_connect() {
#endif
// no output is the default
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
if (getenv("RECVMOCK")) {
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT);
} else {
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
}
if (safety_setter_thread_handle == -1) {
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);

3
selfdrive/can/libdbc_py.py
View File

@ -1,11 +1,12 @@
import os
import sys
import subprocess
from cffi import FFI
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_output(["make"], cwd=can_dir)
subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir)
ffi = FFI()
ffi.cdef("""

2
selfdrive/can/parser.cc
View File

@ -242,7 +242,7 @@ class CANParser {
const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
if (state.seen > 0) {
INFO("%X TIMEOUT\n", state.address);
DEBUG("%X TIMEOUT\n", state.address);
}
can_valid = false;
}

24
selfdrive/car/__init__.py
View File

@ -6,12 +6,9 @@ from common.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
from .honda.interface import CarInterface as HondaInterface
try:
from .toyota.interface import CarInterface as ToyotaInterface
except ImportError:
ToyotaInterface = None
from selfdrive.car.honda.interface import CarInterface as HondaInterface
from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
from selfdrive.car.mock.interface import CarInterface as MockInterface
try:
from .simulator.interface import CarInterface as SimInterface
@ -31,9 +28,12 @@ interfaces = {
"HONDA CR-V 2016 TOURING": HondaInterface,
"TOYOTA PRIUS 2017": ToyotaInterface,
"TOYOTA RAV4 2017": ToyotaInterface,
"TOYOTA RAV4 2017 HYBRID": ToyotaInterface,
"simulator": SimInterface,
"simulator2": Sim2Interface
"simulator2": Sim2Interface,
"mock": MockInterface
}
# **** for use live only ****
@ -75,12 +75,18 @@ def fingerprint(logcan, timeout):
return (candidate_cars[0], finger)
def get_car(logcan, sendcan=None, timeout=None):
def get_car(logcan, sendcan=None, passive=True):
# TODO: timeout only useful for replays so controlsd can start before unlogger
timeout = 1. if passive else None
candidate, fingerprints = fingerprint(logcan, timeout)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
return None, None
if passive:
candidate = "mock"
else:
return None, None
interface_cls = interfaces[candidate]
params = interface_cls.get_params(candidate, fingerprints)

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

@ -122,7 +122,7 @@ class CarController(object):
GAS_MAX = 1004
BRAKE_MAX = 1024/4
if CS.civic:
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
elif CS.crv:
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value

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

@ -1,5 +1,4 @@
import os
import time
from cereal import car
from common.numpy_fast import interp
import selfdrive.messaging as messaging

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

@ -1,11 +1,11 @@
#!/usr/bin/env python
import os
import time
import numpy as np
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 selfdrive.controls.lib.vehicle_model import VehicleModel
from cereal import car
from selfdrive.services import service_list
import selfdrive.messaging as messaging
@ -93,6 +93,7 @@ class CarInterface(object):
# *** init the major players ***
self.CS = CarState(CP)
self.VM = VehicleModel(CP)
# sending if read only is False
if sendcan is not None:
@ -163,7 +164,7 @@ class CarInterface(object):
ret.centerToFront = centerToFront_civic
ret.steerRatio = 13.0
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx']
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.]
@ -252,6 +253,8 @@ class CarInterface(object):
ret.steerLimitAlert = True
ret.startAccel = 0.5
ret.steerRateCost = 0.5
return ret
# returns a car.CarState
@ -270,6 +273,7 @@ class CarInterface(object):
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@ -370,11 +374,11 @@ class CarInterface(object):
else:
self.can_invalid_count = 0
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
elif self.CS.steer_not_allowed:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed:

0
selfdrive/car/mock/__init__.py 100644
View File

121
selfdrive/car/mock/interface.py 100755
View File

@ -0,0 +1,121 @@
#!/usr/bin/env python
import os
import time
import zmq
from common.realtime import sec_since_boot
import common.numpy_fast as np
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
# mocked car interface to work with chffrplus
TS = 0.01 # 100Hz
YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
# low pass gain
LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
class CarInterface(object):
def __init__(self, CP, sendcan=None):
self.CP = CP
print "Using Mock Car Interface"
context = zmq.Context()
# TODO: subscribe to phone sensor
self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port)
self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
self.speed = 0.
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
@staticmethod
def compute_gb(accel, speed):
return accel
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.0
@staticmethod
def get_params(candidate, fingerprint):
ret = car.CarParams.new_message()
ret.carName = "mock"
ret.radarName = "mock"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.noOutput
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
ret.mass = 1700.
ret.rotationalInertia = 2500.
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13. # reasonable
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
ret.steerRatioRear = 0.
ret.steerMaxBP = [0.]
ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [0.]
ret.longitudinalKpBP = [0.]
ret.longitudinalKpV = [0.]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.]
return ret
# returns a car.CarState
def update(self, c):
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
if sensors is not None:
for sensor in sensors.sensorEvents:
if sensor.type == 4: # gyro
self.yaw_rate_meas = -sensor.gyro.v[0]
gps = messaging.recv_sock(self.gps)
if gps is not None:
self.speed = gps.gpsLocation.speed
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.speed
ret.vEgoRaw = self.speed
ret.aEgo = 0.
self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
ret.yawRate = self.yaw_rate
ret.standstill = self.speed < 0.01
ret.wheelSpeeds.fl = self.speed
ret.wheelSpeeds.fr = self.speed
ret.wheelSpeeds.rl = self.speed
ret.wheelSpeeds.rr = self.speed
curvature = self.yaw_rate / max(self.speed, 1.)
ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
events = []
#events.append(create_event('passive', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
ret.events = events
return ret.as_reader()
def apply(self, c):
# in mock no carcontrols
return False

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

@ -6,7 +6,7 @@ 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_fcw_command
from selfdrive.car.toyota.values import CAR, ECU
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
@ -27,60 +27,6 @@ TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
0x383]
# addr, [ecu, bus, 1/freq*100, vl]
STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'),
0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
0x2E6: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
0x2E7: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
0x33E: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
0x365: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
0x366: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 40, '\x06\x00'),
0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
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'),
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'),
}
def check_ecu_msgs(fingerprint, candidate, ecu):
# return True if fingerprint contains messages normally sent by a given ecu
ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and
candidate in STATIC_MSGS[x][1] and
STATIC_MSGS[x][2] == 0)]
return any(msg for msg in fingerprint if msg in ecu_msgs)
def accel_hysteresis(accel, accel_steady, enabled):
# for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command

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

@ -1,5 +1,4 @@
import os
import time
import selfdrive.messaging as messaging
from selfdrive.car.toyota.values import CAR
from selfdrive.can.parser import CANParser
@ -19,7 +18,7 @@ def parse_gear_shifter(can_gear, car_fingerprint):
return "drive"
elif can_gear == 0x4:
return "brake"
elif car_fingerprint == CAR.RAV4:
elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]:
if can_gear == 0x20:
return "park"
elif can_gear == 0x10:
@ -47,6 +46,17 @@ def get_can_parser(CP):
(550, 40),
(581, 33)
]
elif CP.carFingerprint == CAR.RAV4H:
dbc_f = 'toyota_rav4_hybrid_2017_pt.dbc'
signals = [
("GEAR", 956, 0),
("BRAKE_PRESSED", 550, 0),
("GAS_PEDAL", 581, 0),
]
checks = [
(550, 40),
(581, 33)
]
elif CP.carFingerprint == CAR.RAV4:
dbc_f = 'toyota_rav4_2017_pt.dbc'
signals = [
@ -131,6 +141,10 @@ class CarState(object):
can_gear = cp.vl[295]['GEAR']
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
self.pedal_gas = cp.vl[581]['GAS_PEDAL']
elif self.car_fingerprint == CAR.RAV4H:
can_gear = cp.vl[956]['GEAR']
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
self.pedal_gas = cp.vl[581]['GAS_PEDAL']
elif self.car_fingerprint == CAR.RAV4:
can_gear = cp.vl[956]['GEAR']
self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']

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

@ -1,20 +1,24 @@
#!/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, 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.config import Conversions as CV
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.carstate import CarState, get_can_parser
from selfdrive.car.toyota.values import CAR, ECU, check_ecu_msgs
try:
from selfdrive.car.toyota.carcontroller import CarController
except ImportError:
CarController = None
class CarInterface(object):
def __init__(self, CP, sendcan=None):
self.CP = CP
self.VM = VehicleModel(CP)
self.frame = 0
self.can_invalid_count = 0
@ -70,9 +74,8 @@ class CarInterface(object):
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
stop_and_go = True
ret.mass = 3045./2.205 + std_cargo
ret.wheelbase = 2.70
ret.wheelbase = 2.70 if candidate == CAR.PRIUS else 2.65
ret.centerToFront = ret.wheelbase * 0.44
ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
ret.steerKp, ret.steerKi = 0.6, 0.05
@ -83,9 +86,9 @@ class CarInterface(object):
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
if candidate == CAR.PRIUS:
if candidate in [CAR.PRIUS, CAR.RAV4H]: # rav4 hybrid can do stop and go
ret.minEnableSpeed = -1.
elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go
elif candidate == CAR.RAV4: # TODO: hack ICE Rav4 to do stop and go
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
centerToRear = ret.wheelbase - ret.centerToFront
@ -131,6 +134,11 @@ class CarInterface(object):
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
if candidate == CAR.PRIUS:
ret.steerRateCost = 2.
elif candidate in [CAR.RAV4, CAR.RAV4H]:
ret.steerRateCost = 1.
return ret
# returns a car.CarState
@ -150,6 +158,7 @@ class CarInterface(object):
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
@ -180,7 +189,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 = 0.
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
if self.CP.carFingerprint == CAR.RAV4H:
# ignore standstill in hybrid rav4, since pcm allows to restart without
# receiving any special command
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
# TODO: button presses
buttonEvents = []
@ -223,8 +237,8 @@ class CarInterface(object):
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if self.CS.low_speed_lockout:
events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY]))
if self.CS.low_speed_lockout and self.CP.enableDsu:
events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if c.actuators.gas > 0.1:

62
selfdrive/car/toyota/values.py
View File

@ -1,8 +1,64 @@
class CAR:
PRIUS = "TOYOTA PRIUS 2017"
RAV4 = "TOYOTA RAV4 2017"
PRIUS = "TOYOTA PRIUS 2017"
RAV4H = "TOYOTA RAV4 2017 HYBRID"
RAV4 = "TOYOTA RAV4 2017"
class ECU:
CAM = 0 # camera
DSU = 1 # driving support unit
APGS = 2 # advanced parking guidance system
# addr, [ecu, bus, 1/freq*100, vl]
STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'),
0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
0x2E6: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
0x2E7: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
0x33E: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
0x365: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
0x366: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 40, '\x06\x00'),
0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
0x470: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 1, 100, '\x00\x00\x02\x7a'),
}
def check_ecu_msgs(fingerprint, candidate, ecu):
# return True if fingerprint contains messages normally sent by a given ecu
ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and
candidate in STATIC_MSGS[x][1] and
STATIC_MSGS[x][2] == 0)]
return any(msg for msg in fingerprint if msg in ecu_msgs)

2
selfdrive/common/version.h
View File

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

161
selfdrive/controls/controlsd.py
View File

@ -2,7 +2,7 @@
import json
from copy import copy
import zmq
from cereal import car
from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
@ -30,6 +30,7 @@ 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
State = log.Live100Data.ControlState
class Calibration:
UNCALIBRATED = 0
@ -37,38 +38,43 @@ class Calibration:
INVALID = 2
class State:
DISABLED = 'disabled'
ENABLED = 'enabled'
PRE_ENABLED = 'preEnabled'
SOFT_DISABLING = 'softDisabling'
# True when actuators are controlled
def isActive(state):
return state in [State.ENABLED, State.SOFT_DISABLING]
return state in [State.enabled, State.softDisabling]
# True if system is engaged
def isEnabled(state):
return (isActive(state) or state == State.PRE_ENABLED)
return (isActive(state) or state == State.preEnabled)
def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overtemp, free_space):
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
td = None
cal = None
hh = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
# *** thermal checking logic ***
# thermal data, checked every second
td = messaging.recv_sock(thermal)
if td is not None:
# overtemp above 95 deg
overtemp = any(
t > 950
for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
# CPU overtemp above 95 deg
overtemp_proc = any(t > 950 for t in
(td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu))
overtemp_bat = td.thermal.bat > 50000 # 50c
overtemp = overtemp_proc or overtemp_bat
# under 15% of space free no enable allowed
free_space = td.thermal.freeSpace < 0.15
@ -80,7 +86,6 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# *** read calibration status ***
cal = messaging.recv_sock(cal)
if cal is not None:
cal_status = cal.liveCalibration.calStatus
@ -91,11 +96,10 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# *** health checking logic ***
hh = messaging.recv_sock(health)
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed:
events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
return CS, events, cal_status, overtemp, free_space
@ -123,6 +127,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
v_cruise_kph_last = v_cruise_kph
for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise":
@ -138,76 +143,75 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
# ***** handle state transitions *****
# DISABLED
if state == State.DISABLED:
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.PRE_ENABLED
state = State.preEnabled
else:
state = State.ENABLED
state = State.enabled
AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# ENABLED
elif state == State.ENABLED:
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.SOFT_DISABLING
state = State.softDisabling
soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# SOFT DISABLING
elif state == State.SOFT_DISABLING:
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.DISABLED
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.ENABLED
state = State.enabled
elif soft_disable_timer <= 0:
state = State.DISABLED
state = State.disabled
# TODO: PRE ENABLING
elif state == State.PRE_ENABLED:
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.DISABLED
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.DISABLED
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.ENABLED
state = State.enabled
return state, soft_disable_timer, v_cruise_kph
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status,
PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# reset actuators to zero
@ -217,9 +221,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
active = isActive(state)
for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
@ -238,24 +239,20 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
# ***** state specific actions *****
# DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]:
awareness_status = 1.
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]:
elif state in [State.enabled, State.softDisabling]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on
# decrease awareness status
awareness_status -= 0.01/(AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
@ -286,6 +283,17 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# reset conditions for the 6 minutes timout
if CS.buttonEvents or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed or \
state in [State.preEnabled, State.disabled]:
awareness_status = 1.
# parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
AM.add(str(e) + "Permanent", enabled)
# *** process alerts ***
AM.process_alerts(sec_since_boot())
@ -293,7 +301,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate,
def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status,
LaC, LoC, angle_offset, passive):
@ -335,6 +343,8 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle
dat.live100.alertText1 = AM.alert_text_1
dat.live100.alertText2 = AM.alert_text_2
dat.live100.alertSize = AM.alert_size
dat.live100.alertStatus = AM.alert_status
dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0
# what packets were used to process
@ -349,6 +359,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
dat.live100.vEgo = CS.vEgo
dat.live100.vEgoRaw = CS.vEgoRaw
dat.live100.angleSteers = CS.steeringAngle
dat.live100.curvature = VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo)
dat.live100.steerOverride = CS.steeringPressed
# high level control state
@ -410,7 +421,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
def controlsd_thread(gctx, rate=100):
# start the loop
set_realtime_priority(2)
set_realtime_priority(3)
context = zmq.Context()
@ -429,9 +440,11 @@ def controlsd_thread(gctx, rate=100):
sendcan = None
# sub
thermal = messaging.sub_sock(context, service_list['thermal'].port)
health = messaging.sub_sock(context, service_list['health'].port)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port)
poller = zmq.Poller()
thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
@ -439,10 +452,7 @@ def controlsd_thread(gctx, rate=100):
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
if CI is None:
if passive:
return
else:
raise Exception("unsupported car")
raise Exception("unsupported car")
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
@ -461,7 +471,7 @@ def controlsd_thread(gctx, rate=100):
# write CarParams
params.put("CarParams", CP.to_bytes())
state = State.DISABLED
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
overtemp = False
@ -472,6 +482,7 @@ def controlsd_thread(gctx, rate=100):
# 0.0 - 1.0
awareness_status = 1.
v_cruise_kph_last = 0
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
@ -485,14 +496,14 @@ def controlsd_thread(gctx, rate=100):
except (ValueError, KeyError):
pass
prof = Profiler()
prof = Profiler(False) # off by default
while 1:
prof.reset() # avoid memory leak
prof.checkpoint("Ratekeeper", ignore=True) # rk is here
# sample data and compute car events
CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status,
CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status,
overtemp, free_space)
prof.checkpoint("Sample")
@ -502,24 +513,26 @@ def controlsd_thread(gctx, rate=100):
if not passive:
# update control state
state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer,
v_cruise_kph, AM)
prof.checkpoint("State transition")
# compute actuators
actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle)
v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM,
angle_offset, rear_view_allowed, rear_view_toggle)
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph,
rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed,
rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
# *** run loop at fixed rate ***
if rk.keep_time():
prof.display()
rk.keep_time()
prof.display()
def main(gctx=None):

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

@ -1,19 +1,25 @@
from cereal import car
from cereal import car, log
from selfdrive.swaglog import cloudlog
from common.realtime import sec_since_boot
import copy
# Priority
class PT:
class Priority:
HIGH = 3
MID = 2
LOW = 1
LOWEST = 0
AlertSize = log.Live100Data.AlertSize
AlertStatus = log.Live100Data.AlertStatus
class Alert(object):
def __init__(self,
alert_text_1,
alert_text_2,
alert_status,
alert_size,
alert_priority,
visual_alert,
audible_alert,
@ -23,6 +29,8 @@ class Alert(object):
self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2
self.alert_status = alert_status
self.alert_size = alert_size
self.alert_priority = alert_priority
self.visual_alert = visual_alert if visual_alert is not None else "none"
self.audible_alert = audible_alert if audible_alert is not None else "none"
@ -31,6 +39,8 @@ class Alert(object):
self.duration_hud_alert = duration_hud_alert
self.duration_text = duration_text
self.start_time = 0.
# typecheck that enums are valid on startup
tst = car.CarControl.new_message()
tst.hudControl.visualAlert = self.visual_alert
@ -51,347 +61,418 @@ class AlertManager(object):
"enable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
AlertStatus.normal, AlertSize.none,
Priority.MID, None, "beepSingle", .2, 0., 0.),
"disable": Alert(
"",
"",
PT.MID, None, "beepSingle", .2, 0., 0.),
AlertStatus.normal, AlertSize.none,
Priority.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert(
"Brake",
"Brake!",
"Risk of Collision",
PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
"steerSaturated": Alert(
"Take Control",
"Turn Exceeds Limit",
PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.),
"steerTempUnavailable": Alert(
"Take Control",
"Steer Temporarily Unavailable",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": Alert(
"Take Control",
"User Distracted",
PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.),
AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
"driverDistracted": Alert(
"Take Control to Regain Speed",
"User Distracted",
PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
"startup": Alert(
"Always Keep Hands on Wheel",
"Be Ready to Take Over Any Time",
PT.LOW, None, None, 0., 0., 15.),
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, None, None, 0., 0., 15.),
"ethicalDilemma": Alert(
"Take Control Immediately",
"Ethical Dilemma Detected",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Temporary Unavailable",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"manualRestart": Alert(
"Take Control",
"Resume Driving Manually",
PT.LOW, None, None, .0, 0., 1.),
AlertStatus.userPrompt, AlertSize.full,
Priority.LOW, None, None, 0., 0., .2),
# Non-entry only alerts
"wrongCarModeNoEntry": Alert(
"Comma Unavailable",
"Main Switch Off",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"dataNeededNoEntry": Alert(
"Comma Unavailable",
"Data needed for calibration. Upload drive, try again",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"outOfSpaceNoEntry": Alert(
"Comma Unavailable",
"Out of Space",
PT.LOW, None, "chimeDouble", .4, 0., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 0., 3.),
"pedalPressedNoEntry": Alert(
"Comma Unavailable",
"Pedal Pressed",
PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.),
"speedTooLowNoEntry": Alert(
"Comma Unavailable",
"Speed Too Low",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeHoldNoEntry": Alert(
"Comma Unavailable",
"Brake Hold Active",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"parkBrakeNoEntry": Alert(
"Comma Unavailable",
"Park Brake Engaged",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"lowSpeedLockoutNoEntry": Alert(
"Comma Unavailable",
"Cruise Fault: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing disabling
# Cancellation alerts causing soft disabling
"overheat": Alert(
"Take Control Immediately",
"System Overheated",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"wrongGear": Alert(
"Take Control Immediately",
"Gear not D",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInvalid": Alert(
"Take Control Immediately",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"Calibration Invalid: Reposition EON and Recalibrate",
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"calibrationInProgress": Alert(
"Take Control Immediately",
"Calibration in Progress",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"doorOpen": Alert(
"Take Control Immediately",
"Door Open",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"seatbeltNotLatched": Alert(
"Take Control Immediately",
"Seatbelt Unlatched",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
"espDisabled": Alert(
"Take Control Immediately",
"ESP Off",
PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
# Cancellation alerts causing immediate disabling
"radarCommIssue": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"radarFault": Alert(
"Take Control Immediately",
"Radar Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"modelCommIssue": Alert(
"Take Control Immediately",
"Model Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"controlsFailed": Alert(
"Take Control Immediately",
"Controls Failed",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"controlsMismatch": Alert(
"Take Control Immediately",
"Controls Mismatch",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"commIssue": Alert(
"Take Control Immediately",
"CAN Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"steerUnavailable": Alert(
"Take Control Immediately",
"Steer Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"Steer Fault: Restart the Car",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"brakeUnavailable": Alert(
"Take Control Immediately",
"Brake Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"Brake Fault: Restart the Car",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"gasUnavailable": Alert(
"Take Control Immediately",
"Gas Error: Restart the Car",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
"Gas Fault: Restart the Car",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"reverseGear": Alert(
"Take Control Immediately",
"Reverse Gear",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"cruiseDisabled": Alert(
"Take Control Immediately",
"Cruise Is Off",
PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.),
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
# not loud cancellations (user is in control)
"noTarget": Alert(
"Comma Canceled",
"No Close Lead",
PT.HIGH, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
"speedTooLow": Alert(
"Comma Canceled",
"Speed Too Low",
PT.HIGH, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.HIGH, None, "chimeDouble", .4, 2., 3.),
# Cancellation alerts causing non-entry
"overheatNoEntry": Alert(
"Comma Unavailable",
"System Overheated",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"wrongGearNoEntry": Alert(
"Comma Unavailable",
"Gear not D",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInvalidNoEntry": Alert(
"Comma Unavailable",
"Calibration Invalid: Reposition Neo and Recalibrate",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"Calibration Invalid: Reposition EON and Recalibrate",
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"calibrationInProgressNoEntry": Alert(
"Comma Unavailable",
"Calibration in Progress",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"doorOpenNoEntry": Alert(
"Comma Unavailable",
"Door Open",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"seatbeltNotLatchedNoEntry": Alert(
"Comma Unavailable",
"Seatbelt Unlatched",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"espDisabledNoEntry": Alert(
"Comma Unavailable",
"ESP Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"radarCommIssueNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"radarFaultNoEntry": Alert(
"Comma Unavailable",
"Radar Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"modelCommIssueNoEntry": Alert(
"Comma Unavailable",
"Model Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsFailedNoEntry": Alert(
"Comma Unavailable",
"Controls Failed",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"controlsMismatchNoEntry": Alert(
"Comma Unavailable",
"Controls Mismatch",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"commIssueNoEntry": Alert(
"Comma Unavailable",
"CAN Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"steerUnavailableNoEntry": Alert(
"Comma Unavailable",
"Steer Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"Steer Fault: Restart the Car",
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"brakeUnavailableNoEntry": Alert(
"Comma Unavailable",
"Brake Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
"Brake Fault: Restart the Car",
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"gasUnavailableNoEntry": Alert(
"Comma Unavailable",
"Gas Error: Restart the Car",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"reverseGearNoEntry": Alert(
"Comma Unavailable",
"Reverse Gear",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"cruiseDisabledNoEntry": Alert(
"Comma Unavailable",
"Cruise is Off",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"noTargetNoEntry": Alert(
"Comma Unavailable",
"No Close Lead",
PT.LOW, None, "chimeDouble", .4, 2., 3.),
AlertStatus.normal, AlertSize.full,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
# permanent alerts to display on small UI upper box
"steerUnavailablePermanent": Alert(
"STEER FAULT",
"RESTART THE CAR",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, None, None, 0., 0., .2),
"brakeUnavailablePermanent": Alert(
"BRAKE FAULT",
"RESTART THE CAR",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, None, None, 0., 0., .2),
"lowSpeedLockoutPermanent": Alert(
"CRUISE FAULT",
"RESTART THE CAR",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, None, None, 0., 0., .2),
}
def __init__(self):
self.activealerts = []
self.current_alert = None
def alertPresent(self):
return len(self.activealerts) > 0
def add(self, alert_type, enabled=True, extra_text=''):
alert_type = str(alert_type)
this_alert = copy.copy(self.alerts[alert_type])
this_alert.alert_text_2 += extra_text
added_alert = copy.copy(self.alerts[alert_type])
added_alert.alert_text_2 += extra_text
added_alert.start_time = sec_since_boot()
# if new alert is higher priority, log it
if self.current_alert is None or this_alert > self.current_alert:
if not self.alertPresent() or \
added_alert.alert_priority > self.activealerts[0].alert_priority:
cloudlog.event('alert_add',
alert_type=alert_type,
enabled=enabled)
self.activealerts.append(this_alert)
self.activealerts.sort()
self.activealerts.append(added_alert)
self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True)
# TODO: cycle through alerts?
def process_alerts(self, cur_time):
if self.alertPresent():
self.alert_start_time = cur_time
self.current_alert = self.activealerts[0]
print self.current_alert
# first get rid of all the expired alerts
self.activealerts = [a for a in self.activealerts if a.start_time +
max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time]
ca = self.activealerts[0] if self.alertPresent() else None
# start with assuming no alerts
self.alert_text_1 = ""
self.alert_text_2 = ""
self.alert_status = AlertStatus.normal
self.alert_size = AlertSize.none
self.visual_alert = "none"
self.audible_alert = "none"
if self.current_alert is not None:
# ewwwww
if self.alert_start_time + self.current_alert.duration_sound > cur_time:
self.audible_alert = self.current_alert.audible_alert
if ca:
if ca.start_time + ca.duration_sound > cur_time:
self.audible_alert = ca.audible_alert
if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time:
self.visual_alert = self.current_alert.visual_alert
if ca.start_time + ca.duration_hud_alert > cur_time:
self.visual_alert = ca.visual_alert
if self.alert_start_time + self.current_alert.duration_text > cur_time:
self.alert_text_1 = self.current_alert.alert_text_1
self.alert_text_2 = self.current_alert.alert_text_2
# disable current alert
if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert,
self.current_alert.duration_text) < cur_time:
self.current_alert = None
# reset
self.activealerts = []
if ca.start_time + ca.duration_text > cur_time:
self.alert_text_1 = ca.alert_text_1
self.alert_text_2 = ca.alert_text_2
self.alert_status = ca.alert_status
self.alert_size = ca.alert_size

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

@ -10,6 +10,7 @@ class EventTypes:
USER_DISABLE = 'userDisable'
SOFT_DISABLE = 'softDisable'
IMMEDIATE_DISABLE = 'immediateDisable'
PERMANENT = 'permanent'
def create_event(name, types):

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

@ -8,7 +8,7 @@ from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
ACTUATORS_DELAY = 0.2
ACTUATORS_DELAY = 0.1
_DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz
@ -28,11 +28,11 @@ class LatControl(object):
def __init__(self, VM):
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc()
self.setup_mpc(VM.CP.steerRateCost)
def setup_mpc(self):
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init()
self.libmpc.init(steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
@ -83,7 +83,7 @@ class LatControl(object):
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if nans:
self.libmpc.init()
self.libmpc.init(VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
@ -95,7 +95,10 @@ class LatControl(object):
self.pid.reset()
else:
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)
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = self.angle_steers_des_mpc
steers_max = get_steer_max(VM.CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max

38
selfdrive/controls/lib/lateral_mpc/generator.cpp
View File

@ -72,14 +72,12 @@ int main( )
// Angular rate error
h << (v_ref + 1.0 ) * t;
DMatrix Q(5,5);
Q(0,0) = 1.0;
Q(1,1) = 1.0;
Q(2,2) = 1.0;
Q(3,3) = 1.0;
Q(4,4) = 1.0;
BMatrix Q(5,5); Q.setAll(true);
// Q(0,0) = 1.0;
// Q(1,1) = 1.0;
// Q(2,2) = 1.0;
// Q(3,3) = 1.0;
// Q(4,4) = 2.0;
// Terminal cost
Function hN;
@ -92,18 +90,27 @@ int main( )
// Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
DMatrix QN(4,4);
QN(0,0) = 1.0;
QN(1,1) = 1.0;
QN(2,2) = 1.0;
BMatrix QN(4,4); QN.setAll(true);
// QN(0,0) = 1.0;
// QN(1,1) = 1.0;
// QN(2,2) = 1.0;
// QN(3,3) = 1.0;
QN(3,3) = 1.0;
// Non uniform time grid
// First 5 timesteps are 0.05, after that it's 0.15
DMatrix numSteps(20, 1);
for (int i = 0; i < 5; i++){
numSteps(i) = 1;
}
for (int i = 5; i < 20; i++){
numSteps(i) = 3;
}
// Setup Optimal Control Problem
const double tStart = 0.0;
const double tEnd = samplingTime * controlHorizon;
const double tEnd = 2.5;
OCP ocp( tStart, tEnd, controlHorizon );
OCP ocp( tStart, tEnd, numSteps);
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
@ -120,6 +127,7 @@ int main( )
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
mpc.set( MAX_NUM_QP_ITERATIONS, 500);
mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES );

13
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
View File

@ -1,11 +1,12 @@
import os
import sys
import subprocess
from cffi import FFI
mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
ffi = FFI()
ffi.cdef("""
@ -14,13 +15,13 @@ typedef struct {
} state_t;
typedef struct {
double x[50];
double y[50];
double psi[50];
double delta[50];
double x[20];
double y[20];
double psi[20];
double delta[20];
} log_t;
void init();
void init(double steer_rate_cost);
int run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double p_poly[4],
double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);

18
selfdrive/controls/lib/lateral_mpc/mpc.c
View File

@ -28,7 +28,7 @@ typedef struct {
double delta[N];
} log_t;
void init(){
void init(double steerRateCost){
acado_initializeSolver();
int i;
@ -42,6 +42,22 @@ void init(){
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
for (i = 0; i < N; i++) {
int f = 1;
if (i > 4){
f = 3;
}
acadoVariables.W[25 * i + 0] = 1.0 * f;
acadoVariables.W[25 * i + 6] = 1.0 * f;
acadoVariables.W[25 * i + 12] = 1.0 * f;
acadoVariables.W[25 * i + 18] = 1.0 * f;
acadoVariables.W[25 * i + 24] = steerRateCost * f;
}
acadoVariables.WN[0] = 1.0;
acadoVariables.WN[5] = 1.0;
acadoVariables.WN[10] = 1.0;
acadoVariables.WN[15] = 1.0;
}
int run_mpc(state_t * x0, log_t * solution,

140
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h
View File

@ -62,7 +62,7 @@ extern "C"
/** Indicator for fixed initial state. */
#define ACADO_INITIAL_STATE_FIXED 1
/** Number of control/estimation intervals. */
#define ACADO_N 50
#define ACADO_N 20
/** Number of online data values. */
#define ACADO_NOD 18
/** Number of path constraints. */
@ -80,9 +80,7 @@ extern "C"
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 4
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 54
/** Number of integration steps per shooting interval. */
#define ACADO_RK_NIS 1
#define ACADO_QP_NV 24
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
@ -90,7 +88,7 @@ extern "C"
/** 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
#define ACADO_WEIGHTING_MATRICES_TYPE 2
/*
@ -104,36 +102,42 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 51 x 4 (row major format)
/** Matrix of size: 21 x 4 (row major format)
*
* Matrix containing 51 differential variable vectors.
* Matrix containing 21 differential variable vectors.
*/
real_t x[ 204 ];
real_t x[ 84 ];
/** Column vector of size: 50
/** Column vector of size: 20
*
* Matrix containing 50 control variable vectors.
* Matrix containing 20 control variable vectors.
*/
real_t u[ 50 ];
real_t u[ 20 ];
/** Matrix of size: 51 x 18 (row major format)
/** Matrix of size: 21 x 18 (row major format)
*
* Matrix containing 51 online data vectors.
* Matrix containing 21 online data vectors.
*/
real_t od[ 918 ];
real_t od[ 378 ];
/** Column vector of size: 250
/** Column vector of size: 100
*
* Matrix containing 50 reference/measurement vectors of size 5 for first 50 nodes.
* Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes.
*/
real_t y[ 250 ];
real_t y[ 100 ];
/** Column vector of size: 4
*
* Reference/measurement vector for the 51. node.
* Reference/measurement vector for the 21. node.
*/
real_t yN[ 4 ];
/** Matrix of size: 100 x 5 (row major format) */
real_t W[ 500 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t WN[ 16 ];
/** Column vector of size: 4
*
* Current state feedback vector.
@ -165,20 +169,20 @@ real_t rk_kkk[ 96 ];
/** Row vector of size: 43 */
real_t state[ 43 ];
/** Column vector of size: 200 */
real_t d[ 200 ];
/** Column vector of size: 80 */
real_t d[ 80 ];
/** Column vector of size: 250 */
real_t Dy[ 250 ];
/** Column vector of size: 100 */
real_t Dy[ 100 ];
/** Column vector of size: 4 */
real_t DyN[ 4 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t evGx[ 800 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t evGx[ 320 ];
/** Column vector of size: 200 */
real_t evGu[ 200 ];
/** Column vector of size: 80 */
real_t evGu[ 80 ];
/** Column vector of size: 19 */
real_t objAuxVar[ 19 ];
@ -189,17 +193,20 @@ real_t objValueIn[ 23 ];
/** Row vector of size: 30 */
real_t objValueOut[ 30 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t Q1[ 800 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t Q1[ 320 ];
/** Matrix of size: 200 x 5 (row major format) */
real_t Q2[ 1000 ];
/** Matrix of size: 80 x 5 (row major format) */
real_t Q2[ 400 ];
/** Column vector of size: 50 */
real_t R1[ 50 ];
/** Column vector of size: 20 */
real_t R1[ 20 ];
/** Matrix of size: 50 x 5 (row major format) */
real_t R2[ 250 ];
/** Matrix of size: 20 x 5 (row major format) */
real_t R2[ 100 ];
/** Column vector of size: 80 */
real_t S1[ 80 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t QN1[ 16 ];
@ -213,50 +220,50 @@ real_t Dx0[ 4 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t T[ 16 ];
/** Column vector of size: 5100 */
real_t E[ 5100 ];
/** Column vector of size: 840 */
real_t E[ 840 ];
/** Column vector of size: 5100 */
real_t QE[ 5100 ];
/** Column vector of size: 840 */
real_t QE[ 840 ];
/** Matrix of size: 200 x 4 (row major format) */
real_t QGx[ 800 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t QGx[ 320 ];
/** Column vector of size: 200 */
real_t Qd[ 200 ];
/** Column vector of size: 80 */
real_t Qd[ 80 ];
/** Column vector of size: 204 */
real_t QDy[ 204 ];
/** Column vector of size: 84 */
real_t QDy[ 84 ];
/** Matrix of size: 50 x 4 (row major format) */
real_t H10[ 200 ];
/** Matrix of size: 20 x 4 (row major format) */
real_t H10[ 80 ];
/** Matrix of size: 54 x 54 (row major format) */
real_t H[ 2916 ];
/** Matrix of size: 24 x 24 (row major format) */
real_t H[ 576 ];
/** Matrix of size: 100 x 54 (row major format) */
real_t A[ 5400 ];
/** Matrix of size: 40 x 24 (row major format) */
real_t A[ 960 ];
/** Column vector of size: 54 */
real_t g[ 54 ];
/** Column vector of size: 24 */
real_t g[ 24 ];
/** Column vector of size: 54 */
real_t lb[ 54 ];
/** Column vector of size: 24 */
real_t lb[ 24 ];
/** Column vector of size: 54 */
real_t ub[ 54 ];
/** Column vector of size: 24 */
real_t ub[ 24 ];
/** Column vector of size: 100 */
real_t lbA[ 100 ];
/** Column vector of size: 40 */
real_t lbA[ 40 ];
/** Column vector of size: 100 */
real_t ubA[ 100 ];
/** Column vector of size: 40 */
real_t ubA[ 40 ];
/** Column vector of size: 54 */
real_t x[ 54 ];
/** Column vector of size: 24 */
real_t x[ 24 ];
/** Column vector of size: 154 */
real_t y[ 154 ];
/** Column vector of size: 64 */
real_t y[ 64 ];
} ACADOworkspace;
@ -270,10 +277,11 @@ real_t y[ 154 ];
*
* \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.
* \param rk_index Number of the shooting interval.
*
* \return Status code of the integrator.
*/
int acado_integrate( real_t* const rk_eta, int resetIntegrator );
int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
/** Export of an ACADO symbolic function.
*
@ -306,7 +314,7 @@ 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 strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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.
*/

198
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c
View File

@ -72,11 +72,13 @@ out[23] = (real_t)(1.0000000000000000e+00);
}
/* Fixed step size:0.05 */
int acado_integrate( real_t* const rk_eta, int resetIntegrator )
int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
{
int error;
int run1;
int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
int numInts = numSteps[rk_index];
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[4] = 1.0000000000000000e+00;
rk_eta[5] = 0.0000000000000000e+00;
@ -120,6 +122,7 @@ acadoWorkspace.rk_xxx[42] = rk_eta[42];
for (run1 = 0; run1 < 1; ++run1)
{
for(run1 = 0; run1 < numInts; run1++ ) {
acadoWorkspace.rk_xxx[0] = + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + rk_eta[2];
@ -145,107 +148,108 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
rk_eta[0] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[72];
rk_eta[1] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[73];
rk_eta[2] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[74];
rk_eta[3] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[75];
rk_eta[4] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[76];
rk_eta[5] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[77];
rk_eta[6] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[78];
rk_eta[7] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[79];
rk_eta[8] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[80];
rk_eta[9] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[81];
rk_eta[10] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[82];
rk_eta[11] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[83];
rk_eta[12] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[84];
rk_eta[13] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[85];
rk_eta[14] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[86];
rk_eta[15] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[87];
rk_eta[16] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[88];
rk_eta[17] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[89];
rk_eta[18] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[90];
rk_eta[19] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[91];
rk_eta[20] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[92];
rk_eta[21] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[93];
rk_eta[22] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[94];
rk_eta[23] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[95];
rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72];
rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73];
rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74];
rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75];
rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76];
rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77];
rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78];
rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79];
rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80];
rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81];
rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82];
rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83];
rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84];
rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85];
rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86];
rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87];
rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88];
rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89];
rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90];
rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91];
rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92];
rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93];
rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94];
rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
}
error = 0;
return error;
}

2
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp
View File

@ -40,7 +40,7 @@ int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(54, 100);
QProblem qp(24, 40);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);

4
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp
View File

@ -37,9 +37,9 @@
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 54
#define QPOASES_NVMAX 24
/** Maximum number of constraints. */
#define QPOASES_NCMAX 100
#define QPOASES_NCMAX 40
/** Maximum number of working set recalculations. */
#define QPOASES_NWSRMAX 500
/** Print level for qpOASES. */

4947
selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
View File

File diff suppressed because one or more lines are too long

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

@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.pid import PIController
STOPPING_EGO_SPEED = 0.5
MIN_CAN_SPEED = 0.3
MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface
STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
STARTING_TARGET_SPEED = 0.5
BRAKE_THRESHOLD_TO_PID = 0.2
@ -101,7 +101,7 @@ class LongControl(object):
elif self.long_control_state == LongCtrlState.pid:
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.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)

30
selfdrive/controls/lib/longitudinal_mpc/generator.cpp
View File

@ -44,11 +44,8 @@ int main( )
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;
// Weights are defined in mpc.
BMatrix Q(4,4); Q.setAll(true);
// Terminal cost
Function hN;
@ -56,16 +53,24 @@ int main( )
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;
// Weights are defined in mpc.
BMatrix QN(3,3); QN.setAll(true);
// Non uniform time grid
// First 5 timesteps are 0.2, after that it's 0.6
DMatrix numSteps(20, 1);
for (int i = 0; i < 5; i++){
numSteps(i) = 1;
}
for (int i = 5; i < 20; i++){
numSteps(i) = 3;
}
// Setup Optimal Control Problem
const double tStart = 0.0;
const double tEnd = samplingTime * controlHorizon;
const double tEnd = 10.0;
OCP ocp( tStart, tEnd, controlHorizon );
OCP ocp( tStart, tEnd, numSteps);
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
@ -78,8 +83,9 @@ int main( )
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( NUM_INTEGRATOR_STEPS, controlHorizon);
mpc.set( MAX_NUM_QP_ITERATIONS, 500);
mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
mpc.set( QP_SOLVER, QP_QPOASES );

17
selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
View File

@ -1,10 +1,11 @@
import os
import sys
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)
subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir)
def _get_libmpc(mpc_id):
@ -18,13 +19,13 @@ def _get_libmpc(mpc_id):
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];
double x_ego[20];
double v_ego[20];
double a_ego[20];
double j_ego[20];
double x_l[20];
double v_l[20];
double a_l[20];
} log_t;
void init();

19
selfdrive/controls/lib/longitudinal_mpc/mpc.c
View File

@ -45,6 +45,22 @@ void init(){
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
// Set weights
for (i = 0; i < N; i++) {
int f = 1;
if (i > 4){
f = 3;
}
acadoVariables.W[16 * i + 0] = 5.0 * f; // exponential cost for danger zone
acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance
acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration
acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk
}
acadoVariables.WN[0] = 5.0; // exponential cost for danger zone
acadoVariables.WN[4] = 0.1; // desired distance
acadoVariables.WN[8] = 10.0; // acceleration
}
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){
@ -58,6 +74,9 @@ void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, doub
double dt = 0.2;
for (i = 0; i < N + 1; ++i){
if (i > 4){
dt = 0.6;
}
acadoVariables.x[i*NX] = x_ego;
acadoVariables.x[i*NX+1] = v_ego;
acadoVariables.x[i*NX+2] = a_ego;

141
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h
View File

@ -62,7 +62,7 @@ extern "C"
/** Indicator for fixed initial state. */
#define ACADO_INITIAL_STATE_FIXED 1
/** Number of control/estimation intervals. */
#define ACADO_N 50
#define ACADO_N 20
/** Number of online data values. */
#define ACADO_NOD 1
/** Number of path constraints. */
@ -80,9 +80,7 @@ extern "C"
/** 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
#define ACADO_QP_NV 26
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
@ -90,7 +88,7 @@ extern "C"
/** 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
#define ACADO_WEIGHTING_MATRICES_TYPE 2
/*
@ -104,36 +102,42 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 51 x 6 (row major format)
/** Matrix of size: 21 x 6 (row major format)
*
* Matrix containing 51 differential variable vectors.
* Matrix containing 21 differential variable vectors.
*/
real_t x[ 306 ];
real_t x[ 126 ];
/** Column vector of size: 50
/** Column vector of size: 20
*
* Matrix containing 50 control variable vectors.
* Matrix containing 20 control variable vectors.
*/
real_t u[ 50 ];
real_t u[ 20 ];
/** Column vector of size: 51
/** Column vector of size: 21
*
* Matrix containing 51 online data vectors.
* Matrix containing 21 online data vectors.
*/
real_t od[ 51 ];
real_t od[ 21 ];
/** Column vector of size: 200
/** Column vector of size: 80
*
* Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes.
* Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes.
*/
real_t y[ 200 ];
real_t y[ 80 ];
/** Column vector of size: 3
*
* Reference/measurement vector for the 51. node.
* Reference/measurement vector for the 21. node.
*/
real_t yN[ 3 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t W[ 320 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t WN[ 9 ];
/** Column vector of size: 6
*
* Current state feedback vector.
@ -162,20 +166,20 @@ 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: 120 */
real_t d[ 120 ];
/** Column vector of size: 200 */
real_t Dy[ 200 ];
/** Column vector of size: 80 */
real_t Dy[ 80 ];
/** Column vector of size: 3 */
real_t DyN[ 3 ];
/** Matrix of size: 300 x 6 (row major format) */
real_t evGx[ 1800 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t evGx[ 720 ];
/** Column vector of size: 300 */
real_t evGu[ 300 ];
/** Column vector of size: 120 */
real_t evGu[ 120 ];
/** Column vector of size: 32 */
real_t objAuxVar[ 32 ];
@ -186,20 +190,20 @@ 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: 120 x 6 (row major format) */
real_t Q1[ 720 ];
/** Matrix of size: 300 x 4 (row major format) */
real_t Q2[ 1200 ];
/** Matrix of size: 120 x 4 (row major format) */
real_t Q2[ 480 ];
/** Column vector of size: 50 */
real_t R1[ 50 ];
/** Column vector of size: 20 */
real_t R1[ 20 ];
/** Matrix of size: 50 x 4 (row major format) */
real_t R2[ 200 ];
/** Matrix of size: 20 x 4 (row major format) */
real_t R2[ 80 ];
/** Column vector of size: 300 */
real_t S1[ 300 ];
/** Column vector of size: 120 */
real_t S1[ 120 ];
/** Matrix of size: 6 x 6 (row major format) */
real_t QN1[ 36 ];
@ -213,50 +217,50 @@ 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: 1260 */
real_t E[ 1260 ];
/** Column vector of size: 7650 */
real_t QE[ 7650 ];
/** Column vector of size: 1260 */
real_t QE[ 1260 ];
/** Matrix of size: 300 x 6 (row major format) */
real_t QGx[ 1800 ];
/** Matrix of size: 120 x 6 (row major format) */
real_t QGx[ 720 ];
/** Column vector of size: 300 */
real_t Qd[ 300 ];
/** Column vector of size: 120 */
real_t Qd[ 120 ];
/** Column vector of size: 306 */
real_t QDy[ 306 ];
/** Column vector of size: 126 */
real_t QDy[ 126 ];
/** Matrix of size: 50 x 6 (row major format) */
real_t H10[ 300 ];
/** Matrix of size: 20 x 6 (row major format) */
real_t H10[ 120 ];
/** Matrix of size: 56 x 56 (row major format) */
real_t H[ 3136 ];
/** Matrix of size: 26 x 26 (row major format) */
real_t H[ 676 ];
/** Matrix of size: 50 x 56 (row major format) */
real_t A[ 2800 ];
/** Matrix of size: 20 x 26 (row major format) */
real_t A[ 520 ];
/** Column vector of size: 56 */
real_t g[ 56 ];
/** Column vector of size: 26 */
real_t g[ 26 ];
/** Column vector of size: 56 */
real_t lb[ 56 ];
/** Column vector of size: 26 */
real_t lb[ 26 ];
/** Column vector of size: 56 */
real_t ub[ 56 ];
/** Column vector of size: 26 */
real_t ub[ 26 ];
/** Column vector of size: 50 */
real_t lbA[ 50 ];
/** Column vector of size: 20 */
real_t lbA[ 20 ];
/** Column vector of size: 50 */
real_t ubA[ 50 ];
/** Column vector of size: 20 */
real_t ubA[ 20 ];
/** Column vector of size: 56 */
real_t x[ 56 ];
/** Column vector of size: 26 */
real_t x[ 26 ];
/** Column vector of size: 106 */
real_t y[ 106 ];
/** Column vector of size: 46 */
real_t y[ 46 ];
} ACADOworkspace;
@ -270,10 +274,11 @@ real_t y[ 106 ];
*
* \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.
* \param rk_index Number of the shooting interval.
*
* \return Status code of the integrator.
*/
int acado_integrate( real_t* const rk_eta, int resetIntegrator );
int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
/** Export of an ACADO symbolic function.
*
@ -306,7 +311,7 @@ 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 strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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.
*/

390
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c
View File

@ -78,11 +78,13 @@ 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 acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
{
int error;
int run1;
int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
int numInts = numSteps[rk_index];
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[6] = 1.0000000000000000e+00;
rk_eta[7] = 0.0000000000000000e+00;
@ -131,6 +133,7 @@ acadoWorkspace.rk_xxx[49] = rk_eta[49];
for (run1 = 0; run1 < 1; ++run1)
{
for(run1 = 0; run1 < numInts; run1++ ) {
acadoWorkspace.rk_xxx[0] = + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + rk_eta[2];
@ -180,203 +183,204 @@ 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];
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*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];
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[72] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[73] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[74] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[75] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[76] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[77] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[78] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[79] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[80] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[81] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[82] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[83] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[84] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[85] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[86] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[87] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[88] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[89] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[90] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[91] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[92] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[93] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[94] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*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];
acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
acadoWorkspace.rk_xxx[24] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
acadoWorkspace.rk_xxx[25] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
acadoWorkspace.rk_xxx[26] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
acadoWorkspace.rk_xxx[27] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
acadoWorkspace.rk_xxx[28] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
acadoWorkspace.rk_xxx[29] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
acadoWorkspace.rk_xxx[30] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
acadoWorkspace.rk_xxx[31] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
acadoWorkspace.rk_xxx[32] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
acadoWorkspace.rk_xxx[33] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
acadoWorkspace.rk_xxx[34] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
acadoWorkspace.rk_xxx[35] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
acadoWorkspace.rk_xxx[36] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
acadoWorkspace.rk_xxx[37] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
acadoWorkspace.rk_xxx[38] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
acadoWorkspace.rk_xxx[39] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
acadoWorkspace.rk_xxx[40] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
acadoWorkspace.rk_xxx[41] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
acadoWorkspace.rk_xxx[42] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
acadoWorkspace.rk_xxx[43] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
acadoWorkspace.rk_xxx[44] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
acadoWorkspace.rk_xxx[45] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
acadoWorkspace.rk_xxx[46] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
acadoWorkspace.rk_xxx[47] = + (real_t)1.9999999999999993e-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];
rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[144];
rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[145];
rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[146];
rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[147];
rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[148];
rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[149];
rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[150];
rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[151];
rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[152];
rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[153];
rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[154];
rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[155];
rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[156];
rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[157];
rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[158];
rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[159];
rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[160];
rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[161];
rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[162];
rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[163];
rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[164];
rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[165];
rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[166];
rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[167];
rk_eta[24] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[168];
rk_eta[25] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[169];
rk_eta[26] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[170];
rk_eta[27] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[171];
rk_eta[28] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[172];
rk_eta[29] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[173];
rk_eta[30] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[174];
rk_eta[31] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[175];
rk_eta[32] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[176];
rk_eta[33] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[177];
rk_eta[34] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[178];
rk_eta[35] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[179];
rk_eta[36] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[180];
rk_eta[37] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[181];
rk_eta[38] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[182];
rk_eta[39] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[183];
rk_eta[40] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[184];
rk_eta[41] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[185];
rk_eta[42] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[186];
rk_eta[43] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[187];
rk_eta[44] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[188];
rk_eta[45] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[189];
rk_eta[46] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[190];
rk_eta[47] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[191];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
}
error = 0;
return error;
}

2
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp
View File

@ -40,7 +40,7 @@ int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(56, 50);
QProblem qp(26, 20);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);

4
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp
View File

@ -37,9 +37,9 @@
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 56
#define QPOASES_NVMAX 26
/** Maximum number of constraints. */
#define QPOASES_NCMAX 50
#define QPOASES_NCMAX 20
/** Maximum number of working set recalculations. */
#define QPOASES_NWSRMAX 500
/** Print level for qpOASES. */

5021
selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c
View File

File diff suppressed because one or more lines are too long

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

@ -16,7 +16,7 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.pathplanner import PathPlanner
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
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
_DT = 0.01 # 100Hz
_DT_MPC = 0.2 # 5Hz
@ -255,8 +255,9 @@ class Planner(object):
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.poller = zmq.Poller()
self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller)
self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
@ -326,7 +327,15 @@ class Planner(object):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = messaging.recv_sock(self.model)
md = None
l20 = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
@ -334,7 +343,6 @@ class Planner(object):
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
@ -368,17 +376,21 @@ class Planner(object):
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
self.v_cruise = CS.vEgo
self.a_cruise = self.CP.startAccel if starting else a_ego
self.v_acc_start = CS.vEgo
self.a_acc_start = self.CP.startAccel if starting else a_ego
self.v_acc = CS.vEgo
self.a_acc = self.CP.startAccel if starting else a_ego
self.v_acc_sol = CS.vEgo
self.a_acc_sol = self.CP.startAccel if starting else a_ego
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
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)
@ -399,7 +411,7 @@ class Planner(object):
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered")
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True

15
selfdrive/controls/lib/speed_smoother.py
View File

@ -67,15 +67,20 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
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
if aPeak <= 0: # there is no solution, so stop after t1
t2 = t1 + ts + 1e-9
t3 = t2
else:
t2 = t1 + ts
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
t3 = t2 - aPeak / jMin
else:
t1 = (aPeak - aEgo) / jMax
t2 = t1
t3 = t2 - aPeak / jMin
t3 = t2 - aPeak / jMin
dt1 = min(ts, t1)
dt2 = max(min(ts, t2) - t1, 0.)

2
selfdrive/controls/lib/vehicle_model.py
View File

@ -89,6 +89,8 @@ class VehicleModel(object):
A, B = create_dyn_state_matrices(u, self)
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
def yaw_rate(self, sa, u):
return self.calc_curvature(sa, u) * u
if __name__ == '__main__':
from selfdrive.car.honda.interface import CarInterface

73
selfdrive/controls/radard.py
View File

@ -44,11 +44,12 @@ class EKFV1D(EKF):
# fuses camera and radar data for best lead detection
def radard_thread(gctx=None):
set_realtime_priority(1)
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
mocked= CP.radarName == "mock"
VM = VehicleModel(CP)
cloudlog.info("radard got CarParams")
@ -59,8 +60,9 @@ def radard_thread(gctx=None):
context = zmq.Context()
# *** subscribe to features and model from visiond
model = messaging.sub_sock(context, service_list['model'].port)
live100 = messaging.sub_sock(context, service_list['live100'].port)
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
PP = PathPlanner()
RI = RadarInterface()
@ -96,7 +98,6 @@ def radard_thread(gctx=None):
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
while 1:
rr = RI.update()
ar_pts = {}
@ -104,7 +105,15 @@ def radard_thread(gctx=None):
ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured]
# receive the live100s
l100 = messaging.recv_sock(live100)
l100 = None
md = None
for socket, event in poller.poll(0):
if socket is live100:
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
if l100 is not None:
active = l100.live100.active
v_ego = l100.live100.vEgo
@ -119,7 +128,6 @@ def radard_thread(gctx=None):
if v_ego is None:
continue
md = messaging.recv_sock(model)
if md is not None:
last_md_ts = md.logMonoTime
@ -140,9 +148,11 @@ def radard_thread(gctx=None):
del ar_pts[VISION_POINT]
# *** compute the likely path_y ***
if active and not steer_override: # use path from model path_poly
if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy)
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
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
# *** remove missing points from meta data ***
@ -152,8 +162,8 @@ def radard_thread(gctx=None):
# *** compute the tracks ***
for ids in ar_pts:
# ignore the vision point for now
if ids == VISION_POINT:
# ignore standalone vision point, unless we are mocking the radar
if ids == VISION_POINT and not mocked:
continue
rpt = ar_pts[ids]
@ -185,33 +195,11 @@ def radard_thread(gctx=None):
tracks[fused_id].vision_cnt += 1
tracks[fused_id].update_vision_fusion()
# publish tracks (debugging)
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
if DEBUG:
print "NEW CYCLE"
if VISION_POINT in ar_pts:
print "vision", ar_pts[VISION_POINT]
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
dat.liveTracks[cnt].stationary = tracks[ids].stationary
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
liveTracks.send(dat.to_bytes())
idens = tracks.keys()
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
@ -268,6 +256,27 @@ def radard_thread(gctx=None):
dat.live20.cumLagMs = -rk.remaining*1000.
live20.send(dat.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(tracks.keys()):
if DEBUG:
print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \
(ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel,
tracks[ids].dPath, tracks[ids].vLat,
tracks[ids].vLead, tracks[ids].vLeadK,
tracks[ids].aLeadK,
tracks[ids].stationary)
dat.liveTracks[cnt].trackId = ids
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
dat.liveTracks[cnt].stationary = tracks[ids].stationary
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
liveTracks.send(dat.to_bytes())
rk.monitor_time()
def main(gctx=None):

BIN
selfdrive/loggerd/loggerd
View File

Binary file not shown.

32
selfdrive/loggerd/uploader.py
View File

@ -9,6 +9,7 @@ import inspect
import requests
import traceback
import threading
import subprocess
from collections import Counter
from selfdrive.swaglog import cloudlog
@ -66,6 +67,13 @@ def clear_locks(root):
except OSError:
cloudlog.exception("clear_locks failed")
def is_on_wifi():
# ConnectivityManager.getActiveNetworkInfo()
result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n")
data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:])
return "\x00".join("WIFI") in data
class Uploader(object):
def __init__(self, dongle_id, access_token, root):
@ -111,21 +119,22 @@ class Uploader(object):
total_size += os.stat(fn).st_size
return dict(name_counts), total_size
def next_file_to_upload(self):
def next_file_to_upload(self, with_video):
# try to upload log files first
for name, key, fn in self.gen_upload_files():
if name in ["rlog", "rlog.bz2"]:
return (key, fn, 0)
# then upload compressed camera file
for name, key, fn in self.gen_upload_files():
if name in ["fcamera.hevc"]:
return (key, fn, 1)
if with_video:
# then upload compressed camera file
for name, key, fn in self.gen_upload_files():
if name in ["fcamera.hevc"]:
return (key, fn, 1)
# then upload other files
for name, key, fn in self.gen_upload_files():
if not name.endswith('.lock') and not name.endswith(".tmp"):
return (key, fn, 1)
# then upload other files
for name, key, fn in self.gen_upload_files():
if not name.endswith('.lock') and not name.endswith(".tmp"):
return (key, fn, 1)
return None
@ -240,13 +249,16 @@ def uploader_fn(exit_event):
uploader = Uploader(dongle_id, access_token, ROOT)
while True:
upload_video = (params.get("IsUploadVideoOverCellularEnabled") != "0") or is_on_wifi()
backoff = 0.1
while True:
if exit_event.is_set():
return
d = uploader.next_file_to_upload()
d = uploader.next_file_to_upload(upload_video)
if d is None:
break

190
selfdrive/manager.py
View File

@ -7,13 +7,9 @@ import errno
import signal
if __name__ == "__main__":
if os.path.isfile("/init.qcom.rc"):
# check if NEOS update is required
while ((not os.path.isfile("/VERSION")
or int(open("/VERSION").read()) < 3)
and not os.path.isfile("/data/media/0/noupdate")):
os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater")
time.sleep(10)
if os.path.isfile("/init.qcom.rc") \
and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 4):
raise Exception("NEOS outdated")
# get a non-blocking stdout
child_pid, child_pty = os.forkpty()
@ -44,6 +40,7 @@ if __name__ == "__main__":
os._exit(os.wait()[1])
import shutil
import hashlib
import importlib
import subprocess
@ -219,22 +216,13 @@ def kill_managed_process(name):
def cleanup_all_processes(signal, frame):
cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
manage_baseui(False)
for p in ("com.waze", "com.spotify.music", "ai.comma.plus.offroad", "ai.comma.plus.frame"):
system("am force-stop %s" % p)
for name in running.keys():
kill_managed_process(name)
sys.exit(0)
baseui_running = False
def manage_baseui(start):
global baseui_running
if start and not baseui_running:
cloudlog.info("starting baseui")
os.system("am start -n com.baseui/.MainActivity")
baseui_running = True
elif not start and baseui_running:
cloudlog.info("stopping baseui")
os.system("am force-stop com.baseui")
baseui_running = False
cloudlog.info("everything is dead")
# ****************** run loop ******************
@ -252,7 +240,11 @@ def manager_init():
cloudlog.info("dongle id is " + dongle_id)
os.environ['DONGLE_ID'] = dongle_id
dirty = subprocess.call(["git", "diff-index", "--quiet", "origin/release", "--"]) != 0
if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
upstream = "origin/master"
else:
upstream = "origin/release"
dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0
cloudlog.info("dirty is %d" % dirty)
if not dirty:
os.environ['CLEAN'] = '1'
@ -280,12 +272,9 @@ def system(cmd):
output=e.output[-1024:],
returncode=e.returncode)
# TODO: this is not proper gating for EON
try:
EON = os.path.exists("/EON")
if EON:
from smbus2 import SMBus
EON = True
except ImportError:
EON = False
def setup_eon_fan():
if not EON:
@ -322,8 +311,10 @@ _TEMP_THRS_H = [50., 65., 80., 10000]
_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000]
# fan speed options
_FAN_SPEEDS = [0, 16384, 32768, 65535]
# max fan speed only allowed if battery if hot
_BAT_TEMP_THERSHOLD = 45.
def handle_fan(max_temp, fan_speed):
def handle_fan(max_temp, bat_temp, fan_speed):
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp)
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp)
@ -334,6 +325,10 @@ def handle_fan(max_temp, fan_speed):
# update speed if using the low thresholds results in fan speed decrement
fan_speed = new_speed_l
if bat_temp < _BAT_TEMP_THERSHOLD:
# no max fan speed unless battery is hot
fan_speed = min(fan_speed, _FAN_SPEEDS[-2])
set_eon_fan(fan_speed/16384)
return fan_speed
@ -369,8 +364,6 @@ class LocationStarter(object):
return location.speed*3.6 > 10
def manager_thread():
global baseui_running
# now loop
context = zmq.Context()
thermal_sock = messaging.pub_sock(context, service_list['thermal'].port)
@ -383,7 +376,8 @@ def manager_thread():
for p in persistent_processes:
start_managed_process(p)
manage_baseui(True)
# start frame
system("am start -n ai.comma.plus.frame/.MainActivity")
# do this before panda flashing
setup_eon_fan()
@ -391,7 +385,9 @@ def manager_thread():
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
passive = bool(os.getenv("PASSIVE"))
params = Params()
passive = params.get("Passive") == "1"
passive_starter = LocationStarter()
started_ts = None
@ -399,6 +395,7 @@ def manager_thread():
count = 0
fan_speed = 0
ignition_seen = False
battery_was_high = False
health_sock.RCVTIMEO = 1500
@ -424,13 +421,19 @@ def manager_thread():
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/usb/online") as f:
msg.thermal.usbOnline = bool(int(f.read()))
# TODO: add car battery voltage check
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
fan_speed = handle_fan(max_temp, fan_speed)
bat_temp = msg.thermal.bat/1000.
fan_speed = handle_fan(max_temp, bat_temp, fan_speed)
msg.thermal.fanSpeed = fan_speed
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0))
thermal_sock.send(msg.to_bytes())
print msg
@ -448,8 +451,10 @@ def manager_thread():
ignition = td is not None and td.health.started
ignition_seen = ignition_seen or ignition
params = Params()
should_start = ignition and (params.get("HasAcceptedTerms") == "1")
do_uninstall = params.get("DoUninstall") == "1"
accepted_terms = params.get("HasAcceptedTerms") == "1"
should_start = ignition
# start on gps in passive mode
if passive and not ignition_seen:
@ -458,6 +463,15 @@ def manager_thread():
# with 2% left, we killall, otherwise the phone is bricked
should_start = should_start and avail > 0.02
# require usb power
should_start = should_start and msg.thermal.usbOnline
should_start = should_start and accepted_terms and (not do_uninstall)
# if any CPU gets above 107 or the battery gets above 53, kill all processes
# controls will warn with CPU above 95 or battery above 50
if max_temp > 107.0 or msg.thermal.bat >= 53000:
should_start = False
if should_start:
if not started_ts:
@ -468,20 +482,17 @@ def manager_thread():
kill_managed_process(p)
else:
start_managed_process(p)
manage_baseui(False)
else:
manage_baseui(True)
started_ts = None
logger_dead = False
for p in car_started_processes:
kill_managed_process(p)
# shutdown if the battery gets lower than 10%, we aren't running, and we are discharging
if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging":
# shutdown if the battery gets lower than 5%, we aren't running, and we are discharging
if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
# check the status of baseui
baseui_running = 'com.baseui' in subprocess.check_output(["ps"])
if msg.thermal.batteryPercent > 10:
battery_was_high = True
# check the status of all processes, did any of them die?
for p in running:
@ -495,10 +506,13 @@ def manager_thread():
health=(td.to_dict() if td else None),
thermal=msg.to_dict())
if do_uninstall:
break
count += 1
def get_installed_apks():
dat = subprocess.check_output(["pm", "list", "packages", "-3", "-f"]).strip().split("\n")
dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n")
ret = {}
for x in dat:
if x.startswith("package:"):
@ -506,12 +520,49 @@ def get_installed_apks():
ret[k] = v
return ret
# optional, build the c++ binaries and preimport the python for speed
def install_apk(path):
# can only install from world readable path
install_path = "/sdcard/%s" % os.path.basename(path)
shutil.copyfile(path, install_path)
ret = subprocess.call(["pm", "install", "-r", install_path])
os.remove(install_path)
return ret == 0
def update_apks():
# install apks
installed = get_installed_apks()
for app in os.listdir(os.path.join(BASEDIR, "apk/")):
if ".apk" in app:
app = app.split(".apk")[0]
if app not in installed:
installed[app] = None
cloudlog.info("installed apks %s" % (str(installed), ))
for app in installed.iterkeys():
apk_path = os.path.join(BASEDIR, "apk/"+app+".apk")
if os.path.isfile(apk_path):
h1 = hashlib.sha1(open(apk_path).read()).hexdigest()
h2 = None
if installed[app] is not None:
h2 = hashlib.sha1(open(installed[app]).read()).hexdigest()
cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2))
if h2 is None or h1 != h2:
cloudlog.info("installing %s" % app)
success = install_apk(apk_path)
if not success:
cloudlog.info("needing to uninstall %s" % app)
system("pm uninstall %s" % app)
success = install_apk(apk_path)
assert success
def manager_update():
update_apks()
def manager_prepare():
if os.path.exists(os.path.join(BASEDIR, ".gitmodules")):
# update submodules
system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR)
system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR)
# build cereal first
subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal"))
@ -521,32 +572,12 @@ def manager_prepare():
for p in managed_processes:
prepare_managed_process(p)
# install apks
installed = get_installed_apks()
for app in os.listdir(os.path.join(BASEDIR, "apk/")):
if ".apk" in app:
app = app.split(".apk")[0]
if app not in installed:
installed[app] = None
cloudlog.info("installed apks %s" % (str(installed), ))
for app in installed:
apk_path = os.path.join(BASEDIR, "apk/"+app+".apk")
if os.path.isfile(apk_path):
h1 = hashlib.sha1(open(apk_path).read()).hexdigest()
h2 = None
if installed[app] is not None:
h2 = hashlib.sha1(open(installed[app]).read()).hexdigest()
cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2))
if h2 is None or h1 != h2:
cloudlog.info("installing %s" % app)
for do_uninstall in [False, True]:
if do_uninstall:
cloudlog.info("needing to uninstall %s" % app)
os.system("pm uninstall %s" % app)
ret = os.system("cp %s /sdcard/%s.apk && pm install -r /sdcard/%s.apk && rm /sdcard/%s.apk" % (apk_path, app, app, app))
if ret == 0:
break
assert ret == 0
def uninstall():
cloudlog.warning("uninstalling")
with open('/cache/recovery/command', 'w') as f:
f.write('--wipe_data\n')
# IPowerManager.reboot(confirm=false, reason="recovery", wait=true)
os.system("service call power 16 i32 0 s16 recovery i32 1")
def main():
if os.getenv("NOLOG") is not None:
@ -581,11 +612,13 @@ def main():
if params.get("IsMetric") is None:
params.put("IsMetric", "0")
if params.get("IsRearViewMirror") is None:
params.put("IsRearViewMirror", "1")
params.put("IsRearViewMirror", "0")
if params.get("IsFcwEnabled") is None:
params.put("IsFcwEnabled", "1")
if params.get("HasAcceptedTerms") is None:
params.put("HasAcceptedTerms", "0")
if params.get("IsUploadVideoOverCellularEnabled") is None:
params.put("IsUploadVideoOverCellularEnabled", "1")
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
@ -597,6 +630,7 @@ def main():
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True)
try:
manager_update()
manager_init()
manager_prepare()
finally:
@ -606,6 +640,9 @@ def main():
if os.getenv("PREPAREONLY") is not None:
return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try:
manager_thread()
except Exception:
@ -614,6 +651,9 @@ def main():
finally:
cleanup_all_processes(None, None)
if params.get("DoUninstall") == "1":
uninstall()
if __name__ == "__main__":
main()
# manual exit because we are forked

0
selfdrive/radar/mock/__init__.py 100644
View File

24
selfdrive/radar/mock/interface.py 100755
View File

@ -0,0 +1,24 @@
#!/usr/bin/env python
from cereal import car
import time
class RadarInterface(object):
def __init__(self):
# radar
self.pts = {}
self.delay = 0.1
def update(self):
ret = car.RadarState.new_message()
time.sleep(0.05) # radard runs on RI updates
return ret
if __name__ == "__main__":
RI = RadarInterface()
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret

1
selfdrive/radar/nidec/interface.py
View File

@ -33,7 +33,6 @@ class RadarInterface(object):
self.radar_fault = False
self.delay = 0.1 # Delay of radar
self.cutin_prediction = True
# Nidec
self.rcp = _create_nidec_can_parser()

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

@ -34,7 +34,6 @@ class RadarInterface(object):
# Nidec
self.rcp = _create_radard_can_parser()
self.cutin_prediction = False
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)

19
selfdrive/registration.py
View File

@ -19,16 +19,23 @@ def get_git_commit():
def get_git_branch():
return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip()
def get_git_remote():
return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip()
def register():
params = Params()
try:
params.put("Version", version)
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
params.put("Version", version)
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
params.put("GitRemote", get_git_remote())
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
try:
if dongle_id is None or access_token is None:
resp = api_get("v1/pilotauth/", method='POST', imei=get_imei(), serial=get_serial())
cloudlog.info("getting pilotauth")
resp = api_get("v1/pilotauth/", method='POST', timeout=15,
imei=get_imei(), serial=get_serial())
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii')

BIN
selfdrive/sensord/gpsd
View File

Binary file not shown.

BIN
selfdrive/sensord/sensord
View File

Binary file not shown.

2
selfdrive/service_list.yaml
View File

@ -46,6 +46,8 @@ ubloxGnss: [8033, true]
clocks: [8034, true]
liveMpc: [8035, true]
liveLongitudinalMpc: [8036, true]
plusFrame: [8037, false]
navStatus: [8038, true]
testModel: [8040, false]

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

@ -60,6 +60,7 @@ class Maneuver(object):
gas=gas, brake=brake, steer_torque=steer_torque,
distance=distance, speed=speed, acceleration=acceleration,
up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd,
uf_accel_cmd=last_live100.ufAccelCmd,
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
cruise_speed=last_live100.vCruise,

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

@ -20,6 +20,7 @@ class ManeuverPlot(object):
self.up_accel_cmd_array = []
self.ui_accel_cmd_array = []
self.uf_accel_cmd_array = []
self.d_rel_array = []
self.v_rel_array = []
@ -38,8 +39,8 @@ class ManeuverPlot(object):
self.title = title
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, fcw):
acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel,
v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw):
self.time_array.append(time)
self.gas_array.append(gas)
self.brake_array.append(brake)
@ -49,6 +50,7 @@ class ManeuverPlot(object):
self.acceleration_array.append(acceleration)
self.up_accel_cmd_array.append(up_accel_cmd)
self.ui_accel_cmd_array.append(ui_accel_cmd)
self.uf_accel_cmd_array.append(uf_accel_cmd)
self.d_rel_array.append(d_rel)
self.v_rel_array.append(v_rel)
self.v_lead_array.append(v_lead)
@ -117,12 +119,13 @@ class ManeuverPlot(object):
plt.figure(plt_num)
plt.plot(
np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g',
np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b'
np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b',
np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r'
)
plt.xlabel("Time, [s]")
plt.ylabel("Accel Cmd [m/s^2]")
plt.grid()
plt.legend(["Proportional", "Integral"], loc=0)
plt.legend(["Proportional", "Integral", "feedforward"], loc=0)
pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000)
# relative distances chart =======

8
selfdrive/test/test_openpilot.py
View File

@ -4,7 +4,6 @@ os.environ['FAKEUPLOAD'] = "1"
from common.testing import phone_only
from selfdrive.manager import manager_init, manager_prepare
from selfdrive.manager import start_managed_process, kill_managed_process, get_running
from selfdrive.manager import manage_baseui
from selfdrive.config import CruiseButtons
from functools import wraps
import time
@ -93,10 +92,3 @@ def test_ui():
def test_uploader():
print "UPLOADER"
time.sleep(10.0)
@phone_only
def test_baseui():
manage_baseui(True)
time.sleep(10.0)
manage_baseui(False)

1
selfdrive/ui/Makefile
View File

@ -52,6 +52,7 @@ ui: $(OBJS)
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
-L/system/vendor/lib64 \
-lhardware \
$(OPENGL_LIBS) \
-lcutils -lm -llog

745
selfdrive/ui/ui.c
View File

File diff suppressed because it is too large Load Diff

3
selfdrive/updated.py
View File

@ -7,9 +7,6 @@ import time
import subprocess
def main(gctx=None):
if not os.getenv("CLEAN"):
return
while True:
# try network
r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.