openpilot v0.3.9 release
parent
2cfdbefde8
commit
5627d0d7fd
22
README.md
22
README.md
|
@ -30,21 +30,25 @@ Supported Cars
|
||||||
- Can only be enabled above 25 mph
|
- Can only be enabled above 25 mph
|
||||||
|
|
||||||
- Toyota RAV-4 2016+ with TSS-P (alpha!)
|
- Toyota RAV-4 2016+ with TSS-P (alpha!)
|
||||||
- Can only be enabled above 20 mph
|
- By default it uses stock Toyota ACC for longitudinal control
|
||||||
|
- openpilot longitudinal control available after unplugging the Driving Support ECU and can be enabled above 20 mph
|
||||||
|
|
||||||
|
- Toyota Prius 2017 (alpha!)
|
||||||
|
- By default it uses stock Toyota ACC for longitudinal control
|
||||||
|
- openpilot longitudinal control available after unplugging the Driving Support ECU
|
||||||
|
|
||||||
In Progress Cars
|
In Progress Cars
|
||||||
------
|
------
|
||||||
|
- Probably all TSS-P Toyota with Steering Assist.
|
||||||
|
- 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option.
|
||||||
|
- Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported.
|
||||||
|
|
||||||
- Toyota Prius 2017
|
Community WIP Cars
|
||||||
|
|
||||||
- Probably all TSS-P Toyota
|
|
||||||
|
|
||||||
Community Ported Cars
|
|
||||||
------
|
------
|
||||||
|
|
||||||
- Chevy Volt 2016-2018 Premier with Driver Confidence II
|
- [Chevy Volt 2016-2018 Premier with Driver Confidence II](https://github.com/commaai/openpilot/pull/104)
|
||||||
|
|
||||||
- Classic Tesla Model S (pre-AP)
|
- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
|
||||||
|
|
||||||
Directory structure
|
Directory structure
|
||||||
------
|
------
|
||||||
|
@ -98,7 +102,7 @@ User Data / chffr Account / Crash Reporting
|
||||||
|
|
||||||
By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
|
By default openpilot creates an account and includes a client for chffr, our dashcam app. We use your data to train better models and improve openpilot for everyone.
|
||||||
|
|
||||||
It's open source software, so you are free to disable it if you wish.
|
It's open source software, so you are free to disable it if you wish.
|
||||||
|
|
||||||
It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
|
It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
|
||||||
It does not log the user facing camera or the microphone.
|
It does not log the user facing camera or the microphone.
|
||||||
|
|
14
RELEASES.md
14
RELEASES.md
|
@ -1,3 +1,10 @@
|
||||||
|
Version 0.3.9 (2017-11-21)
|
||||||
|
==========================
|
||||||
|
* Add alpha support for 2017 Toyota Prius
|
||||||
|
* Improved longitudinal control using model predictive control
|
||||||
|
* Enable Forward Collision Warning
|
||||||
|
* Acura ILX now maintains openpilot engaged at standstill when brakes are applied
|
||||||
|
|
||||||
Version 0.3.8.2 (2017-10-30)
|
Version 0.3.8.2 (2017-10-30)
|
||||||
==========================
|
==========================
|
||||||
* Add alpha support for 2017 Toyota RAV4
|
* Add alpha support for 2017 Toyota RAV4
|
||||||
|
@ -17,9 +24,9 @@ Version 0.3.7 (2017-09-30)
|
||||||
* Fixed sporadic longitudinal pulsing in Civic
|
* Fixed sporadic longitudinal pulsing in Civic
|
||||||
* Cleanups to vehicle interface
|
* Cleanups to vehicle interface
|
||||||
|
|
||||||
Version 0.3.6.1 (2017-08-15)
|
Version 0.3.6.1 (2017-08-15)
|
||||||
============================
|
============================
|
||||||
* Mitigate low speed steering oscillations on some vehicles
|
* Mitigate low speed steering oscillations on some vehicles
|
||||||
* Include board steering check for CR-V
|
* Include board steering check for CR-V
|
||||||
|
|
||||||
Version 0.3.6 (2017-08-08)
|
Version 0.3.6 (2017-08-08)
|
||||||
|
@ -139,4 +146,3 @@ Version 0.1 (2016-11-29)
|
||||||
* Lane keep assist is working
|
* Lane keep assist is working
|
||||||
* Support for Acura ILX 2016 with AcuraWatch Plus
|
* Support for Acura ILX 2016 with AcuraWatch Plus
|
||||||
* Support for Honda Civic 2016 Touring Edition
|
* Support for Honda Civic 2016 Touring Edition
|
||||||
|
|
||||||
|
|
Binary file not shown.
|
@ -77,6 +77,7 @@ struct CarState {
|
||||||
# brake pedal, 0.0-1.0
|
# brake pedal, 0.0-1.0
|
||||||
brake @5 :Float32; # this is user pedal only
|
brake @5 :Float32; # this is user pedal only
|
||||||
brakePressed @6 :Bool; # this is user pedal only
|
brakePressed @6 :Bool; # this is user pedal only
|
||||||
|
brakeLights @19 :Bool;
|
||||||
|
|
||||||
# steering wheel
|
# steering wheel
|
||||||
steeringAngle @7 :Float32; # deg
|
steeringAngle @7 :Float32; # deg
|
||||||
|
@ -92,6 +93,8 @@ struct CarState {
|
||||||
|
|
||||||
# button presses
|
# button presses
|
||||||
buttonEvents @11 :List(ButtonEvent);
|
buttonEvents @11 :List(ButtonEvent);
|
||||||
|
leftBlinker @20 :Bool;
|
||||||
|
rightBlinker @21 :Bool;
|
||||||
|
|
||||||
# which packets this state came from
|
# which packets this state came from
|
||||||
canMonoTimes @12: List(UInt64);
|
canMonoTimes @12: List(UInt64);
|
||||||
|
@ -109,6 +112,7 @@ struct CarState {
|
||||||
speed @1 :Float32;
|
speed @1 :Float32;
|
||||||
available @2 :Bool;
|
available @2 :Bool;
|
||||||
speedOffset @3 :Float32;
|
speedOffset @3 :Float32;
|
||||||
|
standstill @4 :Bool;
|
||||||
}
|
}
|
||||||
|
|
||||||
enum GearShifter {
|
enum GearShifter {
|
||||||
|
@ -291,10 +295,16 @@ struct CarParams {
|
||||||
steerKi @17 :Float32;
|
steerKi @17 :Float32;
|
||||||
steerKf @26 :Float32;
|
steerKf @26 :Float32;
|
||||||
|
|
||||||
|
# Kp and Ki for the longitudinal control
|
||||||
|
longitudinalKpBP @37 :List(Float32);
|
||||||
|
longitudinalKpV @38 :List(Float32);
|
||||||
|
longitudinalKiBP @39 :List(Float32);
|
||||||
|
longitudinalKiV @40 :List(Float32);
|
||||||
|
|
||||||
steerLimitAlert @30 :Bool;
|
steerLimitAlert @30 :Bool;
|
||||||
|
|
||||||
vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state
|
vEgoStopping @31 :Float32; # Speed at which the car goes into stopping state
|
||||||
directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake
|
directAccelControl @32 :Bool; # Does the car have direct accel control or just gas/brake
|
||||||
|
stoppingControl @35 :Bool; # Does the car allows full control even at lows speeds when stopping
|
||||||
# TODO: Kp and Ki for long control, perhaps not needed?
|
startAccel @36 :Float32; # Required acceleraton to overcome creep braking
|
||||||
}
|
}
|
||||||
|
|
|
@ -337,12 +337,15 @@ struct Live100Data {
|
||||||
vTargetLead @3 :Float32;
|
vTargetLead @3 :Float32;
|
||||||
upAccelCmd @4 :Float32;
|
upAccelCmd @4 :Float32;
|
||||||
uiAccelCmd @5 :Float32;
|
uiAccelCmd @5 :Float32;
|
||||||
|
ufAccelCmd @33 :Float32;
|
||||||
yActualDEPRECATED @6 :Float32;
|
yActualDEPRECATED @6 :Float32;
|
||||||
yDes @7 :Float32;
|
yDesDEPRECATED @7 :Float32;
|
||||||
upSteer @8 :Float32;
|
upSteer @8 :Float32;
|
||||||
uiSteer @9 :Float32;
|
uiSteer @9 :Float32;
|
||||||
aTargetMin @10 :Float32;
|
ufSteer @34 :Float32;
|
||||||
aTargetMax @11 :Float32;
|
aTargetMinDEPRECATED @10 :Float32;
|
||||||
|
aTargetMaxDEPRECATED @11 :Float32;
|
||||||
|
aTarget @35 :Float32;
|
||||||
jerkFactor @12 :Float32;
|
jerkFactor @12 :Float32;
|
||||||
angleSteers @13 :Float32; # Steering angle in degrees.
|
angleSteers @13 :Float32; # Steering angle in degrees.
|
||||||
angleSteersDes @29 :Float32;
|
angleSteersDes @29 :Float32;
|
||||||
|
@ -350,6 +353,7 @@ struct Live100Data {
|
||||||
cumLagMs @15 :Float32;
|
cumLagMs @15 :Float32;
|
||||||
|
|
||||||
enabled @19 :Bool;
|
enabled @19 :Bool;
|
||||||
|
active @36 :Bool;
|
||||||
steerOverride @20 :Bool;
|
steerOverride @20 :Bool;
|
||||||
|
|
||||||
vCruise @22 :Float32;
|
vCruise @22 :Float32;
|
||||||
|
@ -388,7 +392,7 @@ struct ModelData {
|
||||||
leftLane @2 :PathData;
|
leftLane @2 :PathData;
|
||||||
rightLane @3 :PathData;
|
rightLane @3 :PathData;
|
||||||
lead @4 :LeadData;
|
lead @4 :LeadData;
|
||||||
leadNew @6 :List(Float32);
|
freePath @6 :List(Float32);
|
||||||
|
|
||||||
settings @5 :ModelSettings;
|
settings @5 :ModelSettings;
|
||||||
|
|
||||||
|
@ -472,10 +476,13 @@ struct Plan {
|
||||||
|
|
||||||
# longitudinal
|
# longitudinal
|
||||||
longitudinalValid @2 :Bool;
|
longitudinalValid @2 :Bool;
|
||||||
|
vCruise @16 :Float32;
|
||||||
|
aCruise @17 :Float32;
|
||||||
vTarget @3 :Float32;
|
vTarget @3 :Float32;
|
||||||
vTargetFuture @14 :Float32;
|
vTargetFuture @14 :Float32;
|
||||||
aTargetMin @4 :Float32;
|
aTargetMinDEPRECATED @4 :Float32;
|
||||||
aTargetMax @5 :Float32;
|
aTargetMaxDEPRECATED @5 :Float32;
|
||||||
|
aTarget @18 :Float32;
|
||||||
jerkFactor @6 :Float32;
|
jerkFactor @6 :Float32;
|
||||||
hasLead @7 :Bool;
|
hasLead @7 :Bool;
|
||||||
fcw @8 :Bool;
|
fcw @8 :Bool;
|
||||||
|
|
|
@ -143,7 +143,7 @@ class dbc(object):
|
||||||
msg = self.msgs.get(x[0])
|
msg = self.msgs.get(x[0])
|
||||||
if msg is None:
|
if msg is None:
|
||||||
if x[0] not in self._warned_addresses:
|
if x[0] not in self._warned_addresses:
|
||||||
print("WARNING: Unknown message address {}".format(x[0]))
|
#print("WARNING: Unknown message address {}".format(x[0]))
|
||||||
self._warned_addresses.add(x[0])
|
self._warned_addresses.add(x[0])
|
||||||
return None, None
|
return None, None
|
||||||
|
|
||||||
|
|
|
@ -19,6 +19,9 @@ _FINGERPRINTS = {
|
||||||
"TOYOTA RAV4 2017": {
|
"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
|
36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||||
},
|
},
|
||||||
|
"TOYOTA PRIUS 2017": {
|
||||||
|
36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
|
||||||
|
},
|
||||||
}
|
}
|
||||||
|
|
||||||
# support additional internal only fingerprints
|
# support additional internal only fingerprints
|
||||||
|
|
|
@ -56,6 +56,7 @@ keys = {
|
||||||
# read: ui, controls
|
# read: ui, controls
|
||||||
"IsMetric": TxType.PERSISTANT,
|
"IsMetric": TxType.PERSISTANT,
|
||||||
"IsRearViewMirror": TxType.PERSISTANT,
|
"IsRearViewMirror": TxType.PERSISTANT,
|
||||||
|
"IsFcwEnabled": TxType.PERSISTANT,
|
||||||
# written: visiond
|
# written: visiond
|
||||||
# read: visiond, controlsd
|
# read: visiond, controlsd
|
||||||
"CalibrationParams": TxType.PERSISTANT,
|
"CalibrationParams": TxType.PERSISTANT,
|
||||||
|
|
2
opendbc
2
opendbc
|
@ -1 +1 @@
|
||||||
Subproject commit 835a9739d6721e351e1814439b55b6c4212f7b85
|
Subproject commit c8eeedce1717c6e05acd77f8b893908667baea21
|
2
panda
2
panda
|
@ -1 +1 @@
|
||||||
Subproject commit 849f68879d1ceacbf1f9d4174e16e1cd14527383
|
Subproject commit 3cab37297566962fd6e48a674db3e1f6de8fa4da
|
|
@ -85,4 +85,4 @@ def get_car(logcan, sendcan=None, timeout=None):
|
||||||
interface_cls = interfaces[candidate]
|
interface_cls = interfaces[candidate]
|
||||||
params = interface_cls.get_params(candidate, fingerprints)
|
params = interface_cls.get_params(candidate, fingerprints)
|
||||||
|
|
||||||
return interface_cls(params, logcan, sendcan), params
|
return interface_cls(params, sendcan), params
|
||||||
|
|
|
@ -18,7 +18,7 @@ CAMERA_MSGS = [0xe4, 0x194]
|
||||||
|
|
||||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
|
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
|
||||||
# hyst params... TODO: move these to VehicleParams
|
# hyst params... TODO: move these to VehicleParams
|
||||||
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
|
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||||
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
|
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
|
||||||
|
|
||||||
|
@ -36,14 +36,8 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic):
|
||||||
brake_steady = brake + brake_hyst_gap
|
brake_steady = brake + brake_hyst_gap
|
||||||
brake = brake_steady
|
brake = brake_steady
|
||||||
|
|
||||||
if not civic:
|
if not civic and brake > 0.0:
|
||||||
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
|
brake += 0.15
|
||||||
brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds
|
|
||||||
# offset the brake command for threshold in the brake system. no brake torque perceived below it
|
|
||||||
brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
|
|
||||||
brake_offset = brake_on_offset - brake_hyst_on
|
|
||||||
if brake > 0.0:
|
|
||||||
brake += brake_offset
|
|
||||||
|
|
||||||
return brake, braking, brake_steady
|
return brake, braking, brake_steady
|
||||||
|
|
||||||
|
@ -69,6 +63,7 @@ HUDData = namedtuple("HUDData",
|
||||||
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
|
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
|
||||||
"lanes", "beep", "X8", "chime", "acc_alert"])
|
"lanes", "beep", "X8", "chime", "acc_alert"])
|
||||||
|
|
||||||
|
|
||||||
class CarController(object):
|
class CarController(object):
|
||||||
def __init__(self, enable_camera=True):
|
def __init__(self, enable_camera=True):
|
||||||
self.braking = False
|
self.braking = False
|
||||||
|
@ -95,8 +90,7 @@ class CarController(object):
|
||||||
pcm_cancel_cmd = True
|
pcm_cancel_cmd = True
|
||||||
|
|
||||||
# *** rate limit after the enable check ***
|
# *** rate limit after the enable check ***
|
||||||
brake = rate_limit(brake, self.brake_last, -2., 1./100)
|
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
|
||||||
self.brake_last = brake
|
|
||||||
|
|
||||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||||
#TODO: use enum!!
|
#TODO: use enum!!
|
||||||
|
@ -142,17 +136,9 @@ class CarController(object):
|
||||||
|
|
||||||
# steer torque is converted back to CAN reference (positive when steering right)
|
# steer torque is converted back to CAN reference (positive when steering right)
|
||||||
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
|
apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1))
|
||||||
apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
||||||
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||||
|
|
||||||
# no gas if you are hitting the brake or the user is
|
|
||||||
if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
|
|
||||||
apply_gas = 0
|
|
||||||
|
|
||||||
# no computer brake if the gas is being pressed
|
|
||||||
if CS.car_gas > 0 and apply_brake != 0:
|
|
||||||
apply_brake = 0
|
|
||||||
|
|
||||||
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
|
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
|
||||||
if CS.steer_not_allowed:
|
if CS.steer_not_allowed:
|
||||||
apply_steer = 0
|
apply_steer = 0
|
||||||
|
|
|
@ -2,7 +2,6 @@ import os
|
||||||
import time
|
import time
|
||||||
from cereal import car
|
from cereal import car
|
||||||
from common.numpy_fast import interp
|
from common.numpy_fast import interp
|
||||||
from common.realtime import sec_since_boot
|
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
from selfdrive.can.parser import CANParser
|
from selfdrive.can.parser import CANParser
|
||||||
from selfdrive.config import Conversions as CV
|
from selfdrive.config import Conversions as CV
|
||||||
|
@ -275,7 +274,7 @@ def get_can_parser(CP):
|
||||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
|
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
|
||||||
|
|
||||||
class CarState(object):
|
class CarState(object):
|
||||||
def __init__(self, CP, logcan):
|
def __init__(self, CP):
|
||||||
self.acura = False
|
self.acura = False
|
||||||
self.civic = False
|
self.civic = False
|
||||||
self.accord = False
|
self.accord = False
|
||||||
|
@ -294,9 +293,6 @@ class CarState(object):
|
||||||
self.brake_only = CP.enableCruise
|
self.brake_only = CP.enableCruise
|
||||||
self.CP = CP
|
self.CP = CP
|
||||||
|
|
||||||
# initialize can parser
|
|
||||||
self.cp = get_can_parser(CP)
|
|
||||||
|
|
||||||
self.user_gas, self.user_gas_pressed = 0., 0
|
self.user_gas, self.user_gas_pressed = 0., 0
|
||||||
self.brake_switch_prev = 0
|
self.brake_switch_prev = 0
|
||||||
self.brake_switch_ts = 0
|
self.brake_switch_ts = 0
|
||||||
|
@ -315,14 +311,13 @@ class CarState(object):
|
||||||
self.v_ego_C = np.matrix([1.0, 0.0])
|
self.v_ego_C = np.matrix([1.0, 0.0])
|
||||||
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||||
self.v_ego_R = 1e3
|
self.v_ego_R = 1e3
|
||||||
|
self.v_ego = 0.0
|
||||||
# import control
|
# import control
|
||||||
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
||||||
# self.v_ego_K = np.transpose(K)
|
# self.v_ego_K = np.transpose(K)
|
||||||
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
||||||
|
|
||||||
def update(self, can_pub_main=None):
|
def update(self, cp):
|
||||||
cp = self.cp
|
|
||||||
cp.update(int(sec_since_boot() * 1e9), False)
|
|
||||||
|
|
||||||
# copy can_valid
|
# copy can_valid
|
||||||
self.can_valid = cp.can_valid
|
self.can_valid = cp.can_valid
|
||||||
|
@ -368,6 +363,9 @@ class CarState(object):
|
||||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||||
self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
|
self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
|
||||||
speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
||||||
|
|
||||||
|
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||||
|
self.v_ego_x = np.matrix([[speed], [0.0]])
|
||||||
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed)
|
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed)
|
||||||
|
|
||||||
self.v_ego_raw = speed
|
self.v_ego_raw = speed
|
||||||
|
@ -449,13 +447,15 @@ class CarState(object):
|
||||||
else:
|
else:
|
||||||
self.car_gas = cp.vl[0x130]['CAR_GAS']
|
self.car_gas = cp.vl[0x130]['CAR_GAS']
|
||||||
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
||||||
|
self.steer_torque_driver = cp.vl[0x18F]['STEER_TORQUE_SENSOR']
|
||||||
|
|
||||||
# brake switch has shown some single time step noise, so only considered when
|
# brake switch has shown some single time step noise, so only considered when
|
||||||
# switch is on for at least 2 consecutive CAN samples
|
# switch is on for at least 2 consecutive CAN samples
|
||||||
|
self.brake_switch = cp.vl[0x17C]['BRAKE_SWITCH']
|
||||||
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \
|
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \
|
||||||
(cp.vl[0x17C]['BRAKE_SWITCH'] and self.brake_switch_prev and \
|
(self.brake_switch and self.brake_switch_prev and \
|
||||||
cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||||
self.brake_switch_prev = cp.vl[0x17C]['BRAKE_SWITCH']
|
self.brake_switch_prev = self.brake_switch
|
||||||
self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH']
|
self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH']
|
||||||
|
|
||||||
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
|
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
|
||||||
|
@ -472,7 +472,6 @@ if __name__ == '__main__':
|
||||||
import time
|
import time
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
context = zmq.Context()
|
context = zmq.Context()
|
||||||
logcan = messaging.sub_sock(context, service_list['can'].port)
|
|
||||||
|
|
||||||
class CarParams(object):
|
class CarParams(object):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
@ -480,7 +479,7 @@ if __name__ == '__main__':
|
||||||
self.enableGas = 0
|
self.enableGas = 0
|
||||||
self.enableCruise = 0
|
self.enableCruise = 0
|
||||||
CP = CarParams()
|
CP = CarParams()
|
||||||
CS = CarState(CP, logcan)
|
CS = CarState(CP)
|
||||||
|
|
||||||
while 1:
|
while 1:
|
||||||
CS.update()
|
CS.update()
|
||||||
|
|
|
@ -2,23 +2,34 @@
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from common.numpy_fast import clip
|
from common.numpy_fast import clip, interp
|
||||||
from common.realtime import sec_since_boot
|
from common.realtime import sec_since_boot
|
||||||
from selfdrive.config import Conversions as CV
|
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.drive_helpers import create_event, EventTypes as ET, get_events
|
||||||
from cereal import car
|
from cereal import car
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
from selfdrive.car.honda.carstate import CarState
|
from selfdrive.car.honda.carstate import CarState, get_can_parser
|
||||||
from selfdrive.car.honda.carcontroller import CAMERA_MSGS
|
from selfdrive.car.honda.carcontroller import CAMERA_MSGS
|
||||||
from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
|
from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
|
||||||
|
from selfdrive.controls.lib.planner import A_ACC_MAX
|
||||||
|
|
||||||
try:
|
try:
|
||||||
from .carcontroller import CarController
|
from .carcontroller import CarController
|
||||||
except ImportError:
|
except ImportError:
|
||||||
CarController = None
|
CarController = None
|
||||||
|
|
||||||
def get_compute_gb():
|
|
||||||
|
def compute_gb_honda(accel, speed):
|
||||||
|
creep_brake = 0.0
|
||||||
|
creep_speed = 2.3
|
||||||
|
creep_brake_value = 0.15
|
||||||
|
if speed < creep_speed:
|
||||||
|
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
|
||||||
|
return float(accel) / 4.8 - creep_brake
|
||||||
|
|
||||||
|
|
||||||
|
def get_compute_gb_acura():
|
||||||
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
|
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
|
||||||
# where -1.0 is max brake and 1.0 is max gas
|
# where -1.0 is max brake and 1.0 is max gas
|
||||||
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
|
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
|
||||||
|
@ -45,8 +56,8 @@ def get_compute_gb():
|
||||||
def leakyrelu(x, alpha):
|
def leakyrelu(x, alpha):
|
||||||
return np.maximum(x, alpha * x)
|
return np.maximum(x, alpha * x)
|
||||||
|
|
||||||
def _compute_gb(accel, speed):
|
def _compute_gb_acura(accel, speed):
|
||||||
#linearly extrap below v1 using v1 and v2 data
|
# linearly extrap below v1 using v1 and v2 data
|
||||||
v1 = 5.
|
v1 = 5.
|
||||||
v2 = 10.
|
v2 = 10.
|
||||||
dat = np.array([accel, speed])
|
dat = np.array([accel, speed])
|
||||||
|
@ -60,12 +71,11 @@ def get_compute_gb():
|
||||||
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
|
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
|
||||||
return float(m4)
|
return float(m4)
|
||||||
|
|
||||||
return _compute_gb
|
return _compute_gb_acura
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(object):
|
class CarInterface(object):
|
||||||
def __init__(self, CP, logcan, sendcan=None):
|
def __init__(self, CP, sendcan=None):
|
||||||
self.logcan = logcan
|
|
||||||
self.CP = CP
|
self.CP = CP
|
||||||
|
|
||||||
self.frame = 0
|
self.frame = 0
|
||||||
|
@ -75,8 +85,10 @@ class CarInterface(object):
|
||||||
self.brake_pressed_prev = False
|
self.brake_pressed_prev = False
|
||||||
self.can_invalid_count = 0
|
self.can_invalid_count = 0
|
||||||
|
|
||||||
|
self.cp = get_can_parser(CP)
|
||||||
|
|
||||||
# *** init the major players ***
|
# *** init the major players ***
|
||||||
self.CS = CarState(CP, self.logcan)
|
self.CS = CarState(CP)
|
||||||
|
|
||||||
# sending if read only is False
|
# sending if read only is False
|
||||||
if sendcan is not None:
|
if sendcan is not None:
|
||||||
|
@ -87,6 +99,25 @@ class CarInterface(object):
|
||||||
# self.accord_msg = []
|
# self.accord_msg = []
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
|
if not self.CS.civic:
|
||||||
|
self.compute_gb = get_compute_gb_acura()
|
||||||
|
else:
|
||||||
|
self.compute_gb = compute_gb_honda
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||||
|
eA = a_ego - a_target
|
||||||
|
valuesA = [1.0, 0.1]
|
||||||
|
bpA = [0.0, 0.5]
|
||||||
|
|
||||||
|
eV = v_ego - v_target
|
||||||
|
valuesV = [1.0, 0.1]
|
||||||
|
bpV = [0.0, 0.5]
|
||||||
|
|
||||||
|
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
|
||||||
|
# unless aTargetMax is very high and then we scale with it; this help in quicker restart
|
||||||
|
return float(max(0.714, a_target / A_ACC_MAX)) * min(interp(eA, bpA, valuesA), interp(eV, bpV, valuesV))
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint):
|
def get_params(candidate, fingerprint):
|
||||||
|
|
||||||
|
@ -130,6 +161,11 @@ class CarInterface(object):
|
||||||
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
|
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
|
||||||
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
|
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.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
|
||||||
|
|
||||||
|
ret.longitudinalKpBP = [0., 5., 35.]
|
||||||
|
ret.longitudinalKpV = [3.6, 2.4, 1.5]
|
||||||
|
ret.longitudinalKiBP = [0., 35.]
|
||||||
|
ret.longitudinalKiV = [0.54, 0.36]
|
||||||
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
|
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
|
||||||
stop_and_go = False
|
stop_and_go = False
|
||||||
ret.m = 3095./2.205 + std_cargo
|
ret.m = 3095./2.205 + std_cargo
|
||||||
|
@ -139,6 +175,11 @@ class CarInterface(object):
|
||||||
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
|
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
|
||||||
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
|
is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee']
|
||||||
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
|
ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24]
|
||||||
|
|
||||||
|
ret.longitudinalKpBP = [0., 5., 35.]
|
||||||
|
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||||
|
ret.longitudinalKiBP = [0., 35.]
|
||||||
|
ret.longitudinalKiV = [0.18, 0.12]
|
||||||
elif candidate == "HONDA ACCORD 2016 TOURING":
|
elif candidate == "HONDA ACCORD 2016 TOURING":
|
||||||
stop_and_go = False
|
stop_and_go = False
|
||||||
ret.m = 3580./2.205 + std_cargo
|
ret.m = 3580./2.205 + std_cargo
|
||||||
|
@ -146,6 +187,11 @@ class CarInterface(object):
|
||||||
ret.aF = ret.l * 0.38
|
ret.aF = ret.l * 0.38
|
||||||
ret.sR = 15.3
|
ret.sR = 15.3
|
||||||
ret.steerKp, ret.steerKi = 0.8, 0.24
|
ret.steerKp, ret.steerKi = 0.8, 0.24
|
||||||
|
|
||||||
|
ret.longitudinalKpBP = [0., 5., 35.]
|
||||||
|
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||||
|
ret.longitudinalKiBP = [0., 35.]
|
||||||
|
ret.longitudinalKiV = [0.18, 0.12]
|
||||||
elif candidate == "HONDA CR-V 2016 TOURING":
|
elif candidate == "HONDA CR-V 2016 TOURING":
|
||||||
stop_and_go = False
|
stop_and_go = False
|
||||||
ret.m = 3572./2.205 + std_cargo
|
ret.m = 3572./2.205 + std_cargo
|
||||||
|
@ -153,6 +199,11 @@ class CarInterface(object):
|
||||||
ret.aF = ret.l * 0.41
|
ret.aF = ret.l * 0.41
|
||||||
ret.sR = 15.3
|
ret.sR = 15.3
|
||||||
ret.steerKp, ret.steerKi = 0.8, 0.24
|
ret.steerKp, ret.steerKi = 0.8, 0.24
|
||||||
|
|
||||||
|
ret.longitudinalKpBP = [0., 5., 35.]
|
||||||
|
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||||
|
ret.longitudinalKiBP = [0., 35.]
|
||||||
|
ret.longitudinalKiV = [0.18, 0.12]
|
||||||
else:
|
else:
|
||||||
raise ValueError("unsupported car %s" % candidate)
|
raise ValueError("unsupported car %s" % candidate)
|
||||||
|
|
||||||
|
@ -188,19 +239,20 @@ class CarInterface(object):
|
||||||
ret.longPidDeadzoneBP = [0.]
|
ret.longPidDeadzoneBP = [0.]
|
||||||
ret.longPidDeadzoneV = [0.]
|
ret.longPidDeadzoneV = [0.]
|
||||||
|
|
||||||
|
ret.stoppingControl = True
|
||||||
ret.steerLimitAlert = True
|
ret.steerLimitAlert = True
|
||||||
|
ret.startAccel = 0.5
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
compute_gb = staticmethod(get_compute_gb())
|
|
||||||
|
|
||||||
# returns a car.CarState
|
# returns a car.CarState
|
||||||
def update(self, c):
|
def update(self, c):
|
||||||
# ******************* do can recv *******************
|
# ******************* do can recv *******************
|
||||||
can_pub_main = []
|
|
||||||
canMonoTimes = []
|
canMonoTimes = []
|
||||||
|
|
||||||
self.CS.update(can_pub_main)
|
self.cp.update(int(sec_since_boot() * 1e9), False)
|
||||||
|
|
||||||
|
self.CS.update(self.cp)
|
||||||
|
|
||||||
# create message
|
# create message
|
||||||
ret = car.CarState.new_message()
|
ret = car.CarState.new_message()
|
||||||
|
@ -225,6 +277,10 @@ class CarInterface(object):
|
||||||
# brake pedal
|
# brake pedal
|
||||||
ret.brake = self.CS.user_brake
|
ret.brake = self.CS.user_brake
|
||||||
ret.brakePressed = self.CS.brake_pressed != 0
|
ret.brakePressed = self.CS.brake_pressed != 0
|
||||||
|
# FIXME: read sendcan for brakelights
|
||||||
|
brakelights_threshold = 0.02 if self.CS.civic else 0.1
|
||||||
|
ret.brakeLights = bool(self.CS.brake_switch or
|
||||||
|
c.actuators.brake > brakelights_threshold)
|
||||||
|
|
||||||
# steering wheel
|
# steering wheel
|
||||||
ret.steeringAngle = self.CS.angle_steers
|
ret.steeringAngle = self.CS.angle_steers
|
||||||
|
@ -233,7 +289,7 @@ class CarInterface(object):
|
||||||
# gear shifter lever
|
# gear shifter lever
|
||||||
ret.gearShifter = self.CS.gear_shifter
|
ret.gearShifter = self.CS.gear_shifter
|
||||||
|
|
||||||
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
|
ret.steeringTorque = self.CS.steer_torque_driver
|
||||||
ret.steeringPressed = self.CS.steer_override
|
ret.steeringPressed = self.CS.steer_override
|
||||||
|
|
||||||
# cruise state
|
# cruise state
|
||||||
|
@ -241,9 +297,12 @@ class CarInterface(object):
|
||||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||||
ret.cruiseState.available = bool(self.CS.main_on)
|
ret.cruiseState.available = bool(self.CS.main_on)
|
||||||
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
|
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
|
||||||
|
ret.cruiseState.standstill = False
|
||||||
|
|
||||||
# TODO: button presses
|
# TODO: button presses
|
||||||
buttonEvents = []
|
buttonEvents = []
|
||||||
|
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||||
|
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||||
|
|
||||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||||
be = car.CarState.ButtonEvent.new_message()
|
be = car.CarState.ButtonEvent.new_message()
|
||||||
|
@ -332,16 +391,16 @@ class CarInterface(object):
|
||||||
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
|
||||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||||
|
|
||||||
#if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
|
|
||||||
if ret.gasPressed:
|
if ret.gasPressed:
|
||||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||||
|
|
||||||
# it can happen that car cruise disables while comma system is enabled: need to
|
# it can happen that car cruise disables while comma system is enabled: need to
|
||||||
# keep braking if needed or if the speed is very low
|
# keep braking if needed or if the speed is very low
|
||||||
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
|
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
|
||||||
if self.CP.enableCruise and not ret.cruiseState.enabled and \
|
if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.:
|
||||||
(c.actuators.brake <= 0. or ret.vEgo < 0.3):
|
|
||||||
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
||||||
|
if not self.CS.civic and ret.vEgo < 0.001:
|
||||||
|
events.append(create_event('manualRestart', [ET.WARNING]))
|
||||||
|
|
||||||
cur_time = sec_since_boot()
|
cur_time = sec_since_boot()
|
||||||
enable_pressed = False
|
enable_pressed = False
|
||||||
|
@ -380,14 +439,11 @@ class CarInterface(object):
|
||||||
self.brake_pressed_prev = ret.brakePressed
|
self.brake_pressed_prev = ret.brakePressed
|
||||||
|
|
||||||
# cast to reader so it can't be modified
|
# cast to reader so it can't be modified
|
||||||
#print ret
|
|
||||||
return ret.as_reader()
|
return ret.as_reader()
|
||||||
|
|
||||||
# pass in a car.CarControl
|
# pass in a car.CarControl
|
||||||
# to be called @ 100hz
|
# to be called @ 100hz
|
||||||
def apply(self, c):
|
def apply(self, c):
|
||||||
#print c
|
|
||||||
|
|
||||||
if c.hudControl.speedVisible:
|
if c.hudControl.speedVisible:
|
||||||
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
|
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
|
||||||
else:
|
else:
|
||||||
|
|
|
@ -4,7 +4,9 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||||
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
||||||
create_steer_command, create_ui_command, \
|
create_steer_command, create_ui_command, \
|
||||||
create_ipas_steer_command, create_accel_command
|
create_ipas_steer_command, create_accel_command, \
|
||||||
|
create_fcw_command
|
||||||
|
from selfdrive.car.toyota.values import CAR, ECU
|
||||||
|
|
||||||
|
|
||||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||||
|
@ -14,22 +16,12 @@ ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||||
|
|
||||||
STEER_MAX = 1500
|
STEER_MAX = 1500
|
||||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||||
STEER_DELTA_DOWN = 45 # lower than 50 otherwise the Rav4 faults (Prius seems ok though)
|
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||||
STEER_ERROR_MAX = 500 # max delta between torque cmd and torque motor
|
STEER_ERROR_MAX = 500 # max delta between torque cmd and torque motor
|
||||||
|
|
||||||
STEER_IPAS_MAX = 340
|
STEER_IPAS_MAX = 340
|
||||||
STEER_IPAS_DELTA_MAX = 3
|
STEER_IPAS_DELTA_MAX = 3
|
||||||
|
|
||||||
class CAR:
|
|
||||||
PRIUS = "TOYOTA PRIUS 2017"
|
|
||||||
RAV4 = "TOYOTA RAV4 2017"
|
|
||||||
|
|
||||||
class ECU:
|
|
||||||
CAM = 0 # camera
|
|
||||||
DSU = 1 # driving support unit
|
|
||||||
APGS = 2 # advanced parking guidance system
|
|
||||||
|
|
||||||
|
|
||||||
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
||||||
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
||||||
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
|
0x373, 0x374, 0x375, 0x380, 0x381, 0x382,
|
||||||
|
@ -75,7 +67,6 @@ STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46
|
||||||
0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
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'),
|
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'),
|
0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||||
0x411: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x20\x00\x00\x10\x00\x80\x00'),
|
|
||||||
0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
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'),
|
0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'),
|
||||||
}
|
}
|
||||||
|
@ -107,18 +98,23 @@ def accel_hysteresis(accel, accel_steady, enabled):
|
||||||
|
|
||||||
def process_hud_alert(hud_alert, audible_alert):
|
def process_hud_alert(hud_alert, audible_alert):
|
||||||
# initialize to no alert
|
# initialize to no alert
|
||||||
hud1 = 0
|
steer = 0
|
||||||
hud2 = 0
|
fcw = 0
|
||||||
if hud_alert in ['steerRequired', 'fcw']:
|
sound1 = 0
|
||||||
if audible_alert == 'chimeRepeated':
|
sound2 = 0
|
||||||
hud1 = 3
|
|
||||||
else:
|
|
||||||
hud1 = 1
|
|
||||||
if audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
|
|
||||||
# TODO: find a way to send single chimes
|
|
||||||
hud2 = 1
|
|
||||||
|
|
||||||
return hud1, hud2
|
if hud_alert == 'fcw':
|
||||||
|
fcw = 1
|
||||||
|
elif hud_alert == 'steerRequired':
|
||||||
|
steer = 1
|
||||||
|
|
||||||
|
if audible_alert == 'chimeRepeated':
|
||||||
|
sound1 = 1
|
||||||
|
elif audible_alert in ['beepSingle', 'chimeSingle', 'chimeDouble']:
|
||||||
|
# TODO: find a way to send single chimes
|
||||||
|
sound2 = 1
|
||||||
|
|
||||||
|
return steer, fcw, sound1, sound2
|
||||||
|
|
||||||
|
|
||||||
class CarController(object):
|
class CarController(object):
|
||||||
|
@ -130,6 +126,8 @@ class CarController(object):
|
||||||
self.accel_steady = 0.
|
self.accel_steady = 0.
|
||||||
self.car_fingerprint = car_fingerprint
|
self.car_fingerprint = car_fingerprint
|
||||||
self.alert_active = False
|
self.alert_active = False
|
||||||
|
self.last_standstill = False
|
||||||
|
self.standstill_req = False
|
||||||
|
|
||||||
self.fake_ecus = set()
|
self.fake_ecus = set()
|
||||||
if enable_camera: self.fake_ecus.add(ECU.CAM)
|
if enable_camera: self.fake_ecus.add(ECU.CAM)
|
||||||
|
@ -140,7 +138,7 @@ class CarController(object):
|
||||||
pcm_cancel_cmd, hud_alert, audible_alert):
|
pcm_cancel_cmd, hud_alert, audible_alert):
|
||||||
|
|
||||||
# *** compute control surfaces ***
|
# *** compute control surfaces ***
|
||||||
tt = sec_since_boot()
|
ts = sec_since_boot()
|
||||||
|
|
||||||
# steer torque is converted back to CAN reference (positive when steering right)
|
# steer torque is converted back to CAN reference (positive when steering right)
|
||||||
apply_accel = actuators.gas - actuators.brake
|
apply_accel = actuators.gas - actuators.brake
|
||||||
|
@ -170,8 +168,16 @@ class CarController(object):
|
||||||
if not enabled or CS.steer_error:
|
if not enabled or CS.steer_error:
|
||||||
apply_steer = 0
|
apply_steer = 0
|
||||||
|
|
||||||
|
# on entering standstill, send standstill request
|
||||||
|
if CS.standstill and not self.last_standstill:
|
||||||
|
self.standstill_req = True
|
||||||
|
if CS.pcm_acc_status != 8:
|
||||||
|
# pcm entered standstill or it's disabled
|
||||||
|
self.standstill_req = False
|
||||||
|
|
||||||
self.last_steer = apply_steer
|
self.last_steer = apply_steer
|
||||||
self.last_accel = apply_accel
|
self.last_accel = apply_accel
|
||||||
|
self.last_standstill = CS.standstill
|
||||||
|
|
||||||
can_sends = []
|
can_sends = []
|
||||||
|
|
||||||
|
@ -190,9 +196,9 @@ class CarController(object):
|
||||||
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
|
# accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control
|
||||||
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
|
if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus):
|
||||||
if ECU.DSU in self.fake_ecus:
|
if ECU.DSU in self.fake_ecus:
|
||||||
can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd))
|
can_sends.append(create_accel_command(apply_accel, pcm_cancel_cmd, self.standstill_req))
|
||||||
else:
|
else:
|
||||||
can_sends.append(create_accel_command(0, pcm_cancel_cmd))
|
can_sends.append(create_accel_command(0, pcm_cancel_cmd, False))
|
||||||
|
|
||||||
if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
|
if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
|
||||||
for addr in TARGET_IDS:
|
for addr in TARGET_IDS:
|
||||||
|
@ -201,24 +207,25 @@ class CarController(object):
|
||||||
# ui mesg is at 100Hz but we send asap if:
|
# ui mesg is at 100Hz but we send asap if:
|
||||||
# - there is something to display
|
# - there is something to display
|
||||||
# - there is something to stop displaying
|
# - there is something to stop displaying
|
||||||
hud1, hud2 = process_hud_alert(hud_alert, audible_alert)
|
alert_out = process_hud_alert(hud_alert, audible_alert)
|
||||||
if ((hud1 or hud2) and not self.alert_active) or \
|
steer, fcw, sound1, sound2 = alert_out
|
||||||
(not (hud1 or hud2) and self.alert_active):
|
|
||||||
|
if (any(alert_out) and not self.alert_active) or \
|
||||||
|
(not any(alert_out) and self.alert_active):
|
||||||
send_ui = True
|
send_ui = True
|
||||||
self.alert_active = not self.alert_active
|
self.alert_active = not self.alert_active
|
||||||
else:
|
else:
|
||||||
send_ui = False
|
send_ui = False
|
||||||
|
|
||||||
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
|
||||||
can_sends.append(create_ui_command(hud1, hud2))
|
can_sends.append(create_ui_command(steer, sound1, sound2))
|
||||||
|
can_sends.append(create_fcw_command(fcw))
|
||||||
|
|
||||||
#*** static msgs ***
|
#*** static msgs ***
|
||||||
|
|
||||||
for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems():
|
for addr, (ecu, cars, bus, fr_step, vl) in STATIC_MSGS.iteritems():
|
||||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
|
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
|
||||||
# send msg!
|
|
||||||
# special cases
|
# special cases
|
||||||
|
|
||||||
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
|
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
|
||||||
cnt = (((frame / 5) % 7) + 1) << 5
|
cnt = (((frame / 5) % 7) + 1) << 5
|
||||||
vl = chr(cnt) + vl
|
vl = chr(cnt) + vl
|
||||||
|
|
|
@ -1,8 +1,7 @@
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
from common.realtime import sec_since_boot
|
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
from selfdrive.car.toyota.carcontroller import CAR
|
from selfdrive.car.toyota.values import CAR
|
||||||
from selfdrive.can.parser import CANParser
|
from selfdrive.can.parser import CANParser
|
||||||
from selfdrive.config import Conversions as CV
|
from selfdrive.config import Conversions as CV
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
@ -85,6 +84,7 @@ def get_can_parser(CP):
|
||||||
("STEER_TORQUE_EPS", 608, 0),
|
("STEER_TORQUE_EPS", 608, 0),
|
||||||
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
|
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
|
||||||
("LKA_STATE", 610, 0),
|
("LKA_STATE", 610, 0),
|
||||||
|
("BRAKE_LIGHTS_ACC", 951, 0),
|
||||||
]
|
]
|
||||||
|
|
||||||
checks += [
|
checks += [
|
||||||
|
@ -100,14 +100,13 @@ def get_can_parser(CP):
|
||||||
|
|
||||||
|
|
||||||
class CarState(object):
|
class CarState(object):
|
||||||
def __init__(self, CP, logcan):
|
def __init__(self, CP):
|
||||||
|
|
||||||
self.CP = CP
|
self.CP = CP
|
||||||
self.left_blinker_on = 0
|
self.left_blinker_on = 0
|
||||||
self.right_blinker_on = 0
|
self.right_blinker_on = 0
|
||||||
|
|
||||||
# initialize can parser
|
# initialize can parser
|
||||||
self.cp = get_can_parser(CP)
|
|
||||||
self.car_fingerprint = CP.carFingerprint
|
self.car_fingerprint = CP.carFingerprint
|
||||||
|
|
||||||
# vEgo kalman filter
|
# vEgo kalman filter
|
||||||
|
@ -117,14 +116,13 @@ class CarState(object):
|
||||||
self.v_ego_C = np.matrix([1.0, 0.0])
|
self.v_ego_C = np.matrix([1.0, 0.0])
|
||||||
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||||
self.v_ego_R = 1e3
|
self.v_ego_R = 1e3
|
||||||
|
self.v_ego = 0.0
|
||||||
# import control
|
# import control
|
||||||
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
||||||
# self.v_ego_K = np.transpose(K)
|
# self.v_ego_K = np.transpose(K)
|
||||||
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
||||||
|
|
||||||
def update(self):
|
def update(self, cp):
|
||||||
cp = self.cp
|
|
||||||
cp.update(int(sec_since_boot() * 1e9), False)
|
|
||||||
|
|
||||||
# copy can_valid
|
# copy can_valid
|
||||||
self.can_valid = cp.can_valid
|
self.can_valid = cp.can_valid
|
||||||
|
@ -160,6 +158,8 @@ class CarState(object):
|
||||||
cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
|
cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
|
||||||
|
|
||||||
# Kalman filter
|
# Kalman filter
|
||||||
|
if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||||
|
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
|
||||||
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
|
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
|
||||||
self.v_ego_raw = self.v_wheel
|
self.v_ego_raw = self.v_wheel
|
||||||
self.v_ego = float(self.v_ego_x[0])
|
self.v_ego = float(self.v_ego_x[0])
|
||||||
|
@ -185,3 +185,4 @@ class CarState(object):
|
||||||
self.car_gas = self.pedal_gas
|
self.car_gas = self.pedal_gas
|
||||||
self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
|
self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
|
||||||
self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2
|
self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2
|
||||||
|
self.brake_lights = bool(cp.vl[951]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||||
|
|
|
@ -1,19 +1,19 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
|
from common.realtime import sec_since_boot
|
||||||
import common.numpy_fast as np
|
import common.numpy_fast as np
|
||||||
from selfdrive.config import Conversions as CV
|
from selfdrive.config import Conversions as CV
|
||||||
from selfdrive.car.toyota.carstate import CarState, CAR
|
from selfdrive.car.toyota.carstate import CarState, get_can_parser
|
||||||
from selfdrive.car.toyota.carcontroller import CarController, ECU, check_ecu_msgs
|
from selfdrive.car.toyota.values import CAR, ECU
|
||||||
|
from selfdrive.car.toyota.carcontroller import CarController, check_ecu_msgs
|
||||||
from cereal import car
|
from cereal import car
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(object):
|
class CarInterface(object):
|
||||||
def __init__(self, CP, logcan, sendcan=None):
|
def __init__(self, CP, sendcan=None):
|
||||||
self.logcan = logcan
|
|
||||||
self.CP = CP
|
self.CP = CP
|
||||||
|
|
||||||
self.frame = 0
|
self.frame = 0
|
||||||
|
@ -23,13 +23,23 @@ class CarInterface(object):
|
||||||
self.cruise_enabled_prev = False
|
self.cruise_enabled_prev = False
|
||||||
|
|
||||||
# *** init the major players ***
|
# *** init the major players ***
|
||||||
self.CS = CarState(CP, self.logcan)
|
self.CS = CarState(CP)
|
||||||
|
|
||||||
|
self.cp = get_can_parser(CP)
|
||||||
|
|
||||||
# sending if read only is False
|
# sending if read only is False
|
||||||
if sendcan is not None:
|
if sendcan is not None:
|
||||||
self.sendcan = sendcan
|
self.sendcan = sendcan
|
||||||
self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
self.CC = CarController(CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def compute_gb(accel, speed):
|
||||||
|
return float(accel) / 3.0
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||||
|
return 1.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint):
|
def get_params(candidate, fingerprint):
|
||||||
|
|
||||||
|
@ -108,21 +118,25 @@ class CarInterface(object):
|
||||||
ret.enableGas = True
|
ret.enableGas = True
|
||||||
|
|
||||||
ret.steerLimitAlert = False
|
ret.steerLimitAlert = False
|
||||||
|
ret.stoppingControl = False
|
||||||
|
ret.startAccel = 0.0
|
||||||
|
|
||||||
|
ret.longitudinalKpBP = [0., 5., 35.]
|
||||||
|
ret.longitudinalKpV = [3.6, 2.4, 1.5]
|
||||||
|
ret.longitudinalKiBP = [0., 35.]
|
||||||
|
ret.longitudinalKiV = [0.54, 0.36]
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
@staticmethod
|
|
||||||
def compute_gb(accel, speed):
|
|
||||||
# toyota interface is already in accelration cmd, so conversion to gas-brake it's a pass-through.
|
|
||||||
return accel
|
|
||||||
|
|
||||||
# returns a car.CarState
|
# returns a car.CarState
|
||||||
def update(self, c):
|
def update(self, c):
|
||||||
# ******************* do can recv *******************
|
# ******************* do can recv *******************
|
||||||
can_pub_main = []
|
can_pub_main = []
|
||||||
canMonoTimes = []
|
canMonoTimes = []
|
||||||
|
|
||||||
self.CS.update()
|
self.cp.update(int(sec_since_boot() * 1e9), False)
|
||||||
|
|
||||||
|
self.CS.update(self.cp)
|
||||||
|
|
||||||
# create message
|
# create message
|
||||||
ret = car.CarState.new_message()
|
ret = car.CarState.new_message()
|
||||||
|
@ -147,6 +161,7 @@ class CarInterface(object):
|
||||||
# brake pedal
|
# brake pedal
|
||||||
ret.brake = self.CS.user_brake
|
ret.brake = self.CS.user_brake
|
||||||
ret.brakePressed = self.CS.brake_pressed != 0
|
ret.brakePressed = self.CS.brake_pressed != 0
|
||||||
|
ret.brakeLights = self.CS.brake_lights
|
||||||
|
|
||||||
# steering wheel
|
# steering wheel
|
||||||
ret.steeringAngle = self.CS.angle_steers
|
ret.steeringAngle = self.CS.angle_steers
|
||||||
|
@ -160,6 +175,7 @@ class CarInterface(object):
|
||||||
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
|
||||||
ret.cruiseState.available = bool(self.CS.main_on)
|
ret.cruiseState.available = bool(self.CS.main_on)
|
||||||
ret.cruiseState.speedOffset = 0.
|
ret.cruiseState.speedOffset = 0.
|
||||||
|
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||||
|
|
||||||
# TODO: button presses
|
# TODO: button presses
|
||||||
buttonEvents = []
|
buttonEvents = []
|
||||||
|
@ -177,6 +193,8 @@ class CarInterface(object):
|
||||||
buttonEvents.append(be)
|
buttonEvents.append(be)
|
||||||
|
|
||||||
ret.buttonEvents = buttonEvents
|
ret.buttonEvents = buttonEvents
|
||||||
|
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||||
|
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||||
|
|
||||||
# events
|
# events
|
||||||
events = []
|
events = []
|
||||||
|
|
|
@ -68,16 +68,24 @@ def create_steer_command(steer, raw_cnt):
|
||||||
return make_can_msg(0x2e4, msg, 0, True)
|
return make_can_msg(0x2e4, msg, 0, True)
|
||||||
|
|
||||||
|
|
||||||
def create_accel_command(accel, pcm_cancel):
|
def create_accel_command(accel, pcm_cancel, standstill_req):
|
||||||
# TODO: find the exact canceling bit
|
# TODO: find the exact canceling bit
|
||||||
state = 0xc0 + pcm_cancel # this allows automatic restart from hold without driver cmd
|
state = 0x40 if standstill_req else 0xC0
|
||||||
|
state += pcm_cancel # this allows automatic restart from hold without driver cmd
|
||||||
|
|
||||||
msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
|
msg = struct.pack("!hBBBBB", accel, 0x63, state, 0x00, 0x00, 0x00)
|
||||||
|
|
||||||
return make_can_msg(0x343, msg, 0, True)
|
return make_can_msg(0x343, msg, 0, True)
|
||||||
|
|
||||||
|
def create_fcw_command(fcw):
|
||||||
|
|
||||||
def create_ui_command(hud1, hud2):
|
msg = struct.pack("!BBBBBBBB", fcw<<4, 0x20, 0x00, 0x00, 0x10, 0x00, 0x80, 0x00)
|
||||||
msg = struct.pack('!BBBBBBBB', 0x54, 0x04 + hud1 + (hud2 << 4), 0x0C,
|
|
||||||
0x00, 0x00, 0x2C, 0x38, 0x02)
|
return make_can_msg(0x411, msg, 0, False)
|
||||||
|
|
||||||
|
|
||||||
|
def create_ui_command(steer, sound1, sound2):
|
||||||
|
|
||||||
|
msg = struct.pack("!BBBBBBBB", 0x54, 0x04 + steer + (sound2<<4), 0x0C, 0x00,
|
||||||
|
sound1, 0x2C, 0x38, 0x02)
|
||||||
return make_can_msg(0x412, msg, 0, False)
|
return make_can_msg(0x412, msg, 0, False)
|
||||||
|
|
|
@ -0,0 +1,8 @@
|
||||||
|
class CAR:
|
||||||
|
PRIUS = "TOYOTA PRIUS 2017"
|
||||||
|
RAV4 = "TOYOTA RAV4 2017"
|
||||||
|
|
||||||
|
class ECU:
|
||||||
|
CAM = 0 # camera
|
||||||
|
DSU = 1 # driving support unit
|
||||||
|
APGS = 2 # advanced parking guidance system
|
|
@ -1 +1 @@
|
||||||
#define COMMA_VERSION "0.3.8.2-openpilot"
|
#define COMMA_VERSION "0.3.9-openpilot"
|
||||||
|
|
|
@ -21,12 +21,14 @@ class Conversions:
|
||||||
|
|
||||||
|
|
||||||
# Car button codes
|
# Car button codes
|
||||||
|
# TODO: this is Honda specific, move to honda/interface.py
|
||||||
class CruiseButtons:
|
class CruiseButtons:
|
||||||
RES_ACCEL = 4
|
RES_ACCEL = 4
|
||||||
DECEL_SET = 3
|
DECEL_SET = 3
|
||||||
CANCEL = 2
|
CANCEL = 2
|
||||||
MAIN = 1
|
MAIN = 1
|
||||||
|
|
||||||
|
RADAR_TO_CENTER = 2.7 # RADAR is ~ 2.7m ahead from center of car
|
||||||
|
|
||||||
# Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
|
# Image params for color cam on acura, calibrated on pre las vegas drive (2016-05-21)
|
||||||
class ImageParams:
|
class ImageParams:
|
||||||
|
|
|
@ -1,15 +1,13 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
import os
|
|
||||||
import json
|
import json
|
||||||
from copy import copy
|
from copy import copy
|
||||||
import zmq
|
import zmq
|
||||||
from cereal import car, log
|
from cereal import car
|
||||||
from common.numpy_fast import clip
|
from common.numpy_fast import clip
|
||||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
||||||
from common.profiler import Profiler
|
from common.profiler import Profiler
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
from selfdrive.swaglog import cloudlog
|
|
||||||
from selfdrive.config import Conversions as CV
|
from selfdrive.config import Conversions as CV
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
from selfdrive.car import get_car
|
from selfdrive.car import get_car
|
||||||
|
@ -22,7 +20,7 @@ from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEE
|
||||||
from selfdrive.controls.lib.latcontrol import LatControl
|
from selfdrive.controls.lib.latcontrol import LatControl
|
||||||
from selfdrive.controls.lib.alertmanager import AlertManager
|
from selfdrive.controls.lib.alertmanager import AlertManager
|
||||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||||
from selfdrive.controls.lib.adaptivecruise import A_ACC_MAX
|
|
||||||
|
|
||||||
V_CRUISE_MAX = 144
|
V_CRUISE_MAX = 144
|
||||||
V_CRUISE_MIN = 8
|
V_CRUISE_MIN = 8
|
||||||
|
@ -31,7 +29,6 @@ V_CRUISE_ENABLE_MIN = 40
|
||||||
|
|
||||||
AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels
|
AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels
|
||||||
AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
|
AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car
|
||||||
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
|
|
||||||
|
|
||||||
|
|
||||||
class Calibration:
|
class Calibration:
|
||||||
|
@ -103,9 +100,9 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space):
|
||||||
return CS, events, cal_status, overtemp, free_space
|
return CS, events, cal_status, overtemp, free_space
|
||||||
|
|
||||||
|
|
||||||
def calc_plan(CS, events, PL, LoC):
|
def calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status):
|
||||||
# plan runs always, independently of the state
|
# plan runs always, independently of the state
|
||||||
plan_packet = PL.update(CS, LoC)
|
plan_packet = PL.update(CS, LoC, v_cruise_kph, awareness_status < -0.)
|
||||||
plan = plan_packet.plan
|
plan = plan_packet.plan
|
||||||
plan_ts = plan_packet.logMonoTime
|
plan_ts = plan_packet.logMonoTime
|
||||||
|
|
||||||
|
@ -114,7 +111,7 @@ def calc_plan(CS, events, PL, LoC):
|
||||||
|
|
||||||
# disable if lead isn't close when system is active and brake is pressed to avoid
|
# disable if lead isn't close when system is active and brake is pressed to avoid
|
||||||
# unexpected vehicle accelerations
|
# unexpected vehicle accelerations
|
||||||
if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED:
|
if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED:
|
||||||
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||||
|
|
||||||
return plan, plan_ts
|
return plan, plan_ts
|
||||||
|
@ -265,24 +262,18 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s
|
||||||
for e in get_events(events, [ET.WARNING]):
|
for e in get_events(events, [ET.WARNING]):
|
||||||
AM.add(e, enabled)
|
AM.add(e, enabled)
|
||||||
|
|
||||||
# if user is not responsive to awareness alerts, then start a smooth deceleration
|
|
||||||
if awareness_status < -0.:
|
|
||||||
plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
|
|
||||||
plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
|
|
||||||
|
|
||||||
# *** angle offset learning ***
|
# *** angle offset learning ***
|
||||||
|
|
||||||
if rk.frame % 5 == 2 and plan.lateralValid:
|
if rk.frame % 5 == 2 and plan.lateralValid:
|
||||||
# *** run this at 20hz again ***
|
# *** run this at 20hz again ***
|
||||||
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
|
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
|
||||||
PL.PP.c_poly, PL.PP.c_prob, LaC.y_des,
|
PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle,
|
||||||
CS.steeringPressed)
|
CS.steeringPressed)
|
||||||
|
|
||||||
# *** gas/brake PID loop ***
|
# *** gas/brake PID loop ***
|
||||||
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill,
|
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
|
||||||
v_cruise_kph, plan.vTarget,
|
v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget,
|
||||||
[plan.aTargetMin, plan.aTargetMax],
|
CP, PL.lead_1)
|
||||||
plan.jerkFactor, CP)
|
|
||||||
|
|
||||||
# *** steering PID loop ***
|
# *** steering PID loop ***
|
||||||
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
|
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
|
||||||
|
@ -323,11 +314,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
|
||||||
# brake discount removes a sharp nonlinearity
|
# brake discount removes a sharp nonlinearity
|
||||||
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
|
brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0))
|
||||||
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
|
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
|
||||||
|
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget)
|
||||||
# TODO: parametrize 0.714 in interface?
|
|
||||||
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
|
|
||||||
# unless aTargetMax is very high and then we scale with it; this helpw in quicker restart
|
|
||||||
CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX))
|
|
||||||
|
|
||||||
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
|
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
|
||||||
CC.hudControl.speedVisible = isEnabled(state)
|
CC.hudControl.speedVisible = isEnabled(state)
|
||||||
|
@ -356,6 +343,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
|
||||||
|
|
||||||
# if controls is enabled
|
# if controls is enabled
|
||||||
dat.live100.enabled = isEnabled(state)
|
dat.live100.enabled = isEnabled(state)
|
||||||
|
dat.live100.active = isActive(state)
|
||||||
|
|
||||||
# car state
|
# car state
|
||||||
dat.live100.vEgo = CS.vEgo
|
dat.live100.vEgo = CS.vEgo
|
||||||
|
@ -372,17 +360,17 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph,
|
||||||
dat.live100.vCruise = float(v_cruise_kph)
|
dat.live100.vCruise = float(v_cruise_kph)
|
||||||
dat.live100.upAccelCmd = float(LoC.pid.p)
|
dat.live100.upAccelCmd = float(LoC.pid.p)
|
||||||
dat.live100.uiAccelCmd = float(LoC.pid.i)
|
dat.live100.uiAccelCmd = float(LoC.pid.i)
|
||||||
|
dat.live100.ufAccelCmd = float(LoC.pid.f)
|
||||||
|
|
||||||
# lateral control state
|
# lateral control state
|
||||||
dat.live100.yDes = float(LaC.y_des)
|
|
||||||
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
|
dat.live100.angleSteersDes = float(LaC.angle_steers_des)
|
||||||
dat.live100.upSteer = float(LaC.pid.p)
|
dat.live100.upSteer = float(LaC.pid.p)
|
||||||
dat.live100.uiSteer = float(LaC.pid.i)
|
dat.live100.uiSteer = float(LaC.pid.i)
|
||||||
|
dat.live100.ufSteer = float(LaC.pid.f)
|
||||||
|
|
||||||
# processed radar state, should add a_pcm?
|
# processed radar state, should add a_pcm?
|
||||||
dat.live100.vTargetLead = float(plan.vTarget)
|
dat.live100.vTargetLead = float(plan.vTarget)
|
||||||
dat.live100.aTargetMin = float(plan.aTargetMin)
|
dat.live100.aTarget = float(plan.aTarget)
|
||||||
dat.live100.aTargetMax = float(plan.aTargetMax)
|
|
||||||
dat.live100.jerkFactor = float(plan.jerkFactor)
|
dat.live100.jerkFactor = float(plan.jerkFactor)
|
||||||
|
|
||||||
# log learned angle offset
|
# log learned angle offset
|
||||||
|
@ -459,8 +447,10 @@ def controlsd_thread(gctx, rate=100):
|
||||||
if passive:
|
if passive:
|
||||||
CP.safetyModel = car.CarParams.SafetyModels.noOutput
|
CP.safetyModel = car.CarParams.SafetyModels.noOutput
|
||||||
|
|
||||||
PL = Planner(CP)
|
fcw_enabled = params.get("IsFcwEnabled") == "1"
|
||||||
LoC = LongControl(CI.compute_gb)
|
|
||||||
|
PL = Planner(CP, fcw_enabled)
|
||||||
|
LoC = LongControl(CP, CI.compute_gb)
|
||||||
VM = VehicleModel(CP)
|
VM = VehicleModel(CP)
|
||||||
LaC = LatControl(VM)
|
LaC = LatControl(VM)
|
||||||
AM = AlertManager()
|
AM = AlertManager()
|
||||||
|
@ -481,7 +471,7 @@ def controlsd_thread(gctx, rate=100):
|
||||||
rear_view_allowed = params.get("IsRearViewMirror") == "1"
|
rear_view_allowed = params.get("IsRearViewMirror") == "1"
|
||||||
|
|
||||||
# 0.0 - 1.0
|
# 0.0 - 1.0
|
||||||
awareness_status = 0.
|
awareness_status = 1.
|
||||||
|
|
||||||
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
|
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
|
||||||
|
|
||||||
|
@ -507,7 +497,7 @@ def controlsd_thread(gctx, rate=100):
|
||||||
prof.checkpoint("Sample")
|
prof.checkpoint("Sample")
|
||||||
|
|
||||||
# define plan
|
# define plan
|
||||||
plan, plan_ts = calc_plan(CS, events, PL, LoC)
|
plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status)
|
||||||
prof.checkpoint("Plan")
|
prof.checkpoint("Plan")
|
||||||
|
|
||||||
if not passive:
|
if not passive:
|
||||||
|
|
|
@ -1,294 +0,0 @@
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
from common.numpy_fast import clip, interp
|
|
||||||
import selfdrive.messaging as messaging
|
|
||||||
|
|
||||||
# TODO: we compute a_pcm but we don't use it, as accelOverride is hardcoded in controlsd
|
|
||||||
|
|
||||||
# lookup tables VS speed to determine min and max accels in cruise
|
|
||||||
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
|
|
||||||
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
|
||||||
|
|
||||||
# need fast accel at very low speed for stop and go
|
|
||||||
_A_CRUISE_MAX_V = [1., 1., .8, .5, .30]
|
|
||||||
_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
|
|
||||||
|
|
||||||
def calc_cruise_accel_limits(v_ego):
|
|
||||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
|
||||||
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
|
||||||
return np.vstack([a_cruise_min, a_cruise_max])
|
|
||||||
|
|
||||||
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
|
|
||||||
_A_TOTAL_MAX_BP = [0., 20., 40.]
|
|
||||||
|
|
||||||
def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP):
|
|
||||||
#*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
|
||||||
# this should avoid accelerating when losing the target in turns
|
|
||||||
deg_to_rad = np.pi / 180. # from can reading to rad
|
|
||||||
|
|
||||||
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
|
||||||
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
|
|
||||||
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
|
|
||||||
|
|
||||||
a_target[1] = min(a_target[1], a_x_allowed)
|
|
||||||
a_pcm = min(a_pcm, a_x_allowed)
|
|
||||||
return a_target, a_pcm
|
|
||||||
|
|
||||||
def process_a_lead(a_lead):
|
|
||||||
# soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
|
|
||||||
a_lead_threshold = 0.5
|
|
||||||
a_lead = min(a_lead + a_lead_threshold, 0)
|
|
||||||
return a_lead
|
|
||||||
|
|
||||||
def calc_desired_distance(v_lead):
|
|
||||||
#*** compute desired distance ***
|
|
||||||
t_gap = 1.7 # good to be far away
|
|
||||||
d_offset = 4 # distance when at zero speed
|
|
||||||
return d_offset + v_lead * t_gap
|
|
||||||
|
|
||||||
|
|
||||||
#linear slope
|
|
||||||
_L_SLOPE_V = [0.40, 0.10]
|
|
||||||
_L_SLOPE_BP = [0., 40]
|
|
||||||
|
|
||||||
# parabola slope
|
|
||||||
_P_SLOPE_V = [1.0, 0.25]
|
|
||||||
_P_SLOPE_BP = [0., 40]
|
|
||||||
|
|
||||||
def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
|
|
||||||
#*** compute desired speed ***
|
|
||||||
# the desired speed curve is divided in 4 portions:
|
|
||||||
# 1-constant
|
|
||||||
# 2-linear to regain distance
|
|
||||||
# 3-linear to shorten distance
|
|
||||||
# 4-parabolic (constant decel)
|
|
||||||
|
|
||||||
max_runaway_speed = -2. # no slower than 2m/s over the lead
|
|
||||||
|
|
||||||
# interpolate the lookups to find the slopes for a give lead speed
|
|
||||||
l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
|
|
||||||
p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
|
|
||||||
|
|
||||||
# this is where parabola and linear curves are tangents
|
|
||||||
x_linear_to_parabola = p_slope / l_slope**2
|
|
||||||
|
|
||||||
# parabola offset to have the parabola being tangent to the linear curve
|
|
||||||
x_parabola_offset = p_slope / (2 * l_slope**2)
|
|
||||||
|
|
||||||
if d_lead < d_des:
|
|
||||||
# calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
|
|
||||||
v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
|
|
||||||
# calculate v_rel_des on one third of the linear slope
|
|
||||||
v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
|
|
||||||
# take the min of the 2 above
|
|
||||||
v_rel_des = min(v_rel_des_1, v_rel_des_2)
|
|
||||||
v_rel_des = max(v_rel_des, max_runaway_speed)
|
|
||||||
elif d_lead < d_des + x_linear_to_parabola:
|
|
||||||
v_rel_des = (d_lead - d_des) * l_slope
|
|
||||||
v_rel_des = max(v_rel_des, max_runaway_speed)
|
|
||||||
else:
|
|
||||||
v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
|
|
||||||
|
|
||||||
# compute desired speed
|
|
||||||
v_target = v_rel_des + v_lead
|
|
||||||
|
|
||||||
# compute v_coast: above this speed we want to coast
|
|
||||||
t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region
|
|
||||||
v_coast_shift = max(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0
|
|
||||||
v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line
|
|
||||||
v_coast = min(v_coast, v_target)
|
|
||||||
|
|
||||||
return v_target, v_coast
|
|
||||||
|
|
||||||
def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
|
|
||||||
# this function computes the required decel to avoid crashing, given safety offsets
|
|
||||||
a_critical = - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5)
|
|
||||||
return a_critical
|
|
||||||
|
|
||||||
|
|
||||||
# maximum acceleration adjustment
|
|
||||||
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
|
|
||||||
# speeds
|
|
||||||
_A_CORR_BY_SPEED_BP = [0., 2., 10.]
|
|
||||||
|
|
||||||
# max acceleration allowed in acc, which happens in restart
|
|
||||||
A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V)
|
|
||||||
|
|
||||||
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
|
|
||||||
a_coast_min = -1.0 # never coast faster then -1m/s^2
|
|
||||||
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
|
|
||||||
# regardless v_target
|
|
||||||
if v_ref > min(v_coast, v_target):
|
|
||||||
# for smooth coast we can be aggressive and target a point where car would actually crash
|
|
||||||
v_offset_coast = 0.
|
|
||||||
d_offset_coast = d_des/2. - 4.
|
|
||||||
|
|
||||||
# acceleration value to smoothly coast until we hit v_target
|
|
||||||
if d_lead > d_offset_coast + 0.1:
|
|
||||||
a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
|
|
||||||
# if lead is decelerating, then offset the coast decel
|
|
||||||
a_coast += a_lead_contr
|
|
||||||
a_max = max(a_coast, a_coast_min)
|
|
||||||
else:
|
|
||||||
a_max = a_coast_min
|
|
||||||
else:
|
|
||||||
# same as cruise accel, plus add a small correction based on relative lead speed
|
|
||||||
# if the lead car is faster, we can accelerate more, if the car is slower, then we can reduce acceleration
|
|
||||||
a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
|
|
||||||
* clip(-v_rel / 4., -.5, 1)
|
|
||||||
return a_max
|
|
||||||
|
|
||||||
# arbitrary limits to avoid too high accel being computed
|
|
||||||
_A_SAT = [-10., 5.]
|
|
||||||
|
|
||||||
# do not consider a_lead at 0m/s, fully consider it at 10m/s
|
|
||||||
_A_LEAD_LOW_SPEED_V = [0., 1.]
|
|
||||||
|
|
||||||
# speed break points
|
|
||||||
_A_LEAD_LOW_SPEED_BP = [0., 10.]
|
|
||||||
|
|
||||||
# add a small offset to the desired decel, just for safety margin
|
|
||||||
_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3]
|
|
||||||
|
|
||||||
# speed bp: different offset based on the likelyhood that lead decels abruptly
|
|
||||||
_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.]
|
|
||||||
|
|
||||||
|
|
||||||
def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
|
|
||||||
v_target, v_coast, a_target, a_pcm):
|
|
||||||
#*** compute max accel ***
|
|
||||||
# v_rel is now your velocity in lead car frame
|
|
||||||
v_rel *= -1 # this simplifies things when thinking in d_rel-v_rel diagram
|
|
||||||
|
|
||||||
v_rel_pid = v_pid - v_lead
|
|
||||||
|
|
||||||
# this is how much lead accel we consider in assigning the desired decel
|
|
||||||
a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP,
|
|
||||||
_A_LEAD_LOW_SPEED_V) * 0.8
|
|
||||||
|
|
||||||
# first call of calc_positive_accel_limit is used to shape v_pid
|
|
||||||
a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
|
|
||||||
v_rel_pid, v_coast, v_target,
|
|
||||||
a_lead_contr, a_target[1])
|
|
||||||
# second call of calc_positive_accel_limit is used to limit the pcm throttle
|
|
||||||
# control (only useful when we don't control throttle directly)
|
|
||||||
a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego,
|
|
||||||
v_rel, v_coast, v_target,
|
|
||||||
a_lead_contr, a_pcm)
|
|
||||||
|
|
||||||
#*** compute max decel ***
|
|
||||||
v_offset = 1. # assume the car is 1m/s slower
|
|
||||||
d_offset = 1. # assume the distance is 1m lower
|
|
||||||
if v_target - v_ego > 0.5:
|
|
||||||
pass # acc target speed is above vehicle speed, so we can use the cruise limits
|
|
||||||
elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions
|
|
||||||
# compute needed accel to get to 1m distance with -1m/s rel speed
|
|
||||||
decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
|
|
||||||
|
|
||||||
critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
|
|
||||||
a_target[0] = min(decel_offset + critical_decel + a_lead_contr,
|
|
||||||
a_target[0])
|
|
||||||
else:
|
|
||||||
a_target[0] = _A_SAT[0]
|
|
||||||
# a_min can't be higher than a_max
|
|
||||||
a_target[0] = min(a_target[0], a_target[1])
|
|
||||||
# final check on limits
|
|
||||||
a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
|
|
||||||
a_target = a_target.tolist()
|
|
||||||
return a_target, a_pcm
|
|
||||||
|
|
||||||
def calc_jerk_factor(d_lead, v_rel):
|
|
||||||
# we don't have an explicit jerk limit, so this function calculates a factor
|
|
||||||
# that is used by the PID controller to scale the gains. Not the cleanest solution
|
|
||||||
# but we need this for the demo.
|
|
||||||
# TODO: Calculate Kp and Ki directly in this function.
|
|
||||||
|
|
||||||
# the higher is the decel required to avoid a crash, the higher is the PI factor scaling
|
|
||||||
d_offset = 0.5
|
|
||||||
v_offset = 2.
|
|
||||||
a_offset = 1.
|
|
||||||
jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
|
|
||||||
if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
|
|
||||||
jerk_factor = jerk_factor_max
|
|
||||||
else:
|
|
||||||
a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
|
|
||||||
# increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
|
|
||||||
jerk_factor = max(a_critical - a_offset, 0.)/5.
|
|
||||||
jerk_factor = min(jerk_factor, jerk_factor_max)
|
|
||||||
return jerk_factor
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
MAX_SPEED_POSSIBLE = 55.
|
|
||||||
|
|
||||||
def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP):
|
|
||||||
# drive limits
|
|
||||||
# TODO: Make lims function of speed (more aggressive at low speed).
|
|
||||||
a_lim = [-3., 1.5]
|
|
||||||
|
|
||||||
#*** set target speed pretty high, as lead hasn't been considered yet
|
|
||||||
v_target_lead = MAX_SPEED_POSSIBLE
|
|
||||||
|
|
||||||
#*** set accel limits as cruise accel/decel limits ***
|
|
||||||
a_target = calc_cruise_accel_limits(v_ego)
|
|
||||||
|
|
||||||
# start with 1
|
|
||||||
a_pcm = 1.
|
|
||||||
|
|
||||||
#*** limit max accel in sharp turns
|
|
||||||
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, CP)
|
|
||||||
jerk_factor = 0.
|
|
||||||
|
|
||||||
if l1 is not None and l1.status:
|
|
||||||
#*** process noisy a_lead signal from radar processing ***
|
|
||||||
a_lead_p = process_a_lead(l1.aLeadK)
|
|
||||||
|
|
||||||
#*** compute desired distance ***
|
|
||||||
d_des = calc_desired_distance(l1.vLead)
|
|
||||||
|
|
||||||
#*** compute desired speed ***
|
|
||||||
v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
|
|
||||||
|
|
||||||
if l2 is not None and l2.status:
|
|
||||||
#*** process noisy a_lead signal from radar processing ***
|
|
||||||
a_lead_p2 = process_a_lead(l2.aLeadK)
|
|
||||||
|
|
||||||
#*** compute desired distance ***
|
|
||||||
d_des2 = calc_desired_distance(l2.vLead)
|
|
||||||
|
|
||||||
#*** compute desired speed ***
|
|
||||||
v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
|
|
||||||
|
|
||||||
# listen to lead that makes you go slower
|
|
||||||
if v_target_lead2 < v_target_lead:
|
|
||||||
l1 = l2
|
|
||||||
d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
|
|
||||||
|
|
||||||
# l1 is the main lead now
|
|
||||||
|
|
||||||
#*** compute accel limits ***
|
|
||||||
a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
|
|
||||||
l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
|
|
||||||
|
|
||||||
# we can now limit a_target to a_lim
|
|
||||||
a_target = np.clip(a_target1, a_lim[0], a_lim[1])
|
|
||||||
a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
|
|
||||||
|
|
||||||
#*** compute max factor ***
|
|
||||||
jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
|
|
||||||
|
|
||||||
# force coasting decel if driver hasn't been controlling car in a while
|
|
||||||
return v_target_lead, a_target, a_pcm, jerk_factor
|
|
||||||
|
|
||||||
|
|
||||||
class AdaptiveCruise(object):
|
|
||||||
def __init__(self):
|
|
||||||
self.l1, self.l2 = None, None
|
|
||||||
def update(self, v_ego, angle_steers, v_pid, CP, l20):
|
|
||||||
if l20 is not None:
|
|
||||||
self.l1 = l20.live20.leadOne
|
|
||||||
self.l2 = l20.live20.leadTwo
|
|
||||||
|
|
||||||
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
|
|
||||||
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP)
|
|
||||||
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
|
|
|
@ -59,9 +59,9 @@ class AlertManager(object):
|
||||||
PT.MID, None, "beepSingle", .2, 0., 0.),
|
PT.MID, None, "beepSingle", .2, 0., 0.),
|
||||||
|
|
||||||
"fcw": Alert(
|
"fcw": Alert(
|
||||||
"",
|
"Brake",
|
||||||
"",
|
"Risk of Collision",
|
||||||
PT.LOW, None, None, .1, .1, .1),
|
PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.),
|
||||||
|
|
||||||
"steerSaturated": Alert(
|
"steerSaturated": Alert(
|
||||||
"Take Control",
|
"Take Control",
|
||||||
|
|
|
@ -33,7 +33,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
|
||||||
return clip(new_value, last_value + dw_step, last_value + up_step)
|
return clip(new_value, last_value + dw_step, last_value + up_step)
|
||||||
|
|
||||||
|
|
||||||
def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
|
def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override):
|
||||||
# simple integral controller that learns how much steering offset to put to have the car going straight
|
# simple integral controller that learns how much steering offset to put to have the car going straight
|
||||||
# while being in the middle of the lane
|
# while being in the middle of the lane
|
||||||
min_offset = -5. # deg
|
min_offset = -5. # deg
|
||||||
|
@ -42,7 +42,7 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_d
|
||||||
min_learn_speed = 1.
|
min_learn_speed = 1.
|
||||||
|
|
||||||
# learn less at low speed or when turning
|
# learn less at low speed or when turning
|
||||||
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.5*abs(y_des))
|
alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.2 * abs(angle_steers))
|
||||||
|
|
||||||
# only learn if lateral control is active and if driver is not overriding:
|
# only learn if lateral control is active and if driver is not overriding:
|
||||||
if lateral_control and not steer_override:
|
if lateral_control and not steer_override:
|
||||||
|
|
|
@ -1,66 +0,0 @@
|
||||||
import numpy as np
|
|
||||||
from common.realtime import sec_since_boot
|
|
||||||
|
|
||||||
#Time to collisions greater than 5s are iognored
|
|
||||||
MAX_TTC = 5.
|
|
||||||
|
|
||||||
def calc_ttc(l1):
|
|
||||||
# if l1 is None, return max ttc immediately
|
|
||||||
if not l1:
|
|
||||||
return MAX_TTC
|
|
||||||
# this function returns the time to collision (ttc), assuming that
|
|
||||||
# ARel will stay constant TODO: review this assumptions
|
|
||||||
# change sign to rel quantities as it's going to be easier for calculations
|
|
||||||
vRel = -l1.vRel
|
|
||||||
aRel = -l1.aRel
|
|
||||||
|
|
||||||
# assuming that closing gap ARel comes from lead vehicle decel,
|
|
||||||
# then limit ARel so that v_lead will get to zero in no sooner than t_decel.
|
|
||||||
# This helps underweighting ARel when v_lead is close to zero.
|
|
||||||
t_decel = 2.
|
|
||||||
aRel = np.minimum(aRel, l1.vLead/t_decel)
|
|
||||||
|
|
||||||
# delta of the quadratic equation to solve for ttc
|
|
||||||
delta = vRel**2 + 2 * l1.dRel * aRel
|
|
||||||
|
|
||||||
# assign an arbitrary high ttc value if there is no solution to ttc
|
|
||||||
if delta < 0.1 or (np.sqrt(delta) + vRel < 0.1):
|
|
||||||
ttc = MAX_TTC
|
|
||||||
else:
|
|
||||||
ttc = np.minimum(2 * l1.dRel / (np.sqrt(delta) + vRel), MAX_TTC)
|
|
||||||
return ttc
|
|
||||||
|
|
||||||
class ForwardCollisionWarning(object):
|
|
||||||
def __init__(self, dt):
|
|
||||||
self.last_active = 0.
|
|
||||||
self.violation_time = 0.
|
|
||||||
self.active = False
|
|
||||||
self.dt = dt # time step
|
|
||||||
|
|
||||||
def process(self, CS, AC):
|
|
||||||
# send an fcw alert if the violation time > violation_thrs
|
|
||||||
violation_thrs = 0.3 # fcw turns on after a continuous violation for this time
|
|
||||||
fcw_t_delta = 5. # no more than one fcw alert within this time
|
|
||||||
a_acc_on = -2.0 # with system on, above this limit of desired decel, we should trigger fcw
|
|
||||||
a_acc_off = -2.5 # with system off, above this limit of desired decel, we should trigger fcw
|
|
||||||
ttc_thrs = 2.5 # ttc threshold for fcw
|
|
||||||
v_fcw_min = 5. # no fcw below 5m/s
|
|
||||||
steer_angle_th = 40. # deg, no fcw above this steer angle
|
|
||||||
cur_time = sec_since_boot()
|
|
||||||
|
|
||||||
ttc = calc_ttc(AC.l1)
|
|
||||||
a_fcw = a_acc_on if CS.cruiseState.enabled else a_acc_off
|
|
||||||
|
|
||||||
# increase violation time if we want to decelerate quite fast
|
|
||||||
if AC.l1 and ( \
|
|
||||||
(CS.vEgo > v_fcw_min) and (CS.vEgo > AC.v_target_lead) and (AC.a_target[0] < a_fcw) \
|
|
||||||
and not CS.brakePressed and ttc < ttc_thrs and abs(CS.steeringAngle) < steer_angle_th \
|
|
||||||
and AC.l1.fcw):
|
|
||||||
self.violation_time = np.minimum(self.violation_time + self.dt, violation_thrs)
|
|
||||||
else:
|
|
||||||
self.violation_time = np.maximum(self.violation_time - 2*self.dt, 0)
|
|
||||||
|
|
||||||
# fire FCW
|
|
||||||
self.active = self.violation_time >= violation_thrs and cur_time > (self.last_active + fcw_t_delta)
|
|
||||||
if self.active:
|
|
||||||
self.last_active = cur_time
|
|
|
@ -2,11 +2,14 @@ import math
|
||||||
from selfdrive.controls.lib.pid import PIController
|
from selfdrive.controls.lib.pid import PIController
|
||||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||||
from common.numpy_fast import clip, interp
|
from common.numpy_fast import clip, interp
|
||||||
|
from common.realtime import sec_since_boot
|
||||||
|
|
||||||
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
|
# 100ms is a rule of thumb estimation of lag from image processing to actuator command
|
||||||
ACTUATORS_DELAY = 0.1
|
ACTUATORS_DELAY = 0.1
|
||||||
|
|
||||||
|
_DT = 0.01 # 100Hz
|
||||||
|
_DT_MPC = 0.05 # 20Hz
|
||||||
|
|
||||||
|
|
||||||
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
|
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio):
|
||||||
states[0].x = v_ego * ACTUATORS_DELAY
|
states[0].x = v_ego * ACTUATORS_DELAY
|
||||||
|
@ -23,8 +26,6 @@ class LatControl(object):
|
||||||
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
|
self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0)
|
||||||
self.setup_mpc()
|
self.setup_mpc()
|
||||||
|
|
||||||
self.y_des = -1 # Legacy
|
|
||||||
|
|
||||||
def setup_mpc(self):
|
def setup_mpc(self):
|
||||||
self.libmpc = libmpc_py.libmpc
|
self.libmpc = libmpc_py.libmpc
|
||||||
self.libmpc.init()
|
self.libmpc.init()
|
||||||
|
@ -38,15 +39,20 @@ class LatControl(object):
|
||||||
self.cur_state[0].delta = 0.0
|
self.cur_state[0].delta = 0.0
|
||||||
|
|
||||||
self.last_mpc_ts = 0.0
|
self.last_mpc_ts = 0.0
|
||||||
self.angle_steers_des = 0
|
self.angle_steers_des = 0.0
|
||||||
|
self.angle_steers_des_mpc = 0.0
|
||||||
|
self.angle_steers_des_prev = 0.0
|
||||||
|
self.angle_steers_des_time = 0.0
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.pid.reset()
|
self.pid.reset()
|
||||||
|
|
||||||
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
|
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
|
||||||
|
cur_time = sec_since_boot()
|
||||||
self.mpc_updated = False
|
self.mpc_updated = False
|
||||||
if self.last_mpc_ts + 0.001 < PL.last_md_ts:
|
if self.last_mpc_ts < PL.last_md_ts:
|
||||||
self.last_mpc_ts = PL.last_md_ts
|
self.last_mpc_ts = PL.last_md_ts
|
||||||
|
self.angle_steers_des_prev = self.angle_steers_des_mpc
|
||||||
|
|
||||||
curvature_factor = VM.curvature_factor(v_ego)
|
curvature_factor = VM.curvature_factor(v_ego)
|
||||||
|
|
||||||
|
@ -65,16 +71,19 @@ class LatControl(object):
|
||||||
delta_desired = self.mpc_solution[0].delta[1]
|
delta_desired = self.mpc_solution[0].delta[1]
|
||||||
self.cur_state[0].delta = delta_desired
|
self.cur_state[0].delta = delta_desired
|
||||||
|
|
||||||
self.angle_steers_des = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
|
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.CP.sR) + angle_offset)
|
||||||
|
self.angle_steers_des_time = cur_time
|
||||||
self.mpc_updated = True
|
self.mpc_updated = True
|
||||||
|
|
||||||
if v_ego < 0.3 or not active:
|
if v_ego < 0.3 or not active:
|
||||||
output_steer = 0.0
|
output_steer = 0.0
|
||||||
self.pid.reset()
|
self.pid.reset()
|
||||||
else:
|
else:
|
||||||
steer_max = get_steer_max(VM.CP, v_ego)
|
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.pid.pos_limit = steer_max
|
self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
|
||||||
self.pid.neg_limit = -steer_max
|
steers_max = get_steer_max(VM.CP, v_ego)
|
||||||
|
self.pid.pos_limit = steers_max
|
||||||
|
self.pid.neg_limit = -steers_max
|
||||||
steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
|
steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
|
||||||
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
|
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward)
|
||||||
|
|
||||||
|
|
|
@ -4,7 +4,8 @@ from selfdrive.config import Conversions as CV
|
||||||
from selfdrive.controls.lib.pid import PIController
|
from selfdrive.controls.lib.pid import PIController
|
||||||
|
|
||||||
STOPPING_EGO_SPEED = 0.5
|
STOPPING_EGO_SPEED = 0.5
|
||||||
STOPPING_TARGET_SPEED = 0.3
|
MIN_CAN_SPEED = 0.3
|
||||||
|
STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01
|
||||||
STARTING_TARGET_SPEED = 0.5
|
STARTING_TARGET_SPEED = 0.5
|
||||||
BRAKE_THRESHOLD_TO_PID = 0.2
|
BRAKE_THRESHOLD_TO_PID = 0.2
|
||||||
|
|
||||||
|
@ -18,12 +19,16 @@ class LongCtrlState:
|
||||||
starting = 'starting' # starting and releasing brakes in open loop before giving back to PID
|
starting = 'starting' # starting and releasing brakes in open loop before giving back to PID
|
||||||
|
|
||||||
|
|
||||||
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
|
||||||
output_gb, brake_pressed):
|
|
||||||
|
|
||||||
stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \
|
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
||||||
(((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or
|
output_gb, brake_pressed, cruise_standstill):
|
||||||
(brake_pressed))
|
|
||||||
|
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
|
||||||
|
(v_ego < STOPPING_EGO_SPEED and \
|
||||||
|
((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or
|
||||||
|
brake_pressed))
|
||||||
|
|
||||||
|
starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
|
||||||
|
|
||||||
if not active:
|
if not active:
|
||||||
long_control_state = LongCtrlState.off
|
long_control_state = LongCtrlState.off
|
||||||
|
@ -38,7 +43,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
||||||
long_control_state = LongCtrlState.stopping
|
long_control_state = LongCtrlState.stopping
|
||||||
|
|
||||||
elif long_control_state == LongCtrlState.stopping:
|
elif long_control_state == LongCtrlState.stopping:
|
||||||
if (v_target > STARTING_TARGET_SPEED):
|
if starting_condition:
|
||||||
long_control_state = LongCtrlState.starting
|
long_control_state = LongCtrlState.starting
|
||||||
|
|
||||||
elif long_control_state == LongCtrlState.starting:
|
elif long_control_state == LongCtrlState.starting:
|
||||||
|
@ -50,15 +55,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
||||||
return long_control_state
|
return long_control_state
|
||||||
|
|
||||||
|
|
||||||
_KP_BP = [0., 5., 35.]
|
|
||||||
_KP_V = [1.2, 0.8, 0.5]
|
|
||||||
|
|
||||||
_KI_BP = [0., 35.]
|
|
||||||
_KI_V = [0.18, 0.12]
|
|
||||||
|
|
||||||
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
|
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
|
||||||
starting_brake_rate = 0.8 # brake_travel/s while releasing on restart
|
starting_brake_rate = 0.8 # brake_travel/s while releasing on restart
|
||||||
starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative
|
|
||||||
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
|
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
|
||||||
|
|
||||||
_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
|
_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints
|
||||||
|
@ -66,10 +64,10 @@ _MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this
|
||||||
|
|
||||||
|
|
||||||
class LongControl(object):
|
class LongControl(object):
|
||||||
def __init__(self, compute_gb):
|
def __init__(self, CP, compute_gb):
|
||||||
self.long_control_state = LongCtrlState.off # initialized to off
|
self.long_control_state = LongCtrlState.off # initialized to off
|
||||||
self.pid = PIController((_KP_BP, _KP_V),
|
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
|
||||||
(_KI_BP, _KI_V),
|
(CP.longitudinalKiBP, CP.longitudinalKiV),
|
||||||
rate=100.0,
|
rate=100.0,
|
||||||
sat_limit=0.8,
|
sat_limit=0.8,
|
||||||
convert=compute_gb)
|
convert=compute_gb)
|
||||||
|
@ -80,27 +78,18 @@ class LongControl(object):
|
||||||
self.pid.reset()
|
self.pid.reset()
|
||||||
self.v_pid = v_pid
|
self.v_pid = v_pid
|
||||||
|
|
||||||
def update(self, active, v_ego, brake_pressed, standstill, v_cruise, v_target_lead, a_target,
|
def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1):
|
||||||
jerk_factor, CP):
|
|
||||||
|
|
||||||
# actuation limits
|
# actuation limits
|
||||||
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
|
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
|
||||||
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
|
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
|
||||||
|
|
||||||
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
|
|
||||||
|
|
||||||
output_gb = self.last_output_gb
|
output_gb = self.last_output_gb
|
||||||
|
|
||||||
# limit max target speed based on cruise setting
|
|
||||||
v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS)
|
|
||||||
rate = 100.0
|
rate = 100.0
|
||||||
max_speed_delta_up = a_target[1] * 1.0 / rate
|
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
|
||||||
max_speed_delta_down = a_target[0] * 1.0 / rate
|
v_target_future, self.v_pid, output_gb,
|
||||||
|
brake_pressed, cruise_standstill)
|
||||||
|
|
||||||
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\
|
v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
|
||||||
v_target, self.v_pid, output_gb, brake_pressed)
|
|
||||||
|
|
||||||
v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
|
|
||||||
|
|
||||||
# *** long control behavior based on state
|
# *** long control behavior based on state
|
||||||
if self.long_control_state == LongCtrlState.off:
|
if self.long_control_state == LongCtrlState.off:
|
||||||
|
@ -110,29 +99,15 @@ class LongControl(object):
|
||||||
|
|
||||||
# tracking objects and driving
|
# tracking objects and driving
|
||||||
elif self.long_control_state == LongCtrlState.pid:
|
elif self.long_control_state == LongCtrlState.pid:
|
||||||
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
|
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
|
||||||
if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)):
|
|
||||||
self.v_pid = max(v_target, v_ego + overshoot_allowance)
|
|
||||||
elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)):
|
|
||||||
self.v_pid = min(v_target, v_ego - overshoot_allowance)
|
|
||||||
|
|
||||||
# move v_pid no faster than allowed accel limits
|
|
||||||
if (v_target > self.v_pid + max_speed_delta_up):
|
|
||||||
self.v_pid += max_speed_delta_up
|
|
||||||
elif (v_target < self.v_pid + max_speed_delta_down):
|
|
||||||
self.v_pid += max_speed_delta_down
|
|
||||||
else:
|
|
||||||
self.v_pid = v_target
|
|
||||||
|
|
||||||
# to avoid too much wind up on acceleration, limit positive speed error
|
|
||||||
if CP.enableGas:
|
|
||||||
max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V)
|
|
||||||
self.v_pid = min(self.v_pid, v_ego + max_speed_error)
|
|
||||||
|
|
||||||
|
self.v_pid = max(v_target, MIN_CAN_SPEED)
|
||||||
self.pid.pos_limit = gas_max
|
self.pid.pos_limit = gas_max
|
||||||
self.pid.neg_limit = - brake_max
|
self.pid.neg_limit = - brake_max
|
||||||
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
|
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
|
||||||
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor, deadzone=deadzone)
|
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
|
||||||
|
if prevent_overshoot:
|
||||||
|
output_gb = min(output_gb, 0.0)
|
||||||
|
|
||||||
# intention is to stop, switch to a different brake control until we stop
|
# intention is to stop, switch to a different brake control until we stop
|
||||||
elif self.long_control_state == LongCtrlState.stopping:
|
elif self.long_control_state == LongCtrlState.stopping:
|
||||||
|
@ -150,7 +125,6 @@ class LongControl(object):
|
||||||
output_gb += starting_brake_rate / rate
|
output_gb += starting_brake_rate / rate
|
||||||
self.v_pid = v_ego
|
self.v_pid = v_ego
|
||||||
self.pid.reset()
|
self.pid.reset()
|
||||||
self.pid.i = starting_Ui
|
|
||||||
|
|
||||||
self.last_output_gb = output_gb
|
self.last_output_gb = output_gb
|
||||||
final_gas = clip(output_gb, 0., gas_max)
|
final_gas = clip(output_gb, 0., gas_max)
|
||||||
|
|
|
@ -0,0 +1,94 @@
|
||||||
|
CC = clang
|
||||||
|
CXX = clang++
|
||||||
|
|
||||||
|
PHONELIBS = ../../../../phonelibs
|
||||||
|
|
||||||
|
UNAME_M := $(shell uname -m)
|
||||||
|
|
||||||
|
CFLAGS = -O3 -fPIC -I.
|
||||||
|
CXXFLAGS = -O3 -fPIC -I.
|
||||||
|
|
||||||
|
QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC
|
||||||
|
|
||||||
|
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
|
||||||
|
|
||||||
|
ifeq ($(UNAME_M),aarch64)
|
||||||
|
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||||
|
else
|
||||||
|
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||||
|
endif
|
||||||
|
|
||||||
|
OBJS = \
|
||||||
|
qp/Bounds.o \
|
||||||
|
qp/Constraints.o \
|
||||||
|
qp/CyclingManager.o \
|
||||||
|
qp/Indexlist.o \
|
||||||
|
qp/MessageHandling.o \
|
||||||
|
qp/QProblem.o \
|
||||||
|
qp/QProblemB.o \
|
||||||
|
qp/SubjectTo.o \
|
||||||
|
qp/Utils.o \
|
||||||
|
qp/EXTRAS/SolutionAnalysis.o \
|
||||||
|
mpc_export/acado_qpoases_interface.o \
|
||||||
|
mpc_export/acado_integrator.o \
|
||||||
|
mpc_export/acado_solver.o \
|
||||||
|
mpc_export/acado_auxiliary_functions.o \
|
||||||
|
mpc.o
|
||||||
|
|
||||||
|
DEPS := $(OBJS:.o=.d)
|
||||||
|
|
||||||
|
.PHONY: all
|
||||||
|
all: libcommampc1.so libcommampc2.so
|
||||||
|
|
||||||
|
libcommampc1.so: $(OBJS)
|
||||||
|
$(CXX) -shared -o '$@' $^ -lm
|
||||||
|
|
||||||
|
libcommampc2.so: libcommampc1.so
|
||||||
|
cp libcommampc1.so libcommampc2.so
|
||||||
|
|
||||||
|
qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||||
|
@echo "[ CXX ] $@"
|
||||||
|
mkdir -p qp
|
||||||
|
$(CXX) $(CXXFLAGS) -MMD \
|
||||||
|
-I mpc_export/ \
|
||||||
|
$(QPOASES_FLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
qp/EXTRAS/%.o: $(PHONELIBS)/qpoases/SRC/EXTRAS/%.cpp
|
||||||
|
@echo "[ CXX ] $@"
|
||||||
|
mkdir -p qp/EXTRAS
|
||||||
|
$(CXX) $(CXXFLAGS) -MMD \
|
||||||
|
-I mpc_export/ \
|
||||||
|
$(QPOASES_FLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
%.o: %.cpp
|
||||||
|
@echo "[ CXX ] $@"
|
||||||
|
$(CXX) $(CXXFLAGS) -MMD \
|
||||||
|
-I mpc_export/ \
|
||||||
|
$(QPOASES_FLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
%.o: %.c
|
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) -MMD \
|
||||||
|
-I mpc_export/ \
|
||||||
|
$(QPOASES_FLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
generator: generator.cpp
|
||||||
|
$(CXX) -Wall -std=c++11 \
|
||||||
|
generator.cpp \
|
||||||
|
-o generator \
|
||||||
|
$(ACADO_FLAGS) \
|
||||||
|
$(ACADO_LIBS)
|
||||||
|
|
||||||
|
.PHONY: generate
|
||||||
|
generate: generator
|
||||||
|
./generator
|
||||||
|
|
||||||
|
.PHONY: clean
|
||||||
|
clean:
|
||||||
|
rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS)
|
||||||
|
|
||||||
|
-include $(DEPS)
|
|
@ -0,0 +1,98 @@
|
||||||
|
#include <acado_code_generation.hpp>
|
||||||
|
|
||||||
|
const int controlHorizon = 50;
|
||||||
|
const double samplingTime = 0.2;
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
#define G 9.81
|
||||||
|
#define TR 1.8
|
||||||
|
|
||||||
|
#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
|
||||||
|
#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1))
|
||||||
|
|
||||||
|
int main( )
|
||||||
|
{
|
||||||
|
USING_NAMESPACE_ACADO
|
||||||
|
|
||||||
|
|
||||||
|
DifferentialEquation f;
|
||||||
|
|
||||||
|
DifferentialState x_ego, v_ego, a_ego;
|
||||||
|
DifferentialState x_l, v_l, a_l;
|
||||||
|
|
||||||
|
OnlineData lambda;
|
||||||
|
|
||||||
|
Control j_ego;
|
||||||
|
|
||||||
|
auto desired = 4.0 + RW(v_ego, v_l);
|
||||||
|
auto d_l = x_l - x_ego;
|
||||||
|
|
||||||
|
// Equations of motion
|
||||||
|
f << dot(x_ego) == v_ego;
|
||||||
|
f << dot(v_ego) == a_ego;
|
||||||
|
f << dot(a_ego) == j_ego;
|
||||||
|
|
||||||
|
f << dot(x_l) == v_l;
|
||||||
|
f << dot(v_l) == a_l;
|
||||||
|
f << dot(a_l) == -lambda * a_l;
|
||||||
|
|
||||||
|
// Running cost
|
||||||
|
Function h;
|
||||||
|
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
|
||||||
|
h << (d_l - desired) / (0.1 * v_ego + 0.5);
|
||||||
|
h << a_ego * (1.0 + v_ego / 10.0);
|
||||||
|
h << j_ego * (1.0 + v_ego / 10.0);
|
||||||
|
|
||||||
|
DMatrix Q(4,4);
|
||||||
|
Q(0,0) = 5.0;
|
||||||
|
Q(1,1) = 0.1;
|
||||||
|
Q(2,2) = 10.0;
|
||||||
|
Q(3,3) = 20.0;
|
||||||
|
|
||||||
|
// Terminal cost
|
||||||
|
Function hN;
|
||||||
|
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
|
||||||
|
hN << (d_l - desired) / (0.1 * v_ego + 0.5);
|
||||||
|
hN << a_ego * (1.0 + v_ego / 10.0);
|
||||||
|
|
||||||
|
DMatrix QN(3,3);
|
||||||
|
QN(0,0) = 5.0;
|
||||||
|
QN(1,1) = 0.1;
|
||||||
|
QN(2,2) = 10.0;
|
||||||
|
|
||||||
|
// Setup Optimal Control Problem
|
||||||
|
const double tStart = 0.0;
|
||||||
|
const double tEnd = samplingTime * controlHorizon;
|
||||||
|
|
||||||
|
OCP ocp( tStart, tEnd, controlHorizon );
|
||||||
|
ocp.subjectTo(f);
|
||||||
|
|
||||||
|
ocp.minimizeLSQ(Q, h);
|
||||||
|
ocp.minimizeLSQEndTerm(QN, hN);
|
||||||
|
|
||||||
|
ocp.subjectTo( 0.0 <= v_ego);
|
||||||
|
ocp.setNOD(1);
|
||||||
|
|
||||||
|
OCPexport mpc(ocp);
|
||||||
|
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
|
||||||
|
mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
|
||||||
|
mpc.set( INTEGRATOR_TYPE, INT_RK4 );
|
||||||
|
mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon);
|
||||||
|
mpc.set( MAX_NUM_QP_ITERATIONS, 500);
|
||||||
|
|
||||||
|
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
|
||||||
|
mpc.set( QP_SOLVER, QP_QPOASES );
|
||||||
|
mpc.set( HOTSTART_QP, YES );
|
||||||
|
mpc.set( GENERATE_TEST_FILE, NO);
|
||||||
|
mpc.set( GENERATE_MAKE_FILE, NO );
|
||||||
|
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
|
||||||
|
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
|
||||||
|
|
||||||
|
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
|
||||||
|
exit( EXIT_FAILURE );
|
||||||
|
|
||||||
|
mpc.printDimensionsQP( );
|
||||||
|
|
||||||
|
return EXIT_SUCCESS;
|
||||||
|
}
|
|
@ -0,0 +1,43 @@
|
||||||
|
import os
|
||||||
|
import subprocess
|
||||||
|
|
||||||
|
from cffi import FFI
|
||||||
|
|
||||||
|
mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
|
||||||
|
subprocess.check_output(["make", "-j4"], cwd=mpc_dir)
|
||||||
|
|
||||||
|
|
||||||
|
def _get_libmpc(mpc_id):
|
||||||
|
libmpc_fn = os.path.join(mpc_dir, "libcommampc%d.so" % mpc_id)
|
||||||
|
|
||||||
|
ffi = FFI()
|
||||||
|
ffi.cdef("""
|
||||||
|
typedef struct {
|
||||||
|
double x_ego, v_ego, a_ego, x_l, v_l, a_l;
|
||||||
|
} state_t;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double x_ego[50];
|
||||||
|
double v_ego[50];
|
||||||
|
double a_ego[50];
|
||||||
|
double j_ego[50];
|
||||||
|
double x_l[50];
|
||||||
|
double v_l[50];
|
||||||
|
double a_l[50];
|
||||||
|
} log_t;
|
||||||
|
|
||||||
|
void init();
|
||||||
|
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
|
||||||
|
int run_mpc(state_t * x0, log_t * solution,
|
||||||
|
double l);
|
||||||
|
""")
|
||||||
|
|
||||||
|
return (ffi, ffi.dlopen(libmpc_fn))
|
||||||
|
|
||||||
|
|
||||||
|
mpcs = [_get_libmpc(1), _get_libmpc(2)]
|
||||||
|
|
||||||
|
|
||||||
|
def get_libmpc(mpc_id):
|
||||||
|
return mpcs[mpc_id - 1]
|
|
@ -0,0 +1,118 @@
|
||||||
|
#include "acado_common.h"
|
||||||
|
#include "acado_auxiliary_functions.h"
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#define NX ACADO_NX /* Number of differential state variables. */
|
||||||
|
#define NXA ACADO_NXA /* Number of algebraic variables. */
|
||||||
|
#define NU ACADO_NU /* Number of control inputs. */
|
||||||
|
#define NOD ACADO_NOD /* Number of online data values. */
|
||||||
|
|
||||||
|
#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */
|
||||||
|
#define NYN ACADO_NYN /* Number of measurements/references on node N. */
|
||||||
|
|
||||||
|
#define N ACADO_N /* Number of intervals in the horizon. */
|
||||||
|
|
||||||
|
ACADOvariables acadoVariables;
|
||||||
|
ACADOworkspace acadoWorkspace;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double x_ego, v_ego, a_ego, x_l, v_l, a_l;
|
||||||
|
} state_t;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double x_ego[N];
|
||||||
|
double v_ego[N];
|
||||||
|
double a_ego[N];
|
||||||
|
double j_ego[N];
|
||||||
|
double x_l[N];
|
||||||
|
double v_l[N];
|
||||||
|
double a_l[N];
|
||||||
|
} log_t;
|
||||||
|
|
||||||
|
void init(){
|
||||||
|
acado_initializeSolver();
|
||||||
|
int i;
|
||||||
|
|
||||||
|
/* Initialize the states and controls. */
|
||||||
|
for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0;
|
||||||
|
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0;
|
||||||
|
|
||||||
|
/* Initialize the measurements/reference. */
|
||||||
|
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
|
||||||
|
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
|
||||||
|
|
||||||
|
/* MPC: initialize the current state feedback. */
|
||||||
|
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){
|
||||||
|
int i;
|
||||||
|
double x_ego = 0.0;
|
||||||
|
double a_ego = 0.0;
|
||||||
|
|
||||||
|
if (v_ego > v_l){
|
||||||
|
a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
|
||||||
|
}
|
||||||
|
double dt = 0.2;
|
||||||
|
|
||||||
|
for (i = 0; i < N + 1; ++i){
|
||||||
|
acadoVariables.x[i*NX] = x_ego;
|
||||||
|
acadoVariables.x[i*NX+1] = v_ego;
|
||||||
|
acadoVariables.x[i*NX+2] = a_ego;
|
||||||
|
|
||||||
|
acadoVariables.x[i*NX+3] = x_l;
|
||||||
|
acadoVariables.x[i*NX+4] = v_l;
|
||||||
|
acadoVariables.x[i*NX+5] = a_l;
|
||||||
|
|
||||||
|
x_ego += v_ego * dt;
|
||||||
|
v_ego += a_ego * dt;
|
||||||
|
|
||||||
|
x_l += v_l * dt;
|
||||||
|
v_l += a_l * dt;
|
||||||
|
a_l += -l * a_l * dt;
|
||||||
|
|
||||||
|
if (v_ego <= 0.0) {
|
||||||
|
v_ego = 0.0;
|
||||||
|
a_ego = 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0;
|
||||||
|
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
|
||||||
|
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int run_mpc(state_t * x0, log_t * solution, double l){
|
||||||
|
int i;
|
||||||
|
|
||||||
|
for (i = 0; i <= NOD * N; i+= NOD){
|
||||||
|
acadoVariables.od[i] = l;
|
||||||
|
}
|
||||||
|
|
||||||
|
acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
|
||||||
|
acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
|
||||||
|
acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
|
||||||
|
acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l;
|
||||||
|
acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
|
||||||
|
acadoVariables.x[5] = acadoVariables.x0[5] = x0->a_l;
|
||||||
|
|
||||||
|
acado_preparationStep();
|
||||||
|
acado_feedbackStep();
|
||||||
|
|
||||||
|
for (i = 0; i <= N; i++){
|
||||||
|
solution->x_ego[i] = acadoVariables.x[i*NX];
|
||||||
|
solution->v_ego[i] = acadoVariables.x[i*NX+1];
|
||||||
|
solution->a_ego[i] = acadoVariables.x[i*NX+2];
|
||||||
|
solution->x_l[i] = acadoVariables.x[i*NX+3];
|
||||||
|
solution->v_l[i] = acadoVariables.x[i*NX+4];
|
||||||
|
solution->a_l[i] = acadoVariables.x[i*NX+5];
|
||||||
|
|
||||||
|
solution->j_ego[i] = acadoVariables.u[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Dont shift states here. Current solution is closer to next timestep than if
|
||||||
|
// we shift by 0.2 seconds.
|
||||||
|
|
||||||
|
return acado_getNWSR();
|
||||||
|
}
|
|
@ -0,0 +1,212 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include "acado_auxiliary_functions.h"
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
real_t* acado_getVariablesX( )
|
||||||
|
{
|
||||||
|
return acadoVariables.x;
|
||||||
|
}
|
||||||
|
|
||||||
|
real_t* acado_getVariablesU( )
|
||||||
|
{
|
||||||
|
return acadoVariables.u;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if ACADO_NY > 0
|
||||||
|
real_t* acado_getVariablesY( )
|
||||||
|
{
|
||||||
|
return acadoVariables.y;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if ACADO_NYN > 0
|
||||||
|
real_t* acado_getVariablesYN( )
|
||||||
|
{
|
||||||
|
return acadoVariables.yN;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
real_t* acado_getVariablesX0( )
|
||||||
|
{
|
||||||
|
#if ACADO_INITIAL_VALUE_FIXED
|
||||||
|
return acadoVariables.x0;
|
||||||
|
#else
|
||||||
|
return 0;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Print differential variables. */
|
||||||
|
void acado_printDifferentialVariables( )
|
||||||
|
{
|
||||||
|
int i, j;
|
||||||
|
printf("\nDifferential variables:\n[\n");
|
||||||
|
for (i = 0; i < ACADO_N + 1; ++i)
|
||||||
|
{
|
||||||
|
for (j = 0; j < ACADO_NX; ++j)
|
||||||
|
printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
printf("]\n\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Print control variables. */
|
||||||
|
void acado_printControlVariables( )
|
||||||
|
{
|
||||||
|
int i, j;
|
||||||
|
printf("\nControl variables:\n[\n");
|
||||||
|
for (i = 0; i < ACADO_N; ++i)
|
||||||
|
{
|
||||||
|
for (j = 0; j < ACADO_NU; ++j)
|
||||||
|
printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
|
||||||
|
printf("\n");
|
||||||
|
}
|
||||||
|
printf("]\n\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Print ACADO code generation notice. */
|
||||||
|
void acado_printHeader( )
|
||||||
|
{
|
||||||
|
printf(
|
||||||
|
"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
|
||||||
|
"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n"
|
||||||
|
"Milan Vukov and Rien Quirynen, KU Leuven.\n"
|
||||||
|
);
|
||||||
|
|
||||||
|
printf(
|
||||||
|
"Developed within the Optimization in Engineering Center (OPTEC) under\n"
|
||||||
|
"supervision of Moritz Diehl. All rights reserved.\n\n"
|
||||||
|
"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
|
||||||
|
"General Public License 3 in the hope that it will be useful,\n"
|
||||||
|
"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
|
||||||
|
"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
|
||||||
|
"GNU Lesser General Public License for more details.\n\n"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if !(defined _DSPACE)
|
||||||
|
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
|
||||||
|
|
||||||
|
void acado_tic( acado_timer* t )
|
||||||
|
{
|
||||||
|
QueryPerformanceFrequency(&t->freq);
|
||||||
|
QueryPerformanceCounter(&t->tic);
|
||||||
|
}
|
||||||
|
|
||||||
|
real_t acado_toc( acado_timer* t )
|
||||||
|
{
|
||||||
|
QueryPerformanceCounter(&t->toc);
|
||||||
|
return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#elif (defined __APPLE__)
|
||||||
|
|
||||||
|
void acado_tic( acado_timer* t )
|
||||||
|
{
|
||||||
|
/* read current clock cycles */
|
||||||
|
t->tic = mach_absolute_time();
|
||||||
|
}
|
||||||
|
|
||||||
|
real_t acado_toc( acado_timer* t )
|
||||||
|
{
|
||||||
|
|
||||||
|
uint64_t duration; /* elapsed time in clock cycles*/
|
||||||
|
|
||||||
|
t->toc = mach_absolute_time();
|
||||||
|
duration = t->toc - t->tic;
|
||||||
|
|
||||||
|
/*conversion from clock cycles to nanoseconds*/
|
||||||
|
mach_timebase_info(&(t->tinfo));
|
||||||
|
duration *= t->tinfo.numer;
|
||||||
|
duration /= t->tinfo.denom;
|
||||||
|
|
||||||
|
return (real_t)duration / 1e9;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
#if __STDC_VERSION__ >= 199901L
|
||||||
|
/* C99 mode */
|
||||||
|
|
||||||
|
/* read current time */
|
||||||
|
void acado_tic( acado_timer* t )
|
||||||
|
{
|
||||||
|
gettimeofday(&t->tic, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* return time passed since last call to tic on this timer */
|
||||||
|
real_t acado_toc( acado_timer* t )
|
||||||
|
{
|
||||||
|
struct timeval temp;
|
||||||
|
|
||||||
|
gettimeofday(&t->toc, 0);
|
||||||
|
|
||||||
|
if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
|
||||||
|
{
|
||||||
|
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
|
||||||
|
temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
|
||||||
|
temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
/* ANSI */
|
||||||
|
|
||||||
|
/* read current time */
|
||||||
|
void acado_tic( acado_timer* t )
|
||||||
|
{
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &t->tic);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* return time passed since last call to tic on this timer */
|
||||||
|
real_t acado_toc( acado_timer* t )
|
||||||
|
{
|
||||||
|
struct timespec temp;
|
||||||
|
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &t->toc);
|
||||||
|
|
||||||
|
if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
|
||||||
|
{
|
||||||
|
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
|
||||||
|
temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
|
||||||
|
temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* __STDC_VERSION__ >= 199901L */
|
||||||
|
|
||||||
|
#endif /* (defined _WIN32 || _WIN64) */
|
||||||
|
|
||||||
|
#endif
|
|
@ -0,0 +1,138 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ACADO_AUXILIARY_FUNCTIONS_H
|
||||||
|
#define ACADO_AUXILIARY_FUNCTIONS_H
|
||||||
|
|
||||||
|
#include "acado_common.h"
|
||||||
|
|
||||||
|
#ifndef __MATLAB__
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
#endif /* __cplusplus */
|
||||||
|
#endif /* __MATLAB__ */
|
||||||
|
|
||||||
|
/** Get pointer to the matrix with differential variables. */
|
||||||
|
real_t* acado_getVariablesX( );
|
||||||
|
|
||||||
|
/** Get pointer to the matrix with control variables. */
|
||||||
|
real_t* acado_getVariablesU( );
|
||||||
|
|
||||||
|
#if ACADO_NY > 0
|
||||||
|
/** Get pointer to the matrix with references/measurements. */
|
||||||
|
real_t* acado_getVariablesY( );
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if ACADO_NYN > 0
|
||||||
|
/** Get pointer to the vector with references/measurement on the last node. */
|
||||||
|
real_t* acado_getVariablesYN( );
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
|
||||||
|
real_t* acado_getVariablesX0( );
|
||||||
|
|
||||||
|
/** Print differential variables. */
|
||||||
|
void acado_printDifferentialVariables( );
|
||||||
|
|
||||||
|
/** Print control variables. */
|
||||||
|
void acado_printControlVariables( );
|
||||||
|
|
||||||
|
/** Print ACADO code generation notice. */
|
||||||
|
void acado_printHeader( );
|
||||||
|
|
||||||
|
/*
|
||||||
|
* A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for
|
||||||
|
* providing us with the following timing routines.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#if !(defined _DSPACE)
|
||||||
|
#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
|
||||||
|
|
||||||
|
/* Use Windows QueryPerformanceCounter for timing. */
|
||||||
|
#include <Windows.h>
|
||||||
|
|
||||||
|
/** A structure for keeping internal timer data. */
|
||||||
|
typedef struct acado_timer_
|
||||||
|
{
|
||||||
|
LARGE_INTEGER tic;
|
||||||
|
LARGE_INTEGER toc;
|
||||||
|
LARGE_INTEGER freq;
|
||||||
|
} acado_timer;
|
||||||
|
|
||||||
|
|
||||||
|
#elif (defined __APPLE__)
|
||||||
|
|
||||||
|
#include "unistd.h"
|
||||||
|
#include <mach/mach_time.h>
|
||||||
|
|
||||||
|
/** A structure for keeping internal timer data. */
|
||||||
|
typedef struct acado_timer_
|
||||||
|
{
|
||||||
|
uint64_t tic;
|
||||||
|
uint64_t toc;
|
||||||
|
mach_timebase_info_data_t tinfo;
|
||||||
|
} acado_timer;
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Use POSIX clock_gettime() for timing on non-Windows machines. */
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
#if __STDC_VERSION__ >= 199901L
|
||||||
|
/* C99 mode of operation. */
|
||||||
|
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
typedef struct acado_timer_
|
||||||
|
{
|
||||||
|
struct timeval tic;
|
||||||
|
struct timeval toc;
|
||||||
|
} acado_timer;
|
||||||
|
|
||||||
|
#else
|
||||||
|
/* ANSI C */
|
||||||
|
|
||||||
|
/** A structure for keeping internal timer data. */
|
||||||
|
typedef struct acado_timer_
|
||||||
|
{
|
||||||
|
struct timespec tic;
|
||||||
|
struct timespec toc;
|
||||||
|
} acado_timer;
|
||||||
|
|
||||||
|
#endif /* __STDC_VERSION__ >= 199901L */
|
||||||
|
|
||||||
|
#endif /* (defined _WIN32 || defined _WIN64) */
|
||||||
|
|
||||||
|
/** A function for measurement of the current time. */
|
||||||
|
void acado_tic( acado_timer* t );
|
||||||
|
|
||||||
|
/** A function which returns the elapsed time. */
|
||||||
|
real_t acado_toc( acado_timer* t );
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef __MATLAB__
|
||||||
|
#ifdef __cplusplus
|
||||||
|
} /* extern "C" */
|
||||||
|
#endif /* __cplusplus */
|
||||||
|
#endif /* __MATLAB__ */
|
||||||
|
|
||||||
|
#endif /* ACADO_AUXILIARY_FUNCTIONS_H */
|
|
@ -0,0 +1,349 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ACADO_COMMON_H
|
||||||
|
#define ACADO_COMMON_H
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#ifndef __MATLAB__
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
#endif /* __cplusplus */
|
||||||
|
#endif /* __MATLAB__ */
|
||||||
|
|
||||||
|
/** \defgroup ACADO ACADO CGT generated module. */
|
||||||
|
/** @{ */
|
||||||
|
|
||||||
|
/** qpOASES QP solver indicator. */
|
||||||
|
#define ACADO_QPOASES 0
|
||||||
|
#define ACADO_QPOASES3 1
|
||||||
|
/** FORCES QP solver indicator.*/
|
||||||
|
#define ACADO_FORCES 2
|
||||||
|
/** qpDUNES QP solver indicator.*/
|
||||||
|
#define ACADO_QPDUNES 3
|
||||||
|
/** HPMPC QP solver indicator. */
|
||||||
|
#define ACADO_HPMPC 4
|
||||||
|
#define ACADO_GENERIC 5
|
||||||
|
|
||||||
|
/** Indicator for determining the QP solver used by the ACADO solver code. */
|
||||||
|
#define ACADO_QP_SOLVER ACADO_QPOASES
|
||||||
|
|
||||||
|
#include "acado_qpoases_interface.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Common definitions
|
||||||
|
*/
|
||||||
|
/** User defined block based condensing. */
|
||||||
|
#define ACADO_BLOCK_CONDENSING 0
|
||||||
|
/** Compute covariance matrix of the last state estimate. */
|
||||||
|
#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
|
||||||
|
/** Flag indicating whether constraint values are hard-coded or not. */
|
||||||
|
#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
|
||||||
|
/** Indicator for fixed initial state. */
|
||||||
|
#define ACADO_INITIAL_STATE_FIXED 1
|
||||||
|
/** Number of control/estimation intervals. */
|
||||||
|
#define ACADO_N 50
|
||||||
|
/** Number of online data values. */
|
||||||
|
#define ACADO_NOD 1
|
||||||
|
/** Number of path constraints. */
|
||||||
|
#define ACADO_NPAC 0
|
||||||
|
/** Number of control variables. */
|
||||||
|
#define ACADO_NU 1
|
||||||
|
/** Number of differential variables. */
|
||||||
|
#define ACADO_NX 6
|
||||||
|
/** Number of algebraic variables. */
|
||||||
|
#define ACADO_NXA 0
|
||||||
|
/** Number of differential derivative variables. */
|
||||||
|
#define ACADO_NXD 0
|
||||||
|
/** Number of references/measurements per node on the first N nodes. */
|
||||||
|
#define ACADO_NY 4
|
||||||
|
/** Number of references/measurements on the last (N + 1)st node. */
|
||||||
|
#define ACADO_NYN 3
|
||||||
|
/** Total number of QP optimization variables. */
|
||||||
|
#define ACADO_QP_NV 56
|
||||||
|
/** Number of integration steps per shooting interval. */
|
||||||
|
#define ACADO_RK_NIS 1
|
||||||
|
/** Number of Runge-Kutta stages per integration step. */
|
||||||
|
#define ACADO_RK_NSTAGES 4
|
||||||
|
/** Providing interface for arrival cost. */
|
||||||
|
#define ACADO_USE_ARRIVAL_COST 0
|
||||||
|
/** Indicator for usage of non-hard-coded linear terms in the objective. */
|
||||||
|
#define ACADO_USE_LINEAR_TERMS 0
|
||||||
|
/** Indicator for type of fixed weighting matrices. */
|
||||||
|
#define ACADO_WEIGHTING_MATRICES_TYPE 0
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Globally used structure definitions
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** The structure containing the user data.
|
||||||
|
*
|
||||||
|
* Via this structure the user "communicates" with the solver code.
|
||||||
|
*/
|
||||||
|
typedef struct ACADOvariables_
|
||||||
|
{
|
||||||
|
int dummy;
|
||||||
|
/** Matrix of size: 51 x 6 (row major format)
|
||||||
|
*
|
||||||
|
* Matrix containing 51 differential variable vectors.
|
||||||
|
*/
|
||||||
|
real_t x[ 306 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 50
|
||||||
|
*
|
||||||
|
* Matrix containing 50 control variable vectors.
|
||||||
|
*/
|
||||||
|
real_t u[ 50 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 51
|
||||||
|
*
|
||||||
|
* Matrix containing 51 online data vectors.
|
||||||
|
*/
|
||||||
|
real_t od[ 51 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 200
|
||||||
|
*
|
||||||
|
* Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes.
|
||||||
|
*/
|
||||||
|
real_t y[ 200 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 3
|
||||||
|
*
|
||||||
|
* Reference/measurement vector for the 51. node.
|
||||||
|
*/
|
||||||
|
real_t yN[ 3 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 6
|
||||||
|
*
|
||||||
|
* Current state feedback vector.
|
||||||
|
*/
|
||||||
|
real_t x0[ 6 ];
|
||||||
|
|
||||||
|
|
||||||
|
} ACADOvariables;
|
||||||
|
|
||||||
|
/** Private workspace used by the auto-generated code.
|
||||||
|
*
|
||||||
|
* Data members of this structure are private to the solver.
|
||||||
|
* In other words, the user code should not modify values of this
|
||||||
|
* structure.
|
||||||
|
*/
|
||||||
|
typedef struct ACADOworkspace_
|
||||||
|
{
|
||||||
|
real_t rk_ttt;
|
||||||
|
|
||||||
|
/** Row vector of size: 50 */
|
||||||
|
real_t rk_xxx[ 50 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 4 x 48 (row major format) */
|
||||||
|
real_t rk_kkk[ 192 ];
|
||||||
|
|
||||||
|
/** Row vector of size: 50 */
|
||||||
|
real_t state[ 50 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 300 */
|
||||||
|
real_t d[ 300 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 200 */
|
||||||
|
real_t Dy[ 200 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 3 */
|
||||||
|
real_t DyN[ 3 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 300 x 6 (row major format) */
|
||||||
|
real_t evGx[ 1800 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 300 */
|
||||||
|
real_t evGu[ 300 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 32 */
|
||||||
|
real_t objAuxVar[ 32 ];
|
||||||
|
|
||||||
|
/** Row vector of size: 8 */
|
||||||
|
real_t objValueIn[ 8 ];
|
||||||
|
|
||||||
|
/** Row vector of size: 32 */
|
||||||
|
real_t objValueOut[ 32 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 300 x 6 (row major format) */
|
||||||
|
real_t Q1[ 1800 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 300 x 4 (row major format) */
|
||||||
|
real_t Q2[ 1200 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 50 */
|
||||||
|
real_t R1[ 50 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 50 x 4 (row major format) */
|
||||||
|
real_t R2[ 200 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 300 */
|
||||||
|
real_t S1[ 300 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 6 x 6 (row major format) */
|
||||||
|
real_t QN1[ 36 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 6 x 3 (row major format) */
|
||||||
|
real_t QN2[ 18 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 6 */
|
||||||
|
real_t Dx0[ 6 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 6 x 6 (row major format) */
|
||||||
|
real_t T[ 36 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 7650 */
|
||||||
|
real_t E[ 7650 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 7650 */
|
||||||
|
real_t QE[ 7650 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 300 x 6 (row major format) */
|
||||||
|
real_t QGx[ 1800 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 300 */
|
||||||
|
real_t Qd[ 300 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 306 */
|
||||||
|
real_t QDy[ 306 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 50 x 6 (row major format) */
|
||||||
|
real_t H10[ 300 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 56 x 56 (row major format) */
|
||||||
|
real_t H[ 3136 ];
|
||||||
|
|
||||||
|
/** Matrix of size: 50 x 56 (row major format) */
|
||||||
|
real_t A[ 2800 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 56 */
|
||||||
|
real_t g[ 56 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 56 */
|
||||||
|
real_t lb[ 56 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 56 */
|
||||||
|
real_t ub[ 56 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 50 */
|
||||||
|
real_t lbA[ 50 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 50 */
|
||||||
|
real_t ubA[ 50 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 56 */
|
||||||
|
real_t x[ 56 ];
|
||||||
|
|
||||||
|
/** Column vector of size: 106 */
|
||||||
|
real_t y[ 106 ];
|
||||||
|
|
||||||
|
|
||||||
|
} ACADOworkspace;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Forward function declarations.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/** Performs the integration and sensitivity propagation for one shooting interval.
|
||||||
|
*
|
||||||
|
* \param rk_eta Working array to pass the input values and return the results.
|
||||||
|
* \param resetIntegrator The internal memory of the integrator can be reset.
|
||||||
|
*
|
||||||
|
* \return Status code of the integrator.
|
||||||
|
*/
|
||||||
|
int acado_integrate( real_t* const rk_eta, int resetIntegrator );
|
||||||
|
|
||||||
|
/** Export of an ACADO symbolic function.
|
||||||
|
*
|
||||||
|
* \param in Input to the exported function.
|
||||||
|
* \param out Output of the exported function.
|
||||||
|
*/
|
||||||
|
void acado_rhs_forw(const real_t* in, real_t* out);
|
||||||
|
|
||||||
|
/** Preparation step of the RTI scheme.
|
||||||
|
*
|
||||||
|
* \return Status of the integration module. =0: OK, otherwise the error code.
|
||||||
|
*/
|
||||||
|
int acado_preparationStep( );
|
||||||
|
|
||||||
|
/** Feedback/estimation step of the RTI scheme.
|
||||||
|
*
|
||||||
|
* \return Status code of the qpOASES QP solver.
|
||||||
|
*/
|
||||||
|
int acado_feedbackStep( );
|
||||||
|
|
||||||
|
/** Solver initialization. Must be called once before any other function call.
|
||||||
|
*
|
||||||
|
* \return =0: OK, otherwise an error code of a QP solver.
|
||||||
|
*/
|
||||||
|
int acado_initializeSolver( );
|
||||||
|
|
||||||
|
/** Initialize shooting nodes by a forward simulation starting from the first node.
|
||||||
|
*/
|
||||||
|
void acado_initializeNodesByForwardSimulation( );
|
||||||
|
|
||||||
|
/** Shift differential variables vector by one interval.
|
||||||
|
*
|
||||||
|
* \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation.
|
||||||
|
* \param xEnd Value for the x vector on the last node. If =0 the old value is used.
|
||||||
|
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
|
||||||
|
*/
|
||||||
|
void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
|
||||||
|
|
||||||
|
/** Shift controls vector by one interval.
|
||||||
|
*
|
||||||
|
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
|
||||||
|
*/
|
||||||
|
void acado_shiftControls( real_t* const uEnd );
|
||||||
|
|
||||||
|
/** Get the KKT tolerance of the current iterate.
|
||||||
|
*
|
||||||
|
* \return The KKT tolerance value.
|
||||||
|
*/
|
||||||
|
real_t acado_getKKT( );
|
||||||
|
|
||||||
|
/** Calculate the objective value.
|
||||||
|
*
|
||||||
|
* \return Value of the objective function.
|
||||||
|
*/
|
||||||
|
real_t acado_getObjective( );
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Extern declarations.
|
||||||
|
*/
|
||||||
|
|
||||||
|
extern ACADOworkspace acadoWorkspace;
|
||||||
|
extern ACADOvariables acadoVariables;
|
||||||
|
|
||||||
|
/** @} */
|
||||||
|
|
||||||
|
#ifndef __MATLAB__
|
||||||
|
#ifdef __cplusplus
|
||||||
|
} /* extern "C" */
|
||||||
|
#endif /* __cplusplus */
|
||||||
|
#endif /* __MATLAB__ */
|
||||||
|
|
||||||
|
#endif /* ACADO_COMMON_H */
|
|
@ -0,0 +1,383 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include "acado_common.h"
|
||||||
|
|
||||||
|
|
||||||
|
void acado_rhs_forw(const real_t* in, real_t* out)
|
||||||
|
{
|
||||||
|
const real_t* xd = in;
|
||||||
|
const real_t* u = in + 48;
|
||||||
|
const real_t* od = in + 49;
|
||||||
|
|
||||||
|
/* Compute outputs: */
|
||||||
|
out[0] = xd[1];
|
||||||
|
out[1] = xd[2];
|
||||||
|
out[2] = u[0];
|
||||||
|
out[3] = xd[4];
|
||||||
|
out[4] = xd[5];
|
||||||
|
out[5] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[5]);
|
||||||
|
out[6] = xd[12];
|
||||||
|
out[7] = xd[13];
|
||||||
|
out[8] = xd[14];
|
||||||
|
out[9] = xd[15];
|
||||||
|
out[10] = xd[16];
|
||||||
|
out[11] = xd[17];
|
||||||
|
out[12] = xd[18];
|
||||||
|
out[13] = xd[19];
|
||||||
|
out[14] = xd[20];
|
||||||
|
out[15] = xd[21];
|
||||||
|
out[16] = xd[22];
|
||||||
|
out[17] = xd[23];
|
||||||
|
out[18] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[19] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[20] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[21] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[22] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[23] = (real_t)(0.0000000000000000e+00);
|
||||||
|
out[24] = xd[30];
|
||||||
|
out[25] = xd[31];
|
||||||
|
out[26] = xd[32];
|
||||||
|
out[27] = xd[33];
|
||||||
|
out[28] = xd[34];
|
||||||
|
out[29] = xd[35];
|
||||||
|
out[30] = xd[36];
|
||||||
|
out[31] = xd[37];
|
||||||
|
out[32] = xd[38];
|
||||||
|
out[33] = xd[39];
|
||||||
|
out[34] = xd[40];
|
||||||
|
out[35] = xd[41];
|
||||||
|
out[36] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[36]);
|
||||||
|
out[37] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[37]);
|
||||||
|
out[38] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[38]);
|
||||||
|
out[39] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[39]);
|
||||||
|
out[40] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[40]);
|
||||||
|
out[41] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[41]);
|
||||||
|
out[42] = xd[43];
|
||||||
|
out[43] = xd[44];
|
||||||
|
out[44] = (real_t)(1.0000000000000000e+00);
|
||||||
|
out[45] = xd[46];
|
||||||
|
out[46] = xd[47];
|
||||||
|
out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Fixed step size:0.2 */
|
||||||
|
int acado_integrate( real_t* const rk_eta, int resetIntegrator )
|
||||||
|
{
|
||||||
|
int error;
|
||||||
|
|
||||||
|
int run1;
|
||||||
|
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
|
||||||
|
rk_eta[6] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[7] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[8] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[9] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[10] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[11] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[12] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[13] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[14] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[15] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[16] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[17] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[18] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[19] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[20] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[21] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[22] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[23] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[24] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[25] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[26] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[27] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[28] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[29] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[30] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[31] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[32] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[33] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[34] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[35] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[36] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[37] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[38] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[39] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[40] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[41] = 1.0000000000000000e+00;
|
||||||
|
rk_eta[42] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[43] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[44] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[45] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[46] = 0.0000000000000000e+00;
|
||||||
|
rk_eta[47] = 0.0000000000000000e+00;
|
||||||
|
acadoWorkspace.rk_xxx[48] = rk_eta[48];
|
||||||
|
acadoWorkspace.rk_xxx[49] = rk_eta[49];
|
||||||
|
|
||||||
|
for (run1 = 0; run1 < 1; ++run1)
|
||||||
|
{
|
||||||
|
acadoWorkspace.rk_xxx[0] = + rk_eta[0];
|
||||||
|
acadoWorkspace.rk_xxx[1] = + rk_eta[1];
|
||||||
|
acadoWorkspace.rk_xxx[2] = + rk_eta[2];
|
||||||
|
acadoWorkspace.rk_xxx[3] = + rk_eta[3];
|
||||||
|
acadoWorkspace.rk_xxx[4] = + rk_eta[4];
|
||||||
|
acadoWorkspace.rk_xxx[5] = + rk_eta[5];
|
||||||
|
acadoWorkspace.rk_xxx[6] = + rk_eta[6];
|
||||||
|
acadoWorkspace.rk_xxx[7] = + rk_eta[7];
|
||||||
|
acadoWorkspace.rk_xxx[8] = + rk_eta[8];
|
||||||
|
acadoWorkspace.rk_xxx[9] = + rk_eta[9];
|
||||||
|
acadoWorkspace.rk_xxx[10] = + rk_eta[10];
|
||||||
|
acadoWorkspace.rk_xxx[11] = + rk_eta[11];
|
||||||
|
acadoWorkspace.rk_xxx[12] = + rk_eta[12];
|
||||||
|
acadoWorkspace.rk_xxx[13] = + rk_eta[13];
|
||||||
|
acadoWorkspace.rk_xxx[14] = + rk_eta[14];
|
||||||
|
acadoWorkspace.rk_xxx[15] = + rk_eta[15];
|
||||||
|
acadoWorkspace.rk_xxx[16] = + rk_eta[16];
|
||||||
|
acadoWorkspace.rk_xxx[17] = + rk_eta[17];
|
||||||
|
acadoWorkspace.rk_xxx[18] = + rk_eta[18];
|
||||||
|
acadoWorkspace.rk_xxx[19] = + rk_eta[19];
|
||||||
|
acadoWorkspace.rk_xxx[20] = + rk_eta[20];
|
||||||
|
acadoWorkspace.rk_xxx[21] = + rk_eta[21];
|
||||||
|
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
|
||||||
|
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
|
||||||
|
acadoWorkspace.rk_xxx[24] = + rk_eta[24];
|
||||||
|
acadoWorkspace.rk_xxx[25] = + rk_eta[25];
|
||||||
|
acadoWorkspace.rk_xxx[26] = + rk_eta[26];
|
||||||
|
acadoWorkspace.rk_xxx[27] = + rk_eta[27];
|
||||||
|
acadoWorkspace.rk_xxx[28] = + rk_eta[28];
|
||||||
|
acadoWorkspace.rk_xxx[29] = + rk_eta[29];
|
||||||
|
acadoWorkspace.rk_xxx[30] = + rk_eta[30];
|
||||||
|
acadoWorkspace.rk_xxx[31] = + rk_eta[31];
|
||||||
|
acadoWorkspace.rk_xxx[32] = + rk_eta[32];
|
||||||
|
acadoWorkspace.rk_xxx[33] = + rk_eta[33];
|
||||||
|
acadoWorkspace.rk_xxx[34] = + rk_eta[34];
|
||||||
|
acadoWorkspace.rk_xxx[35] = + rk_eta[35];
|
||||||
|
acadoWorkspace.rk_xxx[36] = + rk_eta[36];
|
||||||
|
acadoWorkspace.rk_xxx[37] = + rk_eta[37];
|
||||||
|
acadoWorkspace.rk_xxx[38] = + rk_eta[38];
|
||||||
|
acadoWorkspace.rk_xxx[39] = + rk_eta[39];
|
||||||
|
acadoWorkspace.rk_xxx[40] = + rk_eta[40];
|
||||||
|
acadoWorkspace.rk_xxx[41] = + rk_eta[41];
|
||||||
|
acadoWorkspace.rk_xxx[42] = + rk_eta[42];
|
||||||
|
acadoWorkspace.rk_xxx[43] = + rk_eta[43];
|
||||||
|
acadoWorkspace.rk_xxx[44] = + rk_eta[44];
|
||||||
|
acadoWorkspace.rk_xxx[45] = + rk_eta[45];
|
||||||
|
acadoWorkspace.rk_xxx[46] = + rk_eta[46];
|
||||||
|
acadoWorkspace.rk_xxx[47] = + rk_eta[47];
|
||||||
|
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
|
||||||
|
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[0] + rk_eta[0];
|
||||||
|
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[1] + rk_eta[1];
|
||||||
|
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[2] + rk_eta[2];
|
||||||
|
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[3] + rk_eta[3];
|
||||||
|
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[4] + rk_eta[4];
|
||||||
|
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[5] + rk_eta[5];
|
||||||
|
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[6] + rk_eta[6];
|
||||||
|
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[7] + rk_eta[7];
|
||||||
|
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[8] + rk_eta[8];
|
||||||
|
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[9] + rk_eta[9];
|
||||||
|
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[10] + rk_eta[10];
|
||||||
|
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[11] + rk_eta[11];
|
||||||
|
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[12] + rk_eta[12];
|
||||||
|
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[13] + rk_eta[13];
|
||||||
|
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[14] + rk_eta[14];
|
||||||
|
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[15] + rk_eta[15];
|
||||||
|
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[16] + rk_eta[16];
|
||||||
|
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[17] + rk_eta[17];
|
||||||
|
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[18] + rk_eta[18];
|
||||||
|
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[19] + rk_eta[19];
|
||||||
|
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[20] + rk_eta[20];
|
||||||
|
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[21] + rk_eta[21];
|
||||||
|
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[22] + rk_eta[22];
|
||||||
|
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[23] + rk_eta[23];
|
||||||
|
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[24] + rk_eta[24];
|
||||||
|
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[25] + rk_eta[25];
|
||||||
|
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[26] + rk_eta[26];
|
||||||
|
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[27] + rk_eta[27];
|
||||||
|
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[28] + rk_eta[28];
|
||||||
|
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[29] + rk_eta[29];
|
||||||
|
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[30] + rk_eta[30];
|
||||||
|
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[31] + rk_eta[31];
|
||||||
|
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[32] + rk_eta[32];
|
||||||
|
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[33] + rk_eta[33];
|
||||||
|
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[34] + rk_eta[34];
|
||||||
|
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[35] + rk_eta[35];
|
||||||
|
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[36] + rk_eta[36];
|
||||||
|
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[37] + rk_eta[37];
|
||||||
|
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[38] + rk_eta[38];
|
||||||
|
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[39] + rk_eta[39];
|
||||||
|
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[40] + rk_eta[40];
|
||||||
|
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[41] + rk_eta[41];
|
||||||
|
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[42] + rk_eta[42];
|
||||||
|
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[43] + rk_eta[43];
|
||||||
|
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[44] + rk_eta[44];
|
||||||
|
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[45] + rk_eta[45];
|
||||||
|
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[46] + rk_eta[46];
|
||||||
|
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[47] + rk_eta[47];
|
||||||
|
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
|
||||||
|
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0];
|
||||||
|
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1];
|
||||||
|
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2];
|
||||||
|
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3];
|
||||||
|
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4];
|
||||||
|
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5];
|
||||||
|
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6];
|
||||||
|
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7];
|
||||||
|
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8];
|
||||||
|
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9];
|
||||||
|
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10];
|
||||||
|
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11];
|
||||||
|
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12];
|
||||||
|
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13];
|
||||||
|
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14];
|
||||||
|
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15];
|
||||||
|
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16];
|
||||||
|
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17];
|
||||||
|
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18];
|
||||||
|
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19];
|
||||||
|
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20];
|
||||||
|
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21];
|
||||||
|
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22];
|
||||||
|
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23];
|
||||||
|
acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[72] + rk_eta[24];
|
||||||
|
acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[73] + rk_eta[25];
|
||||||
|
acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[74] + rk_eta[26];
|
||||||
|
acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[75] + rk_eta[27];
|
||||||
|
acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[76] + rk_eta[28];
|
||||||
|
acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[77] + rk_eta[29];
|
||||||
|
acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[78] + rk_eta[30];
|
||||||
|
acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[79] + rk_eta[31];
|
||||||
|
acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[80] + rk_eta[32];
|
||||||
|
acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[81] + rk_eta[33];
|
||||||
|
acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[82] + rk_eta[34];
|
||||||
|
acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[83] + rk_eta[35];
|
||||||
|
acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[84] + rk_eta[36];
|
||||||
|
acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[85] + rk_eta[37];
|
||||||
|
acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[86] + rk_eta[38];
|
||||||
|
acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[87] + rk_eta[39];
|
||||||
|
acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[88] + rk_eta[40];
|
||||||
|
acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[89] + rk_eta[41];
|
||||||
|
acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[90] + rk_eta[42];
|
||||||
|
acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[91] + rk_eta[43];
|
||||||
|
acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[92] + rk_eta[44];
|
||||||
|
acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[93] + rk_eta[45];
|
||||||
|
acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[94] + rk_eta[46];
|
||||||
|
acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[95] + rk_eta[47];
|
||||||
|
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) );
|
||||||
|
acadoWorkspace.rk_xxx[0] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
|
||||||
|
acadoWorkspace.rk_xxx[1] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
|
||||||
|
acadoWorkspace.rk_xxx[2] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
|
||||||
|
acadoWorkspace.rk_xxx[3] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
|
||||||
|
acadoWorkspace.rk_xxx[4] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
|
||||||
|
acadoWorkspace.rk_xxx[5] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
|
||||||
|
acadoWorkspace.rk_xxx[6] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
|
||||||
|
acadoWorkspace.rk_xxx[7] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
|
||||||
|
acadoWorkspace.rk_xxx[8] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
|
||||||
|
acadoWorkspace.rk_xxx[9] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
|
||||||
|
acadoWorkspace.rk_xxx[10] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
|
||||||
|
acadoWorkspace.rk_xxx[11] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
|
||||||
|
acadoWorkspace.rk_xxx[12] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
|
||||||
|
acadoWorkspace.rk_xxx[13] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
|
||||||
|
acadoWorkspace.rk_xxx[14] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
|
||||||
|
acadoWorkspace.rk_xxx[15] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
|
||||||
|
acadoWorkspace.rk_xxx[16] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
|
||||||
|
acadoWorkspace.rk_xxx[17] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
|
||||||
|
acadoWorkspace.rk_xxx[18] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
|
||||||
|
acadoWorkspace.rk_xxx[19] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
|
||||||
|
acadoWorkspace.rk_xxx[20] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
|
||||||
|
acadoWorkspace.rk_xxx[21] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
|
||||||
|
acadoWorkspace.rk_xxx[22] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
|
||||||
|
acadoWorkspace.rk_xxx[23] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
|
||||||
|
acadoWorkspace.rk_xxx[24] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
|
||||||
|
acadoWorkspace.rk_xxx[25] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
|
||||||
|
acadoWorkspace.rk_xxx[26] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
|
||||||
|
acadoWorkspace.rk_xxx[27] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
|
||||||
|
acadoWorkspace.rk_xxx[28] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
|
||||||
|
acadoWorkspace.rk_xxx[29] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
|
||||||
|
acadoWorkspace.rk_xxx[30] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
|
||||||
|
acadoWorkspace.rk_xxx[31] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
|
||||||
|
acadoWorkspace.rk_xxx[32] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
|
||||||
|
acadoWorkspace.rk_xxx[33] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
|
||||||
|
acadoWorkspace.rk_xxx[34] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
|
||||||
|
acadoWorkspace.rk_xxx[35] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
|
||||||
|
acadoWorkspace.rk_xxx[36] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
|
||||||
|
acadoWorkspace.rk_xxx[37] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
|
||||||
|
acadoWorkspace.rk_xxx[38] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
|
||||||
|
acadoWorkspace.rk_xxx[39] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
|
||||||
|
acadoWorkspace.rk_xxx[40] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
|
||||||
|
acadoWorkspace.rk_xxx[41] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
|
||||||
|
acadoWorkspace.rk_xxx[42] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
|
||||||
|
acadoWorkspace.rk_xxx[43] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
|
||||||
|
acadoWorkspace.rk_xxx[44] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
|
||||||
|
acadoWorkspace.rk_xxx[45] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
|
||||||
|
acadoWorkspace.rk_xxx[46] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
|
||||||
|
acadoWorkspace.rk_xxx[47] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47];
|
||||||
|
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) );
|
||||||
|
rk_eta[0] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[144];
|
||||||
|
rk_eta[1] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[145];
|
||||||
|
rk_eta[2] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[146];
|
||||||
|
rk_eta[3] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[147];
|
||||||
|
rk_eta[4] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[148];
|
||||||
|
rk_eta[5] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[149];
|
||||||
|
rk_eta[6] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[150];
|
||||||
|
rk_eta[7] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[151];
|
||||||
|
rk_eta[8] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[152];
|
||||||
|
rk_eta[9] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[153];
|
||||||
|
rk_eta[10] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[154];
|
||||||
|
rk_eta[11] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[155];
|
||||||
|
rk_eta[12] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[156];
|
||||||
|
rk_eta[13] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[157];
|
||||||
|
rk_eta[14] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[158];
|
||||||
|
rk_eta[15] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[159];
|
||||||
|
rk_eta[16] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[160];
|
||||||
|
rk_eta[17] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[161];
|
||||||
|
rk_eta[18] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[162];
|
||||||
|
rk_eta[19] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[163];
|
||||||
|
rk_eta[20] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[164];
|
||||||
|
rk_eta[21] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[165];
|
||||||
|
rk_eta[22] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[166];
|
||||||
|
rk_eta[23] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[167];
|
||||||
|
rk_eta[24] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[168];
|
||||||
|
rk_eta[25] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[169];
|
||||||
|
rk_eta[26] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[170];
|
||||||
|
rk_eta[27] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[171];
|
||||||
|
rk_eta[28] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[172];
|
||||||
|
rk_eta[29] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[173];
|
||||||
|
rk_eta[30] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[174];
|
||||||
|
rk_eta[31] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[175];
|
||||||
|
rk_eta[32] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[176];
|
||||||
|
rk_eta[33] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[177];
|
||||||
|
rk_eta[34] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[178];
|
||||||
|
rk_eta[35] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[179];
|
||||||
|
rk_eta[36] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[180];
|
||||||
|
rk_eta[37] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[181];
|
||||||
|
rk_eta[38] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[182];
|
||||||
|
rk_eta[39] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[183];
|
||||||
|
rk_eta[40] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[184];
|
||||||
|
rk_eta[41] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[185];
|
||||||
|
rk_eta[42] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[186];
|
||||||
|
rk_eta[43] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[187];
|
||||||
|
rk_eta[44] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[188];
|
||||||
|
rk_eta[45] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[189];
|
||||||
|
rk_eta[46] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[190];
|
||||||
|
rk_eta[47] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[191];
|
||||||
|
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
|
||||||
|
}
|
||||||
|
error = 0;
|
||||||
|
return error;
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,70 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
#include "acado_common.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "INCLUDE/QProblem.hpp"
|
||||||
|
|
||||||
|
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
|
||||||
|
#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
|
||||||
|
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
|
||||||
|
|
||||||
|
static int acado_nWSR;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
|
||||||
|
static SolutionAnalysis acado_sa;
|
||||||
|
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
|
||||||
|
|
||||||
|
int acado_solve( void )
|
||||||
|
{
|
||||||
|
acado_nWSR = QPOASES_NWSRMAX;
|
||||||
|
|
||||||
|
QProblem qp(56, 50);
|
||||||
|
|
||||||
|
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
|
||||||
|
|
||||||
|
qp.getPrimalSolution( acadoWorkspace.x );
|
||||||
|
qp.getDualSolution( acadoWorkspace.y );
|
||||||
|
|
||||||
|
#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
|
||||||
|
|
||||||
|
if (retVal != SUCCESSFUL_RETURN)
|
||||||
|
return (int)retVal;
|
||||||
|
|
||||||
|
retVal = acado_sa.getHessianInverse( &qp,var );
|
||||||
|
|
||||||
|
#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
|
||||||
|
|
||||||
|
return (int)retVal;
|
||||||
|
}
|
||||||
|
|
||||||
|
int acado_getNWSR( void )
|
||||||
|
{
|
||||||
|
return acado_nWSR;
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* acado_getErrorString( int error )
|
||||||
|
{
|
||||||
|
return MessageHandling::getErrorString( error );
|
||||||
|
}
|
|
@ -0,0 +1,65 @@
|
||||||
|
/*
|
||||||
|
* This file was auto-generated using the ACADO Toolkit.
|
||||||
|
*
|
||||||
|
* While ACADO Toolkit is free software released under the terms of
|
||||||
|
* the GNU Lesser General Public License (LGPL), the generated code
|
||||||
|
* as such remains the property of the user who used ACADO Toolkit
|
||||||
|
* to generate this code. In particular, user dependent data of the code
|
||||||
|
* do not inherit the GNU LGPL license. On the other hand, parts of the
|
||||||
|
* generated code that are a direct copy of source code from the
|
||||||
|
* ACADO Toolkit or the software tools it is based on, remain, as derived
|
||||||
|
* work, automatically covered by the LGPL license.
|
||||||
|
*
|
||||||
|
* ACADO Toolkit is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef QPOASES_HEADER
|
||||||
|
#define QPOASES_HEADER
|
||||||
|
|
||||||
|
#ifdef PC_DEBUG
|
||||||
|
#include <stdio.h>
|
||||||
|
#endif /* PC_DEBUG */
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
#define EXTERNC extern "C"
|
||||||
|
#else
|
||||||
|
#define EXTERNC
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/*
|
||||||
|
* A set of options for qpOASES
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** Maximum number of optimization variables. */
|
||||||
|
#define QPOASES_NVMAX 56
|
||||||
|
/** Maximum number of constraints. */
|
||||||
|
#define QPOASES_NCMAX 50
|
||||||
|
/** Maximum number of working set recalculations. */
|
||||||
|
#define QPOASES_NWSRMAX 500
|
||||||
|
/** Print level for qpOASES. */
|
||||||
|
#define QPOASES_PRINTLEVEL PL_NONE
|
||||||
|
/** The value of EPS */
|
||||||
|
#define QPOASES_EPS 2.221e-16
|
||||||
|
/** Internally used floating point type */
|
||||||
|
typedef double real_t;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Forward function declarations
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** A function that calls the QP solver */
|
||||||
|
EXTERNC int acado_solve( void );
|
||||||
|
|
||||||
|
/** Get the number of active set changes */
|
||||||
|
EXTERNC int acado_getNWSR( void );
|
||||||
|
|
||||||
|
/** Get the error string. */
|
||||||
|
const char* acado_getErrorString( int error );
|
||||||
|
|
||||||
|
#endif /* QPOASES_HEADER */
|
File diff suppressed because one or more lines are too long
|
@ -12,7 +12,7 @@ def apply_deadzone(error, deadzone):
|
||||||
return error
|
return error
|
||||||
|
|
||||||
class PIController(object):
|
class PIController(object):
|
||||||
def __init__(self, k_p, k_i, k_f=0., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
||||||
self._k_p = k_p # proportional gain
|
self._k_p = k_p # proportional gain
|
||||||
self._k_i = k_i # integrale gain
|
self._k_i = k_i # integrale gain
|
||||||
self.k_f = k_f # feedforward gain
|
self.k_f = k_f # feedforward gain
|
||||||
|
@ -24,7 +24,6 @@ class PIController(object):
|
||||||
self.i_unwind_rate = 0.3 / rate
|
self.i_unwind_rate = 0.3 / rate
|
||||||
self.i_rate = 1.0 / rate
|
self.i_rate = 1.0 / rate
|
||||||
self.sat_limit = sat_limit
|
self.sat_limit = sat_limit
|
||||||
self.jerk_factor = 0.0
|
|
||||||
self.convert = convert
|
self.convert = convert
|
||||||
|
|
||||||
self.reset()
|
self.reset()
|
||||||
|
@ -36,7 +35,7 @@ class PIController(object):
|
||||||
else:
|
else:
|
||||||
kp = interp(self.speed, self._k_p[0], self._k_p[1])
|
kp = interp(self.speed, self._k_p[0], self._k_p[1])
|
||||||
|
|
||||||
return (1.0 + self.jerk_factor) * kp
|
return kp
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def k_i(self):
|
def k_i(self):
|
||||||
|
@ -45,7 +44,7 @@ class PIController(object):
|
||||||
else:
|
else:
|
||||||
ki = interp(self.speed, self._k_i[0], self._k_i[1])
|
ki = interp(self.speed, self._k_i[0], self._k_i[1])
|
||||||
|
|
||||||
return (1.0 + self.jerk_factor) * ki
|
return ki
|
||||||
|
|
||||||
def _check_saturation(self, control, override, error):
|
def _check_saturation(self, control, override, error):
|
||||||
saturated = (control < self.neg_limit) or (control > self.pos_limit)
|
saturated = (control < self.neg_limit) or (control > self.pos_limit)
|
||||||
|
@ -62,34 +61,35 @@ class PIController(object):
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.p = 0.0
|
self.p = 0.0
|
||||||
self.i = 0.0
|
self.i = 0.0
|
||||||
|
self.f = 0.0
|
||||||
self.sat_count = 0.0
|
self.sat_count = 0.0
|
||||||
self.saturated = False
|
self.saturated = False
|
||||||
self.control = 0
|
self.control = 0
|
||||||
|
|
||||||
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False, feedforward=0., deadzone=0.):
|
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
|
||||||
self.speed = speed
|
self.speed = speed
|
||||||
self.jerk_factor = jerk_factor
|
|
||||||
|
|
||||||
error = float(apply_deadzone(setpoint - measurement, deadzone))
|
error = float(apply_deadzone(setpoint - measurement, deadzone))
|
||||||
self.p = error * self.k_p
|
self.p = error * self.k_p
|
||||||
f = feedforward * self.k_f
|
self.f = feedforward * self.k_f
|
||||||
|
|
||||||
if override:
|
if override:
|
||||||
self.i -= self.i_unwind_rate * float(np.sign(self.i))
|
self.i -= self.i_unwind_rate * float(np.sign(self.i))
|
||||||
else:
|
else:
|
||||||
i = self.i + error * self.k_i * self.i_rate
|
i = self.i + error * self.k_i * self.i_rate
|
||||||
control = self.p + f + i
|
control = self.p + self.f + i
|
||||||
|
|
||||||
if self.convert is not None:
|
if self.convert is not None:
|
||||||
control = self.convert(control, speed=self.speed)
|
control = self.convert(control, speed=self.speed)
|
||||||
|
|
||||||
# Update when changing i will move the control away from the limits
|
# Update when changing i will move the control away from the limits
|
||||||
# or when i will move towards the sign of the error
|
# or when i will move towards the sign of the error
|
||||||
if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
|
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
|
||||||
(error <= 0 and (control >= self.neg_limit or i > 0.0)):
|
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
|
||||||
|
not freeze_integrator:
|
||||||
self.i = i
|
self.i = i
|
||||||
|
|
||||||
control = self.p + f + self.i
|
control = self.p + self.f + self.i
|
||||||
if self.convert is not None:
|
if self.convert is not None:
|
||||||
control = self.convert(control, speed=self.speed)
|
control = self.convert(control, speed=self.speed)
|
||||||
|
|
||||||
|
|
|
@ -1,24 +1,230 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
import zmq
|
import zmq
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import math
|
||||||
|
|
||||||
from common.realtime import sec_since_boot
|
from common.realtime import sec_since_boot
|
||||||
|
from common.params import Params
|
||||||
|
from common.numpy_fast import interp
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
|
from selfdrive.swaglog import cloudlog
|
||||||
|
from selfdrive.config import Conversions as CV
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||||
from selfdrive.controls.lib.pathplanner import PathPlanner
|
from selfdrive.controls.lib.pathplanner import PathPlanner
|
||||||
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
|
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||||
from selfdrive.controls.lib.fcw import ForwardCollisionWarning
|
from selfdrive.controls.lib.speed_smoother import speed_smoother
|
||||||
|
from selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||||
|
|
||||||
_DT = 0.01 # 100Hz
|
_DT = 0.01 # 100Hz
|
||||||
|
_DT_MPC = 0.2 # 5Hz
|
||||||
|
MAX_SPEED_ERROR = 2.0
|
||||||
|
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
|
||||||
|
_DEBUG = False
|
||||||
|
_LEAD_ACCEL_TAU = 1.5
|
||||||
|
|
||||||
|
# lookup tables VS speed to determine min and max accels in cruise
|
||||||
|
# make sure these accelerations are smaller than mpc limits
|
||||||
|
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
|
||||||
|
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
||||||
|
|
||||||
|
# need fast accel at very low speed for stop and go
|
||||||
|
# make sure these accelerations are smaller than mpc limits
|
||||||
|
_A_CRUISE_MAX_V = [1., 1., .8, .5, .3]
|
||||||
|
_A_CRUISE_MAX_V_FOLLOWING = [1.5, 1.5, 1.2, .7, .3]
|
||||||
|
_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
|
||||||
|
|
||||||
|
# Lookup table for turns
|
||||||
|
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
|
||||||
|
_A_TOTAL_MAX_BP = [0., 20., 40.]
|
||||||
|
|
||||||
|
# max acceleration allowed in acc, which happens in restart
|
||||||
|
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||||
|
|
||||||
|
|
||||||
|
def calc_cruise_accel_limits(v_ego, following):
|
||||||
|
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||||
|
|
||||||
|
if following:
|
||||||
|
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
|
||||||
|
else:
|
||||||
|
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
||||||
|
return np.vstack([a_cruise_min, a_cruise_max])
|
||||||
|
|
||||||
|
|
||||||
|
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
||||||
|
"""
|
||||||
|
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
||||||
|
this should avoid accelerating when losing the target in turns
|
||||||
|
"""
|
||||||
|
deg_to_rad = np.pi / 180. # from can reading to rad
|
||||||
|
|
||||||
|
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||||
|
a_y = v_ego**2 * angle_steers * deg_to_rad / (CP.sR * CP.l)
|
||||||
|
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
|
||||||
|
|
||||||
|
a_target[1] = min(a_target[1], a_x_allowed)
|
||||||
|
return a_target
|
||||||
|
|
||||||
|
|
||||||
|
class FCWChecker(object):
|
||||||
|
def __init__(self):
|
||||||
|
self.fcw_count = 0
|
||||||
|
self.last_fcw_a = 0.0
|
||||||
|
self.v_lead_max = 0.0
|
||||||
|
self.lead_seen_t = 0.0
|
||||||
|
self.last_fcw_time = 0.0
|
||||||
|
|
||||||
|
def reset_lead(self, cur_time):
|
||||||
|
self.v_lead_max = 0.0
|
||||||
|
self.lead_seen_t = cur_time
|
||||||
|
|
||||||
|
def update(self, mpc_solution, cur_time, v_ego, v_lead, y_lead, vlat_lead, fcw_lead, blinkers):
|
||||||
|
min_a_mpc = min(list(mpc_solution[0].a_ego)[1:])
|
||||||
|
self.v_lead_max = max(self.v_lead_max, v_lead)
|
||||||
|
|
||||||
|
if (fcw_lead > 0.99
|
||||||
|
and v_ego > 5.0
|
||||||
|
and min_a_mpc < -4.0
|
||||||
|
and self.v_lead_max > 2.5
|
||||||
|
and v_ego > v_lead
|
||||||
|
and self.lead_seen_t < cur_time - 2.0
|
||||||
|
and abs(y_lead) < 1.0
|
||||||
|
and abs(vlat_lead) < 0.3
|
||||||
|
and not blinkers):
|
||||||
|
self.fcw_count += 1
|
||||||
|
if self.fcw_count > 10 and self.last_fcw_time + 5.0 < cur_time:
|
||||||
|
self.last_fcw_time = cur_time
|
||||||
|
self.last_fcw_a = min_a_mpc
|
||||||
|
return True
|
||||||
|
else:
|
||||||
|
self.fcw_count = 0
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
class LongitudinalMpc(object):
|
||||||
|
def __init__(self, mpc_id, live_longitudinal_mpc):
|
||||||
|
self.live_longitudinal_mpc = live_longitudinal_mpc
|
||||||
|
self.mpc_id = mpc_id
|
||||||
|
|
||||||
|
self.setup_mpc()
|
||||||
|
self.v_mpc = 0.0
|
||||||
|
self.v_mpc_future = 0.0
|
||||||
|
self.a_mpc = 0.0
|
||||||
|
self.v_cruise = 0.0
|
||||||
|
self.prev_lead_status = False
|
||||||
|
self.prev_lead_x = 0.0
|
||||||
|
self.new_lead = False
|
||||||
|
|
||||||
|
self.last_cloudlog_t = 0.0
|
||||||
|
|
||||||
|
def send_mpc_solution(self, qp_iterations, calculation_time):
|
||||||
|
qp_iterations = max(0, qp_iterations)
|
||||||
|
dat = messaging.new_message()
|
||||||
|
dat.init('liveLongitudinalMpc')
|
||||||
|
dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
|
||||||
|
dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
|
||||||
|
dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
|
||||||
|
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
|
||||||
|
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
|
||||||
|
dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l)
|
||||||
|
dat.liveLongitudinalMpc.aLeadTau = self.l
|
||||||
|
dat.liveLongitudinalMpc.qpIterations = qp_iterations
|
||||||
|
dat.liveLongitudinalMpc.mpcId = self.mpc_id
|
||||||
|
dat.liveLongitudinalMpc.calculationTime = calculation_time
|
||||||
|
self.live_longitudinal_mpc.send(dat.to_bytes())
|
||||||
|
|
||||||
|
def setup_mpc(self):
|
||||||
|
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
|
||||||
|
self.libmpc.init()
|
||||||
|
|
||||||
|
self.mpc_solution = ffi.new("log_t *")
|
||||||
|
self.cur_state = ffi.new("state_t *")
|
||||||
|
self.cur_state[0].v_ego = 0
|
||||||
|
self.cur_state[0].a_ego = 0
|
||||||
|
self.l = _LEAD_ACCEL_TAU
|
||||||
|
|
||||||
|
def set_cur_state(self, v, a):
|
||||||
|
self.cur_state[0].v_ego = v
|
||||||
|
self.cur_state[0].a_ego = a
|
||||||
|
|
||||||
|
def update(self, CS, lead, v_cruise_setpoint):
|
||||||
|
# Setup current mpc state
|
||||||
|
self.cur_state[0].x_ego = 0.0
|
||||||
|
|
||||||
|
if lead is not None and lead.status:
|
||||||
|
x_lead = lead.dRel
|
||||||
|
v_lead = max(0.0, lead.vLead)
|
||||||
|
a_lead = lead.aLeadK
|
||||||
|
|
||||||
|
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
|
||||||
|
v_lead = 0.0
|
||||||
|
a_lead = 0.0
|
||||||
|
|
||||||
|
# Learn if constant acceleration
|
||||||
|
if abs(a_lead) < 0.5:
|
||||||
|
self.l = _LEAD_ACCEL_TAU
|
||||||
|
else:
|
||||||
|
self.l *= 0.9
|
||||||
|
|
||||||
|
l = max(self.l, -a_lead / (v_lead + 0.01))
|
||||||
|
self.new_lead = False
|
||||||
|
if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
|
||||||
|
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, l)
|
||||||
|
self.new_lead = True
|
||||||
|
|
||||||
|
self.prev_lead_status = True
|
||||||
|
self.prev_lead_x = x_lead
|
||||||
|
self.cur_state[0].x_l = x_lead
|
||||||
|
self.cur_state[0].v_l = v_lead
|
||||||
|
self.cur_state[0].a_l = a_lead
|
||||||
|
else:
|
||||||
|
self.prev_lead_status = False
|
||||||
|
# Fake a fast lead car, so mpc keeps running
|
||||||
|
self.cur_state[0].x_l = 50.0
|
||||||
|
self.cur_state[0].v_l = CS.vEgo + 10.0
|
||||||
|
self.cur_state[0].a_l = 0.0
|
||||||
|
l = _LEAD_ACCEL_TAU
|
||||||
|
|
||||||
|
# Calculate mpc
|
||||||
|
t = sec_since_boot()
|
||||||
|
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l)
|
||||||
|
duration = int((sec_since_boot() - t) * 1e9)
|
||||||
|
self.send_mpc_solution(n_its, duration)
|
||||||
|
|
||||||
|
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
|
||||||
|
self.v_mpc = self.mpc_solution[0].v_ego[1]
|
||||||
|
self.a_mpc = self.mpc_solution[0].a_ego[1]
|
||||||
|
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
|
||||||
|
|
||||||
|
# Reset if NaN or goes through lead car
|
||||||
|
dls = np.array(list(self.mpc_solution[0].x_l)[1:]) - np.array(list(self.mpc_solution[0].x_ego)[1:])
|
||||||
|
crashing = min(dls) < -50.0
|
||||||
|
nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
|
||||||
|
backwards = min(list(self.mpc_solution[0].v_ego)[1:]) < -0.01
|
||||||
|
|
||||||
|
if ((backwards or crashing) and self.prev_lead_status) or nans:
|
||||||
|
if t > self.last_cloudlog_t + 5.0:
|
||||||
|
self.last_cloudlog_t = t
|
||||||
|
cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
|
||||||
|
self.mpc_id, backwards, crashing, nans))
|
||||||
|
|
||||||
|
self.libmpc.init()
|
||||||
|
self.cur_state[0].v_ego = CS.vEgo
|
||||||
|
self.cur_state[0].a_ego = 0.0
|
||||||
|
self.prev_lead_status = False
|
||||||
|
|
||||||
|
|
||||||
class Planner(object):
|
class Planner(object):
|
||||||
def __init__(self, CP):
|
def __init__(self, CP, fcw_enabled):
|
||||||
context = zmq.Context()
|
context = zmq.Context()
|
||||||
self.CP = CP
|
self.CP = CP
|
||||||
self.live20 = messaging.sub_sock(context, service_list['live20'].port)
|
self.live20 = messaging.sub_sock(context, service_list['live20'].port)
|
||||||
self.model = messaging.sub_sock(context, service_list['model'].port)
|
self.model = messaging.sub_sock(context, service_list['model'].port)
|
||||||
|
|
||||||
self.plan = messaging.pub_sock(context, service_list['plan'].port)
|
self.plan = messaging.pub_sock(context, service_list['plan'].port)
|
||||||
|
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
|
||||||
|
|
||||||
self.last_md_ts = 0
|
self.last_md_ts = 0
|
||||||
self.last_l20_ts = 0
|
self.last_l20_ts = 0
|
||||||
|
@ -29,36 +235,141 @@ class Planner(object):
|
||||||
self.radar_errors = []
|
self.radar_errors = []
|
||||||
|
|
||||||
self.PP = PathPlanner()
|
self.PP = PathPlanner()
|
||||||
self.AC = AdaptiveCruise()
|
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
|
||||||
self.FCW = ForwardCollisionWarning(_DT)
|
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
|
||||||
|
|
||||||
|
self.v_acc_start = 0.0
|
||||||
|
self.a_acc_start = 0.0
|
||||||
|
self.acc_start_time = sec_since_boot()
|
||||||
|
self.v_acc = 0.0
|
||||||
|
self.v_acc_sol = 0.0
|
||||||
|
self.v_acc_future = 0.0
|
||||||
|
self.a_acc = 0.0
|
||||||
|
self.a_acc_sol = 0.0
|
||||||
|
self.v_cruise = 0.0
|
||||||
|
self.a_cruise = 0.0
|
||||||
|
|
||||||
|
self.lead_1 = None
|
||||||
|
self.lead_2 = None
|
||||||
|
|
||||||
|
self.longitudinalPlanSource = 'cruise'
|
||||||
|
self.fcw = False
|
||||||
|
self.fcw_checker = FCWChecker()
|
||||||
|
self.fcw_enabled = fcw_enabled
|
||||||
|
|
||||||
|
def choose_solution(self, v_cruise_setpoint):
|
||||||
|
solutions = {'cruise': self.v_cruise}
|
||||||
|
if self.mpc1.prev_lead_status:
|
||||||
|
solutions['mpc1'] = self.mpc1.v_mpc
|
||||||
|
if self.mpc2.prev_lead_status:
|
||||||
|
solutions['mpc2'] = self.mpc2.v_mpc
|
||||||
|
|
||||||
|
slowest = min(solutions, key=solutions.get)
|
||||||
|
|
||||||
|
if _DEBUG:
|
||||||
|
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
|
||||||
|
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
|
||||||
|
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
|
||||||
|
|
||||||
|
self.longitudinalPlanSource = slowest
|
||||||
|
|
||||||
|
# Choose lowest of MPC and cruise
|
||||||
|
if slowest == 'mpc1':
|
||||||
|
self.v_acc = self.mpc1.v_mpc
|
||||||
|
self.a_acc = self.mpc1.a_mpc
|
||||||
|
elif slowest == 'mpc2':
|
||||||
|
self.v_acc = self.mpc2.v_mpc
|
||||||
|
self.a_acc = self.mpc2.a_mpc
|
||||||
|
elif slowest == 'cruise':
|
||||||
|
self.v_acc = self.v_cruise
|
||||||
|
self.a_acc = self.a_cruise
|
||||||
|
|
||||||
|
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
|
||||||
|
|
||||||
# this runs whenever we get a packet that can change the plan
|
# this runs whenever we get a packet that can change the plan
|
||||||
def update(self, CS, LoC):
|
def update(self, CS, LoC, v_cruise_kph, user_distracted):
|
||||||
cur_time = sec_since_boot()
|
cur_time = sec_since_boot()
|
||||||
|
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
|
||||||
|
|
||||||
md = messaging.recv_sock(self.model)
|
md = messaging.recv_sock(self.model)
|
||||||
if md is not None:
|
if md is not None:
|
||||||
self.last_md_ts = md.logMonoTime
|
self.last_md_ts = md.logMonoTime
|
||||||
self.last_model = cur_time
|
self.last_model = cur_time
|
||||||
self.model_dead = False
|
self.model_dead = False
|
||||||
if cur_time - self.last_model > 0.5:
|
|
||||||
self.model_dead = True
|
|
||||||
|
|
||||||
l20 = messaging.recv_sock(self.live20)
|
self.PP.update(CS.vEgo, md)
|
||||||
|
|
||||||
|
l20 = messaging.recv_sock(self.live20) if md is None else None
|
||||||
if l20 is not None:
|
if l20 is not None:
|
||||||
self.last_l20_ts = l20.logMonoTime
|
self.last_l20_ts = l20.logMonoTime
|
||||||
self.last_l20 = cur_time
|
self.last_l20 = cur_time
|
||||||
self.radar_dead = False
|
self.radar_dead = False
|
||||||
self.radar_errors = list(l20.live20.radarErrors)
|
self.radar_errors = list(l20.live20.radarErrors)
|
||||||
|
|
||||||
|
self.v_acc_start = self.v_acc_sol
|
||||||
|
self.a_acc_start = self.a_acc_sol
|
||||||
|
self.acc_start_time = cur_time
|
||||||
|
|
||||||
|
self.lead_1 = l20.live20.leadOne
|
||||||
|
self.lead_2 = l20.live20.leadTwo
|
||||||
|
|
||||||
|
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
|
||||||
|
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
|
||||||
|
|
||||||
|
# Calculate speed for normal cruise control
|
||||||
|
if enabled:
|
||||||
|
|
||||||
|
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
|
||||||
|
# TODO: make a separate lookup for jerk tuning
|
||||||
|
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
|
||||||
|
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP)
|
||||||
|
if user_distracted:
|
||||||
|
# if user is not responsive to awareness alerts, then start a smooth deceleration
|
||||||
|
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
|
||||||
|
accel_limits[0] = min(accel_limits[0], accel_limits[1])
|
||||||
|
|
||||||
|
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
|
||||||
|
v_cruise_setpoint,
|
||||||
|
accel_limits[1], accel_limits[0],
|
||||||
|
jerk_limits[1],
|
||||||
|
jerk_limits[0],
|
||||||
|
_DT_MPC)
|
||||||
|
else:
|
||||||
|
starting = LoC.long_control_state == LongCtrlState.starting
|
||||||
|
self.v_cruise = CS.vEgo
|
||||||
|
self.a_cruise = self.CP.startAccel if starting else CS.aEgo
|
||||||
|
self.v_acc_start = CS.vEgo
|
||||||
|
self.a_acc_start = self.CP.startAccel if starting else CS.aEgo
|
||||||
|
self.v_acc = CS.vEgo
|
||||||
|
self.a_acc = self.CP.startAccel if starting else CS.aEgo
|
||||||
|
self.v_acc_sol = CS.vEgo
|
||||||
|
self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo
|
||||||
|
|
||||||
|
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
|
||||||
|
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
|
||||||
|
|
||||||
|
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
|
||||||
|
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
|
||||||
|
|
||||||
|
self.choose_solution(v_cruise_setpoint)
|
||||||
|
|
||||||
|
# determine fcw
|
||||||
|
if self.mpc1.new_lead:
|
||||||
|
self.fcw_checker.reset_lead(cur_time)
|
||||||
|
|
||||||
|
blinkers = CS.leftBlinker or CS.rightBlinker
|
||||||
|
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo,
|
||||||
|
self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat,
|
||||||
|
self.lead_1.fcw, blinkers) \
|
||||||
|
and not CS.brakePressed
|
||||||
|
if self.fcw:
|
||||||
|
cloudlog.info("FCW triggered")
|
||||||
|
|
||||||
|
if cur_time - self.last_model > 0.5:
|
||||||
|
self.model_dead = True
|
||||||
|
|
||||||
if cur_time - self.last_l20 > 0.5:
|
if cur_time - self.last_l20 > 0.5:
|
||||||
self.radar_dead = True
|
self.radar_dead = True
|
||||||
|
|
||||||
self.PP.update(CS.vEgo, md)
|
|
||||||
|
|
||||||
# LoC.v_pid -> CS.vEgo
|
|
||||||
# TODO: is this change okay?
|
|
||||||
self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
|
|
||||||
|
|
||||||
# **** send the plan ****
|
# **** send the plan ****
|
||||||
plan_send = messaging.new_message()
|
plan_send = messaging.new_message()
|
||||||
plan_send.init('plan')
|
plan_send.init('plan')
|
||||||
|
@ -71,8 +382,12 @@ class Planner(object):
|
||||||
if 'fault' in self.radar_errors:
|
if 'fault' in self.radar_errors:
|
||||||
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||||
|
|
||||||
plan_send.plan.events = events
|
# Interpolation of trajectory
|
||||||
|
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
|
||||||
|
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
|
||||||
|
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
|
||||||
|
|
||||||
|
plan_send.plan.events = events
|
||||||
plan_send.plan.mdMonoTime = self.last_md_ts
|
plan_send.plan.mdMonoTime = self.last_md_ts
|
||||||
plan_send.plan.l20MonoTime = self.last_l20_ts
|
plan_send.plan.l20MonoTime = self.last_l20_ts
|
||||||
|
|
||||||
|
@ -83,15 +398,17 @@ class Planner(object):
|
||||||
|
|
||||||
# longitudal plan
|
# longitudal plan
|
||||||
plan_send.plan.longitudinalValid = not self.radar_dead
|
plan_send.plan.longitudinalValid = not self.radar_dead
|
||||||
plan_send.plan.vTarget = float(self.AC.v_target_lead)
|
plan_send.plan.vCruise = self.v_cruise
|
||||||
plan_send.plan.aTargetMin = float(self.AC.a_target[0])
|
plan_send.plan.aCruise = self.a_cruise
|
||||||
plan_send.plan.aTargetMax = float(self.AC.a_target[1])
|
plan_send.plan.vTarget = self.v_acc_sol
|
||||||
plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
|
plan_send.plan.aTarget = self.a_acc_sol
|
||||||
plan_send.plan.hasLead = self.AC.has_lead
|
plan_send.plan.vTargetFuture = self.v_acc_future
|
||||||
|
plan_send.plan.hasLead = self.mpc1.prev_lead_status
|
||||||
|
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
|
||||||
|
|
||||||
# compute risk of collision events: fcw
|
# Send out fcw
|
||||||
self.FCW.process(CS, self.AC)
|
fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
|
||||||
plan_send.plan.fcw = bool(self.FCW.active)
|
plan_send.plan.fcw = fcw
|
||||||
|
|
||||||
self.plan.send(plan_send.to_bytes())
|
self.plan.send(plan_send.to_bytes())
|
||||||
return plan_send
|
return plan_send
|
||||||
|
|
|
@ -0,0 +1,88 @@
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin):
|
||||||
|
|
||||||
|
tDelta = 0.
|
||||||
|
if aEgo > aMax:
|
||||||
|
tDelta = (aMax - aEgo) / jMin
|
||||||
|
elif aEgo < aMin:
|
||||||
|
tDelta = (aMin - aEgo) / jMax
|
||||||
|
|
||||||
|
return tDelta
|
||||||
|
|
||||||
|
|
||||||
|
def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):
|
||||||
|
|
||||||
|
dV = vT - vEgo
|
||||||
|
|
||||||
|
tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)
|
||||||
|
|
||||||
|
if (ts <= tDelta):
|
||||||
|
if (aEgo < aMin):
|
||||||
|
vEgo += ts * aEgo + 0.5 * ts**2 * jMax
|
||||||
|
aEgo += ts * jMax
|
||||||
|
return vEgo, aEgo
|
||||||
|
elif (aEgo > aMax):
|
||||||
|
vEgo += ts * aEgo + 0.5 * ts**2 * jMin
|
||||||
|
aEgo += ts * jMin
|
||||||
|
return vEgo, aEgo
|
||||||
|
|
||||||
|
if aEgo > aMax:
|
||||||
|
dV -= 0.5 * (aMax**2 - aEgo**2) / jMin
|
||||||
|
vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin
|
||||||
|
aEgo += tDelta * jMin
|
||||||
|
elif aEgo < aMin:
|
||||||
|
dV -= 0.5 * (aMin**2 - aEgo**2) / jMax
|
||||||
|
vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax
|
||||||
|
aEgo += tDelta * jMax
|
||||||
|
|
||||||
|
ts -= tDelta
|
||||||
|
|
||||||
|
jLim = jMin if aEgo >= 0 else jMax
|
||||||
|
# if we reduce the accel to zero immediately, how much delta speed we generate?
|
||||||
|
dv_min_shift = - 0.5 * aEgo**2 / jLim
|
||||||
|
|
||||||
|
# flip signs so we can consider only one case
|
||||||
|
flipped = False
|
||||||
|
if dV < dv_min_shift:
|
||||||
|
flipped = True
|
||||||
|
dV *= -1
|
||||||
|
vEgo *= -1
|
||||||
|
aEgo *= -1
|
||||||
|
aMax = -aMin
|
||||||
|
jMaxcopy = -jMin
|
||||||
|
jMin = -jMax
|
||||||
|
jMax = jMaxcopy
|
||||||
|
|
||||||
|
# small addition needed to avoid numerical issues with sqrt of ~zero
|
||||||
|
aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin))
|
||||||
|
|
||||||
|
if aPeak > aMax:
|
||||||
|
aPeak = aMax
|
||||||
|
t1 = (aPeak - aEgo) / jMax
|
||||||
|
vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
|
||||||
|
if vChange < aPeak * ts:
|
||||||
|
t2 = t1 + vChange / aPeak
|
||||||
|
else:
|
||||||
|
t2 = t1 + ts
|
||||||
|
else:
|
||||||
|
t1 = (aPeak - aEgo) / jMax
|
||||||
|
t2 = t1
|
||||||
|
t3 = t2 - aPeak / jMin
|
||||||
|
|
||||||
|
dt1 = min(ts, t1)
|
||||||
|
dt2 = max(min(ts, t2) - t1, 0.)
|
||||||
|
dt3 = max(min(ts, t3) - t2, 0.)
|
||||||
|
|
||||||
|
if ts > t3:
|
||||||
|
vEgo += dV
|
||||||
|
aEgo = 0.
|
||||||
|
else:
|
||||||
|
vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin
|
||||||
|
aEgo += jMax * dt1 + dt3 * jMin
|
||||||
|
|
||||||
|
vEgo *= -1 if flipped else 1
|
||||||
|
aEgo *= -1 if flipped else 1
|
||||||
|
|
||||||
|
return float(vEgo), float(aEgo)
|
|
@ -79,8 +79,9 @@ def radard_thread(gctx=None):
|
||||||
tsv = 1./rate
|
tsv = 1./rate
|
||||||
v_len = 20 # how many speed data points to remember for t alignment with rdr data
|
v_len = 20 # how many speed data points to remember for t alignment with rdr data
|
||||||
|
|
||||||
enabled = 0
|
active = 0
|
||||||
steer_angle = 0.
|
steer_angle = 0.
|
||||||
|
steer_override = False
|
||||||
|
|
||||||
tracks = defaultdict(dict)
|
tracks = defaultdict(dict)
|
||||||
|
|
||||||
|
@ -104,9 +105,10 @@ def radard_thread(gctx=None):
|
||||||
# receive the live100s
|
# receive the live100s
|
||||||
l100 = messaging.recv_sock(live100)
|
l100 = messaging.recv_sock(live100)
|
||||||
if l100 is not None:
|
if l100 is not None:
|
||||||
enabled = l100.live100.enabled
|
active = l100.live100.active
|
||||||
v_ego = l100.live100.vEgo
|
v_ego = l100.live100.vEgo
|
||||||
steer_angle = l100.live100.angleSteers
|
steer_angle = l100.live100.angleSteers
|
||||||
|
steer_override = l100.live100.steerOverride
|
||||||
|
|
||||||
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
|
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
|
||||||
v_ego_array = v_ego_array[:, 1:]
|
v_ego_array = v_ego_array[:, 1:]
|
||||||
|
@ -137,7 +139,7 @@ def radard_thread(gctx=None):
|
||||||
del ar_pts[VISION_POINT]
|
del ar_pts[VISION_POINT]
|
||||||
|
|
||||||
# *** compute the likely path_y ***
|
# *** compute the likely path_y ***
|
||||||
if enabled: # use path from model path_poly
|
if active and not steer_override: # use path from model path_poly
|
||||||
path_y = np.polyval(PP.d_poly, path_x)
|
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 since calibration does not exactly report the physical offset
|
||||||
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
|
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
|
||||||
|
|
Binary file not shown.
|
@ -581,6 +581,8 @@ def main():
|
||||||
params.put("IsMetric", "0")
|
params.put("IsMetric", "0")
|
||||||
if params.get("IsRearViewMirror") is None:
|
if params.get("IsRearViewMirror") is None:
|
||||||
params.put("IsRearViewMirror", "1")
|
params.put("IsRearViewMirror", "1")
|
||||||
|
if params.get("IsFcwEnabled") is None:
|
||||||
|
params.put("IsFcwEnabled", "1")
|
||||||
|
|
||||||
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
|
params.put("Passive", "1" if os.getenv("PASSIVE") else "0")
|
||||||
|
|
||||||
|
|
|
@ -8,16 +8,18 @@ import zmq
|
||||||
from selfdrive.services import service_list
|
from selfdrive.services import service_list
|
||||||
import selfdrive.messaging as messaging
|
import selfdrive.messaging as messaging
|
||||||
|
|
||||||
|
|
||||||
|
RADAR_MSGS = range(0x210, 0x220)
|
||||||
|
|
||||||
def _create_radard_can_parser():
|
def _create_radard_can_parser():
|
||||||
dbc_f = 'toyota_prius_2017_adas.dbc'
|
dbc_f = 'toyota_prius_2017_adas.dbc'
|
||||||
radar_messages = range(0x210, 0x220)
|
msg_n = len(RADAR_MSGS)
|
||||||
msg_n = len(radar_messages)
|
msg_last = RADAR_MSGS[-1]
|
||||||
msg_last = radar_messages[-1]
|
|
||||||
signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n +
|
signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n +
|
||||||
['REL_SPEED'] * msg_n + ['VALID'] * msg_n,
|
['REL_SPEED'] * msg_n + ['VALID'] * msg_n,
|
||||||
radar_messages * 5,
|
RADAR_MSGS * 5,
|
||||||
[255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n)
|
[255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n)
|
||||||
checks = zip(radar_messages, [20]*msg_n)
|
checks = zip(RADAR_MSGS, [20]*msg_n)
|
||||||
|
|
||||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
||||||
|
|
||||||
|
@ -25,6 +27,7 @@ class RadarInterface(object):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
# radar
|
# radar
|
||||||
self.pts = {}
|
self.pts = {}
|
||||||
|
self.ptsValid = {key: False for key in RADAR_MSGS}
|
||||||
self.track_id = 0
|
self.track_id = 0
|
||||||
|
|
||||||
self.delay = 0.0 # Delay of radar
|
self.delay = 0.0 # Delay of radar
|
||||||
|
@ -55,7 +58,17 @@ class RadarInterface(object):
|
||||||
#print "NEW TRACKS"
|
#print "NEW TRACKS"
|
||||||
for ii in updated_messages:
|
for ii in updated_messages:
|
||||||
cpt = self.rcp.vl[ii]
|
cpt = self.rcp.vl[ii]
|
||||||
if cpt['LONG_DIST'] < 255 and cpt['VALID']:
|
|
||||||
|
# a point needs one valid measurement before being considered
|
||||||
|
#if cpt['NEW_TRACK'] or cpt['LONG_DIST'] >= 255:
|
||||||
|
# self.ptsValid[ii] = False # reset validity
|
||||||
|
# TODO: find better way to eliminate both false positive and false negative
|
||||||
|
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
|
||||||
|
self.ptsValid[ii] = True
|
||||||
|
else:
|
||||||
|
self.ptsValid[ii] = False
|
||||||
|
|
||||||
|
if self.ptsValid[ii]:
|
||||||
#print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
|
#print "%5s %5s %5s" % (round(cpt['LONG_DIST'], 1), round(cpt['LAT_DIST'], 1), round(cpt['REL_SPEED'], 1))
|
||||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||||
self.pts[ii] = car.RadarState.RadarPoint.new_message()
|
self.pts[ii] = car.RadarState.RadarPoint.new_message()
|
||||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -45,6 +45,7 @@ gpsLocationExternal: [8032, true]
|
||||||
ubloxGnss: [8033, true]
|
ubloxGnss: [8033, true]
|
||||||
clocks: [8034, true]
|
clocks: [8034, true]
|
||||||
liveMpc: [8035, true]
|
liveMpc: [8035, true]
|
||||||
|
liveLongitudinalMpc: [8036, true]
|
||||||
|
|
||||||
testModel: [8040, false]
|
testModel: [8040, false]
|
||||||
|
|
||||||
|
|
|
@ -64,7 +64,7 @@ class Maneuver(object):
|
||||||
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
|
v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid,
|
||||||
cruise_speed=last_live100.vCruise,
|
cruise_speed=last_live100.vCruise,
|
||||||
jerk_factor=last_live100.jerkFactor,
|
jerk_factor=last_live100.jerkFactor,
|
||||||
a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax)
|
a_target=last_live100.aTarget)
|
||||||
|
|
||||||
print "maneuver end"
|
print "maneuver end"
|
||||||
|
|
||||||
|
|
|
@ -29,8 +29,7 @@ class ManeuverPlot(object):
|
||||||
self.cruise_speed_array = []
|
self.cruise_speed_array = []
|
||||||
self.jerk_factor_array = []
|
self.jerk_factor_array = []
|
||||||
|
|
||||||
self.a_target_min_array = []
|
self.a_target_array = []
|
||||||
self.a_target_max_array = []
|
|
||||||
|
|
||||||
self.v_target_array = []
|
self.v_target_array = []
|
||||||
|
|
||||||
|
@ -38,8 +37,7 @@ class ManeuverPlot(object):
|
||||||
|
|
||||||
def add_data(self, time, gas, brake, steer_torque, distance, speed,
|
def add_data(self, time, gas, brake, steer_torque, distance, speed,
|
||||||
acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead,
|
acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead,
|
||||||
v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target_min,
|
v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target):
|
||||||
a_target_max):
|
|
||||||
self.time_array.append(time)
|
self.time_array.append(time)
|
||||||
self.gas_array.append(gas)
|
self.gas_array.append(gas)
|
||||||
self.brake_array.append(brake)
|
self.brake_array.append(brake)
|
||||||
|
@ -56,8 +54,7 @@ class ManeuverPlot(object):
|
||||||
self.pid_speed_array.append(pid_speed)
|
self.pid_speed_array.append(pid_speed)
|
||||||
self.cruise_speed_array.append(cruise_speed)
|
self.cruise_speed_array.append(cruise_speed)
|
||||||
self.jerk_factor_array.append(jerk_factor)
|
self.jerk_factor_array.append(jerk_factor)
|
||||||
self.a_target_min_array.append(a_target_min)
|
self.a_target_array.append(a_target)
|
||||||
self.a_target_max_array.append(a_target_max)
|
|
||||||
|
|
||||||
|
|
||||||
def write_plot(self, path, maneuver_name):
|
def write_plot(self, path, maneuver_name):
|
||||||
|
@ -90,12 +87,11 @@ class ManeuverPlot(object):
|
||||||
plt.figure(plt_num)
|
plt.figure(plt_num)
|
||||||
plt.plot(
|
plt.plot(
|
||||||
np.array(self.time_array), np.array(self.acceleration_array), 'g',
|
np.array(self.time_array), np.array(self.acceleration_array), 'g',
|
||||||
np.array(self.time_array), np.array(self.a_target_min_array), 'k--',
|
np.array(self.time_array), np.array(self.a_target_array), 'k--',
|
||||||
np.array(self.time_array), np.array(self.a_target_max_array), 'k--',
|
|
||||||
)
|
)
|
||||||
plt.xlabel('Time [s]')
|
plt.xlabel('Time [s]')
|
||||||
plt.ylabel('Acceleration [m/s^2]')
|
plt.ylabel('Acceleration [m/s^2]')
|
||||||
plt.legend(['accel', 'max', 'min'], loc=0)
|
plt.legend(['ego-plant', 'target'], loc=0)
|
||||||
plt.grid()
|
plt.grid()
|
||||||
pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
|
pylab.savefig("/".join([path, maneuver_name, 'acceleration.svg']), dpi=1000)
|
||||||
|
|
||||||
|
|
|
@ -253,7 +253,8 @@ class Plant(object):
|
||||||
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
|
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
|
||||||
|
|
||||||
# ******** publish a fake model going straight and fake calibration ********
|
# ******** publish a fake model going straight and fake calibration ********
|
||||||
if publish_model:
|
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||||
|
if publish_model and self.rk.frame % 5 == 0:
|
||||||
md = messaging.new_message()
|
md = messaging.new_message()
|
||||||
cal = messaging.new_message()
|
cal = messaging.new_message()
|
||||||
md.init('model')
|
md.init('model')
|
||||||
|
|
|
@ -156,6 +156,18 @@ maneuvers = [
|
||||||
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
|
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
|
||||||
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
|
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
|
||||||
),
|
),
|
||||||
|
Maneuver(
|
||||||
|
"stop and go with 1.5m/s2 lead accel and 3.3m/s^2 lead decel, with full stops",
|
||||||
|
duration=45.,
|
||||||
|
initial_speed=0.,
|
||||||
|
lead_relevancy=True,
|
||||||
|
initial_distance_lead=20.,
|
||||||
|
speed_lead_values=[10., 0., 0., 10., 0., 0.] ,
|
||||||
|
speed_lead_breakpoints=[10., 13., 26., 33., 36., 45.],
|
||||||
|
cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3),
|
||||||
|
(CB.RES_ACCEL, 1.4), (0.0, 1.5),
|
||||||
|
(CB.RES_ACCEL, 1.6), (0.0, 1.7)]
|
||||||
|
),
|
||||||
Maneuver(
|
Maneuver(
|
||||||
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
|
"accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2",
|
||||||
duration=30.,
|
duration=30.,
|
||||||
|
|
Binary file not shown.
Loading…
Reference in New Issue