Longitudinal planner: make v_desired a FirstOrderFilter (#23341)
* make v_desired a FirstOrderFilter * forgot one * one more ref * Add a new object for the filter * fix * fix tests * update ref Co-authored-by: Willem Melching <willem.melching@gmail.com>pull/23326/head
parent
a1e201ef5a
commit
9f88ba188a
|
@ -4,6 +4,7 @@ import numpy as np
|
|||
from common.numpy_fast import interp
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from common.filter_simple import FirstOrderFilter
|
||||
from common.realtime import DT_MDL
|
||||
from selfdrive.modeld.constants import T_IDXS
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
@ -48,9 +49,8 @@ class Planner:
|
|||
|
||||
self.fcw = False
|
||||
|
||||
self.v_desired = init_v
|
||||
self.a_desired = init_a
|
||||
self.alpha = np.exp(-DT_MDL / 2.0)
|
||||
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
|
||||
|
||||
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
|
@ -69,14 +69,13 @@ class Planner:
|
|||
|
||||
prev_accel_constraint = True
|
||||
if long_control_state == LongCtrlState.off or sm['carState'].gasPressed:
|
||||
self.v_desired = v_ego
|
||||
self.v_desired_filter.x = v_ego
|
||||
self.a_desired = a_ego
|
||||
# Smoothly changing between accel trajectory is only relevant when OP is driving
|
||||
prev_accel_constraint = False
|
||||
|
||||
# Prevent divergence, smooth in current v_ego
|
||||
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
|
||||
self.v_desired = max(0.0, self.v_desired)
|
||||
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
||||
|
||||
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
|
||||
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
|
||||
|
@ -88,7 +87,7 @@ class Planner:
|
|||
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
|
||||
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
|
||||
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
||||
self.mpc.set_cur_state(self.v_desired, self.a_desired)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
|
||||
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
|
||||
|
@ -102,7 +101,7 @@ class Planner:
|
|||
# Interpolate 0.05 seconds and save as starting point for next iteration
|
||||
a_prev = self.a_desired
|
||||
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
|
||||
self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
|
|
@ -97,7 +97,7 @@ class Plant():
|
|||
'carState': car_state.carState,
|
||||
'controlsState': control.controlsState}
|
||||
self.planner.update(sm)
|
||||
self.speed = self.planner.v_desired
|
||||
self.speed = self.planner.v_desired_filter.x
|
||||
self.acceleration = self.planner.a_desired
|
||||
fcw = self.planner.fcw
|
||||
self.distance_lead = self.distance_lead + v_lead * self.ts
|
||||
|
|
|
@ -1 +1 @@
|
|||
80430e515ea7ca44f2c73f047692d357856e74c1
|
||||
67534ae58a87b6993a0a9653d255e629785a07c3
|
Loading…
Reference in New Issue