diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7c123de97..b9d832644 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 58011f696..21d45f899 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 6c691b6a9..deeb8e08c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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 diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index 1766db311..9e012268f 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/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(); event.setLogMonoTime(nanos_since_boot());