mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-03-01 14:53:56 +08:00
96d4bfbff3
* refactor
* needs casting
* tests pass
* fix that test
* refactor in controls
* lets not go crazy
* change of names
* use constants
* better naming
* renamed
* soft constraints
* compile slack variables
* rm git conflict
* add slack variables
* unused
* new edition
* fcw
* fix tests
* dividing causes problems
* was way too slow
* take a step back
* byeeee
* for another time
* bad idxs
* little more cpu for cruise mpc
* update refs
* these limits seem fine
* rename
* test model timings fails sometimes
* add default
* save some cpu
* Revert "little more cpu for cruise mpc"
This reverts commit f0a8163ec90e8dc1eabb3c4a4268ad330d23374d.
* Revert "test model timings fails sometimes"
This reverts commit d259d845710ed2cbeb28b383e2600476527d4838.
* update refs
* less cpu
* Revert "Revert "test model timings fails sometimes""
This reverts commit e0263050d9929bfc7ee70c9788234541a4a8461c.
* Revert "less cpu"
This reverts commit 679007472bc2013e7fafb7b17de7a43d6f82359a.
* cleanup
* not too much until we clean up mpc
* more cost on jerk
* change ref
* add todo
* new ref
* indentation
old-commit-hash: be5ddd25cd
67 lines
2.0 KiB
Python
67 lines
2.0 KiB
Python
import numpy as np
|
|
import math
|
|
|
|
from selfdrive.swaglog import cloudlog
|
|
from common.realtime import sec_since_boot
|
|
from selfdrive.controls.lib.longitudinal_mpc_lib import libmpc_py
|
|
from selfdrive.controls.lib.drive_helpers import LON_MPC_N
|
|
from selfdrive.modeld.constants import T_IDXS
|
|
|
|
|
|
class LongitudinalMpc():
|
|
def __init__(self):
|
|
self.reset_mpc()
|
|
self.last_cloudlog_t = 0.0
|
|
self.ts = list(range(10))
|
|
self.status = True
|
|
self.min_a = -1.2
|
|
self.max_a = 1.2
|
|
|
|
|
|
def reset_mpc(self):
|
|
self.libmpc = libmpc_py.libmpc
|
|
self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
|
|
|
|
self.mpc_solution = libmpc_py.ffi.new("log_t *")
|
|
self.cur_state = libmpc_py.ffi.new("state_t *")
|
|
|
|
self.cur_state[0].x_ego = 0
|
|
self.cur_state[0].v_ego = 0
|
|
self.cur_state[0].a_ego = 0
|
|
|
|
def set_accel_limits(self, min_a, max_a):
|
|
self.min_a = min_a
|
|
self.max_a = max_a
|
|
|
|
def set_cur_state(self, v, a):
|
|
v_safe = max(v, 1e-2)
|
|
a_safe = min(a, self.max_a - 1e-2)
|
|
a_safe = max(a_safe, self.min_a + 1e-2)
|
|
self.cur_state[0].x_ego = 0.0
|
|
self.cur_state[0].v_ego = v_safe
|
|
self.cur_state[0].a_ego = a_safe
|
|
|
|
def update(self, carstate, model, v_cruise):
|
|
v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
|
|
poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
|
|
speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
|
|
accels = np.zeros(LON_MPC_N+1)
|
|
|
|
# Calculate mpc
|
|
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
|
|
list(poss), list(speeds), list(accels),
|
|
self.min_a, self.max_a)
|
|
|
|
self.v_solution = list(self.mpc_solution.v_ego)
|
|
self.a_solution = list(self.mpc_solution.a_ego)
|
|
|
|
# Reset if NaN or goes through lead car
|
|
nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
|
|
|
|
t = sec_since_boot()
|
|
if nans:
|
|
if t > self.last_cloudlog_t + 5.0:
|
|
self.last_cloudlog_t = t
|
|
cloudlog.warning("Longitudinal model mpc reset - nans")
|
|
self.reset_mpc()
|