openpilot v0.3.6 tweaks

old-commit-hash: 19dd5f3e32

selfdrive/can/parser.cc

@@ -66,13 +66,14 @@ struct MessageState {
   std::vector<Signal> parse_sigs;
   std::vector<double> vals;
 
+  uint16_t ts;
   uint64_t seen;
   uint64_t check_threshold;
 
   uint8_t counter;
   uint8_t counter_fail;
 
-  bool parse(uint64_t sec, uint64_t dat) {
+  bool parse(uint64_t sec, uint16_t ts_, uint64_t dat) {
     for (int i=0; i < parse_sigs.size(); i++) {
       auto& sig = parse_sigs[i];
 
@@ -96,6 +97,7 @@ struct MessageState {
 
       vals[i] = tmp * sig.factor + sig.offset;
     }
+    ts = ts_;
     seen = sec;
 
     return true;
@@ -222,7 +224,7 @@ class CANParser {
 
         DEBUG("  proc %X: %lx\n", cmsg.getAddress(), p);
 
-        state_it->second.parse(sec, p);
+        state_it->second.parse(sec, cmsg.getBusTime(), p);
       }
   }
 
@@ -285,6 +287,7 @@ class CANParser {
         const Signal &sig = state.parse_sigs[i];
         ret.push_back((SignalValue){
           .address = state.address,
+          .ts = state.ts,
           .name = sig.name,
           .value = state.vals[i],
         });

selfdrive/can/parser.py

@@ -25,6 +25,7 @@ typedef struct MessageParseOptions {
 
 typedef struct SignalValue {
   uint32_t address;
+  uint16_t ts;
   const char* name;
   double value;
 } SignalValue;
@@ -45,6 +46,7 @@ class CANParser(object):
   def __init__(self, dbc_name, signals, checks=[], bus=0):
     self.can_valid = True
     self.vl = defaultdict(dict)
+    self.ts = defaultdict(dict)
 
     sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
 
@@ -87,7 +89,9 @@ class CANParser(object):
       cv = self.can_values[i]
       address = cv.address
       # print hex(cv.address), ffi.string(cv.name)
-      self.vl[address][ffi.string(cv.name)] = cv.value
+      name = ffi.string(cv.name)
+      self.vl[address][name] = cv.value
+      self.ts[address][name] = cv.ts
       ret.add(address)
     return ret
 
@@ -161,6 +165,7 @@ if __name__ == "__main__":
 
   while True:
     cp.update(int(sec_since_boot()*1e9), True)
-    print cp.vl
+    # print cp.vl
+    print cp.ts
     print cp.can_valid
     time.sleep(0.01)

selfdrive/can/parser_common.h

@@ -21,6 +21,7 @@ struct MessageParseOptions {
 
 struct SignalValue {
   uint32_t address;
+  uint16_t ts;
   const char* name;
   double value;
 };

selfdrive/car/honda/carstate.py

@@ -259,6 +259,8 @@ class CarState(object):
     self.cp = get_can_parser(CP)
 
     self.user_gas, self.user_gas_pressed = 0., 0
+    self.brake_switch_prev = 0
+    self.brake_switch_ts = 0
 
     self.cruise_buttons = 0
     self.cruise_setting = 0
@@ -311,8 +313,6 @@ class CarState(object):
     else:
       self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
       self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
-      if cp.vl[0x18F]['STEER_STATUS'] != 0:
-        print cp.vl[0x18F]['STEER_STATUS']
     self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
     self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
     # calc best v_ego estimate, by averaging two opposite corners
@@ -371,6 +371,7 @@ class CarState(object):
       self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
       self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
       self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
+
     if self.accord:
       # on the accord, this doesn't seem to include cruise control
       self.car_gas = cp.vl[0x17C]['PEDAL_GAS']
@@ -382,7 +383,15 @@ class CarState(object):
     else:
       self.car_gas = cp.vl[0x130]['CAR_GAS']
       self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
-    self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or cp.vl[0x17C]['BRAKE_SWITCH']
+
+    # brake switch has shown some single time step noise, so only considered when
+    # switch is on for at least 2 consecutive CAN samples
+    self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \
+                         (cp.vl[0x17C]['BRAKE_SWITCH'] and self.brake_switch_prev and \
+                         cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts)
+    self.brake_switch_prev = cp.vl[0x17C]['BRAKE_SWITCH']
+    self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH']
+
     self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
     self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
     self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']

selfdrive/controls/controlsd.py

@@ -274,7 +274,8 @@ class Controls(object):
     if self.rk.frame % 5 == 2 and self.plan.lateralValid: 
       # *** run this at 20hz again *** 
       self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, 
-                                             self.plan.dPoly, self.LaC.y_des, self.CS.steeringPressed) 
+                                             self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des,
+                                             self.CS.steeringPressed) 
 
     # *** gas/brake PID loop *** 
     final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, 

selfdrive/controls/lib/drive_helpers.py

@@ -3,20 +3,20 @@ from common.numpy_fast import clip
 def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
-def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
+def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override):
   # simple integral controller that learns how much steering offset to put to have the car going straight
+  # while being in the middle of the lane
   min_offset = -5.  # deg
   max_offset =  5.  # deg
   alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
   min_learn_speed = 1.
 
   # learn less at low speed or when turning
-  alpha_v = alpha*(max(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
+  alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.5*abs(y_des))
 
   # only learn if lateral control is active and if driver is not overriding:
   if lateral_control and not steer_override:
-    angle_offset += d_poly[3] * alpha_v
+    angle_offset += c_poly[3] * alpha_v
     angle_offset = clip(angle_offset, min_offset, max_offset)
 
   return angle_offset
-

selfdrive/controls/lib/pathplanner.py

@@ -62,6 +62,8 @@ class PathPlanner(object):
   def __init__(self):
     self.dead = True
     self.d_poly = [0., 0., 0., 0.]
+    self.c_poly = [0., 0., 0., 0.]
+    self.c_prob = 0.
     self.last_model = 0.
     self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
     self._path_pinv = compute_path_pinv()
@@ -81,7 +83,7 @@ class PathPlanner(object):
       self.lead_var = md.model.lead.std**2
 
       # compute target path
-      self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
+      self.d_poly, self.c_poly, self.c_prob = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
 
       self.last_model = cur_time
       self.dead = False

selfdrive/test/plant/plant.py

@@ -204,12 +204,12 @@ class Plant(object):
       print "%6.2f m  %6.2f m/s  %6.2f m/s2   %.2f ang   gas: %.2f  brake: %.2f  steer: %5.2f     lead_rel: %6.2f m  %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)
 
     # ******** publish the car ********
-    vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
+    vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
            self.angle_steer, 0, self.gear_choice, speed!=0,
            0, 0, 0, 0,
-           self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
+           self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, 0.,
            self.user_gas, cruise_buttons, self.esp_disabled, 0,
-           self.user_brake, self.steer_error, self.speed_sensor(speed), self.brake_error,
+           self.user_brake, self.steer_error, self.brake_error,
            self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
            self.pedal_gas, self.cruise_setting,
            # left_blinker, right_blinker, counter