mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-10-01 03:43:42 +08:00
openpilot v0.5.12 release
This commit is contained in:
Vehicle Researcher
@@ -2,6 +2,8 @@
|
||||
import gc
|
||||
import zmq
|
||||
import json
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal import car, log
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
||||
@@ -19,7 +21,8 @@ from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
|
||||
update_v_cruise, \
|
||||
initialize_v_cruise
|
||||
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
|
||||
from selfdrive.controls.lib.latcontrol import LatControl
|
||||
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||
from selfdrive.controls.lib.alertmanager import AlertManager
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.controls.lib.driver_monitor import DriverStatus
|
||||
@@ -40,7 +43,7 @@ def isEnabled(state):
|
||||
return (isActive(state) or state == State.preEnabled)
|
||||
|
||||
|
||||
def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
|
||||
def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
|
||||
poller, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, state, mismatch_counter, params, plan, path_plan):
|
||||
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
||||
@@ -57,18 +60,21 @@ def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health,
|
||||
dm = None
|
||||
|
||||
for socket, event in poller.poll(0):
|
||||
msg = messaging.recv_one(socket)
|
||||
rcv_times[msg.which()] = sec_since_boot()
|
||||
|
||||
if socket is thermal:
|
||||
td = messaging.recv_one(socket)
|
||||
td = msg
|
||||
elif socket is calibration:
|
||||
cal = messaging.recv_one(socket)
|
||||
cal = msg
|
||||
elif socket is health:
|
||||
hh = messaging.recv_one(socket)
|
||||
hh = msg
|
||||
elif socket is driver_monitor:
|
||||
dm = messaging.recv_one(socket)
|
||||
dm = msg
|
||||
elif socket is plan_sock:
|
||||
plan = messaging.recv_one(socket)
|
||||
plan = msg
|
||||
elif socket is path_plan_sock:
|
||||
path_plan = messaging.recv_one(socket)
|
||||
path_plan = msg
|
||||
|
||||
if td is not None:
|
||||
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
|
||||
@@ -202,7 +208,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
|
||||
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
|
||||
|
||||
|
||||
def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc):
|
||||
"""Given the state, this function returns an actuators packet"""
|
||||
|
||||
@@ -246,11 +252,11 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
|
||||
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
|
||||
CS.steeringPressed)
|
||||
|
||||
cur_time = sec_since_boot() # TODO: This won't work in replay
|
||||
mpc_time = plan.l20MonoTime / 1e9
|
||||
cur_time = sec_since_boot()
|
||||
radar_time = rcv_times['plan'] - plan.processingDelay # Subtract processing delay to get the original measurement time
|
||||
_DT = 0.01 # 100Hz
|
||||
|
||||
dt = min(cur_time - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
|
||||
dt = min(cur_time - radar_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
|
||||
a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart)
|
||||
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
|
||||
|
||||
@@ -258,8 +264,8 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
|
||||
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
|
||||
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
|
||||
# Steering PID loop and lateral MPC
|
||||
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
|
||||
CS.steeringPressed, CP, VM, path_plan)
|
||||
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
|
||||
CS.steeringPressed, CP, VM, path_plan)
|
||||
|
||||
# Send a "steering required alert" if saturation count has reached the limit
|
||||
if LaC.sat_flag and CP.steerLimitAlert:
|
||||
@@ -278,12 +284,12 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
|
||||
|
||||
AM.process_alerts(sec_since_boot())
|
||||
|
||||
return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
|
||||
return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol, lac_log
|
||||
|
||||
|
||||
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
|
||||
carcontrol, live100, AM, driver_status,
|
||||
LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc):
|
||||
LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log):
|
||||
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
|
||||
plan_ts = plan.logMonoTime
|
||||
plan = plan.plan
|
||||
@@ -361,9 +367,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
|
||||
"uiAccelCmd": float(LoC.pid.i),
|
||||
"ufAccelCmd": float(LoC.pid.f),
|
||||
"angleSteersDes": float(LaC.angle_steers_des),
|
||||
"upSteer": float(LaC.pid.p),
|
||||
"uiSteer": float(LaC.pid.i),
|
||||
"ufSteer": float(LaC.pid.f),
|
||||
"vTargetLead": float(v_acc),
|
||||
"aTarget": float(a_acc),
|
||||
"jerkFactor": float(plan.jerkFactor),
|
||||
@@ -372,10 +375,15 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
|
||||
"vCurvature": plan.vCurvature,
|
||||
"decelForTurn": plan.decelForTurn,
|
||||
"cumLagMs": -rk.remaining * 1000.,
|
||||
"startMonoTime": start_time,
|
||||
"startMonoTime": int(start_time * 1e9),
|
||||
"mapValid": plan.mapValid,
|
||||
"forceDecel": bool(force_decel),
|
||||
}
|
||||
|
||||
if CP.lateralTuning.which() == 'pid':
|
||||
dat.live100.lateralControlState.pidState = lac_log
|
||||
else:
|
||||
dat.live100.lateralControlState.indiState = lac_log
|
||||
live100.send(dat.to_bytes())
|
||||
|
||||
# carState
|
||||
@@ -443,7 +451,12 @@ def controlsd_thread(gctx=None, rate=100):
|
||||
|
||||
LoC = LongControl(CP, CI.compute_gb)
|
||||
VM = VehicleModel(CP)
|
||||
LaC = LatControl(CP)
|
||||
|
||||
if CP.lateralTuning.which() == 'pid':
|
||||
LaC = LatControlPID(CP)
|
||||
else:
|
||||
LaC = LatControlINDI(CP)
|
||||
|
||||
AM = AlertManager()
|
||||
driver_status = DriverStatus()
|
||||
|
||||
@@ -465,6 +478,8 @@ def controlsd_thread(gctx=None, rate=100):
|
||||
mismatch_counter = 0
|
||||
low_battery = False
|
||||
|
||||
rcv_times = defaultdict(int)
|
||||
|
||||
plan = messaging.new_message()
|
||||
plan.init('plan')
|
||||
path_plan = messaging.new_message()
|
||||
@@ -485,23 +500,30 @@ def controlsd_thread(gctx=None, rate=100):
|
||||
prof = Profiler(False) # off by default
|
||||
|
||||
while True:
|
||||
start_time = int(sec_since_boot() * 1e9)
|
||||
start_time = sec_since_boot()
|
||||
prof.checkpoint("Ratekeeper", ignore=True)
|
||||
|
||||
# Sample data and compute car events
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\
|
||||
data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
|
||||
data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
|
||||
poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
|
||||
state, mismatch_counter, params, plan, path_plan)
|
||||
prof.checkpoint("Sample")
|
||||
|
||||
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
|
||||
plan_age = (start_time - plan.logMonoTime) / 1e9
|
||||
# Create alerts
|
||||
path_plan_age = start_time - rcv_times['pathPlan']
|
||||
plan_age = start_time - rcv_times['plan']
|
||||
|
||||
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
|
||||
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not path_plan.pathPlan.paramsValid:
|
||||
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
|
||||
events += list(plan.plan.events)
|
||||
if not path_plan.pathPlan.modelValid:
|
||||
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not plan.plan.radarValid:
|
||||
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if plan.plan.radarCommIssue:
|
||||
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
||||
@@ -514,8 +536,8 @@ def controlsd_thread(gctx=None, rate=100):
|
||||
prof.checkpoint("State transition")
|
||||
|
||||
# Compute actuators (runs PID loops and lateral MPC)
|
||||
actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc = \
|
||||
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
|
||||
actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc, lac_log = \
|
||||
state_control(rcv_times, plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
|
||||
v_cruise_kph_last, AM, rk, driver_status,
|
||||
LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc)
|
||||
|
||||
@@ -523,7 +545,7 @@ def controlsd_thread(gctx=None, rate=100):
|
||||
|
||||
# Publish data
|
||||
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
|
||||
live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc)
|
||||
live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log)
|
||||
prof.checkpoint("Sent")
|
||||
|
||||
rk.keep_time() # Run at 100Hz
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
DT = 0.01 # Controlsd runs at 100Hz
|
||||
|
||||
# kph
|
||||
V_CRUISE_MAX = 144
|
||||
V_CRUISE_MIN = 8
|
||||
@@ -55,6 +57,10 @@ 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 learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
|
||||
# simple integral controller that learns how much steering offset to put to have the car going straight
|
||||
# while being in the middle of the lane
|
||||
|
||||
@@ -16,6 +16,7 @@ _PITCH_WEIGHT = 1.5 # pitch matters a lot more
|
||||
_METRIC_THRESHOLD = 0.4
|
||||
_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch
|
||||
_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
|
||||
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
|
||||
_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid
|
||||
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
_VARIANCE_FILTER_TS = 20. # 0.008Hz
|
||||
@@ -89,7 +90,7 @@ class DriverStatus():
|
||||
def _is_driver_distracted(self, pose):
|
||||
# to be tuned and to learn the driver's normal pose
|
||||
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
|
||||
yaw_error = pose.yaw
|
||||
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
|
||||
# add positive pitch allowance
|
||||
if pitch_error > 0.:
|
||||
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from selfdrive.car.toyota.carcontroller import SteerLimitParams
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max, DT
|
||||
from common.numpy_fast import clip
|
||||
from cereal import log
|
||||
|
||||
|
||||
class LatControlINDI(object):
|
||||
def __init__(self, CP):
|
||||
self.angle_steers_des = 0.
|
||||
|
||||
A = np.matrix([[1.0, DT, 0.0],
|
||||
[0.0, 1.0, DT],
|
||||
[0.0, 0.0, 1.0]])
|
||||
C = np.matrix([[1.0, 0.0, 0.0],
|
||||
[0.0, 1.0, 0.0]])
|
||||
|
||||
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
|
||||
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
|
||||
|
||||
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
||||
# K = np.transpose(K)
|
||||
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
|
||||
[7.29394177e+00, 1.39159419e-02],
|
||||
[1.71022442e+01, 3.38495381e-02]])
|
||||
|
||||
self.K = K
|
||||
self.A_K = A - np.dot(K, C)
|
||||
self.x = np.matrix([[0.], [0.], [0.]])
|
||||
|
||||
self.enfore_rate_limit = CP.carName == "toyota"
|
||||
|
||||
self.RC = CP.lateralTuning.indi.timeConstant
|
||||
self.G = CP.lateralTuning.indi.actuatorEffectiveness
|
||||
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
|
||||
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
|
||||
self.alpha = 1. - DT / (self.RC + DT)
|
||||
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.delayed_output = 0.
|
||||
self.output_steer = 0.
|
||||
self.counter = 0
|
||||
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
|
||||
# Update Kalman filter
|
||||
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
|
||||
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
|
||||
|
||||
indi_log = log.Live100Data.LateralINDIState.new_message()
|
||||
indi_log.steerAngle = math.degrees(self.x[0])
|
||||
indi_log.steerRate = math.degrees(self.x[1])
|
||||
indi_log.steerAccel = math.degrees(self.x[2])
|
||||
|
||||
if v_ego < 0.3 or not active:
|
||||
indi_log.active = False
|
||||
self.output_steer = 0.0
|
||||
self.delayed_output = 0.0
|
||||
else:
|
||||
self.angle_steers_des = path_plan.angleSteers
|
||||
self.rate_steers_des = path_plan.rateSteers
|
||||
|
||||
steers_des = math.radians(self.angle_steers_des)
|
||||
rate_des = math.radians(self.rate_steers_des)
|
||||
|
||||
# Expected actuator value
|
||||
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha)
|
||||
|
||||
# Compute acceleration error
|
||||
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
|
||||
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
|
||||
accel_error = accel_sp - self.x[2]
|
||||
|
||||
# Compute change in actuator
|
||||
g_inv = 1. / self.G
|
||||
delta_u = g_inv * accel_error
|
||||
|
||||
# Enforce rate limit
|
||||
if self.enfore_rate_limit:
|
||||
steer_max = float(SteerLimitParams.STEER_MAX)
|
||||
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
|
||||
prev_output_steer_cmd = steer_max * self.output_steer
|
||||
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
|
||||
self.output_steer = new_output_steer_cmd / steer_max
|
||||
else:
|
||||
self.output_steer = self.delayed_output + delta_u
|
||||
|
||||
steers_max = get_steer_max(CP, v_ego)
|
||||
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
|
||||
|
||||
indi_log.active = True
|
||||
indi_log.rateSetPoint = float(rate_sp)
|
||||
indi_log.accelSetPoint = float(accel_sp)
|
||||
indi_log.accelError = float(accel_error)
|
||||
indi_log.delayedOutput = float(self.delayed_output)
|
||||
indi_log.delta = float(delta_u)
|
||||
indi_log.output = float(self.output_steer)
|
||||
|
||||
self.sat_flag = False
|
||||
return float(self.output_steer), float(self.angle_steers_des), indi_log
|
||||
@@ -1,29 +1,27 @@
|
||||
from selfdrive.controls.lib.pid import PIController
|
||||
from common.numpy_fast import interp
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
from cereal import car
|
||||
|
||||
_DT = 0.01 # 100Hz
|
||||
_DT_MPC = 0.05 # 20Hz
|
||||
from cereal import log
|
||||
|
||||
|
||||
def get_steer_max(CP, v_ego):
|
||||
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
|
||||
|
||||
|
||||
class LatControl(object):
|
||||
class LatControlPID(object):
|
||||
def __init__(self, CP):
|
||||
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
|
||||
(CP.steerKiBP, CP.steerKiV),
|
||||
k_f=CP.steerKf, pos_limit=1.0)
|
||||
self.last_cloudlog_t = 0.0
|
||||
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
|
||||
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
|
||||
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0)
|
||||
self.angle_steers_des = 0.
|
||||
|
||||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
|
||||
pid_log = log.Live100Data.LateralPIDState.new_message()
|
||||
pid_log.steerAngle = float(angle_steers)
|
||||
pid_log.steerRate = float(angle_steers_rate)
|
||||
|
||||
if v_ego < 0.3 or not active:
|
||||
output_steer = 0.0
|
||||
pid_log.active = False
|
||||
self.pid.reset()
|
||||
else:
|
||||
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
|
||||
@@ -43,6 +41,12 @@ class LatControl(object):
|
||||
deadzone = 0.0
|
||||
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
|
||||
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.f = self.pid.f
|
||||
pid_log.output = output_steer
|
||||
pid_log.saturated = bool(self.pid.saturated)
|
||||
|
||||
self.sat_flag = self.pid.saturated
|
||||
return output_steer, float(self.angle_steers_des)
|
||||
return output_steer, float(self.angle_steers_des), pid_log
|
||||
@@ -58,8 +58,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
|
||||
class LongControl(object):
|
||||
def __init__(self, CP, compute_gb):
|
||||
self.long_control_state = LongCtrlState.off # initialized to off
|
||||
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
|
||||
(CP.longitudinalKiBP, CP.longitudinalKiV),
|
||||
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
||||
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
||||
rate=RATE,
|
||||
sat_limit=0.8,
|
||||
convert=compute_gb)
|
||||
@@ -99,7 +99,7 @@ class LongControl(object):
|
||||
# Toyota starts braking more when it thinks you want to stop
|
||||
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
|
||||
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
|
||||
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
|
||||
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
|
||||
|
||||
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
|
||||
|
||||
|
||||
@@ -32,7 +32,7 @@ int main( )
|
||||
|
||||
// Running cost
|
||||
Function h;
|
||||
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
|
||||
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
|
||||
h << (d_l - desired) / (0.05 * v_ego + 0.5);
|
||||
h << a_ego * (0.1 * v_ego + 1.0);
|
||||
h << j_ego * (0.1 * v_ego + 1.0);
|
||||
@@ -42,7 +42,7 @@ int main( )
|
||||
|
||||
// Terminal cost
|
||||
Function hN;
|
||||
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
|
||||
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
|
||||
hN << (d_l - desired) / (0.05 * v_ego + 0.5);
|
||||
hN << a_ego * (0.1 * v_ego + 1.0);
|
||||
|
||||
|
||||
@@ -181,8 +181,8 @@ real_t evGx[ 180 ];
|
||||
/** Column vector of size: 60 */
|
||||
real_t evGu[ 60 ];
|
||||
|
||||
/** Column vector of size: 15 */
|
||||
real_t objAuxVar[ 15 ];
|
||||
/** Column vector of size: 13 */
|
||||
real_t objAuxVar[ 13 ];
|
||||
|
||||
/** Row vector of size: 6 */
|
||||
real_t objValueIn[ 6 ];
|
||||
|
||||
Binary file not shown.
@@ -73,7 +73,7 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
|
||||
const real_t* xd = in;
|
||||
const real_t* u = in + 3;
|
||||
const real_t* od = in + 4;
|
||||
/* Vector of auxiliary variables; number of elements: 15. */
|
||||
/* Vector of auxiliary variables; number of elements: 13. */
|
||||
real_t* a = acadoWorkspace.objAuxVar;
|
||||
|
||||
/* Compute intermediate quantities: */
|
||||
@@ -87,22 +87,20 @@ a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
|
||||
a[8] = (a[2]*a[2]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[10] = (real_t)(0.0000000000000000e+00);
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[13] = (a[11]*a[11]);
|
||||
a[14] = (real_t)(0.0000000000000000e+00);
|
||||
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[12] = (a[10]*a[10]);
|
||||
|
||||
/* Compute outputs: */
|
||||
out[0] = a[1];
|
||||
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[4] = a[4];
|
||||
out[5] = a[9];
|
||||
out[6] = a[10];
|
||||
out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
|
||||
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
|
||||
out[6] = (real_t)(0.0000000000000000e+00);
|
||||
out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
|
||||
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[9] = (real_t)(0.0000000000000000e+00);
|
||||
out[10] = (real_t)(0.0000000000000000e+00);
|
||||
out[11] = (xd[2]*(real_t)(1.0000000000000001e-01));
|
||||
@@ -110,7 +108,7 @@ out[12] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e
|
||||
out[13] = (real_t)(0.0000000000000000e+00);
|
||||
out[14] = (u[0]*(real_t)(1.0000000000000001e-01));
|
||||
out[15] = (real_t)(0.0000000000000000e+00);
|
||||
out[16] = a[14];
|
||||
out[16] = (real_t)(0.0000000000000000e+00);
|
||||
out[17] = (real_t)(0.0000000000000000e+00);
|
||||
out[18] = (real_t)(0.0000000000000000e+00);
|
||||
out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
|
||||
@@ -120,7 +118,7 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
|
||||
{
|
||||
const real_t* xd = in;
|
||||
const real_t* od = in + 3;
|
||||
/* Vector of auxiliary variables; number of elements: 14. */
|
||||
/* Vector of auxiliary variables; number of elements: 13. */
|
||||
real_t* a = acadoWorkspace.objAuxVar;
|
||||
|
||||
/* Compute intermediate quantities: */
|
||||
@@ -134,20 +132,19 @@ a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
|
||||
a[8] = (a[2]*a[2]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[10] = (real_t)(0.0000000000000000e+00);
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[13] = (a[11]*a[11]);
|
||||
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[12] = (a[10]*a[10]);
|
||||
|
||||
/* Compute outputs: */
|
||||
out[0] = a[1];
|
||||
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[3] = a[4];
|
||||
out[4] = a[9];
|
||||
out[5] = a[10];
|
||||
out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
|
||||
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
|
||||
out[5] = (real_t)(0.0000000000000000e+00);
|
||||
out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
|
||||
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[8] = (real_t)(0.0000000000000000e+00);
|
||||
out[9] = (real_t)(0.0000000000000000e+00);
|
||||
out[10] = (xd[2]*(real_t)(1.0000000000000001e-01));
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -49,7 +49,7 @@ class PathPlanner(object):
|
||||
self.angle_steers_des_prev = 0.0
|
||||
self.angle_steers_des_time = 0.0
|
||||
|
||||
def update(self, CP, VM, CS, md, live100, live_parameters):
|
||||
def update(self, rcv_times, CP, VM, CS, md, live100, live_parameters):
|
||||
v_ego = CS.carState.vEgo
|
||||
angle_steers = CS.carState.steeringAngle
|
||||
active = live100.live100.active
|
||||
@@ -104,6 +104,8 @@ class PathPlanner(object):
|
||||
else:
|
||||
self.invalid_counter = 0
|
||||
|
||||
cur_time = sec_since_boot()
|
||||
model_dead = cur_time - rcv_times['model'] > 0.5
|
||||
plan_valid = self.invalid_counter < 2
|
||||
|
||||
plan_send = messaging.new_message()
|
||||
@@ -121,6 +123,7 @@ class PathPlanner(object):
|
||||
plan_send.pathPlan.angleOffset = float(angle_offset_average)
|
||||
plan_send.pathPlan.valid = bool(plan_valid)
|
||||
plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
|
||||
plan_send.pathPlan.modelValid = bool(not model_dead)
|
||||
|
||||
self.plan.send(plan_send.to_bytes())
|
||||
|
||||
|
||||
@@ -6,10 +6,11 @@ from common.params import Params
|
||||
from common.numpy_fast import interp
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from cereal import car
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.speed_smoother import speed_smoother
|
||||
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
|
||||
from selfdrive.controls.lib.fcw import FCWChecker
|
||||
@@ -113,9 +114,9 @@ class Planner(object):
|
||||
|
||||
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
|
||||
|
||||
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
|
||||
def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
|
||||
"""Gets called when new live20 is available"""
|
||||
cur_time = live20.logMonoTime / 1e9
|
||||
cur_time = sec_since_boot()
|
||||
v_ego = CS.carState.vEgo
|
||||
|
||||
long_control_state = live100.live100.longControlState
|
||||
@@ -131,11 +132,13 @@ class Planner(object):
|
||||
|
||||
v_speedlimit = NO_CURVATURE_SPEED
|
||||
v_curvature = NO_CURVATURE_SPEED
|
||||
map_valid = live_map_data.liveMapData.mapValid
|
||||
|
||||
map_age = cur_time - rcv_times['liveMapData']
|
||||
map_valid = live_map_data.liveMapData.mapValid and map_age < 10.0
|
||||
|
||||
# Speed limit and curvature
|
||||
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
|
||||
if set_speed_limit_active:
|
||||
if set_speed_limit_active and map_valid:
|
||||
if live_map_data.liveMapData.speedLimitValid:
|
||||
speed_limit = live_map_data.liveMapData.speedLimit
|
||||
offset = float(self.params.get("SpeedLimitOffset"))
|
||||
@@ -206,24 +209,16 @@ class Planner(object):
|
||||
if fcw:
|
||||
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
|
||||
|
||||
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
|
||||
radar_dead = cur_time - rcv_times['live20'] > 0.5
|
||||
|
||||
radar_errors = list(live20.live20.radarErrors)
|
||||
radar_fault = car.RadarState.Error.fault in radar_errors
|
||||
radar_comm_issue = car.RadarState.Error.commIssue in radar_errors
|
||||
|
||||
# **** send the plan ****
|
||||
plan_send = messaging.new_message()
|
||||
plan_send.init('plan')
|
||||
|
||||
# TODO: Move all these events to controlsd. This has nothing to do with planning
|
||||
events = []
|
||||
if model_dead:
|
||||
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
radar_errors = list(live20.live20.radarErrors)
|
||||
if 'commIssue' in radar_errors:
|
||||
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if 'fault' in radar_errors:
|
||||
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
plan_send.plan.events = events
|
||||
plan_send.plan.mdMonoTime = md.logMonoTime
|
||||
plan_send.plan.l20MonoTime = live20.logMonoTime
|
||||
|
||||
@@ -242,6 +237,12 @@ class Planner(object):
|
||||
plan_send.plan.decelForTurn = decel_for_turn
|
||||
plan_send.plan.mapValid = map_valid
|
||||
|
||||
radar_valid = not (radar_dead or radar_fault)
|
||||
plan_send.plan.radarValid = bool(radar_valid)
|
||||
plan_send.plan.radarCommIssue = bool(radar_comm_issue)
|
||||
|
||||
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['live20']
|
||||
|
||||
# Send out fcw
|
||||
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
|
||||
plan_send.plan.fcw = fcw
|
||||
|
||||
@@ -27,7 +27,7 @@ v_ego_stationary = 4. # no stationary object flag below this speed
|
||||
|
||||
# Lead Kalman Filter params
|
||||
_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
|
||||
_VLEAD_C = [[1.0, 0.0]]
|
||||
_VLEAD_C = [1.0, 0.0]
|
||||
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
|
||||
#_VLEAD_R = 1e3
|
||||
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
#!/usr/bin/env python
|
||||
import zmq
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.controls.lib.planner import Planner
|
||||
@@ -52,22 +54,27 @@ def plannerd_thread():
|
||||
live_parameters.liveParameters.steerRatio = CP.steerRatio
|
||||
live_parameters.liveParameters.stiffnessFactor = 1.0
|
||||
|
||||
rcv_times = defaultdict(int)
|
||||
|
||||
while True:
|
||||
for socket, event in poller.poll():
|
||||
msg = messaging.recv_one(socket)
|
||||
rcv_times[msg.which()] = sec_since_boot()
|
||||
|
||||
if socket is live100_sock:
|
||||
live100 = messaging.recv_one(socket)
|
||||
live100 = msg
|
||||
elif socket is car_state_sock:
|
||||
car_state = messaging.recv_one(socket)
|
||||
car_state = msg
|
||||
elif socket is live_parameters_sock:
|
||||
live_parameters = messaging.recv_one(socket)
|
||||
live_parameters = msg
|
||||
elif socket is model_sock:
|
||||
model = messaging.recv_one(socket)
|
||||
PP.update(CP, VM, car_state, model, live100, live_parameters)
|
||||
model = msg
|
||||
PP.update(rcv_times, CP, VM, car_state, model, live100, live_parameters)
|
||||
elif socket is live_map_data_sock:
|
||||
live_map_data = messaging.recv_one(socket)
|
||||
live_map_data = msg
|
||||
elif socket is live20_sock:
|
||||
live20 = messaging.recv_one(socket)
|
||||
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
|
||||
live20 = msg
|
||||
PL.update(rcv_times, car_state, CP, VM, PP, live20, live100, model, live_map_data)
|
||||
|
||||
|
||||
def main(gctx=None):
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
import unittest
|
||||
import numpy as np
|
||||
|
||||
from cereal import log
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.planner import calc_cruise_accel_limits
|
||||
from selfdrive.controls.lib.speed_smoother import speed_smoother
|
||||
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
|
||||
|
||||
|
||||
def RW(v_ego, v_l):
|
||||
TR = 1.8
|
||||
G = 9.81
|
||||
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
|
||||
|
||||
|
||||
class FakeSocket(object):
|
||||
def send(self, data):
|
||||
assert data
|
||||
|
||||
|
||||
def run_following_distance_simulation(v_lead, t_end=200.0):
|
||||
dt = 0.2
|
||||
t = 0.
|
||||
|
||||
x_lead = 200.0
|
||||
|
||||
x_ego = 0.0
|
||||
v_ego = v_lead
|
||||
a_ego = 0.0
|
||||
|
||||
v_cruise_setpoint = v_lead + 10.
|
||||
|
||||
mpc = LongitudinalMpc(1, FakeSocket())
|
||||
|
||||
first = True
|
||||
while t < t_end:
|
||||
# Run cruise control
|
||||
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
|
||||
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
|
||||
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
|
||||
accel_limits[1], accel_limits[0],
|
||||
jerk_limits[1], jerk_limits[0],
|
||||
dt)
|
||||
|
||||
# Setup CarState
|
||||
CS = messaging.new_message()
|
||||
CS.init('carState')
|
||||
CS.carState.vEgo = v_ego
|
||||
CS.carState.aEgo = a_ego
|
||||
|
||||
# Setup lead packet
|
||||
lead = log.Live20Data.LeadData.new_message()
|
||||
lead.status = True
|
||||
lead.dRel = x_lead - x_ego
|
||||
lead.vLead = v_lead
|
||||
lead.aLeadK = 0.0
|
||||
|
||||
# Run MPC
|
||||
mpc.set_cur_state(v_ego, a_ego)
|
||||
if first: # Make sure MPC is converged on first timestep
|
||||
for _ in range(20):
|
||||
mpc.update(CS, lead, v_cruise_setpoint)
|
||||
mpc.update(CS, lead, v_cruise_setpoint)
|
||||
|
||||
# Choose slowest of two solutions
|
||||
if v_cruise < mpc.v_mpc:
|
||||
v_ego, a_ego = v_cruise, a_cruise
|
||||
else:
|
||||
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
|
||||
|
||||
# Update state
|
||||
x_lead += v_lead * dt
|
||||
x_ego += v_ego * dt
|
||||
t += dt
|
||||
first = False
|
||||
|
||||
return x_lead - x_ego
|
||||
|
||||
|
||||
class TestFollowingDistance(unittest.TestCase):
|
||||
def test_following_distanc(self):
|
||||
for speed_mph in np.linspace(10, 100, num=10):
|
||||
v_lead = float(speed_mph * CV.MPH_TO_MS)
|
||||
|
||||
simulation_steady_state = run_following_distance_simulation(v_lead)
|
||||
correct_steady_state = RW(v_lead, v_lead) + 4.0
|
||||
|
||||
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1)
|
||||
Reference in New Issue
Block a user