openpilot v0.4.3.1 release

old-commit-hash: 3d628a6fe2
commatwo_master
Vehicle Researcher 7 years ago
parent 85171c28fc
commit 171647c2fc
  1. 6
      RELEASES.md
  2. 4
      apk/ai.comma.plus.frame.apk
  3. 4
      apk/ai.comma.plus.offroad.apk
  4. 2
      cereal/Makefile
  5. 1
      cereal/car.capnp
  6. 24
      cereal/log.capnp
  7. 2
      common/transformations/coordinates.py
  8. 1
      selfdrive/car/honda/hondacan.py
  9. 4
      selfdrive/car/honda/interface.py
  10. 2
      selfdrive/common/version.h
  11. 9
      selfdrive/controls/controlsd.py
  12. 14
      selfdrive/controls/lib/alertmanager.py
  13. 6
      selfdrive/controls/lib/latcontrol.py
  14. 5
      selfdrive/controls/lib/lateral_mpc/generator.cpp
  15. 9
      selfdrive/controls/lib/lateral_mpc/libmpc_py.py
  16. 21
      selfdrive/controls/lib/lateral_mpc/mpc.c
  17. 2
      selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c
  18. 15
      selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
  19. 16
      selfdrive/controls/lib/longitudinal_mpc/mpc.c
  20. 13
      selfdrive/controls/lib/planner.py
  21. 2
      selfdrive/loggerd/loggerd
  22. 7
      selfdrive/manager.py
  23. 2
      selfdrive/sensord/gpsd
  24. 2
      selfdrive/sensord/sensord
  25. 7
      selfdrive/ui/ui.c
  26. 5
      selfdrive/version.py
  27. 4
      selfdrive/visiond/visiond

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:384aa635025948050a42eb8994e21452adecaa0ba4909e1aab0b2dd3c64f5b67
size 2105683
oid sha256:02ad55a9fb7472bf4ec72e3392aa3be8369417283d85e40145d7c691e620f9ef
size 2105763

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d994ff54de0a618538a9ada328dc6196604f22a3a25051fe8e477390be29d28a
size 15407210
oid sha256:cd3d8969b69bea8db3ada2d87acea3def3dab6a3cec9c8f24578425aee902343
size 15469848

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

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

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

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

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

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

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

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

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

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

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

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

@ -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,7 +103,9 @@ 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];
@ -110,10 +113,12 @@ int run_mpc(state_t * x0, log_t * solution,
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
}
solution->cost = acado_getObjective();
acado_shiftStates(2, 0, 0);
acado_shiftControls( 0 );
// 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();
}

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2fe4faa6b70f8cb9fe000412093579a5bf96890831d62c83d01deff94cdb8d19
oid sha256:b5b42642b33d01c204d8093bee36923e59f284fae90629c584e1fb6e4369cc2c
size 393169

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

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

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a5ff2875db45291ee03af11b77ad4cf42b8dd4241939261c4990c8b255b894b7
oid sha256:b22914500af41c3602576ed9b953b6eb974bf17e5febea6f5dfbf637d266f0fc
size 1412368

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:01f3447e6bd823579ee85de485fce87d8ebc815d8f51d0679d02b19e3d86eedb
oid sha256:4538d9fc96c9ef164dbf8f39e28c13e07a6f7653caf8eb27821c244d1ce1698e
size 981400

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0895148012045433f3319106d01273a788457146b9b44d36b34dea46a6bbee0c
oid sha256:526aaeeba44256c1ed75c12adede4ddd2a868f9c919e979ee515443e5f8d85e3
size 972296

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

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

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c2353fba048e2c84c75e25cd6801e49cfeab3a95c0b75d583c668a2f377d0fb4
size 13381984
oid sha256:9644aeee10abb334360de52a3e49df288c789132ac4a1c64ac073daf4b950396
size 13382576

Loading…
Cancel
Save