def __init__(self, decel_limit, accel_limit, max_steer_angle,
max_lat_accel, min_speed, wheel_base, steer_ratio,
vehicle_mass, wheel_radius, max_throttle, max_brake):
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.max_steer_angle = max_steer_angle
self.vehicle_mass = vehicle_mass
self.wheel_radius = wheel_radius
# self.throttle_pid = pid.PID(kp = 0.6, ki = 0.004, kd = 0.2, mn=decel_limit, mx=accel_limit)
# self.throttle_pid = pid.PID(kp = 40.0, ki = 0.0, kd = 0.7, mn=max_brake, mx=max_throttle) # percents cte
self.throttle_pid = pid.PID(kp=1.5,
ki=0.0,
kd=0.01,
mn=max_brake,
mx=max_throttle) # cte is m/s
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5,
ki=0.0,
kd=0.2,
mn=-max_steer_angle,
mx=max_steer_angle) # ki = 0.04
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.clk = rospy.get_time()
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
wheel_radius,
vehicle_mass,
fuel_capacity):
self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.clk = rospy.get_time()
self.max_acc = max_throttle
self.max_dec = max_brake
self.wheel_radius = wheel_radius
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.max_brk_trq = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius * self.max_dec # -1809 N*m
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
vehicle_mass,
fuel_capacity,
brake_deadband,
wheel_radius):
self.throttle_pid = pid.PID(kp=3, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.wheel_radius = wheel_radius
self.max_throttle = max_throttle
self.max_brake = max_brake
self.max_brake_torque = (vehicle_mass + fuel_capacity*GAS_DENSITY) * wheel_radius * max_brake
self.clk = rospy.get_time()
self.count = 0
def __init__(self, *args, **kwargs):
# Timesteps
self.dt = 0.1 # 10Hz
self.prevtime = rospy.get_time()
# TODO: Pull PID Gains out to Launch Files
# TODO: Tune Coefficients
# Initialize Gains on PID Controllers
self.PIDCont_Thr = pid.PID(kp=1.0,ki=0.04,kd=0.1,mn= 0.0,mx=1.0)
self.PIDCont_Brk = pid.PID(kp=200.0,ki=0.00,kd=0.0,mn= 0.0,mx=10000.0)
if STEER_CTE:
# cross track error based steering
self.PIDCont_Str = pid.PID(kp=0.25,ki=0.075,kd=1.5,mn=-0.5,mx=0.5)
else:
# angular velocity error based steering
self.PIDCont_Str = pid.PID(kp=4.0,ki=0.5,kd=0.05,mn=-0.5,mx=0.5)
self.YawCont_Str = yc.YawController(wheel_base=2.8498, steer_ratio=14.8, min_speed=10.0, max_lat_accel=3.0, max_steer_angle=8.0)
# Initialize Low Pass Filters
self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
if STEER_CTE:
# cross track error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.7,ts=self.dt)
else:
# angular velocity error based steering
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt)
self.first = True # first pass flag
pass
def __init__(
self,
throttle_kp,
throttle_ki,
throttle_kd,
max_speed,
accel_limit,
decel_limit,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle,
vehicle_mass,
wheel_radius,
**kwargs):
self.throttle_brake_controller = throttle_brake_controller.ThrottleBrakeController(
kp=throttle_kp,
ki=throttle_ki,
kd=throttle_kd,
max_speed=max_speed,
accel_limit=accel_limit,
decel_limit=decel_limit,
brake_coef=vehicle_mass * wheel_radius)
self.yaw_controller = yaw_controller.YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=min_speed,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.lowpass_filter = lowpass.LowPassFilter(kwargs.get('tau', 0.2),kwargs.get('ts', 0.1))
self.last_time = None
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband,
decel_limit, accel_limit, wheel_radius, wheel_base,
steer_ratio, max_lat_accel, max_steer_angle):
# TODO: Implement
self.yaw_controller = yaw_controller.YawController(
wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle)
kp = 0.3
ki = 0.1
kd = 0.
# min and max throttle
mn = 0.
mx = 0.2
self.throttle_controller = pid.PID(kp, ki, kd, mn, mx)
# 1/(2pi*tau) = cutoff freq
tau = 0.5
# sample time
ts = .02
self.vel_lpf = lowpass.LowPassFilter(tau, ts)
self.vehicle_mass = vehicle_mass
self.fuel_capacity = fuel_capacity
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
self.accel_limit = accel_limit
self.wheel_radius = wheel_radius
self.last_time = rospy.get_time()
pass
def __init__(self, vehicle_mass, fuel_capacity, accel_limit, decel_limit,
wheel_base, wheel_radius, steer_ratio, max_lat_accel,
max_steer_angle, brake_deadband):
# TODO: Implement
self.velocity_controller = pid.PID(kp=K_P,
ki=K_I,
kd=K_D,
mn=decel_limit,
mx=accel_limit)
self.yaw_controller = yaw_controller.YawController(
wheel_base=wheel_base,
steer_ratio=steer_ratio,
min_speed=1.,
max_lat_accel=max_lat_accel,
max_steer_angle=max_steer_angle)
self.yaw_filter = lowpass.LowPassFilter(YAW_TAU, YAW_TS)
# need to calculate torque
self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = wheel_radius
self.brake_deadband = brake_deadband
self.decel_limit = decel_limit
def __init__(self, parmams):
self.vehicle_mass = parmams['vehicle_mass']
self.fuel_capacity = parmams['fuel_capacity']
self.brake_deadband = parmams['brake_deadband']
self.decel_limit = parmams['decel_limit']
self.accel_limit = parmams['accel_limit']
self.wheel_radius = parmams['wheel_radius']
self.wheel_base = parmams['wheel_base']
self.steer_ratio = parmams['steer_ratio']
self.max_lat_accel = parmams['max_lat_accel']
self.max_steer_angle = parmams['max_steer_angle']
self.pid_accel = PID.PID(0.3,
0.05,
0.3,
mn=self.decel_limit,
mx=self.accel_limit)
self.yaw_cont = yaw_controller.YawController(self.wheel_base,
self.steer_ratio, 0.1,
self.max_lat_accel,
self.max_steer_angle)
self.vel_LP = lowpass.LowPassFilter(0.5, 0.02)
def estimate(self, data, wind_time_factor):
print("Estimating winds aloft:")
if wind_time_factor:
self.wind_time_factor = wind_time_factor
self.filt_wn = lowpass.LowPassFilter(self.wind_time_factor, 0.0)
self.filt_we = lowpass.LowPassFilter(self.wind_time_factor, 0.0)
self.filt_ps = lowpass.LowPassFilter(self.pitot_time_factor, 1.0)
self.last_time = 0.0
winds = []
airspeed = 0
psi = 0
vn = 0
ve = 0
wind_deg = 0
wind_kt = 0
ps = 1.0
iter = flight_interp.IterateGroup(data)
for i in tqdm(range(iter.size())):
record = iter.next()
if len(record):
t = record['imu']['time']
if 'air' in record:
airspeed = record['air']['airspeed']
if 'filter' in record:
psi = record['filter']['psi']
vn = record['filter']['vn']
ve = record['filter']['ve']
if airspeed > 10.0:
(wn, we, ps) = self.update(t, airspeed, psi, vn, ve)
#print wn, we, math.atan2(wn, we), math.atan2(wn, we)*r2d
wind_deg = 90 - math.atan2(wn, we) * r2d
if wind_deg < 0: wind_deg += 360.0
wind_kt = math.sqrt(we * we + wn * wn) * mps2kt
#print wn, we, ps, wind_deg, wind_kt
# make sure we log one record per each imu record
winds.append({
'time': t,
'wind_deg': wind_deg,
'wind_kt': wind_kt,
'pitot_scale': ps
})
return winds
def __init__(self,
max_brake,
max_throttle,
max_steer_angle,
wheel_base,
steer_ratio,
min_speed,
max_lat_accel):
self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle)
self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle)
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.yaw_controller = yaw_controller.YawController(wheel_base,
steer_ratio,
min_speed,
max_lat_accel,
max_steer_angle)
self.clk = rospy.get_time()
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel,
max_steer_angle, vehicle_mass, brake_deadband, wheel_radius):
# TODO: Implement
self.dt = 0.0
self.timestamp = time.time()
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.vel_pid = pid.PID(kp=1.1, ki=0.05, kd=0.8, mn=-1, mx=1)
self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed,
max_lat_accel, max_steer_angle)
self.vehicle_mass = vehicle_mass
self.brake_deadband = brake_deadband
self.wheel_radius = wheel_radius
def __init__(self, max_steer_angle):
ms = max_steer_angle # max_steer_angle
# create lowpass filters
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
# override filter. It induces lag. Here for reference on how to use
self.steer_filter = None
# create controllers
self.steer_pid = pid.PID(kp=0.5,
ki=0.003,
kd=0.25,
mn=-ms,
mx=ms,
lowpass_filter=self.steer_filter)
# init timestamp
self.timestamp = rospy.get_time()
def __init__(self, wheel_base, steer_ratio, max_lat_accel,
max_steer_angle):
kp = 0.45
ki = 0.003
kd = 0.25
mn = -max_steer_angle
mx = max_steer_angle
tau = 0.5
ts = 0.02
self.vel_lpf = lowpass.LowPassFilter(tau, ts)
self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0,
max_lat_accel, max_steer_angle)
self.steering_correction_pid = pid.PID(kp, ki, kd, mn, mx)
self.timestamp = rospy.get_time()
def __init__(self, *args, **kwargs):
max_abs_angle = kwargs.get('max_steer_angle')
fuel_capacity = kwargs.get('fuel_capacity')
self.vehicle_mass = kwargs.get('vehicle_mass')
self.vehicle_mass = self.vehicle_mass + (fuel_capacity * GAS_DENSITY)
self.wheel_radius = kwargs.get('wheel_radius')
self.accel_limit = kwargs.get('accel_limit')
self.decel_limit = kwargs.get('decel_limit')
self.brake_deadband = kwargs.get('brake_deadband')
self.max_acceleration = kwargs.get('max_acceleration')
self.wheel_base = kwargs.get('wheel_base')
self.steer_ratio = kwargs.get('steer_ratio')
self.max_steer_angle = kwargs.get('max_steer_angle')
self.max_lat_accel = kwargs.get('max_lat_accel')
# PID controllers
if use_steering_pid:
# create lowpass filters
self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0)
self.pid_steer = pid.PID(kp=S_KP,
ki=S_KI,
kd=S_KD,
mn=-max_abs_angle,
mx=max_abs_angle)
self.pid_velocity = pid.PID(kp=V_KP, ki=V_KI, kd=V_KD)
self.yaw_controller = yaw_controller.YawController(
self.wheel_base,
self.steer_ratio,
1.0, #0.0,
self.max_lat_accel,
self.max_steer_angle) # aLEX YAW CONTROLLER SET TO 0
# max torque (1.0 throttle) and max brake torque (deceleration lmt)
self.max_acc_torque = self.vehicle_mass * self.max_acceleration * self.wheel_radius
self.max_brake_torque = self.vehicle_mass * abs(
self.decel_limit) * self.wheel_radius
# 1st timestamp
self.last_time = rospy.get_time()
# Estimate wind vector given indicated airspeed, aircraft heading
# (true), and gps ground velocity vector. This function is designed
# to be called repeatedly to update the wind estimate in real time.
import math
import lowpass
# useful constants
d2r = math.pi / 180.0
r2d = 180.0 / math.pi
mps2kt = 1.94384
kt2mps = 1 / mps2kt
time_factor = 60
filt_wn = lowpass.LowPassFilter(time_factor, 0.0)
filt_we = lowpass.LowPassFilter(time_factor, 0.0)
filt_ps = lowpass.LowPassFilter(time_factor * 4.0, 1.0)
last_time = 0.0
def update_wind(time, airspeed_kt, yaw_rad, vn, ve):
global last_time
global filt_wn
global filt_we
global filt_ps
dt = 0.0
if last_time > 0:
dt = time - last_time
last_time = time
def ResetLPFs(self):
self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt)
self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt)
self.LPFilt_CTE = lowpass.LowPassFilter(tau=0.2,ts=self.dt)
pass
