mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-18 21:03:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.4.3.1 release

Browse Source
This commit is contained in:
Vehicle Researcher
2018-03-19 23:40:24 -07:00
parent 51fad4a6c6
commit 3d628a6fe2
27 changed files with 201 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
RELEASES.md
View File

@@ -1,3 +1,9 @@
Version 0.4.3.1 (2018-03-19)
============================
* Improve autofocus
* Add check for MPC solution error
* Make first distracted warning visual only
Version 0.4.3 (2018-03-13)
==========================
* Add HDR and autofocus

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

Binary file not shown.

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

Binary file not shown.

2
cereal/Makefile
View File

@@ -1,6 +1,6 @@
PWD := $(shell pwd)
SRCS := log.capnp car.capnp
SRCS := log.capnp car.capnp map.capnp
GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++

1
cereal/car.capnp
View File

@@ -54,6 +54,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
parkBrake @29;
manualRestart @30;
lowSpeedLockout @31;
plannerError @32;
}
}

24
cereal/log.capnp
View File

@@ -119,6 +119,8 @@ struct FrameData {
globalGain @5 :Int32;
lensPos @11 :Int32;
lensSag @12 :Float32;
lensErr @13 :Float32;
lensTruePos @14 :Float32;
image @6 :Data;
frameType @7 :FrameType;
@@ -1325,6 +1327,7 @@ struct LiveMpcData {
delta @3 :List(Float32);
qpIterations @4 :UInt32;
calculationTime @5 :UInt64;
cost @6 :Float64;
}
struct LiveLongitudinalMpcData {
@@ -1338,18 +1341,27 @@ struct LiveLongitudinalMpcData {
qpIterations @7 :UInt32;
mpcId @8 :UInt32;
calculationTime @9 :UInt64;
cost @10 :Float64;
}
struct ECEFPoint {
struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 {
x @0 :Float32;
y @1 :Float32;
z @2 :Float32;
}
struct ECEFPoint @0xc25bbbd524983447 {
x @0 :Float64;
y @1 :Float64;
z @2 :Float64;
}
struct GPSPlannerPoints {
curPos @0 :ECEFPoint;
points @1 :List(ECEFPoint);
curPosDEPRECATED @0 :ECEFPointDEPRECATED;
pointsDEPRECATED @1 :List(ECEFPointDEPRECATED);
curPos @6 :ECEFPoint;
points @7 :List(ECEFPoint);
valid @2 :Bool;
trackName @3 :Text;
speedLimit @4 :Float32;
@@ -1362,7 +1374,8 @@ struct GPSPlannerPlan {
trackName @2 :Text;
speed @3 :Float32;
acceleration @4 :Float32;
points @5 :List(ECEFPoint);
pointsDEPRECATED @5 :List(ECEFPointDEPRECATED);
points @6 :List(ECEFPoint);
}
struct TrafficEvent @0xacfa74a094e62626 {
@@ -1408,7 +1421,8 @@ struct UiNavigationEvent {
type @0: Type;
status @1: Status;
distanceTo @2: Float32;
endRoadPoint @3: ECEFPoint;
endRoadPointDEPRECATED @3: ECEFPointDEPRECATED;
endRoadPoint @4: ECEFPoint;
enum Type {
none @0;

2
common/transformations/coordinates.py
View File

@@ -89,9 +89,11 @@ class LocalCoord(object):
def ecef2ned(self, ecef):
ecef = np.array(ecef)
return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T
def ned2ecef(self, ned):
ned = np.array(ned)
# Transpose so that init_ecef will broadcast correctly for 1d or 2d ned.
return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef)

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

@@ -76,7 +76,6 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
"""Creates an iterable of CAN messages for the UIs."""
commands = []
# TODO: Why is X4 always 0xc1? Not implemented yet in canpacker
acc_hud_values = {
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
'PCM_GAS': hud.pcm_accel,

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

@@ -186,7 +186,7 @@ class CarInterface(object):
ret.steerRatio = 15.3
# Acura at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310']
ret.steerKpV, ret.steerKiV = [[0.1], [0.03]] if is_fw_modified else [[0.8], [0.24]]
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
@@ -449,7 +449,7 @@ class CarInterface(object):
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
else:
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
if self.CS.CP.carFingerprint != CAR.CIVIC and ret.vEgo < 0.001:
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.append(create_event('manualRestart', [ET.WARNING]))
cur_time = sec_since_boot()

2
selfdrive/common/version.h
View File

@@ -1 +1 @@
#define COMMA_VERSION "0.4.3-release"
#define COMMA_VERSION "0.4.3.1-release"

9
selfdrive/controls/controlsd.py
View File

@@ -104,9 +104,9 @@ def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overte
return CS, events, cal_status, overtemp, free_space
def calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status):
def calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status):
# plan runs always, independently of the state
plan_packet = PL.update(CS, LoC, v_cruise_kph, awareness_status < -0.)
plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, awareness_status < -0.)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
@@ -401,6 +401,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
cs_send.init('carState')
# TODO: override CS.events with all the cumulated events
cs_send.carState = copy(CS)
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# broadcast carControl
@@ -408,7 +409,6 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
cc_send.init('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
#print [i.name for i in events]
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
@@ -418,6 +418,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
dat.liveMpc.cost = LaC.mpc_solution[0].cost
livempc.send(dat.to_bytes())
return CC
@@ -517,7 +518,7 @@ def controlsd_thread(gctx, rate=100):
prof.checkpoint("Sample")
# define plan
plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status)
plan, plan_ts = calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status)
prof.checkpoint("Plan")
if not passive:

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

@@ -92,7 +92,7 @@ class AlertManager(object):
"TAKE CONTROL",
"User appears distracted",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1),
Priority.LOW, "steerRequired", None, 0., .1, .1),
"driverDistracted": Alert(
"TAKE CONTROL TO REGAIN SPEED",
@@ -283,6 +283,12 @@ class AlertManager(object):
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
"plannerError": Alert(
"TAKE CONTROL IMMEDIATELY",
"Planner Solution Error",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.),
# not loud cancellations (user is in control)
"noTarget": Alert(
"Comma Canceled",
@@ -405,6 +411,12 @@ class AlertManager(object):
AlertStatus.normal, AlertSize.mid,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
"plannerErrorNoEntry": Alert(
"Comma Unavailable",
"Planner Solution Error",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, None, "chimeDouble", .4, 2., 3.),
# permanent alerts to display on small UI upper box
"steerUnavailablePermanent": Alert(
"STEER FAULT: Restart the car to engage",

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

@@ -39,6 +39,7 @@ class LatControl(object):
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.mpc_nans = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
@@ -56,6 +57,7 @@ class LatControl(object):
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
@@ -87,9 +89,9 @@ class LatControl(object):
self.mpc_updated = True
# Check for infeasable MPC solution
nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if nans:
if self.mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio

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

@@ -115,9 +115,10 @@ int main( )
ocp.minimizeLSQ(Q, h);
ocp.minimizeLSQEndTerm(QN, hN);
// car can't go backward to avoid "circles"
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
ocp.subjectTo( -0.1 <= t <= 0.1);
// more than absolute max steer angle
ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
ocp.setNOD(18);
OCPexport mpc(ocp);

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

@@ -14,10 +14,11 @@ typedef struct {
} state_t;
typedef struct {
double x[20];
double y[20];
double psi[20];
double delta[20];
double x[21];
double y[21];
double psi[21];
double delta[21];
double cost;
} log_t;
void init(double pathCost, double laneCost, double headingCost, double steerRateCost);

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

@@ -22,10 +22,11 @@ typedef struct {
typedef struct {
double x[N];
double y[N];
double psi[N];
double delta[N];
double x[N+1];
double y[N+1];
double psi[N+1];
double delta[N+1];
double cost;
} log_t;
void init(double pathCost, double laneCost, double headingCost, double steerRateCost){
@@ -102,18 +103,22 @@ int run_mpc(state_t * x0, log_t * solution,
acado_preparationStep();
acado_feedbackStep();
/* printf("lat its: %d\n", acado_getNWSR()); */
/* printf("lat its: %d\n", acado_getNWSR()); // n iterations
printf("Objective: %.6f\n", acado_getObjective()); // solution cost */
for (i = 0; i <= N; i++){
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
acado_shiftStates(2, 0, 0);
acado_shiftControls( 0 );
for (i = 0; i <= N; i++){
solution->x[i] = acadoVariables.x[i*NX];
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
solution->cost = acado_getObjective();
// Dont shift states here. Current solution is closer to next timestep than if
// we use the old solution as a starting point
//acado_shiftStates(2, 0, 0);
//acado_shiftControls( 0 );
return acado_getNWSR();
}

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

@@ -3948,46 +3948,46 @@ acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 21 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 22 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.g[ 22 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.g[ 23 ]) );
acadoWorkspace.lb[4] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[0];
acadoWorkspace.lb[5] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[1];
acadoWorkspace.lb[6] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[2];
acadoWorkspace.lb[7] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[3];
acadoWorkspace.lb[8] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[4];
acadoWorkspace.lb[9] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[5];
acadoWorkspace.lb[10] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[6];
acadoWorkspace.lb[11] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[7];
acadoWorkspace.lb[12] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[8];
acadoWorkspace.lb[13] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[9];
acadoWorkspace.lb[14] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[10];
acadoWorkspace.lb[15] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[11];
acadoWorkspace.lb[16] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[12];
acadoWorkspace.lb[17] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[13];
acadoWorkspace.lb[18] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[14];
acadoWorkspace.lb[19] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[15];
acadoWorkspace.lb[20] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[16];
acadoWorkspace.lb[21] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[17];
acadoWorkspace.lb[22] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[18];
acadoWorkspace.lb[23] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[19];
acadoWorkspace.ub[4] = (real_t)1.0000000000000001e-01 - acadoVariables.u[0];
acadoWorkspace.ub[5] = (real_t)1.0000000000000001e-01 - acadoVariables.u[1];
acadoWorkspace.ub[6] = (real_t)1.0000000000000001e-01 - acadoVariables.u[2];
acadoWorkspace.ub[7] = (real_t)1.0000000000000001e-01 - acadoVariables.u[3];
acadoWorkspace.ub[8] = (real_t)1.0000000000000001e-01 - acadoVariables.u[4];
acadoWorkspace.ub[9] = (real_t)1.0000000000000001e-01 - acadoVariables.u[5];
acadoWorkspace.ub[10] = (real_t)1.0000000000000001e-01 - acadoVariables.u[6];
acadoWorkspace.ub[11] = (real_t)1.0000000000000001e-01 - acadoVariables.u[7];
acadoWorkspace.ub[12] = (real_t)1.0000000000000001e-01 - acadoVariables.u[8];
acadoWorkspace.ub[13] = (real_t)1.0000000000000001e-01 - acadoVariables.u[9];
acadoWorkspace.ub[14] = (real_t)1.0000000000000001e-01 - acadoVariables.u[10];
acadoWorkspace.ub[15] = (real_t)1.0000000000000001e-01 - acadoVariables.u[11];
acadoWorkspace.ub[16] = (real_t)1.0000000000000001e-01 - acadoVariables.u[12];
acadoWorkspace.ub[17] = (real_t)1.0000000000000001e-01 - acadoVariables.u[13];
acadoWorkspace.ub[18] = (real_t)1.0000000000000001e-01 - acadoVariables.u[14];
acadoWorkspace.ub[19] = (real_t)1.0000000000000001e-01 - acadoVariables.u[15];
acadoWorkspace.ub[20] = (real_t)1.0000000000000001e-01 - acadoVariables.u[16];
acadoWorkspace.ub[21] = (real_t)1.0000000000000001e-01 - acadoVariables.u[17];
acadoWorkspace.ub[22] = (real_t)1.0000000000000001e-01 - acadoVariables.u[18];
acadoWorkspace.ub[23] = (real_t)1.0000000000000001e-01 - acadoVariables.u[19];
acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
for (lRun1 = 0; lRun1 < 40; ++lRun1)
{
@@ -4477,122 +4477,122 @@ tmp = acadoVariables.x[6] + acadoWorkspace.d[2];
acadoWorkspace.lbA[0] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[0] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[7] + acadoWorkspace.d[3];
acadoWorkspace.lbA[1] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[1] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[1] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[1] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[10] + acadoWorkspace.d[6];
acadoWorkspace.lbA[2] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[2] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[11] + acadoWorkspace.d[7];
acadoWorkspace.lbA[3] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[3] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[3] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[3] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[14] + acadoWorkspace.d[10];
acadoWorkspace.lbA[4] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[4] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[15] + acadoWorkspace.d[11];
acadoWorkspace.lbA[5] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[5] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[5] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[5] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[18] + acadoWorkspace.d[14];
acadoWorkspace.lbA[6] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[6] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[19] + acadoWorkspace.d[15];
acadoWorkspace.lbA[7] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[7] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[7] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[7] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[22] + acadoWorkspace.d[18];
acadoWorkspace.lbA[8] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[8] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[23] + acadoWorkspace.d[19];
acadoWorkspace.lbA[9] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[9] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[9] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[9] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[26] + acadoWorkspace.d[22];
acadoWorkspace.lbA[10] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[10] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[27] + acadoWorkspace.d[23];
acadoWorkspace.lbA[11] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[11] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[11] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[11] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[30] + acadoWorkspace.d[26];
acadoWorkspace.lbA[12] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[12] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[31] + acadoWorkspace.d[27];
acadoWorkspace.lbA[13] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[13] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[13] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[13] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[34] + acadoWorkspace.d[30];
acadoWorkspace.lbA[14] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[14] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[35] + acadoWorkspace.d[31];
acadoWorkspace.lbA[15] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[15] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[15] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[15] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[38] + acadoWorkspace.d[34];
acadoWorkspace.lbA[16] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[16] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[39] + acadoWorkspace.d[35];
acadoWorkspace.lbA[17] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[17] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[17] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[17] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[42] + acadoWorkspace.d[38];
acadoWorkspace.lbA[18] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[18] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[43] + acadoWorkspace.d[39];
acadoWorkspace.lbA[19] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[19] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[19] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[19] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[46] + acadoWorkspace.d[42];
acadoWorkspace.lbA[20] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[20] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[47] + acadoWorkspace.d[43];
acadoWorkspace.lbA[21] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[21] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[21] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[21] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[50] + acadoWorkspace.d[46];
acadoWorkspace.lbA[22] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[22] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[51] + acadoWorkspace.d[47];
acadoWorkspace.lbA[23] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[23] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[23] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[23] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[54] + acadoWorkspace.d[50];
acadoWorkspace.lbA[24] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[24] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[55] + acadoWorkspace.d[51];
acadoWorkspace.lbA[25] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[25] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[25] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[25] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[58] + acadoWorkspace.d[54];
acadoWorkspace.lbA[26] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[26] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[59] + acadoWorkspace.d[55];
acadoWorkspace.lbA[27] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[27] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[27] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[27] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[62] + acadoWorkspace.d[58];
acadoWorkspace.lbA[28] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[28] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[63] + acadoWorkspace.d[59];
acadoWorkspace.lbA[29] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[29] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[29] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[29] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[66] + acadoWorkspace.d[62];
acadoWorkspace.lbA[30] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[30] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[67] + acadoWorkspace.d[63];
acadoWorkspace.lbA[31] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[31] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[31] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[31] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[70] + acadoWorkspace.d[66];
acadoWorkspace.lbA[32] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[32] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[71] + acadoWorkspace.d[67];
acadoWorkspace.lbA[33] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[33] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[33] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[33] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[74] + acadoWorkspace.d[70];
acadoWorkspace.lbA[34] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[34] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[75] + acadoWorkspace.d[71];
acadoWorkspace.lbA[35] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[35] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[35] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[35] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[78] + acadoWorkspace.d[74];
acadoWorkspace.lbA[36] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[36] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[79] + acadoWorkspace.d[75];
acadoWorkspace.lbA[37] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[37] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[37] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[37] = (real_t)8.7266462600000005e-01 - tmp;
tmp = acadoVariables.x[82] + acadoWorkspace.d[78];
acadoWorkspace.lbA[38] = (real_t)-1.5707963268000000e+00 - tmp;
acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp;
tmp = acadoVariables.x[83] + acadoWorkspace.d[79];
acadoWorkspace.lbA[39] = (real_t)-4.3633231300000003e-01 - tmp;
acadoWorkspace.ubA[39] = (real_t)4.3633231300000003e-01 - tmp;
acadoWorkspace.lbA[39] = (real_t)-8.7266462600000005e-01 - tmp;
acadoWorkspace.ubA[39] = (real_t)8.7266462600000005e-01 - tmp;
}

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

@@ -18,13 +18,14 @@ def _get_libmpc(mpc_id):
typedef struct {
double x_ego[20];
double v_ego[20];
double a_ego[20];
double j_ego[20];
double x_l[20];
double v_l[20];
double a_l[20];
double x_ego[21];
double v_ego[21];
double a_ego[21];
double j_ego[21];
double x_l[21];
double v_l[21];
double a_l[21];
double cost;
} log_t;
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);

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

@@ -22,13 +22,14 @@ typedef struct {
typedef struct {
double x_ego[N];
double v_ego[N];
double a_ego[N];
double j_ego[N];
double x_l[N];
double v_l[N];
double a_l[N];
double x_ego[N+1];
double v_ego[N+1];
double a_ego[N+1];
double j_ego[N+1];
double x_l[N+1];
double v_l[N+1];
double a_l[N+1];
double cost;
} log_t;
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
@@ -53,7 +54,7 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
if (i > 4){
f = STEP_MULTIPLIER;
}
acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance
acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration
acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk
@@ -130,6 +131,7 @@ int run_mpc(state_t * x0, log_t * solution, double l){
solution->j_ego[i] = acadoVariables.u[i];
}
solution->cost = acado_getObjective();
// Dont shift states here. Current solution is closer to next timestep than if
// we shift by 0.2 seconds.

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

@@ -112,7 +112,7 @@ class FCWChecker(object):
def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
mpc_solution_a = list(mpc_solution[0].a_ego)
self.last_min_a = min(mpc_solution_a[1:])
self.last_min_a = min(mpc_solution_a)
self.v_lead_max = max(self.v_lead_max, v_lead)
if (fcw_lead > 0.99):
@@ -127,7 +127,7 @@ class FCWChecker(object):
self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
a_delta = min(mpc_solution_a[1:15]) - min(0.0, a_ego)
a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
fcw_allowed = all(c >= 10 for c in self.counters.values())
if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
@@ -164,6 +164,7 @@ class LongitudinalMpc(object):
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
dat.liveLongitudinalMpc.aLead = list(self.mpc_solution[0].a_l)
dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
dat.liveLongitudinalMpc.aLeadTau = self.l
dat.liveLongitudinalMpc.qpIterations = qp_iterations
dat.liveLongitudinalMpc.mpcId = self.mpc_id
@@ -235,10 +236,10 @@ class LongitudinalMpc(object):
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
# Reset if NaN or goes through lead car
dls = np.array(list(self.mpc_solution[0].x_l)[1:]) - np.array(list(self.mpc_solution[0].x_ego)[1:])
dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
crashing = min(dls) < -50.0
nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
backwards = min(list(self.mpc_solution[0].v_ego)[1:]) < -0.01
backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
if ((backwards or crashing) and self.prev_lead_status) or nans:
if t > self.last_cloudlog_t + 5.0:
@@ -336,7 +337,7 @@ class Planner(object):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
# this runs whenever we get a packet that can change the plan
def update(self, CS, LoC, v_cruise_kph, user_distracted):
def update(self, CS, LaC, LoC, v_cruise_kph, user_distracted):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
@@ -459,6 +460,8 @@ class Planner(object):
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps

BIN
selfdrive/loggerd/loggerd
View File

Binary file not shown.

7
selfdrive/manager.py
View File

@@ -466,9 +466,10 @@ def manager_thread():
# have we seen a panda?
panda_seen = panda_seen or td is not None
# start on gps if we have no connection to a panda
if not panda_seen:
should_start = should_start or passive_starter.update(started_ts, location)
# start on gps movement if we haven't seen ignition and are in passive mode
should_start = should_start or (not (ignition_seen and td) # seen ignition and panda is connected
and params.get("Passive") == "1"
and passive_starter.update(started_ts, location))
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and avail > 0.02

BIN
selfdrive/sensord/gpsd
View File

Binary file not shown.

BIN
selfdrive/sensord/sensord
View File

Binary file not shown.

9
selfdrive/ui/ui.c
View File

@@ -921,7 +921,7 @@ static void ui_draw_vision_lanes(UIState *s) {
// draw MPC only if engaged
if (scene->engaged) {
draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 19, nvgRGBA(255, 0, 0, 255));
draw_x_y(s, &scene->mpc_x[0], &scene->mpc_y[0], 20, nvgRGBA(255, 0, 0, 255));
}
}
}
@@ -1190,7 +1190,12 @@ static void ui_draw_vision_speed(UIState *s) {
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 36*2.5);
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
if (s->is_metric) {
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "kph", NULL);
} else {
nvgText(s->vg, viz_speed_x+viz_speed_w/2, 320, "mph", NULL);
}
}
static void ui_draw_vision_wheel(UIState *s) {

5
selfdrive/version.py
View File

@@ -4,10 +4,11 @@ with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "ve
version = _versionf.read().split('"')[1]
try:
if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]):
origin = subprocess.check_output(["git", "config", "--get", "remote.origin.url"])
if "-private" in origin:
upstream = "origin/master"
else:
if 'chffrplus' in version:
if 'chffrplus' in origin:
upstream = "origin/release"
else:
upstream = "origin/release2"

BIN
selfdrive/visiond/visiond
View File

Binary file not shown.