You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
2.4 KiB
53 lines
2.4 KiB
9 years ago
|
import numpy as np
|
||
|
|
|
||
|
|
def rate_limit(new_value, last_value, dw_step, up_step):
|
||
|
|
return np.clip(new_value, last_value + dw_step, last_value + up_step)
|
||
|
|
|
||
|
|
def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
|
||
|
|
# simple integral controller that learns how much steering offset to put to have the car going straight
|
||
|
|
min_offset = -1. # deg
|
||
|
|
max_offset = 1. # deg
|
||
|
|
alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
|
||
|
|
min_learn_speed = 1.
|
||
|
|
|
||
|
|
# learn less at low speed or when turning
|
||
|
|
alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
|
||
|
|
|
||
|
|
# only learn if lateral control is active and if driver is not overriding:
|
||
|
|
if lateral_control and not steer_override:
|
||
|
|
angle_offset += d_poly[3] * alpha_v
|
||
|
|
angle_offset = np.clip(angle_offset, min_offset, max_offset)
|
||
|
|
|
||
|
|
return angle_offset
|
||
|
|
|
||
|
|
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
|
||
|
|
# hyst params... TODO: move these to VehicleParams
|
||
|
|
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
|
||
|
|
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||
|
|
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
|
||
|
|
|
||
|
|
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
|
||
|
|
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
|
||
|
|
final_brake = 0.
|
||
|
|
braking = final_brake > 0.
|
||
|
|
|
||
|
|
# for small brake oscillations within brake_hyst_gap, don't change the brake command
|
||
|
|
if final_brake == 0.:
|
||
|
|
brake_steady = 0.
|
||
|
|
elif final_brake > brake_steady + brake_hyst_gap:
|
||
|
|
brake_steady = final_brake - brake_hyst_gap
|
||
|
|
elif final_brake < brake_steady - brake_hyst_gap:
|
||
|
|
brake_steady = final_brake + brake_hyst_gap
|
||
|
|
final_brake = brake_steady
|
||
|
|
|
||
|
|
if not civic:
|
||
|
|
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
|
||
|
|
brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds
|
||
|
|
# offset the brake command for threshold in the brake system. no brake torque perceived below it
|
||
|
|
brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
|
||
|
|
brake_offset = brake_on_offset - brake_hyst_on
|
||
|
|
if final_brake > 0.0:
|
||
|
|
final_brake += brake_offset
|
||
|
|
|
||
|
|
return final_brake, braking, brake_steady
|