def __init__(self, ts_control): # instantiate lateral controllers self.roll_from_aileron = pid_control(kp=AP.roll_kp, kd=AP.roll_kd, limit=np.radians(45)) self.course_from_roll = pid_control(kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit=np.radians(30)) # self.sideslip_from_rudder = pid_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 = pid_control(kp=AP.pitch_kp, kd=AP.pitch_kd, limit=np.radians(45)) self.altitude_from_pitch = pid_control(kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit=np.radians(30)) self.airspeed_from_throttle = pid_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)
def __init__(self, ts_control): # instantiate lateral controllers self.roll_from_aileron = pid_control( #pd_control_with_rate( kp=AP.roll_kp, kd=AP.roll_kd, Ts=ts_control, limit=np.radians(45)) self.course_from_roll = pid_control( #pi_control( kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit=np.radians(30)) self.sideslip_from_rudder = pid_control( #pi_control( kp=AP.sideslip_kp, ki=AP.sideslip_ki, Ts=ts_control, limit=np.radians(45)) self.yaw_damper = matlab.tf([0.5, 0.], [ 1.0, ], ts_control) # # 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 = pid_control( #pd_control_with_rate( kp=AP.pitch_kp, kd=AP.pitch_kd, limit=np.radians(45)) self.altitude_from_pitch = pid_control( #pi_control( kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit=np.radians(30)) self.airspeed_from_throttle = pid_control( #pi_control( kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, Ts=ts_control, limit=1.5, throttle_flag=True) self.commanded_state = msg_state()