mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-20 05:22:03 +08:00
Merge branch 'devel' of https://github.com/commaai/openpilot into devel-lexus-ish-ave30r
This commit is contained in:
Rick Lan
@@ -17,3 +17,5 @@ COPY ./phonelibs /tmp/openpilot/phonelibs
|
||||
COPY ./pyextra /tmp/openpilot/pyextra
|
||||
|
||||
RUN mkdir -p /tmp/openpilot/selfdrive/test/out
|
||||
RUN make -C /tmp/openpilot/selfdrive/controls/lib/longitudinal_mpc clean
|
||||
RUN make -C /tmp/openpilot/selfdrive/controls/lib/lateral_mpc clean
|
||||
|
||||
+11
@@ -1,3 +1,14 @@
|
||||
Version 0.5.4 (2018-09-25)
|
||||
========================
|
||||
* New Driving Model
|
||||
* New Driver Monitoring Model
|
||||
* Improve longitudinal mpc in mid-low speed braking
|
||||
* Honda Accord hybrid support thanks to energee!
|
||||
* Ship mpc binaries and sensibly reduce build time
|
||||
* Calibration more stable
|
||||
* More Hyundai and Kia cars supported thanks to emmertex!
|
||||
* Various GM Volt improvements thanks to vntarasov!
|
||||
|
||||
Version 0.5.3 (2018-09-03)
|
||||
========================
|
||||
* Hyundai Santa Fe support!
|
||||
|
||||
+3
-1
@@ -42,7 +42,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
speedTooLow @17;
|
||||
outOfSpace @18;
|
||||
overheat @19;
|
||||
calibrationInProgress @20;
|
||||
calibrationIncomplete @20;
|
||||
calibrationInvalid @21;
|
||||
controlsMismatch @22;
|
||||
pcmEnable @23;
|
||||
@@ -69,6 +69,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
|
||||
promptDriverUnresponsive @44;
|
||||
driverUnresponsive @45;
|
||||
belowSteerSpeed @46;
|
||||
calibrationProgress @47;
|
||||
lowBattery @48;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -276,6 +276,7 @@ struct ThermalData {
|
||||
startedTs @13 :UInt64;
|
||||
|
||||
thermalStatus @14 :ThermalStatus;
|
||||
chargerDisabled @17 :Bool;
|
||||
|
||||
enum ThermalStatus {
|
||||
green @0; # all processes run
|
||||
|
||||
+82
-45
@@ -1,7 +1,6 @@
|
||||
import re
|
||||
import os
|
||||
import struct
|
||||
import bitstring
|
||||
import sys
|
||||
import numbers
|
||||
from collections import namedtuple, defaultdict
|
||||
@@ -17,6 +16,7 @@ DBCSignal = namedtuple(
|
||||
"DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
|
||||
"factor", "offset", "tmin", "tmax", "units"])
|
||||
|
||||
|
||||
class dbc(object):
|
||||
def __init__(self, fn):
|
||||
self.name, _ = os.path.splitext(os.path.basename(fn))
|
||||
@@ -122,6 +122,16 @@ class dbc(object):
|
||||
msg_id = self.msg_name_to_address[msg_id]
|
||||
return msg_id
|
||||
|
||||
def reverse_bytes(self, x):
|
||||
return ((x & 0xff00000000000000) >> 56) | \
|
||||
((x & 0x00ff000000000000) >> 40) | \
|
||||
((x & 0x0000ff0000000000) >> 24) | \
|
||||
((x & 0x000000ff00000000) >> 8) | \
|
||||
((x & 0x00000000ff000000) << 8) | \
|
||||
((x & 0x0000000000ff0000) << 24) | \
|
||||
((x & 0x000000000000ff00) << 40) | \
|
||||
((x & 0x00000000000000ff) << 56)
|
||||
|
||||
def encode(self, msg_id, dd):
|
||||
"""Encode a CAN message using the dbc.
|
||||
|
||||
@@ -131,35 +141,40 @@ class dbc(object):
|
||||
"""
|
||||
msg_id = self.lookup_msg_id(msg_id)
|
||||
|
||||
# TODO: Stop using bitstring, which is super slow.
|
||||
msg_def = self.msgs[msg_id]
|
||||
size = msg_def[0][1]
|
||||
|
||||
bsf = bitstring.Bits(hex="00"*size)
|
||||
result = 0
|
||||
for s in msg_def[1]:
|
||||
ival = dd.get(s.name)
|
||||
if ival is not None:
|
||||
|
||||
b2 = s.size
|
||||
if s.is_little_endian:
|
||||
b1 = s.start_bit
|
||||
else:
|
||||
b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8
|
||||
bo = 64 - (b1 + s.size)
|
||||
|
||||
ival = (ival / s.factor) - s.offset
|
||||
ival = int(round(ival))
|
||||
|
||||
# should pack this
|
||||
if s.is_signed and ival < 0:
|
||||
ival = (1 << b2) + ival
|
||||
|
||||
shift = b1 if s.is_little_endian else bo
|
||||
mask = ((1 << b2) - 1) << shift
|
||||
dat = (ival & ((1 << b2) - 1)) << shift
|
||||
|
||||
if s.is_little_endian:
|
||||
ss = s.start_bit
|
||||
else:
|
||||
ss = self.bits_index[s.start_bit]
|
||||
mask = self.reverse_bytes(mask)
|
||||
dat = self.reverse_bytes(dat)
|
||||
|
||||
result &= ~mask
|
||||
result |= dat
|
||||
|
||||
if s.is_signed:
|
||||
tbs = bitstring.Bits(int=ival, length=s.size)
|
||||
else:
|
||||
tbs = bitstring.Bits(uint=ival, length=s.size)
|
||||
|
||||
lpad = bitstring.Bits(bin="0b"+"0"*ss)
|
||||
rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
|
||||
tbs = lpad+tbs+rpad
|
||||
|
||||
bsf |= tbs
|
||||
return bsf.tobytes()
|
||||
result = struct.pack('>Q', result)
|
||||
return result[:size]
|
||||
|
||||
def decode(self, x, arr=None, debug=False):
|
||||
"""Decode a CAN message using the dbc.
|
||||
@@ -195,55 +210,77 @@ class dbc(object):
|
||||
if debug:
|
||||
print name
|
||||
|
||||
blen = 8*len(x[2])
|
||||
|
||||
st = x[2].rjust(8, '\x00')
|
||||
st = x[2].ljust(8, '\x00')
|
||||
le, be = None, None
|
||||
size = msg[0][1]
|
||||
|
||||
for s in msg[1]:
|
||||
if arr is not None and s[0] not in arr:
|
||||
continue
|
||||
|
||||
# big or little endian?
|
||||
# see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
|
||||
if s[3] is False:
|
||||
ss = self.bits_index[s[1]]
|
||||
if be is None:
|
||||
be = struct.unpack(">Q", st)[0]
|
||||
x2_int = be
|
||||
data_bit_pos = (blen - (ss + s[2]))
|
||||
start_bit = s[1]
|
||||
signal_size = s[2]
|
||||
little_endian = s[3]
|
||||
signed = s[4]
|
||||
factor = s[5]
|
||||
offset = s[6]
|
||||
|
||||
b2 = signal_size
|
||||
if little_endian:
|
||||
b1 = start_bit
|
||||
else:
|
||||
b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8
|
||||
bo = 64 - (b1 + signal_size)
|
||||
|
||||
if little_endian:
|
||||
if le is None:
|
||||
le = struct.unpack("<Q", st)[0]
|
||||
x2_int = le >> (64 - 8 * size)
|
||||
ss = s[1]
|
||||
data_bit_pos = ss
|
||||
shift_amount = b1
|
||||
tmp = le
|
||||
else:
|
||||
if be is None:
|
||||
be = struct.unpack(">Q", st)[0]
|
||||
shift_amount = bo
|
||||
tmp = be
|
||||
|
||||
if data_bit_pos < 0:
|
||||
if shift_amount < 0:
|
||||
continue
|
||||
ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
|
||||
|
||||
if s[4] and (ival & (1<<(s[2]-1))): # signed
|
||||
ival -= (1<<s[2])
|
||||
tmp = (tmp >> shift_amount) & ((1 << b2) - 1)
|
||||
if signed and (tmp >> (b2 - 1)):
|
||||
tmp -= (1 << b2)
|
||||
|
||||
# control the offset
|
||||
ival = (ival * s[5]) + s[6]
|
||||
#if debug:
|
||||
# print "%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
|
||||
tmp = tmp * factor + offset
|
||||
|
||||
# if debug:
|
||||
# print "%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])
|
||||
|
||||
if arr is None:
|
||||
out[s[0]] = ival
|
||||
out[s[0]] = tmp
|
||||
else:
|
||||
out[arr.index(s[0])] = ival
|
||||
out[arr.index(s[0])] = tmp
|
||||
return name, out
|
||||
|
||||
def get_signals(self, msg):
|
||||
msg = self.lookup_msg_id(msg)
|
||||
return [sgs.name for sgs in self.msgs[msg][1]]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from opendbc import DBC_PATH
|
||||
import numpy as np
|
||||
|
||||
dbc_test = dbc(os.path.join(DBC_PATH, sys.argv[1]))
|
||||
print dbc_test.get_signals(0xe4)
|
||||
dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc'))
|
||||
msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2})
|
||||
encoded = dbc_test.encode(*msg)
|
||||
decoded = dbc_test.decode((0x25, 0, encoded))
|
||||
assert decoded == msg
|
||||
|
||||
dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc'))
|
||||
decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12"))
|
||||
assert np.isclose(decoded[1]['SAS_Angle'], -26.2)
|
||||
|
||||
msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0})
|
||||
encoded = dbc_test.encode(*msg)
|
||||
decoded = dbc_test.decode((0x2b0, 0, encoded))
|
||||
|
||||
assert decoded == msg
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
class FirstOrderFilter():
|
||||
# first order filter
|
||||
def __init__(self, x0, ts, dt):
|
||||
self.k = (dt / ts) / (1. + dt / ts)
|
||||
self.x = x0
|
||||
|
||||
def update(self, x):
|
||||
self.x = (1. - self.k) * self.x + self.k * x
|
||||
|
||||
|
||||
@@ -57,6 +57,8 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
|
||||
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
|
||||
get_view_frame_from_road_frame(0, 0, 0, model_height))
|
||||
|
||||
model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
|
||||
|
||||
# 'camera from model camera'
|
||||
def get_model_height_transform(camera_frame_from_road_frame, height):
|
||||
camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([
|
||||
|
||||
@@ -189,7 +189,7 @@ BO_ 1033 ASCMKeepAlive: 7 NEO
|
||||
SG_ ASCMKeepAliveAllZero : 7|56@0+ (1,0) [0|0] "" NEO
|
||||
|
||||
BO_ 1217 ECMEngineCoolantTemp: 8 K20_ECM
|
||||
SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C" NEO
|
||||
SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C" NEO
|
||||
|
||||
BO_ 1249 VIN_Part2: 8 K20_ECM
|
||||
SG_ VINPart2 : 7|64@0+ (1,0) [0|0] "" NEO
|
||||
|
||||
@@ -126,7 +126,7 @@ BO_ 1302 ODOMETER: 8 XXX
|
||||
|
||||
CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
|
||||
CM_ SG_ 420 BRAKE_HOLD_RELATED "On when Brake Hold engaged";
|
||||
CM_ SG_ 450 EPB_STATE "3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged"";
|
||||
CM_ SG_ 450 EPB_STATE "3 \"engaged\" 2 \"disengaging\" 1 \"engaging\" 0 \"disengaged\"";
|
||||
CM_ SG_ 806 REVERSE_LIGHT "Might be reverse gear selected and not the lights";
|
||||
|
||||
VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
@@ -180,7 +180,7 @@ BO_ 1033 ASCMKeepAlive: 7 NEO
|
||||
BO_ 1034 ASCM_40A: 7 K124_ASCM
|
||||
|
||||
BO_ 1217 ECMEngineCoolantTemp: 8 K20_ECM
|
||||
SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C" NEO
|
||||
SG_ EngineCoolantTemp : 23|8@0+ (1,-40) [0|0] "°C" NEO
|
||||
|
||||
BO_ 1249 VIN_Part2: 8 K20_ECM
|
||||
SG_ VINPart2 : 7|64@0+ (1,0) [0|0] "" NEO
|
||||
|
||||
@@ -355,7 +355,7 @@ BO_ 1302 ODOMETER: 8 XXX
|
||||
|
||||
CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
|
||||
CM_ SG_ 420 BRAKE_HOLD_RELATED "On when Brake Hold engaged";
|
||||
CM_ SG_ 450 EPB_STATE "3 "engaged" 2 "disengaging" 1 "engaging" 0 "disengaged"";
|
||||
CM_ SG_ 450 EPB_STATE "3 \"engaged\" 2 \"disengaging\" 1 \"engaging\" 0 \"disengaged\"";
|
||||
CM_ SG_ 806 REVERSE_LIGHT "Might be reverse gear selected and not the lights";
|
||||
|
||||
VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
|
||||
@@ -0,0 +1,295 @@
|
||||
VERSION ""
|
||||
|
||||
|
||||
NS_ :
|
||||
NS_DESC_
|
||||
CM_
|
||||
BA_DEF_
|
||||
BA_
|
||||
VAL_
|
||||
CAT_DEF_
|
||||
CAT_
|
||||
FILTER
|
||||
BA_DEF_DEF_
|
||||
EV_DATA_
|
||||
ENVVAR_DATA_
|
||||
SGTYPE_
|
||||
SGTYPE_VAL_
|
||||
BA_DEF_SGTYPE_
|
||||
BA_SGTYPE_
|
||||
SIG_TYPE_REF_
|
||||
VAL_TABLE_
|
||||
SIG_GROUP_
|
||||
SIG_VALTYPE_
|
||||
SIGTYPE_VALTYPE_
|
||||
BO_TX_BU_
|
||||
BA_DEF_REL_
|
||||
BA_REL_
|
||||
BA_DEF_DEF_REL_
|
||||
BU_SG_REL_
|
||||
BU_EV_REL_
|
||||
BU_BO_REL_
|
||||
SG_MUL_VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
BU_: EBCM ADAS PCM EPS VSA SCM BDY XXX EPB EON INTERCEPTOR
|
||||
|
||||
|
||||
BO_ 344 ENGINE_DATA: 8 PCM
|
||||
SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ ZEROS : 23|16@0+ (1,0) [0|15000] "" EON
|
||||
|
||||
BO_ 380 POWERTRAIN_DATA: 8 PCM
|
||||
SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON
|
||||
SG_ GAS_PRESSED : 39|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ ACC_STATUS : 38|1@0+ (1,0) [0|1] "rpm" EON
|
||||
SG_ BOH_17C : 37|5@0+ (1,0) [0|1] "rpm" EON
|
||||
SG_ BRAKE_SWITCH : 32|1@0+ (1,0) [0|1] "rpm" EON
|
||||
SG_ BOH2_17C : 47|10@0+ (1,0) [0|1] "rpm" EON
|
||||
SG_ BRAKE_PRESSED : 53|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ BOH3_17C : 52|5@0+ (1,0) [0|1] "rpm" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 432 STANDSTILL: 7 VSA
|
||||
SG_ CONTROLLED_STANDSTILL : 0|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ WHEELS_MOVING : 12|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ BRAKE_ERROR_1 : 11|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ BRAKE_ERROR_2 : 9|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 464 WHEEL_SPEEDS: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|15@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ WHEEL_SPEED_FR : 8|15@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ WHEEL_SPEED_RL : 25|15@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ WHEEL_SPEED_RR : 42|15@0+ (0.01,0) [0|250] "kph" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 490 VEHICLE_DYNAMICS: 8 VSA
|
||||
SG_ LONG_ACCEL : 23|16@0- (0.0015384,0) [-20|20] "m/s2" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 506 BRAKE_COMMAND: 8 ADAS
|
||||
SG_ COMPUTER_BRAKE : 7|10@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ ZEROS_BOH : 13|5@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ BRAKE_PUMP_REQUEST : 8|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_BOH2 : 23|3@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_OVERRIDE : 20|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_BOH3 : 19|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_FAULT_CMD : 18|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_CANCEL_CMD : 17|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ COMPUTER_BRAKE_REQUEST : 16|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ SET_ME_0X80 : 31|8@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ BRAKE_LIGHTS : 39|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CRUISE_STATES : 38|7@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ CHIME : 47|3@0+ (1,0) [0|7] "" EBCM
|
||||
SG_ ZEROS_BOH6 : 44|1@0+ (1,0) [0|1] "" EBCM
|
||||
SG_ FCW : 43|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ ZEROS_BOH4 : 55|8@0+ (1,0) [0|0] "" EBCM
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EBCM
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EBCM
|
||||
|
||||
BO_ 512 GAS_COMMAND: 6 EON
|
||||
SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR
|
||||
SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR
|
||||
SG_ ENABLE : 39|1@0+ (1,0) [0|1] "" INTERCEPTOR
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" INTERCEPTOR
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|3] "" INTERCEPTOR
|
||||
|
||||
BO_ 513 GAS_SENSOR: 6 INTERCEPTOR
|
||||
SG_ INTERCEPTOR_GAS : 7|16@0+ (0.253984064,-83.3) [0|1] "" EON
|
||||
SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" EON
|
||||
SG_ STATE : 39|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA
|
||||
SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_RL : 23|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ WHEEL_SPEED_RR : 31|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ SET_TO_X55 : 39|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 773 SEATBELT_STATUS: 7 BDY
|
||||
SG_ SEATBELT_DRIVER_LAMP : 7|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ SEATBELT_PASS_UNLATCHED : 10|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ SEATBELT_PASS_LATCHED : 11|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ SEATBELT_DRIVER_UNLATCHED : 12|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ SEATBELT_DRIVER_LATCHED : 13|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ PASS_AIRBAG_OFF : 14|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ PASS_AIRBAG_ON : 15|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 777 LOCK_STATUS: 8 XXX
|
||||
SG_ DOORS_UNLOCKED : 54|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ DOORS_LOCKED : 55|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 780 ACC_HUD: 8 ADAS
|
||||
SG_ PCM_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" BDY
|
||||
SG_ PCM_GAS : 23|8@0+ (1,0) [0|127] "" BDY
|
||||
SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "" BDY
|
||||
SG_ DTC_MODE : 39|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_OFF : 36|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BOH_2 : 35|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X03 : 47|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY
|
||||
SG_ HUD_DISTANCE_3 : 52|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X03_2 : 55|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ SET_ME_X01 : 48|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" BDY
|
||||
|
||||
BO_ 804 CRUISE: 8 PCM
|
||||
SG_ HUD_SPEED_KPH : 7|8@0+ (1,0) [0|255] "kph" EON
|
||||
SG_ HUD_SPEED_MPH : 15|8@0+ (1,0) [0|255] "mph" EON
|
||||
SG_ TRIP_FUEL_CONSUMED : 23|16@0+ (1,0) [0|255] "" EON
|
||||
SG_ CRUISE_SPEED_PCM : 39|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ BOH2 : 47|8@0- (1,0) [0|255] "" EON
|
||||
SG_ BOH3 : 55|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 829 LKAS_HUD: 5 ADAS
|
||||
SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY
|
||||
SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY
|
||||
SG_ BOH : 6|7@0+ (1,0) [0|127] "" BDY
|
||||
SG_ DASHED_LANES : 14|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ DTC : 13|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ LKAS_PROBLEM : 12|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ LKAS_OFF : 11|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BOH2 : 23|2@0+ (1,0) [0|4] "" BDY
|
||||
SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY
|
||||
SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ LDW_OFF : 27|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ CLEAN_WINDSHIELD : 26|1@0+ (1,0) [0|1] "" BDY
|
||||
SG_ SET_ME_X48 : 31|8@0+ (1,0) [0|255] "" BDY
|
||||
SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" BDY
|
||||
SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
BO_ 892 CRUISE_PARAMS: 8 PCM
|
||||
SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 316 GAS_PEDAL: 8 PCM
|
||||
SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON
|
||||
|
||||
BO_ 342 STEERING_SENSORS: 6 EPS
|
||||
SG_ STEER_ANGLE_RATE : 23|16@0- (1,0) [-3000|3000] "deg/s" EON
|
||||
SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ STEER_ANGLE : 7|16@0- (-0.1,0) [-500|500] "deg" EON
|
||||
|
||||
BO_ 399 STEER_STATUS: 7 EPS
|
||||
SG_ STEER_TORQUE_SENSOR : 7|16@0- (1,0) [-2985|2985] "tbd" EON
|
||||
SG_ STEER_TORQUE_MOTOR : 23|16@0- (1,0) [-31000|31000] "tbd" EON
|
||||
SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ STEER_STATUS : 43|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ STEER_CONTROL_ACTIVE : 35|1@0+ (1,0) [0|1] "" EON
|
||||
|
||||
BO_ 401 GEARBOX: 8 PCM
|
||||
SG_ GEAR_SHIFTER : 5|6@0+ (1,0) [0|63] "" EON
|
||||
SG_ GEAR : 35|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 404 STEERING_CONTROL: 4 EON
|
||||
SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS
|
||||
SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS
|
||||
SG_ COUNTER : 29|2@0+ (1,0) [0|15] "" EPS
|
||||
SG_ CHECKSUM : 27|4@0+ (1,0) [0|3] "" EPS
|
||||
SG_ STEER_TORQUE : 7|16@0- (-1,0) [-32767|32767] "" EPS
|
||||
|
||||
BO_ 420 VSA_STATUS: 8 VSA
|
||||
SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON
|
||||
SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 422 SCM_BUTTONS: 8 SCM
|
||||
SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON
|
||||
SG_ LIGHTS_SETTING : 1|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ MAIN_ON : 47|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ CRUISE_SETTING : 43|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ DRIVERS_DOOR_OPEN : 63|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 450 EPB_STATUS: 8 XXX
|
||||
SG_ EPB_BRAKE_AND_PULL : 6|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ EPB_STATE : 29|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 660 SCM_FEEDBACK: 8 SCM
|
||||
SG_ RIGHT_BLINKER : 6|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ LEFT_BLINKER : 5|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ WIPERS_SPEED : 4|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
BO_ 862 HIGHBEAM_CONTROL: 8 ADAS
|
||||
SG_ ZEROS_BOH : 7|50@0+ (1,0) [0|127] "" BDY
|
||||
SG_ ZEROS_BOH_2 : 48|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ HIGHBEAMS_ON : 52|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ AUTO_HIGHBEAMS_ACTIVE : 53|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 1302 ODOMETER: 8 XXX
|
||||
SG_ ODOMETER : 7|24@0+ (1,0) [0|16777215] "km" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
|
||||
|
||||
|
||||
CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping";
|
||||
CM_ SG_ 773 PASS_AIRBAG_OFF "Might just be indicator light";
|
||||
CM_ SG_ 773 PASS_AIRBAG_ON "Might just be indicator light";
|
||||
CM_ SG_ 780 CRUISE_SPEED "255 = no speed";
|
||||
CM_ SG_ 804 CRUISE_SPEED_PCM "255 = no speed";
|
||||
CM_ SG_ 829 BEEP "beeps are pleasant, chimes are for warnngs etc...";
|
||||
CM_ SG_ 401 GEAR "10 = reverse, 11 = transition";
|
||||
VAL_ 506 CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime" ;
|
||||
VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw" ;
|
||||
VAL_ 513 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ;
|
||||
VAL_ 780 CRUISE_SPEED 255 "no_speed" 252 "stopped" ;
|
||||
VAL_ 780 HUD_LEAD 3 "acc_off" 2 "solid_car" 1 "dashed_car" 0 "no_car" ;
|
||||
VAL_ 829 BEEP 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep" ;
|
||||
VAL_ 399 STEER_STATUS 5 "fault" 4 "no_torque_alert_2" 2 "no_torque_alert_1" 0 "normal" ;
|
||||
VAL_ 401 GEAR_SHIFTER 32 "L" 16 "S" 8 "D" 4 "N" 2 "R" 1 "P" ;
|
||||
VAL_ 401 GEAR 7 "L" 10 "S" 4 "D" 3 "N" 2 "R" 1 "P" ;
|
||||
VAL_ 422 CRUISE_BUTTONS 7 "tbd" 6 "tbd" 5 "tbd" 4 "accel_res" 3 "decel_set" 2 "cancel" 1 "main" 0 "none" ;
|
||||
VAL_ 422 LIGHTS_SETTING 3 "high_beam" 2 "low_beam" 1 "position" 0 "no_lights" ;
|
||||
VAL_ 422 CRUISE_SETTING 3 "distance_adj" 2 "tbd" 1 "lkas_button" 0 "none" ;
|
||||
CM_ "CHFFR_METRIC 342 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0";
|
||||
@@ -36,27 +36,141 @@ BS_:
|
||||
BU_: XXX
|
||||
|
||||
|
||||
BO_ 3 NEW_MSG_1: 8 XXX
|
||||
SG_ STEERING_ANGLE : 7|32@0- (1,0) [0|4294967295] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
BO_ 3 STEER_SENSOR: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEER_RATE : 19|12@0- (0.5,0) [0|255] "" XXX
|
||||
SG_ STEER_DIRECTION : 4|1@0+ (1,2) [0|1] "" XXX
|
||||
SG_ STEER_ANGLE : 3|12@0- (-0.5,0) [-500|500] "degrees" XXX
|
||||
|
||||
BO_ 5 NEW_MSG_2: 8 XXX
|
||||
SG_ BRAKE_POSITION : 23|16@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ BRAKE_PRESSED : 0|8@1+ (1,0) [0|17] "" XXX
|
||||
BO_ 5 BRAKE_MODULE: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BRAKE_HOLD : 2|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_POSITION : 17|10@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ DRIVER_BRAKE : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COMPUTER_BRAKE : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BRAKE_PRESSED : 0|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 69 NEW_MSG_3: 8 XXX
|
||||
SG_ TURN_SIGNAL_LEVER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ CRUISE_CONTROL_LEVER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ STEERING_WHEEL_BUTTONS : 32|8@1+ (1,0) [0|255] "4 directional, 2 volume control, & 2 phone buttons" XXX
|
||||
SG_ MORE_STEERING_WHEELS_BUTTONS : 40|8@1+ (1,0) [0|255] "" XXX
|
||||
BO_ 69 DRIVER_CONTROLS: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ STEERING_WHEEL_BUTTONS : 32|16@1+ (1,0) [0|255] "4 directional, 2 volume control & 2 phone buttons" XXX
|
||||
SG_ LEFT_BLINKER : 16|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RIGHT_BLINKER : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ HIGHBEAM_TOGGLE : 18|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ HIGHBEAM_MOMENTARY : 19|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_CANCEL : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_RESUME : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_ACCEL_HIGH : 2|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_DECEL_HIGH : 3|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_ACCEL_LOW : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_CONTROL_DECEL_LOW : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_XFF : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 1 NEW_MSG_4: 8 XXX
|
||||
SG_ DOOR_LOCK_STATUS : 31|16@0+ (1,0) [0|65535] "" XXX
|
||||
BO_ 513 WHEEL_ENCODERS: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ WHEEL_ENCODER_2 : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ WHEEL_ENCODER_3 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ WHEEL_ENCODER_4 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 56|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ WHEEL_ENCODER_1 : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 261 GAS_PEDAL: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ ENGINE_RPM : 4|5@0+ (1,0) [0|255] "" XXX
|
||||
SG_ GAS_PEDAL : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COMBINED_GAS : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 643 DOOR_SENSORS: 8 XXX
|
||||
SG_ BRAKE_PRESSED : 27|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_OPEN_FR : 3|1@1+ (1,0) [0|3] "" XXX
|
||||
SG_ DOOR_OPEN_RL : 5|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ DOOR_OPEN_RR : 7|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ DOOR_OPEN_FL : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_CLOSED_FL : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_CLOSED_FR : 2|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_CLOSED_RL : 4|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DOOR_CLOSED_RR : 6|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 885 SEATBELT_SENSORS: 8 XXX
|
||||
SG_ SEATBELT_DRIVER_LATCHED : 16|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SEATBELT_PASSENGER_LATCHED : 18|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 257 CRUISE_CONTROL: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 6|1@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CRUISE_DISABLED : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X002 : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X003 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_1 : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_ACCELERATING : 22|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LONGITUDINAL_ACCEL_REQUEST : 15|8@0- (1,0) [0|127] "" XXX
|
||||
|
||||
BO_ 260 CRUISE_CONTROL2: 8 XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_XFF : 31|8@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ SET_ME_X02 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_XFF2 : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 7|4@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 14 STEER_TORQUE: 8 XXX
|
||||
SG_ STEER_TORQUE : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 888 CRUISE_CONTROL3: 8 XXX
|
||||
SG_ NEW_SIGNAL_2 : 7|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_5 : 32|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_DISABLED : 36|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_ENABLED : 34|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X003 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X004 : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X002 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X00 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CRUISE_SET_SPEED : 15|8@0+ (1,0) [0|63] "mph" XXX
|
||||
SG_ CRUISE_SPEED_CHANGE : 55|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 307 POWER_SEATS: 8 XXX
|
||||
SG_ DRIVER_FORWARD : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DRIVER_BACK : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 109 GEAR_LEVER: 8 XXX
|
||||
SG_ PARK_BUTTON : 12|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEUTRAL_UP : 9|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEUTRAL_DOWN : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DRIVE : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ REVERSE : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 23|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ CHECKSUM : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 115 GEAR_PACKET: 8 XXX
|
||||
SG_ GEAR : 0|4@1+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 581 IGNITION: 8 XXX
|
||||
|
||||
BO_ 515 WHEEL_SPEEDS: 8 XXX
|
||||
SG_ WHEEL_MOVING_FR : 22|1@1+ (1,0) [0|15] "" XXX
|
||||
SG_ WHEEL_MOVING_RL : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WHEEL_MOVING_FL : 6|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WHEEL_MOVING_RR : 54|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ WHEEL_SPEED_FL : 2|11@0+ (0.0375,0) [0|255] "mph" XXX
|
||||
SG_ WHEEL_SPEED_FR : 18|11@0+ (0.0375,0) [0|255] "mph" XXX
|
||||
SG_ WHEEL_SPEED_RL : 34|11@0+ (0.0375,0) [0|255] "mph" XXX
|
||||
SG_ WHEEL_SPEED_RR : 50|11@0+ (0.0375,0) [0|255] "mph" XXX
|
||||
|
||||
|
||||
|
||||
|
||||
CM_ SG_ 5 BRAKE_PRESSED "appears to be boolean (brake pressed)";
|
||||
CM_ SG_ 69 MORE_STEERING_WHEELS_BUTTONS "back, ok, voice assistance, and mute buttons";
|
||||
CM_ SG_ 3 STEER_DIRECTION "0 = left, 1 = right";
|
||||
CM_ SG_ 5 BRAKE_POSITION "computer and driver";
|
||||
CM_ SG_ 5 BRAKE_PRESSED "computer and driver";
|
||||
CM_ SG_ 261 GAS_PEDAL "user gas input";
|
||||
CM_ SG_ 261 COMBINED_GAS "computer and driver gas";
|
||||
CM_ SG_ 257 CRUISE_ACCELERATING "";
|
||||
+1
-1
@@ -1 +1 @@
|
||||
v1.1.4
|
||||
v1.1.5
|
||||
@@ -8,7 +8,7 @@
|
||||
// brake rising edge
|
||||
// brake > 0mph
|
||||
|
||||
const int GM_MAX_STEER = 255;
|
||||
const int GM_MAX_STEER = 300;
|
||||
const int GM_MAX_RT_DELTA = 128; // max delta torque allowed for real time checks
|
||||
const int32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int GM_MAX_RATE_UP = 7;
|
||||
|
||||
@@ -5,7 +5,7 @@ import libpandasafety_py
|
||||
|
||||
MAX_RATE_UP = 7
|
||||
MAX_RATE_DOWN = 17
|
||||
MAX_STEER = 255
|
||||
MAX_STEER = 300
|
||||
MAX_BRAKE = 350
|
||||
MAX_GAS = 3072
|
||||
MAX_REGEN = 1404
|
||||
|
||||
@@ -13,8 +13,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||
-Werror=return-type \
|
||||
-Werror=format-extra-args
|
||||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -I../../ -O2 $(WARN_FLAGS)
|
||||
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
||||
|
||||
@@ -4,14 +4,14 @@ from selfdrive.config import Conversions as CV
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
from selfdrive.car.gm.values import CAR, DBC
|
||||
from selfdrive.car.gm.values import CAR, DBC, AccState
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
|
||||
class CarControllerParams():
|
||||
def __init__(self, car_fingerprint):
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
self.STEER_MAX = 255
|
||||
self.STEER_MAX = 300
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
|
||||
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
|
||||
@@ -29,11 +29,11 @@ class CarControllerParams():
|
||||
self.ADAS_KEEPALIVE_STEP = 10
|
||||
# pedal lookups, only for Volt
|
||||
MAX_GAS = 3072 # Only a safety limit
|
||||
ZERO_GAS = 2048
|
||||
self.ZERO_GAS = 2048
|
||||
MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen
|
||||
self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle
|
||||
self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, MAX_GAS]
|
||||
self.BRAKE_LOOKUP_BP = [-1., -0.25]
|
||||
self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
|
||||
|
||||
@@ -83,7 +83,6 @@ class CarController(object):
|
||||
return
|
||||
|
||||
P = self.params
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
canbus = self.canbus
|
||||
@@ -91,14 +90,11 @@ class CarController(object):
|
||||
### STEER ###
|
||||
|
||||
if (frame % P.STEER_STEP) == 0:
|
||||
final_steer = actuators.steer if enabled else 0.
|
||||
apply_steer = final_steer * P.STEER_MAX
|
||||
|
||||
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
|
||||
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3.
|
||||
|
||||
if not lkas_enabled:
|
||||
if lkas_enabled:
|
||||
apply_steer = actuators.steer * P.STEER_MAX
|
||||
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
|
||||
else:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
@@ -115,7 +111,7 @@ class CarController(object):
|
||||
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
# no output if not enabled, but keep sending keepalive messages
|
||||
# threat pedals as one
|
||||
# treat pedals as one
|
||||
final_pedal = actuators.gas - actuators.brake
|
||||
|
||||
# *** apply pedal hysteresis ***
|
||||
@@ -123,7 +119,8 @@ class CarController(object):
|
||||
final_pedal, self.pedal_steady)
|
||||
|
||||
if not enabled:
|
||||
apply_gas = P.MAX_ACC_REGEN # TODO: do we really need to send max regen when not enabled?
|
||||
# Stock ECU sends max regen when not enabled.
|
||||
apply_gas = P.MAX_ACC_REGEN
|
||||
apply_brake = 0
|
||||
else:
|
||||
apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
|
||||
@@ -133,12 +130,18 @@ class CarController(object):
|
||||
if (frame % 4) == 0:
|
||||
idx = (frame / 4) % 4
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE)
|
||||
car_stopping = apply_gas < P.ZERO_GAS
|
||||
standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
||||
at_full_stop = enabled and standstill and car_stopping
|
||||
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop))
|
||||
|
||||
at_full_stop = enabled and CS.standstill
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop))
|
||||
# Auto-resume from full stop by resetting ACC control
|
||||
acc_enabled = enabled
|
||||
if standstill and not car_stopping:
|
||||
acc_enabled = False
|
||||
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, at_full_stop))
|
||||
|
||||
# Send dashboard UI commands (ACC status), 25hz
|
||||
if (frame % 4) == 0:
|
||||
|
||||
@@ -60,17 +60,15 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
|
||||
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
|
||||
|
||||
if apply_brake == 0:
|
||||
mode = 0x1
|
||||
else:
|
||||
mode = 0x1
|
||||
if apply_brake > 0:
|
||||
mode = 0xa
|
||||
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
# TODO: this is to have GM bringing the car to complete stop,
|
||||
# but currently it conflicts with OP controls, so turned off.
|
||||
#elif near_stop:
|
||||
# mode = 0xb
|
||||
if near_stop:
|
||||
mode = 0xb
|
||||
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
|
||||
brake = (0x1000 - apply_brake) & 0xfff
|
||||
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
|
||||
|
||||
@@ -4,7 +4,7 @@ from common.realtime import sec_since_boot
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.gm.values import DBC, CAR
|
||||
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS
|
||||
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
|
||||
|
||||
try:
|
||||
@@ -28,10 +28,6 @@ class CanBus(object):
|
||||
self.chassis = 2
|
||||
self.sw_gmlan = 3
|
||||
|
||||
# 384 = "ASCMLKASteeringCmd"
|
||||
# 715 = "ASCMGasRegenCmd"
|
||||
CONTROL_MSGS = [384, 715]
|
||||
|
||||
class CarInterface(object):
|
||||
def __init__(self, CP, sendcan=None):
|
||||
self.CP = CP
|
||||
@@ -74,7 +70,7 @@ class CarInterface(object):
|
||||
# Presence of a camera on the object bus is ok.
|
||||
# Have to go passive if ASCM is online (ACC-enabled cars),
|
||||
# or camera is on powertrain bus (LKA cars without ACC).
|
||||
ret.enableCamera = not any(x for x in CONTROL_MSGS if x in fingerprint)
|
||||
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
|
||||
|
||||
std_cargo = 136
|
||||
|
||||
@@ -197,7 +193,7 @@ class CarInterface(object):
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
cruiseEnabled = self.CS.pcm_acc_status != 0
|
||||
ret.cruiseState.enabled = cruiseEnabled
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
|
||||
ret.cruiseState.standstill = False
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
@@ -280,8 +276,6 @@ class CarInterface(object):
|
||||
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gasPressed:
|
||||
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
|
||||
if ret.cruiseState.standstill:
|
||||
events.append(create_event('resumeRequired', [ET.WARNING]))
|
||||
|
||||
# handle button presses
|
||||
for b in ret.buttonEvents:
|
||||
@@ -298,7 +292,7 @@ class CarInterface(object):
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
if not self.CS.acc_active:
|
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
|
||||
|
||||
|
||||
ret.events = events
|
||||
|
||||
# update previous brake/gas pressed
|
||||
|
||||
@@ -12,6 +12,12 @@ class CruiseButtons:
|
||||
MAIN = 5
|
||||
CANCEL = 6
|
||||
|
||||
class AccState:
|
||||
OFF = 0
|
||||
ACTIVE = 1
|
||||
FAULTED = 3
|
||||
STANDSTILL = 4
|
||||
|
||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||
valid_eps_status = []
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
@@ -49,6 +55,12 @@ FINGERPRINTS = {
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
|
||||
STOCK_CONTROL_MSGS = {
|
||||
CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'),
|
||||
|
||||
@@ -17,7 +17,7 @@ def get_can_parser(CP):
|
||||
("YAW_RATE", "ESP12", 0),
|
||||
|
||||
("CF_Gway_DrvSeatBeltInd", "CGW4", 1),
|
||||
|
||||
|
||||
("CF_Gway_DrvSeatBeltSw", "CGW1", 0),
|
||||
("CF_Gway_TSigLHSw", "CGW1", 0),
|
||||
("CF_Gway_TurnSigLh", "CGW1", 0),
|
||||
|
||||
@@ -70,13 +70,18 @@ class CarInterface(object):
|
||||
tireStiffnessRear_civic = 202500
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
if candidate == CAR.SANTA_FE:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 3982 * CV.LB_TO_KG + std_cargo
|
||||
ret.wheelbase = 2.766
|
||||
ret.steerRatio = 13.8 * 1.15 # 15% higher at the center seems reasonable
|
||||
|
||||
# Values from optimizer
|
||||
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
|
||||
tire_stiffness_factor = 0.82
|
||||
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.37], [0.1]]
|
||||
ret.minSteerSpeed = 0.
|
||||
@@ -122,7 +127,6 @@ class CarInterface(object):
|
||||
ret.longitudinalKpV = [0.]
|
||||
ret.longitudinalKiBP = [0.]
|
||||
ret.longitudinalKiV = [0.]
|
||||
tire_stiffness_factor = 1.
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
|
||||
|
||||
@@ -145,6 +145,9 @@ class CarController(object):
|
||||
# only cut torque when steer state is a known fault
|
||||
if not enabled or CS.steer_state in [9, 25]:
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
apply_steer_req = 1
|
||||
|
||||
self.steer_angle_enabled, self.ipas_reset_counter = \
|
||||
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
|
||||
@@ -192,12 +195,12 @@ class CarController(object):
|
||||
# on consecutive messages
|
||||
if ECU.CAM in self.fake_ecus:
|
||||
if self.angle_control:
|
||||
can_sends.append(create_steer_command(self.packer, 0., frame))
|
||||
can_sends.append(create_steer_command(self.packer, 0., 0, frame))
|
||||
else:
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, frame))
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
|
||||
|
||||
if self.angle_control:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
|
||||
can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled,
|
||||
ECU.APGS in self.fake_ecus))
|
||||
elif ECU.APGS in self.fake_ecus:
|
||||
can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True))
|
||||
|
||||
@@ -52,11 +52,11 @@ def create_ipas_steer_command(packer, steer, enabled, apgs_enabled):
|
||||
return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values)
|
||||
|
||||
|
||||
def create_steer_command(packer, steer, raw_cnt):
|
||||
def create_steer_command(packer, steer, steer_req, raw_cnt):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
|
||||
values = {
|
||||
"STEER_REQUEST": abs(steer) > 0.001,
|
||||
"STEER_REQUEST": steer_req,
|
||||
"STEER_TORQUE_CMD": steer,
|
||||
"COUNTER": raw_cnt,
|
||||
"SET_ME_1": 1,
|
||||
|
||||
@@ -124,6 +124,10 @@ FINGERPRINTS = {
|
||||
#XLE and LE
|
||||
{
|
||||
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
#XSE and SE
|
||||
{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.CAMRYH: [
|
||||
#LE
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/un.h>
|
||||
|
||||
#include "ipc.h"
|
||||
|
||||
int ipc_connect(const char* socket_path) {
|
||||
int err;
|
||||
|
||||
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
|
||||
assert(sock >= 0);
|
||||
struct sockaddr_un addr = {
|
||||
.sun_family = AF_UNIX,
|
||||
};
|
||||
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
|
||||
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
|
||||
if (err != 0) {
|
||||
close(sock);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return sock;
|
||||
}
|
||||
|
||||
int ipc_bind(const char* socket_path) {
|
||||
int err;
|
||||
|
||||
unlink(socket_path);
|
||||
|
||||
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
|
||||
struct sockaddr_un addr = {
|
||||
.sun_family = AF_UNIX,
|
||||
};
|
||||
snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
|
||||
err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
|
||||
assert(err == 0);
|
||||
|
||||
err = listen(sock, 3);
|
||||
assert(err == 0);
|
||||
|
||||
return sock;
|
||||
}
|
||||
|
||||
|
||||
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
|
||||
int *out_num_fds) {
|
||||
int err;
|
||||
|
||||
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
|
||||
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
|
||||
|
||||
struct iovec iov = {
|
||||
.iov_base = buf,
|
||||
.iov_len = buf_size,
|
||||
};
|
||||
struct msghdr msg = {
|
||||
.msg_iov = &iov,
|
||||
.msg_iovlen = 1,
|
||||
};
|
||||
|
||||
if (num_fds > 0) {
|
||||
assert(fds);
|
||||
|
||||
msg.msg_control = control_buf;
|
||||
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
|
||||
}
|
||||
|
||||
if (send) {
|
||||
if (num_fds) {
|
||||
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
|
||||
assert(cmsg);
|
||||
cmsg->cmsg_level = SOL_SOCKET;
|
||||
cmsg->cmsg_type = SCM_RIGHTS;
|
||||
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
|
||||
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
|
||||
// printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
|
||||
}
|
||||
return sendmsg(fd, &msg, 0);
|
||||
} else {
|
||||
int r = recvmsg(fd, &msg, 0);
|
||||
if (r < 0) return r;
|
||||
|
||||
int recv_fds = 0;
|
||||
if (msg.msg_controllen > 0) {
|
||||
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
|
||||
assert(cmsg);
|
||||
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
|
||||
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
|
||||
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
|
||||
recv_fds /= sizeof(int);
|
||||
// printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
|
||||
// assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
|
||||
|
||||
assert(fds && recv_fds <= num_fds);
|
||||
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
|
||||
}
|
||||
|
||||
if (msg.msg_flags) {
|
||||
for (int i=0; i<recv_fds; i++) {
|
||||
close(fds[i]);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (fds) {
|
||||
assert(out_num_fds);
|
||||
*out_num_fds = recv_fds;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef IPC_H
|
||||
#define IPC_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
int ipc_connect(const char* socket_path);
|
||||
int ipc_bind(const char* socket_path);
|
||||
int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
|
||||
int *out_num_fds);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "selfdrive/common/params.h"
|
||||
#include "common/params.h"
|
||||
|
||||
#ifndef _GNU_SOURCE
|
||||
#define _GNU_SOURCE
|
||||
@@ -13,8 +13,8 @@
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "selfdrive/common/util.h"
|
||||
#include "selfdrive/common/utilpp.h"
|
||||
#include "common/util.h"
|
||||
#include "common/utilpp.h"
|
||||
|
||||
namespace {
|
||||
|
||||
|
||||
@@ -9,10 +9,6 @@
|
||||
#include <sstream>
|
||||
#include <fstream>
|
||||
|
||||
#ifdef __x86_64
|
||||
#include <linux/limits.h>
|
||||
#endif
|
||||
|
||||
namespace util {
|
||||
|
||||
inline bool starts_with(std::string s, std::string prefix) {
|
||||
@@ -56,13 +52,13 @@ inline std::string dir_name(std::string const & path) {
|
||||
}
|
||||
|
||||
inline std::string readlink(std::string path) {
|
||||
char buff[PATH_MAX];
|
||||
ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
|
||||
if (len != -1) {
|
||||
buff[len] = '\0';
|
||||
return std::string(buff);
|
||||
}
|
||||
return "";
|
||||
char buff[4096];
|
||||
ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
|
||||
if (len != -1) {
|
||||
buff[len] = '\0';
|
||||
return std::string(buff);
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.5.3-release"
|
||||
#define COMMA_VERSION "0.5.4-release"
|
||||
|
||||
@@ -36,14 +36,17 @@ extern "C" void compute_aligned_width_and_height(int width,
|
||||
int *aligned_h);
|
||||
#endif
|
||||
|
||||
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
|
||||
|
||||
int aligned_w = 0, aligned_h = 0;
|
||||
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
|
||||
#ifdef QCOM
|
||||
compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, &aligned_w, &aligned_h);
|
||||
compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h);
|
||||
#else
|
||||
aligned_w = width; aligned_h = height;
|
||||
*aligned_w = width; *aligned_h = height;
|
||||
#endif
|
||||
}
|
||||
|
||||
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
|
||||
int aligned_w = 0, aligned_h = 0;
|
||||
visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h);
|
||||
|
||||
int stride = aligned_w * 3;
|
||||
size_t size = aligned_w * aligned_h * 3;
|
||||
|
||||
@@ -23,6 +23,7 @@ typedef struct VisionImg {
|
||||
size_t size;
|
||||
} VisionImg;
|
||||
|
||||
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
|
||||
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
|
||||
|
||||
#ifdef QCOM
|
||||
|
||||
@@ -10,6 +10,8 @@
|
||||
#include <sys/socket.h>
|
||||
#include <sys/un.h>
|
||||
|
||||
#include "ipc.h"
|
||||
|
||||
#include "visionipc.h"
|
||||
|
||||
typedef struct VisionPacketWire {
|
||||
@@ -18,95 +20,14 @@ typedef struct VisionPacketWire {
|
||||
} VisionPacketWire;
|
||||
|
||||
int vipc_connect() {
|
||||
int err;
|
||||
|
||||
int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
|
||||
assert(sock >= 0);
|
||||
struct sockaddr_un addr = {
|
||||
.sun_family = AF_UNIX,
|
||||
.sun_path = VIPC_SOCKET_PATH,
|
||||
};
|
||||
err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
|
||||
if (err != 0) {
|
||||
close(sock);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return sock;
|
||||
return ipc_connect(VIPC_SOCKET_PATH);
|
||||
}
|
||||
|
||||
static int sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
|
||||
int *out_num_fds) {
|
||||
int err;
|
||||
|
||||
char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
|
||||
memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
|
||||
|
||||
struct iovec iov = {
|
||||
.iov_base = buf,
|
||||
.iov_len = buf_size,
|
||||
};
|
||||
struct msghdr msg = {
|
||||
.msg_iov = &iov,
|
||||
.msg_iovlen = 1,
|
||||
};
|
||||
|
||||
if (num_fds > 0) {
|
||||
assert(fds);
|
||||
|
||||
msg.msg_control = control_buf;
|
||||
msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
|
||||
}
|
||||
|
||||
if (send) {
|
||||
if (num_fds) {
|
||||
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
|
||||
assert(cmsg);
|
||||
cmsg->cmsg_level = SOL_SOCKET;
|
||||
cmsg->cmsg_type = SCM_RIGHTS;
|
||||
cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
|
||||
memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
|
||||
// printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
|
||||
}
|
||||
return sendmsg(fd, &msg, 0);
|
||||
} else {
|
||||
int r = recvmsg(fd, &msg, 0);
|
||||
if (r < 0) return r;
|
||||
|
||||
int recv_fds = 0;
|
||||
if (msg.msg_controllen > 0) {
|
||||
struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
|
||||
assert(cmsg);
|
||||
assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
|
||||
recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
|
||||
assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
|
||||
recv_fds /= sizeof(int);
|
||||
// printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
|
||||
// assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
|
||||
|
||||
assert(fds && recv_fds <= num_fds);
|
||||
memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
|
||||
}
|
||||
|
||||
if (msg.msg_flags) {
|
||||
for (int i=0; i<recv_fds; i++) {
|
||||
close(fds[i]);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (fds) {
|
||||
assert(out_num_fds);
|
||||
*out_num_fds = recv_fds;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
}
|
||||
|
||||
int vipc_recv(int fd, VisionPacket *out_p) {
|
||||
VisionPacketWire p = {0};
|
||||
VisionPacket p2 = {0};
|
||||
int ret = sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
|
||||
int ret = ipc_sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
|
||||
if (ret < 0) {
|
||||
printf("vipc_recv err: %s\n", strerror(errno));
|
||||
} else {
|
||||
@@ -124,7 +45,7 @@ int vipc_send(int fd, const VisionPacket *p2) {
|
||||
.type = p2->type,
|
||||
.d = p2->d,
|
||||
};
|
||||
return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
|
||||
return ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
|
||||
}
|
||||
|
||||
void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
|
||||
|
||||
@@ -46,8 +46,8 @@ def isEnabled(state):
|
||||
|
||||
|
||||
def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location,
|
||||
poller, cal_status, overtemp, free_space, driver_status, geofence,
|
||||
state, mismatch_counter, params):
|
||||
poller, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, geofence, state, mismatch_counter, params):
|
||||
|
||||
# *** read can and compute car states ***
|
||||
CS = CI.update(CC)
|
||||
@@ -80,6 +80,12 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
||||
# under 15% of space free no enable allowed
|
||||
free_space = td.thermal.freeSpace < 0.15
|
||||
|
||||
# at zero percent battery, OP should not be allowed
|
||||
low_battery = td.thermal.batteryPercent < 1
|
||||
|
||||
if low_battery:
|
||||
events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if overtemp:
|
||||
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
@@ -89,10 +95,11 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
||||
# *** read calibration status ***
|
||||
if cal is not None:
|
||||
cal_status = cal.liveCalibration.calStatus
|
||||
cal_perc = cal.liveCalibration.calPerc
|
||||
|
||||
if cal_status != Calibration.CALIBRATED:
|
||||
if cal_status == Calibration.UNCALIBRATED:
|
||||
events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
|
||||
else:
|
||||
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
@@ -117,7 +124,7 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
|
||||
if geofence is not None and not geofence.in_geofence:
|
||||
events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
|
||||
|
||||
return CS, events, cal_status, overtemp, free_space, mismatch_counter
|
||||
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
|
||||
|
||||
|
||||
def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence):
|
||||
@@ -224,7 +231,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
||||
|
||||
|
||||
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric):
|
||||
driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
|
||||
# Given the state, this function returns the actuators
|
||||
|
||||
# reset actuators to zero
|
||||
@@ -258,13 +265,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
||||
|
||||
# parse warnings from car specific interface
|
||||
for e in get_events(events, [ET.WARNING]):
|
||||
extra_text = ''
|
||||
extra_text = ""
|
||||
if e == "belowSteerSpeed":
|
||||
if is_metric:
|
||||
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
|
||||
else:
|
||||
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
|
||||
AM.add(e, enabled, extra_text=extra_text)
|
||||
AM.add(e, enabled, extra_text_2=extra_text)
|
||||
|
||||
# *** angle offset learning ***
|
||||
|
||||
@@ -289,10 +296,13 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
||||
|
||||
# parse permanent warnings to display constantly
|
||||
for e in get_events(events, [ET.PERMANENT]):
|
||||
AM.add(str(e) + "Permanent", enabled)
|
||||
extra_text_1, extra_text_2 = "", ""
|
||||
if e == "calibrationIncomplete":
|
||||
extra_text_1 = str(cal_perc) + "%"
|
||||
extra_text_2 = "35 kph" if is_metric else "15 mph"
|
||||
AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
|
||||
|
||||
# *** process alerts ***
|
||||
|
||||
AM.process_alerts(sec_since_boot())
|
||||
|
||||
return actuators, v_cruise_kph, driver_status, angle_offset
|
||||
@@ -475,8 +485,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
||||
v_cruise_kph_last = 0
|
||||
overtemp = False
|
||||
free_space = False
|
||||
cal_status = Calibration.UNCALIBRATED
|
||||
cal_status = Calibration.INVALID
|
||||
cal_perc = 0
|
||||
mismatch_counter = 0
|
||||
low_battery = False
|
||||
|
||||
rk = Ratekeeper(rate, print_delay_threshold=2./1000)
|
||||
|
||||
@@ -497,8 +509,8 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
||||
prof.checkpoint("Ratekeeper", ignore=True)
|
||||
|
||||
# sample data and compute car events
|
||||
CS, events, cal_status, overtemp, free_space, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
|
||||
driver_monitor, gps_location, poller, cal_status, overtemp, free_space, driver_status, geofence, state, mismatch_counter, params)
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health,
|
||||
driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
|
||||
prof.checkpoint("Sample")
|
||||
|
||||
# define plan
|
||||
@@ -513,7 +525,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
||||
|
||||
# compute actuators
|
||||
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph,
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric)
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
|
||||
prof.checkpoint("State Control")
|
||||
|
||||
# publish data
|
||||
|
||||
@@ -6,11 +6,12 @@ import copy
|
||||
|
||||
# Priority
|
||||
class Priority:
|
||||
HIGHEST = 4
|
||||
HIGH = 3
|
||||
MID = 2
|
||||
LOW = 1
|
||||
LOWEST = 0
|
||||
LOW_LOWEST = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
AlertSize = log.Live100Data.AlertSize
|
||||
AlertStatus = log.Live100Data.AlertStatus
|
||||
@@ -155,7 +156,7 @@ class AlertManager(object):
|
||||
"Be ready to take over at any time",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, None, None, 0., 0., 15.),
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., 15.),
|
||||
|
||||
"ethicalDilemma": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
@@ -242,6 +243,12 @@ class AlertManager(object):
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"lowBatteryNoEntry": Alert(
|
||||
"openpilot Unavailable",
|
||||
"Low Battery",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# Cancellation alerts causing soft disabling
|
||||
"overheat": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
@@ -261,7 +268,7 @@ class AlertManager(object):
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"calibrationInProgress": Alert(
|
||||
"calibrationIncomplete": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
@@ -285,6 +292,12 @@ class AlertManager(object):
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
"lowBattery": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Low Battery",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.),
|
||||
|
||||
# Cancellation alerts causing immediate disabling
|
||||
"radarCommIssue": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
@@ -324,13 +337,13 @@ class AlertManager(object):
|
||||
|
||||
"steerUnavailable": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Steer Fault: Restart the Car",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
"brakeUnavailable": Alert(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Brake Fault: Restart the Car",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, "steerRequired", "chimeRepeated", 1., 3., 4.),
|
||||
|
||||
@@ -390,7 +403,7 @@ class AlertManager(object):
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"calibrationInProgressNoEntry": Alert(
|
||||
"calibrationIncompleteNoEntry": Alert(
|
||||
"openpilot Unavailable",
|
||||
"Calibration in Progress",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
@@ -452,13 +465,13 @@ class AlertManager(object):
|
||||
|
||||
"steerUnavailableNoEntry": Alert(
|
||||
"openpilot Unavailable",
|
||||
"Steer Fault: Restart the Car",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
"brakeUnavailableNoEntry": Alert(
|
||||
"openpilot Unavailable",
|
||||
"Brake Fault: Restart the Car",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
@@ -492,23 +505,29 @@ class AlertManager(object):
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
|
||||
|
||||
# permanent alerts to display on small UI upper box
|
||||
# permanent alerts
|
||||
"steerUnavailablePermanent": Alert(
|
||||
"STEER FAULT: Restart the car to engage",
|
||||
"LKAS Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
"brakeUnavailablePermanent": Alert(
|
||||
"BRAKE FAULT: Restart the car to engage",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
"lowSpeedLockoutPermanent": Alert(
|
||||
"CRUISE FAULT: Restart the car to engage",
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW_LOWEST, None, None, 0., 0., .2),
|
||||
|
||||
"calibrationIncompletePermanent": Alert(
|
||||
"Calibration in Progress: ",
|
||||
"Drive Above ",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, None, None, 0., 0., .2),
|
||||
}
|
||||
|
||||
@@ -518,10 +537,11 @@ class AlertManager(object):
|
||||
def alertPresent(self):
|
||||
return len(self.activealerts) > 0
|
||||
|
||||
def add(self, alert_type, enabled=True, extra_text=''):
|
||||
def add(self, alert_type, enabled=True, extra_text_1='', extra_text_2=''):
|
||||
alert_type = str(alert_type)
|
||||
added_alert = copy.copy(self.alerts[alert_type])
|
||||
added_alert.alert_text_2 += extra_text
|
||||
added_alert.alert_text_1 += extra_text_1
|
||||
added_alert.alert_text_2 += extra_text_2
|
||||
added_alert.start_time = sec_since_boot()
|
||||
|
||||
# if new alert is higher priority, log it
|
||||
|
||||
@@ -1,12 +1,14 @@
|
||||
import numpy as np
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from common.filter_simple import FirstOrderFilter
|
||||
|
||||
_DT = 0.01 # update runs at 100Hz
|
||||
_DTM = 0.1 # DM runs at 10Hz
|
||||
_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status
|
||||
_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration
|
||||
_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
|
||||
_DISTRACTED_TIME = 8.
|
||||
_DISTRACTED_TIME = 7.
|
||||
_DISTRACTED_PRE_TIME = 4.
|
||||
_DISTRACTED_PROMPT_TIME = 2.
|
||||
# measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model
|
||||
@@ -18,13 +20,10 @@ _CAMERA_X_CONV = 0.375 # 160*864/320/1152
|
||||
_PITCH_WEIGHT = 1.5 # pitch matters a lot more
|
||||
_METRIC_THRESHOLD = 0.4
|
||||
_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch
|
||||
_DTM = 0.1 # driver monitor runs at 10Hz
|
||||
_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
|
||||
_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid
|
||||
_DISTRACTED_FILTER_F = 0.6 # 0.6Hz, 0.25s ts
|
||||
_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM)
|
||||
_VARIANCE_FILTER_F = 0.008 # 0.008Hz, 20s ts
|
||||
_VARIANCE_FILTER_K = 2 * np.pi * _VARIANCE_FILTER_F * _DTM / (1 + 2 * np.pi * _VARIANCE_FILTER_F * _DTM)
|
||||
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
_VARIANCE_FILTER_TS = 20. # 0.008Hz
|
||||
|
||||
|
||||
class _DriverPose():
|
||||
@@ -47,15 +46,15 @@ class DriverStatus():
|
||||
self.monitor_valid = True # variance needs to be low
|
||||
self.awareness = 1.
|
||||
self.driver_distracted = False
|
||||
self.driver_distraction_level = 0.
|
||||
self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, _DTM)
|
||||
self.variance_high = False
|
||||
self.variance_level = 0.
|
||||
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, _DTM)
|
||||
self.ts_last_check = 0.
|
||||
self._set_timers()
|
||||
|
||||
def _reset_filters(self):
|
||||
self.driver_distraction_level = 0.
|
||||
self.variance_level = 0.
|
||||
self.driver_distraction_filter.x = 0.
|
||||
self.variance_filter.x = 0.
|
||||
self.monitor_valid = True
|
||||
|
||||
def _set_timers(self):
|
||||
@@ -90,11 +89,9 @@ class DriverStatus():
|
||||
self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down
|
||||
self.driver_distracted = self._is_driver_distracted(self.pose)
|
||||
# first order filters
|
||||
self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \
|
||||
_DISTRACTED_FILTER_K * self.driver_distracted
|
||||
self.driver_distraction_filter.update(self.driver_distracted)
|
||||
self.variance_high = driver_monitoring.std > _STD_THRESHOLD
|
||||
self.variance_level = (1. - _VARIANCE_FILTER_K) * self.variance_level + \
|
||||
_VARIANCE_FILTER_K * self.variance_high
|
||||
self.variance_filter.update(self.variance_high)
|
||||
|
||||
monitor_param_on_prev = self.monitor_param_on
|
||||
monitor_valid_prev = self.monitor_valid
|
||||
@@ -105,7 +102,7 @@ class DriverStatus():
|
||||
self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
|
||||
self.ts_last_check = ts
|
||||
|
||||
self.monitor_valid = _monitor_hysteresys(self.variance_level, monitor_valid_prev)
|
||||
self.monitor_valid = _monitor_hysteresys(self.variance_filter.x, monitor_valid_prev)
|
||||
self.monitor_on = self.monitor_valid and self.monitor_param_on
|
||||
if monitor_param_on_prev != self.monitor_param_on:
|
||||
self._reset_filters()
|
||||
@@ -114,13 +111,13 @@ class DriverStatus():
|
||||
|
||||
def update(self, events, driver_engaged, ctrl_active, standstill):
|
||||
|
||||
driver_engaged |= (self.driver_distraction_level < 0.37 and self.monitor_on)
|
||||
driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
|
||||
|
||||
if (driver_engaged and self.awareness > 0.) or not ctrl_active:
|
||||
# always reset if driver is in control (unless we are in red alert state) or op isn't active
|
||||
self.awareness = 1.
|
||||
|
||||
if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \
|
||||
if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted)) and \
|
||||
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
|
||||
self.awareness = max(self.awareness - self.step_change, -0.1)
|
||||
|
||||
@@ -142,11 +139,11 @@ class DriverStatus():
|
||||
|
||||
if __name__ == "__main__":
|
||||
ds = DriverStatus(True)
|
||||
ds.driver_distraction_level = 1.
|
||||
ds.driver_distraction_filter.x = 0.
|
||||
ds.driver_distracted = 1
|
||||
for i in range(1000):
|
||||
ds.update([], False, True, True)
|
||||
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
|
||||
for i in range(10):
|
||||
ds.update([], False, True, False)
|
||||
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
|
||||
ds.update([], True, True, False)
|
||||
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_level)
|
||||
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
|
||||
|
||||
|
||||
@@ -20,58 +20,49 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca
|
||||
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
|
||||
lib_qp/Bounds.o \
|
||||
lib_qp/Constraints.o \
|
||||
lib_qp/CyclingManager.o \
|
||||
lib_qp/Indexlist.o \
|
||||
lib_qp/MessageHandling.o \
|
||||
lib_qp/QProblem.o \
|
||||
lib_qp/QProblemB.o \
|
||||
lib_qp/SubjectTo.o \
|
||||
lib_qp/Utils.o \
|
||||
lib_qp/EXTRAS/SolutionAnalysis.o \
|
||||
lib_mpc_export/acado_qpoases_interface.o \
|
||||
lib_mpc_export/acado_integrator.o \
|
||||
lib_mpc_export/acado_solver.o \
|
||||
lib_mpc_export/acado_auxiliary_functions.o \
|
||||
lateral_mpc.o
|
||||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
.PHONY: all
|
||||
all: libcommampc.so
|
||||
all: libmpc.so
|
||||
|
||||
libcommampc.so: $(OBJS)
|
||||
libmpc.so: $(OBJS)
|
||||
$(CXX) -shared -o '$@' $^ -lm
|
||||
|
||||
|
||||
qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||
lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||
@echo "[ CXX ] $@"
|
||||
mkdir -p qp
|
||||
mkdir -p lib_qp/EXTRAS
|
||||
$(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/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.cpp
|
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.c
|
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
@@ -88,6 +79,6 @@ generate: generator
|
||||
|
||||
.PHONY: clean
|
||||
clean:
|
||||
rm -f libcommampc.so generator $(OBJS) $(DEPS)
|
||||
rm -f *.so generator $(OBJS) $(DEPS)
|
||||
|
||||
-include $(DEPS)
|
||||
|
||||
@@ -141,7 +141,7 @@ int main( )
|
||||
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
|
||||
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
|
||||
|
||||
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
|
||||
if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
|
||||
exit( EXIT_FAILURE );
|
||||
|
||||
mpc.printDimensionsQP( );
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
lateral_mpc.o: lateral_mpc.c lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
lib_mpc_export/acado_auxiliary_functions.h
|
||||
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
lib_mpc_export/acado_auxiliary_functions.o: \
|
||||
lib_mpc_export/acado_auxiliary_functions.c \
|
||||
lib_mpc_export/acado_auxiliary_functions.h \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,3 @@
|
||||
lib_mpc_export/acado_integrator.o: lib_mpc_export/acado_integrator.c \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,24 @@
|
||||
lib_mpc_export/acado_qpoases_interface.o: \
|
||||
lib_mpc_export/acado_qpoases_interface.cpp \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblem.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,3 @@
|
||||
lib_mpc_export/acado_solver.o: lib_mpc_export/acado_solver.c \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,14 @@
|
||||
lib_qp/Bounds.o: ../../../../phonelibs/qpoases/SRC/Bounds.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,14 @@
|
||||
lib_qp/Constraints.o: ../../../../phonelibs/qpoases/SRC/Constraints.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,11 @@
|
||||
lib_qp/CyclingManager.o: \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,24 @@
|
||||
lib_qp/EXTRAS/SolutionAnalysis.o: \
|
||||
../../../../phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblem.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,10 @@
|
||||
lib_qp/Indexlist.o: ../../../../phonelibs/qpoases/SRC/Indexlist.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,9 @@
|
||||
lib_qp/MessageHandling.o: \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,22 @@
|
||||
lib_qp/QProblem.o: ../../../../phonelibs/qpoases/SRC/QProblem.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblem.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,16 @@
|
||||
lib_qp/QProblemB.o: ../../../../phonelibs/qpoases/SRC/QProblemB.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,12 @@
|
||||
lib_qp/SubjectTo.o: ../../../../phonelibs/qpoases/SRC/SubjectTo.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,8 @@
|
||||
lib_qp/Utils.o: ../../../../phonelibs/qpoases/SRC/Utils.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp
|
||||
Binary file not shown.
Executable
BIN
Binary file not shown.
@@ -4,7 +4,7 @@ import subprocess
|
||||
from cffi import FFI
|
||||
|
||||
mpc_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libmpc_fn = os.path.join(mpc_dir, "libcommampc.so")
|
||||
libmpc_fn = os.path.join(mpc_dir, "libmpc.so")
|
||||
subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
|
||||
|
||||
ffi = FFI()
|
||||
|
||||
@@ -19,60 +19,52 @@ ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_ca
|
||||
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
|
||||
lib_qp/Bounds.o \
|
||||
lib_qp/Constraints.o \
|
||||
lib_qp/CyclingManager.o \
|
||||
lib_qp/Indexlist.o \
|
||||
lib_qp/MessageHandling.o \
|
||||
lib_qp/QProblem.o \
|
||||
lib_qp/QProblemB.o \
|
||||
lib_qp/SubjectTo.o \
|
||||
lib_qp/Utils.o \
|
||||
lib_qp/EXTRAS/SolutionAnalysis.o \
|
||||
lib_mpc_export/acado_qpoases_interface.o \
|
||||
lib_mpc_export/acado_integrator.o \
|
||||
lib_mpc_export/acado_solver.o \
|
||||
lib_mpc_export/acado_auxiliary_functions.o \
|
||||
longitudinal_mpc.o
|
||||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
.PHONY: all
|
||||
all: libcommampc1.so libcommampc2.so
|
||||
all: libmpc1.so libmpc2.so
|
||||
|
||||
libcommampc1.so: $(OBJS)
|
||||
libmpc1.so: $(OBJS)
|
||||
$(CXX) -shared -o '$@' $^ -lm
|
||||
|
||||
libcommampc2.so: libcommampc1.so
|
||||
cp libcommampc1.so libcommampc2.so
|
||||
libmpc2.so: libmpc1.so
|
||||
cp libmpc1.so libmpc2.so
|
||||
|
||||
qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||
lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||
@echo "[ CXX ] $@"
|
||||
mkdir -p qp
|
||||
mkdir -p lib_qp/EXTRAS
|
||||
$(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/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.cpp
|
||||
@echo "[ CXX ] $@"
|
||||
$(CXX) $(CXXFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
%.o: %.c
|
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I mpc_export/ \
|
||||
-I lib_mpc_export/ \
|
||||
$(QPOASES_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
@@ -89,6 +81,6 @@ generate: generator
|
||||
|
||||
.PHONY: clean
|
||||
clean:
|
||||
rm -f libcommampc1.so libcommampc2.so generator $(OBJS) $(DEPS)
|
||||
rm -f *.so generator $(OBJS) $(DEPS)
|
||||
|
||||
-include $(DEPS)
|
||||
|
||||
@@ -18,15 +18,18 @@ int main( )
|
||||
DifferentialEquation f;
|
||||
|
||||
DifferentialState x_ego, v_ego, a_ego;
|
||||
DifferentialState x_l, v_l, a_l;
|
||||
DifferentialState x_l, v_l, t;
|
||||
|
||||
OnlineData lambda;
|
||||
OnlineData lambda, a_l_0;
|
||||
|
||||
Control j_ego;
|
||||
|
||||
auto desired = 4.0 + RW(v_ego, v_l);
|
||||
auto d_l = x_l - x_ego;
|
||||
|
||||
// Directly calculate a_l to prevent instabilites due to discretization
|
||||
auto a_l = a_l_0 * exp(-lambda * t * t / 2);
|
||||
|
||||
// Equations of motion
|
||||
f << dot(x_ego) == v_ego;
|
||||
f << dot(v_ego) == a_ego;
|
||||
@@ -34,7 +37,7 @@ int main( )
|
||||
|
||||
f << dot(x_l) == v_l;
|
||||
f << dot(v_l) == a_l;
|
||||
f << dot(a_l) == -lambda * a_l;
|
||||
f << dot(t) == 1;
|
||||
|
||||
// Running cost
|
||||
Function h;
|
||||
@@ -76,7 +79,7 @@ int main( )
|
||||
ocp.minimizeLSQEndTerm(QN, hN);
|
||||
|
||||
ocp.subjectTo( 0.0 <= v_ego);
|
||||
ocp.setNOD(1);
|
||||
ocp.setNOD(2);
|
||||
|
||||
OCPexport mpc(ocp);
|
||||
mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
|
||||
@@ -94,7 +97,7 @@ int main( )
|
||||
mpc.set( GENERATE_MATLAB_INTERFACE, NO );
|
||||
mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
|
||||
|
||||
if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
|
||||
if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
|
||||
exit( EXIT_FAILURE );
|
||||
|
||||
mpc.printDimensionsQP( );
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
lib_mpc_export/acado_auxiliary_functions.o: \
|
||||
lib_mpc_export/acado_auxiliary_functions.c \
|
||||
lib_mpc_export/acado_auxiliary_functions.h \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
+12
-9
@@ -64,7 +64,7 @@ extern "C"
|
||||
/** Number of control/estimation intervals. */
|
||||
#define ACADO_N 20
|
||||
/** Number of online data values. */
|
||||
#define ACADO_NOD 1
|
||||
#define ACADO_NOD 2
|
||||
/** Number of path constraints. */
|
||||
#define ACADO_NPAC 0
|
||||
/** Number of control variables. */
|
||||
@@ -114,11 +114,11 @@ real_t x[ 126 ];
|
||||
*/
|
||||
real_t u[ 20 ];
|
||||
|
||||
/** Column vector of size: 21
|
||||
/** Matrix of size: 21 x 2 (row major format)
|
||||
*
|
||||
* Matrix containing 21 online data vectors.
|
||||
*/
|
||||
real_t od[ 21 ];
|
||||
real_t od[ 42 ];
|
||||
|
||||
/** Column vector of size: 80
|
||||
*
|
||||
@@ -155,16 +155,19 @@ real_t x0[ 6 ];
|
||||
*/
|
||||
typedef struct ACADOworkspace_
|
||||
{
|
||||
/** Column vector of size: 10 */
|
||||
real_t rhs_aux[ 10 ];
|
||||
|
||||
real_t rk_ttt;
|
||||
|
||||
/** Row vector of size: 50 */
|
||||
real_t rk_xxx[ 50 ];
|
||||
/** Row vector of size: 51 */
|
||||
real_t rk_xxx[ 51 ];
|
||||
|
||||
/** Matrix of size: 4 x 48 (row major format) */
|
||||
real_t rk_kkk[ 192 ];
|
||||
|
||||
/** Row vector of size: 50 */
|
||||
real_t state[ 50 ];
|
||||
/** Row vector of size: 51 */
|
||||
real_t state[ 51 ];
|
||||
|
||||
/** Column vector of size: 120 */
|
||||
real_t d[ 120 ];
|
||||
@@ -184,8 +187,8 @@ real_t evGu[ 120 ];
|
||||
/** Column vector of size: 30 */
|
||||
real_t objAuxVar[ 30 ];
|
||||
|
||||
/** Row vector of size: 8 */
|
||||
real_t objValueIn[ 8 ];
|
||||
/** Row vector of size: 9 */
|
||||
real_t objValueIn[ 9 ];
|
||||
|
||||
/** Row vector of size: 32 */
|
||||
real_t objValueOut[ 32 ];
|
||||
+31
-16
@@ -25,14 +25,28 @@ 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;
|
||||
/* Vector of auxiliary variables; number of elements: 10. */
|
||||
real_t* a = acadoWorkspace.rhs_aux;
|
||||
|
||||
/* Compute intermediate quantities: */
|
||||
a[0] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
|
||||
a[1] = ((real_t)(1.0000000000000000e+00)/(real_t)(2.0000000000000000e+00));
|
||||
a[2] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
|
||||
a[3] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[36])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[36]))*a[1])*a[2]);
|
||||
a[4] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[37])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[37]))*a[1])*a[2]);
|
||||
a[5] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[38])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[38]))*a[1])*a[2]);
|
||||
a[6] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[39])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[39]))*a[1])*a[2]);
|
||||
a[7] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[40])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[40]))*a[1])*a[2]);
|
||||
a[8] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[41])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[41]))*a[1])*a[2]);
|
||||
a[9] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[47])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[47]))*a[1])*a[2]);
|
||||
|
||||
/* 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[4] = (od[1]*a[0]);
|
||||
out[5] = (real_t)(1.0000000000000000e+00);
|
||||
out[6] = xd[12];
|
||||
out[7] = xd[13];
|
||||
out[8] = xd[14];
|
||||
@@ -57,24 +71,24 @@ 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[30] = (od[1]*a[3]);
|
||||
out[31] = (od[1]*a[4]);
|
||||
out[32] = (od[1]*a[5]);
|
||||
out[33] = (od[1]*a[6]);
|
||||
out[34] = (od[1]*a[7]);
|
||||
out[35] = (od[1]*a[8]);
|
||||
out[36] = (real_t)(0.0000000000000000e+00);
|
||||
out[37] = (real_t)(0.0000000000000000e+00);
|
||||
out[38] = (real_t)(0.0000000000000000e+00);
|
||||
out[39] = (real_t)(0.0000000000000000e+00);
|
||||
out[40] = (real_t)(0.0000000000000000e+00);
|
||||
out[41] = (real_t)(0.0000000000000000e+00);
|
||||
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]);
|
||||
out[46] = (od[1]*a[9]);
|
||||
out[47] = (real_t)(0.0000000000000000e+00);
|
||||
}
|
||||
|
||||
/* Fixed step size:0.2 */
|
||||
@@ -130,6 +144,7 @@ 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];
|
||||
acadoWorkspace.rk_xxx[50] = rk_eta[50];
|
||||
|
||||
for (run1 = 0; run1 < 1; ++run1)
|
||||
{
|
||||
@@ -0,0 +1,3 @@
|
||||
lib_mpc_export/acado_integrator.o: lib_mpc_export/acado_integrator.c \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,24 @@
|
||||
lib_mpc_export/acado_qpoases_interface.o: \
|
||||
lib_mpc_export/acado_qpoases_interface.cpp \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblem.ipp
|
||||
Binary file not shown.
+14
-7
@@ -45,7 +45,8 @@ acadoWorkspace.state[4] = acadoVariables.x[lRun1 * 6 + 4];
|
||||
acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5];
|
||||
|
||||
acadoWorkspace.state[48] = acadoVariables.u[lRun1];
|
||||
acadoWorkspace.state[49] = acadoVariables.od[lRun1];
|
||||
acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 2];
|
||||
acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 2 + 1];
|
||||
|
||||
ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
|
||||
|
||||
@@ -382,7 +383,8 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 6 + 3];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.x[runObj * 6 + 4];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.x[runObj * 6 + 5];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2];
|
||||
acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1];
|
||||
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
|
||||
@@ -401,7 +403,8 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
|
||||
acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.od[20];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
|
||||
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
|
||||
@@ -5202,7 +5205,8 @@ acadoWorkspace.state[3] = acadoVariables.x[index * 6 + 3];
|
||||
acadoWorkspace.state[4] = acadoVariables.x[index * 6 + 4];
|
||||
acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5];
|
||||
acadoWorkspace.state[48] = acadoVariables.u[index];
|
||||
acadoWorkspace.state[49] = acadoVariables.od[index];
|
||||
acadoWorkspace.state[49] = acadoVariables.od[index * 2];
|
||||
acadoWorkspace.state[50] = acadoVariables.od[index * 2 + 1];
|
||||
|
||||
acado_integrate(acadoWorkspace.state, index == 0, index);
|
||||
|
||||
@@ -5253,7 +5257,8 @@ else
|
||||
{
|
||||
acadoWorkspace.state[48] = acadoVariables.u[19];
|
||||
}
|
||||
acadoWorkspace.state[49] = acadoVariables.od[20];
|
||||
acadoWorkspace.state[49] = acadoVariables.od[40];
|
||||
acadoWorkspace.state[50] = acadoVariables.od[41];
|
||||
|
||||
acado_integrate(acadoWorkspace.state, 1, 19);
|
||||
|
||||
@@ -5328,7 +5333,8 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 6 + 3];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.x[lRun1 * 6 + 4];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.x[lRun1 * 6 + 5];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2];
|
||||
acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1];
|
||||
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4];
|
||||
@@ -5342,7 +5348,8 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
|
||||
acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.od[20];
|
||||
acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
|
||||
acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
|
||||
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
|
||||
@@ -0,0 +1,3 @@
|
||||
lib_mpc_export/acado_solver.o: lib_mpc_export/acado_solver.c \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,14 @@
|
||||
lib_qp/Bounds.o: ../../../../phonelibs/qpoases/SRC/Bounds.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp
|
||||
Binary file not shown.
@@ -0,0 +1,14 @@
|
||||
lib_qp/Constraints.o: ../../../../phonelibs/qpoases/SRC/Constraints.cpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp
|
||||
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user