mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-25 11:33:55 +08:00
c6d9b9565a
* start again
* need that too
* this actually works
* not needed
* do properly
* still works
* still works
* still good
* all G without ll
* still works
* all still good
* cleanup building
* cleanup sconscript
* new lane planner
* how on earth is this silent too....
* update
* add rotation radius
* update
* pathplanner first pass
* misc fixes
* fix
* need deep_interp
* local again
* fix
* fix test
* very old
* new replay
* interp properly
* correct length
* another horrible silent bug
* like master
* fix that
* do doubles
* different delay compensation
* make robust to empty msg
* make pass with hack for now
* add some extra
* update ref for increased leg
* test cpu usage on this pr
* tiny bit faster
* purge numpy
* update ref
* not needed
* ready for merge
* try again after recompile
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 158210cde8
56 lines
1.6 KiB
Python
56 lines
1.6 KiB
Python
from common.numpy_fast import clip, interp
|
|
from selfdrive.config import Conversions as CV
|
|
from cereal import car
|
|
|
|
# kph
|
|
V_CRUISE_MAX = 144
|
|
V_CRUISE_MIN = 8
|
|
V_CRUISE_DELTA = 8
|
|
V_CRUISE_ENABLE_MIN = 40
|
|
MPC_N = 16
|
|
CAR_ROTATION_RADIUS = 1.5
|
|
|
|
|
|
class MPC_COST_LAT:
|
|
PATH = 1.0
|
|
HEADING = 1.0
|
|
STEER_RATE = 1.0
|
|
|
|
|
|
class MPC_COST_LONG:
|
|
TTC = 5.0
|
|
DISTANCE = 0.1
|
|
ACCELERATION = 10.0
|
|
JERK = 20.0
|
|
|
|
|
|
def rate_limit(new_value, last_value, dw_step, up_step):
|
|
return clip(new_value, last_value + dw_step, last_value + up_step)
|
|
|
|
|
|
def get_steer_max(CP, v_ego):
|
|
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
|
|
|
|
|
|
def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
|
|
# handle button presses. TODO: this should be in state_control, but a decelCruise press
|
|
# would have the effect of both enabling and changing speed is checked after the state transition
|
|
for b in buttonEvents:
|
|
if enabled and not b.pressed:
|
|
if b.type == car.CarState.ButtonEvent.Type.accelCruise:
|
|
v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA)
|
|
elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
|
|
v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA)
|
|
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
|
|
|
|
return v_cruise_kph
|
|
|
|
|
|
def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
|
|
for b in buttonEvents:
|
|
# 250kph or above probably means we never had a set speed
|
|
if b.type == car.CarState.ButtonEvent.Type.accelCruise and v_cruise_last < 250:
|
|
return v_cruise_last
|
|
|
|
return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))
|