def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pd_control_with_rate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=np.radians(45))
self.course_from_roll = pi_control(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=np.radians(30))
self.sideslip_from_rudder = pi_control(kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit=np.radians(45))
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pd_control_with_rate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=np.radians(45))
self.altitude_from_pitch = pi_control(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=np.radians(30))
self.airspeed_from_throttle = pi_control(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=1.0)
self.commanded_state = msg_state()
def __init__(self, K, limit, Ts, MAV):
self.Ts = Ts
self.limit = limit
self.MAV = MAV
self.k_T = K.item(0)
self.k_D = K.item(1)
self.k_h = K.item(2)
self.k_Va = K.item(3)
self.h_d = -self.MAV.pd0
self.h_d_dot = 0
self.Va_d = self.MAV.Va0
self.Va_d_dot = 0
self.thrust_from_throttle = pi_control(kp=AP.thrust_throttle_kp,
ki=AP.thrust_throttle_ki,
Ts=Ts,
limit=1.0)
self.flight_path_angle_from_elevator = pid_control(
kp=AP.fpa_elevator_kp,
ki=AP.fpa_elevator_ki,
kd=AP.fpa_elevator_kd,
# Ts=Ts,
limit=1.0)