mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-04 15:56:08 +08:00
FrogAi
86 lines
3.5 KiB
Python
86 lines
3.5 KiB
Python
#!/usr/bin/env python3
|
|
import numpy as np
|
|
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
|
|
|
|
from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
|
|
|
|
A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2
|
|
A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
|
|
|
|
# MPH = [0.0, 11, 22, 34, 45, 56, 89]
|
|
A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.]
|
|
A_CRUISE_MAX_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2]
|
|
A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6]
|
|
A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
|
|
|
|
def get_max_accel_eco(v_ego):
|
|
return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO))
|
|
|
|
def get_max_accel_sport(v_ego):
|
|
return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT))
|
|
|
|
def get_max_accel_sport_plus(v_ego):
|
|
return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS))
|
|
|
|
def get_max_accel_low_speeds(max_accel, v_cruise):
|
|
return float(np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel]))
|
|
|
|
def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
|
|
return float(np.interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel]))
|
|
|
|
def get_max_allowed_accel(v_ego):
|
|
return float(np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018
|
|
|
|
class FrogPilotAcceleration:
|
|
def __init__(self, FrogPilotPlanner):
|
|
self.frogpilot_planner = FrogPilotPlanner
|
|
|
|
self.max_accel = 0
|
|
self.min_accel = 0
|
|
|
|
def update(self, v_ego, sm, frogpilot_toggles):
|
|
eco_gear = sm["frogpilotCarState"].ecoGear
|
|
sport_gear = sm["frogpilotCarState"].sportGear
|
|
|
|
if sm["frogpilotCarState"].trafficModeEnabled:
|
|
self.max_accel = get_max_accel(v_ego)
|
|
elif frogpilot_toggles.map_acceleration and (eco_gear or sport_gear):
|
|
if eco_gear:
|
|
self.max_accel = get_max_accel_eco(v_ego)
|
|
else:
|
|
if frogpilot_toggles.sport_plus:
|
|
self.max_accel = get_max_accel_sport_plus(v_ego)
|
|
else:
|
|
self.max_accel = get_max_accel_sport(v_ego)
|
|
else:
|
|
if frogpilot_toggles.acceleration_profile == 1:
|
|
self.max_accel = get_max_accel_eco(v_ego)
|
|
elif frogpilot_toggles.acceleration_profile == 2:
|
|
self.max_accel = get_max_accel_sport(v_ego)
|
|
elif frogpilot_toggles.sport_plus:
|
|
self.max_accel = get_max_accel_sport_plus(v_ego)
|
|
else:
|
|
self.max_accel = get_max_accel(v_ego)
|
|
|
|
if frogpilot_toggles.human_acceleration:
|
|
if self.frogpilot_planner.frogpilot_following.following_lead and not sm["frogpilotCarState"].trafficModeEnabled:
|
|
self.max_accel = float(np.clip(self.frogpilot_planner.lead_one.aLeadK, self.max_accel, get_max_allowed_accel(v_ego)))
|
|
self.max_accel = min(get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise), self.max_accel)
|
|
self.max_accel = min(get_max_accel_ramp_off(self.max_accel, self.frogpilot_planner.v_cruise, v_ego), self.max_accel)
|
|
|
|
if sm["frogpilotCarState"].forceCoast:
|
|
self.min_accel = A_CRUISE_MIN_ECO
|
|
elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear):
|
|
if eco_gear:
|
|
self.min_accel = A_CRUISE_MIN_ECO
|
|
else:
|
|
self.min_accel = A_CRUISE_MIN_SPORT
|
|
else:
|
|
if frogpilot_toggles.deceleration_profile == 1:
|
|
self.min_accel = A_CRUISE_MIN_ECO
|
|
elif frogpilot_toggles.deceleration_profile == 2:
|
|
self.min_accel = A_CRUISE_MIN_SPORT
|
|
else:
|
|
self.min_accel = A_CRUISE_MIN
|