Skip to content

Commit

Permalink
Merge branch 'tesla_alpha' of https://github.com/boggyver/openpilot i…
Browse files Browse the repository at this point in the history
…nto tesla_alpha
  • Loading branch information
Comma Device committed Aug 21, 2020
2 parents dfb9739 + 233f9f6 commit 2756e41
Show file tree
Hide file tree
Showing 5 changed files with 30 additions and 49 deletions.
9 changes: 6 additions & 3 deletions selfdrive/car/tesla/PCC_module.py
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,9 @@ def update_stat(self, CS, frame):

return can_sends

def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, set_speed_limit_active, speed_limit_offset,alca_enabled):
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, pcm_override,
speed_limit_ms, set_speed_limit_active, speed_limit_offset,
alca_enabled):
idx = self.pedal_idx

self.prev_speed_limit_kph = self.speed_limit_kph
Expand Down Expand Up @@ -385,6 +387,7 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
MPC_BRAKE_MULTIPLIER = 6.
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [LongCtrlState.pid, LongCtrlState.stopping]
# determine if pedal is pressed by human
self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed
Expand Down Expand Up @@ -463,12 +466,12 @@ def update_pdl(self, enabled, CS, frame, actuators, pcm_speed, speed_limit_ms, s
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
self.v_pid = -10.
self.v_pid = pcm_override
MPC_BRAKE_MULTIPLIER = 12.

self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., 1) #_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1, self.prev_tesla_accel, CS))
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, _brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS, self.pedal_speed_kph), 0.)

# if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero
Expand Down
8 changes: 7 additions & 1 deletion selfdrive/car/tesla/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -625,7 +625,13 @@ def update(self, enabled, CS, frame, actuators, \
pedalcan = 2
if CS.useWithoutHarness:
pedalcan = 0
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(enabled, CS, frame, actuators, pcm_speed, \
apply_accel, accel_needed, accel_idx = self.PCC.update_pdl(
enabled,
CS,
frame,
actuators,
pcm_speed,
pcm_override,
self.speed_limit_ms,
self.set_speed_limit_active, self.speed_limit_offset * CV.KPH_TO_MS, self.alca_enabled)
can_sends.append(teslacan.create_pedal_command_msg(apply_accel, int(accel_needed), accel_idx,pedalcan))
Expand Down
3 changes: 2 additions & 1 deletion selfdrive/car/tesla/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -556,7 +556,8 @@ def update(self, cp, epas_cp, pedal_cp):
self.gpsVehicleSpeed = cp.vl['UI_gpsVehicleSpeed']["UI_gpsVehicleSpeed"] * CV.KPH_TO_MS

if (self.hasTeslaIcIntegration):
self.apEnabled = (cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1)
#BB: AutoSteer enabled does not work unless we do old style port mapping on MCU
#self.apEnabled = (cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1)
self.summonButton = int(cp.vl["UI_driverAssistControl"]["UI_autoSummonEnable"])
self.apFollowTimeInS = 1 + cp.vl["MCU_chassisControl"]["MCU_fcwSensitivity"] * 0.5
self.keepEonOff = cp.vl["MCU_chassisControl"]["MCU_ldwEnable"] == 1
Expand Down
57 changes: 14 additions & 43 deletions selfdrive/car/tesla/speed_utils/movingaverage.py
Original file line number Diff line number Diff line change
@@ -1,49 +1,20 @@
import queue

class MovingAverage():
def __init__(self, length):
self.position = 0
self.length = length
self.sum = 0.
self.no_items = 0
self.values = [0.] * length

self.reset()

def reset(self):
self.position = 0
self.sum = 0.
self.no_items = 0
self.values = [0.] * self.length
self.queue = queue.Queue(maxsize=self.length)
self.sum = 0

def add(self,element):
if self.no_items == self.length:
self.no_items -= 1
self.sum -= self.values[self.position]
self.values[self.position] = element
self.sum += self.values[self.position]
self.no_items += 1
self.position += 1
if self.sum == 0.:
#all empty so initialize
self.position = 0
self.sum = 0.
self.no_items = 0
return 0.
self.position = self.position % self.length
return self.sum/self.no_items
def add(self, sample):
if self.queue.full():
self.sum -= self.queue.get_nowait()
self.queue.put_nowait(sample)
self.sum += sample
return self.sum / self.queue.qsize()

def dele(self):
if self.no_items == 0:
return 0.
if self.no_items > 0:
self.no_items -= 1
self.sum -= self.values[self.position]
self.values[self.position] = 0.
self.position -= 1
if self.position < 0:
self.position = self.length-1
if self.sum == 0. or self.no_items == 0.:
#all empty so initialize
self.position = 0
self.sum = 0.
self.no_items = 0
return 0.
self.position = self.position % self.length
return self.sum/self.no_items
def full(self):
return self.queue.full()
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/pid_real.py
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ def update(self, setpoint, measurement, speed=0.0, check_saturation=True, overri

self.f = feedforward * self.k_f
self.d = 0.0
if self.past_errors.no_items == self.past_errors.length:
if self.past_errors.full():
self.d = self.k_d * ((error - self.past_errors_avg) / self.d_rate)
self.past_errors_avg = self.past_errors.add(error)

Expand Down

0 comments on commit 2756e41

Please sign in to comment.