Skip to content

Commit

Permalink
Ford: small cc clean up
Browse files Browse the repository at this point in the history
  • Loading branch information
sshane committed Oct 25, 2024
1 parent 04b17b9 commit c883a63
Showing 1 changed file with 12 additions and 10 deletions.
22 changes: 12 additions & 10 deletions opendbc/car/ford/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -102,31 +102,33 @@ def update(self, CC, CS, now_nanos):
# Compensate for engine creep at low speed.
# Either the ABS does not account for engine creep, or the correction is very slow
# TODO: verify this applies to EV/hybrid
self.accel = actuators.accel
accel = actuators.accel
if CC.longActive:
self.accel = apply_creep_compensation(self.accel, CS.out.vEgo)
self.accel = clip(self.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
accel = apply_creep_compensation(accel, CS.out.vEgo)
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

# Both gas and accel are in m/s^2, accel is used solely for braking
self.gas = self.accel
if not CC.longActive or self.gas < CarControllerParams.MIN_GAS:
self.gas = CarControllerParams.INACTIVE_GAS
gas = accel
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
gas = CarControllerParams.INACTIVE_GAS

# PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
accel_due_to_pitch = 0.0
if len(CC.orientationNED) == 3:
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
else:
accel_due_to_pitch = 0.0

accel_pitch_compensated = self.accel + accel_due_to_pitch
accel_pitch_compensated = accel + accel_due_to_pitch
if accel_pitch_compensated > 0.3 or not CC.longActive:
self.brake_request = False
elif accel_pitch_compensated < 0.0:
self.brake_request = True

stopping = CC.actuators.longControlState == LongCtrlState.stopping
# TODO: look into using the actuators packet to send the desired speed
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, self.gas, self.accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))

self.accel = accel
self.gas = gas

### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
Expand Down

0 comments on commit c883a63

Please sign in to comment.