Add fast angle bias (#11)

* fixed pathplanner

* fixed bias

* Remove multi-stacking of angle-offset

The existing pathplanner bug results in the previous desired angle being stacked on top of the previous desired angle before the actual angle has time to catch up.  Since the desired angle currently includes the angle offset and bias, these values get stacked onto the desired angle also.  This results in a tsunami wave of torque favoring the offset / bias.

* The same angle / bias should be applied to both

* This item was correct

* Reverted both changes to offset

* Fix timeout in longitudinal test (#772)

* Fix timeout in longitudinal test

* Update hyundaican for Correct Message ID on LKAS11 (#746)

This is the only trace of CF_Lkas_Icon found under /car/hyundai relative to open .dbc

commaai/opendbc#172

* Revert "Update hyundaican for Correct Message ID on LKAS11 (#746)" (#775)

This reverts commit 1f1893a.

* Correct Message ID on LKAS11 under Openpilot .dbc (#747)

commaai/opendbc#172

* Revert "Revert "Update hyundaican for Correct Message ID on LKAS11 (#746)" (#775)"

This reverts commit d5242c5.

* fixed cur_state.delta

* Revert "fixed cur_state.delta"

This reverts commit 227dccd3596594a8d7162a46c9fb771b9f6b6d75.

* fixed cur_state.delta

* added fast angle bias

* fixed merge conflict

Author: Gernby

selfdrive/controls/controlsd.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+import os
 import gc
 import capnp
 from cereal import car, log
@@ -206,7 +207,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
   return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
 
 
-def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
+def state_control(frame, rcv_frame, plan, path_plan, live_params, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
                   AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc):
   """Given the state, this function returns an actuators packet"""
 
@@ -255,7 +256,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
   # Steering PID loop and lateral MPC
   actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
-                                                              CS.steeringPressed, CP, VM, path_plan)
+                                                              CS.steeringPressed, CP, VM, path_plan, live_params)
 
   # Send a "steering required alert" if saturation count has reached the limit
   if LaC.sat_flag and CP.steerLimitAlert:
@@ -427,7 +428,7 @@ def controlsd_thread(gctx=None):
   is_metric = params.get("IsMetric") == "1"
   passive = params.get("Passive") != "0"
 
-  sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
+  sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', 'liveParameters'])
 
   logcan = messaging.sub_sock(service_list['can'].port)
 
@@ -440,7 +441,8 @@ def controlsd_thread(gctx=None):
   logcan.close()
 
   # TODO: Use the logcan socket from above, but that will currenly break the tests
-  can_sock = messaging.sub_sock(service_list['can'].port, timeout=100)
+  can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
+  can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
 
   car_recognized = CP.carName != 'mock'
   # If stock camera is disconnected, we loaded car controls and it's not chffrplus
@@ -529,7 +531,7 @@ def controlsd_thread(gctx=None):
 
     # Compute actuators (runs PID loops and lateral MPC)
     actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
-      state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
+      state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], sm['liveParameters'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
                     driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
 
     rk.keep_time(1. / 10000)  # Run at 100Hz

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,7 +14,7 @@ def __init__(self, CP):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan, live_params):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steerAngle = float(angle_steers)
     pid_log.steerRate = float(angle_steers_rate)
@@ -24,7 +24,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override,
       pid_log.active = False
       self.pid.reset()
     else:
-      self.angle_steers_des = path_plan.angleSteers  # get from MPC/PathPlanner
+      angle_bias = live_params.angleOffset - live_params.angleOffsetAverage
+      self.angle_steers_des = path_plan.angleSteers + angle_bias  # get from MPC/PathPlanner
 
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max

selfdrive/controls/lib/pathplanner.py

@@ -57,10 +57,11 @@ def setup_mpc(self, steer_rate_cost):
   def update(self, sm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
+    #angle_steers_des = sm['controlsState'].angleSteersDes
     active = sm['controlsState'].active
 
     angle_offset_average = sm['liveParameters'].angleOffsetAverage
-    angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average
+    angle_offset = sm['liveParameters'].angleOffset
 
     self.MP.update(v_ego, sm['model'],sm['carState'])
 
@@ -73,7 +74,7 @@ def update(self, sm, CP, VM):
     self.p_poly = list(self.MP.p_poly)
 
     # account for actuation delay
-    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay, CP.eonToFront)
+    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay, CP.eonToFront)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -85,19 +86,19 @@ def update(self, sm, CP, VM):
       delta_desired = self.mpc_solution[0].delta[1]
       rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
     else:
-      delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
+      delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
       rate_desired = 0.0
 
     self.cur_state[0].delta = delta_desired
 
-    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
+    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_average)
 
     #  Check for infeasable MPC solution
     mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
     t = sec_since_boot()
     if mpc_nans:
       self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
-      self.cur_state[0].delta = math.radians(angle_steers - angle_offset_bias) / VM.sR
+      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
 
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t

selfdrive/locationd/paramsd.cc

@@ -140,8 +140,8 @@ int main(int argc, char *argv[]) {
           double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
           double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
 
-          // Send parameters at 10 Hz
-          if (save_counter % 10 == 0){
+          // Send parameters at 100 Hz
+          if (save_counter % 1 == 0){
             capnp::MallocMessageBuilder msg;
             cereal::Event::Builder event = msg.initRoot<cereal::Event>();
             event.setLogMonoTime(nanos_since_boot());