def __init__(self):
'''
Creates a new Quadrotor object.
'''
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(
IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
self.roll_Stability_PID = Stability_PID_Controller(
IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
# Special handling for yaw from IMU
#self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
self.yaw_IMU_PID = Hover_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd,
IMU_YAW_Ki)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp, ALTITUDE_Kd)
# Altitude hold z-pisition
self.target_altitude = 0.5
self.target_yaw = 0.0
def __init__(self, logfile=None):
'''
Creates a new Quadrotor object with optional logfile.
'''
# Store logfile handle
self.logfile = logfile
# Create PD controllers for pitch, roll based on angles from Inertial Measurement Unit (IMU)
self.pitch_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
self.roll_Stability_PID = Stability_PID_Controller(IMU_PITCH_ROLL_Kp, IMU_PITCH_ROLL_Kd, IMU_PITCH_ROLL_Ki)
# Special handling for yaw from IMU
self.yaw_IMU_PID = Yaw_PID_Controller(IMU_YAW_Kp, IMU_YAW_Kd, IMU_YAW_Ki)
# Create PD controller for altitude-hold
self.altitude_PID = Hover_PID_Controller(ALTITUDE_Kp, ALTITUDE_Kd)
# For altitude hold
self.switch_prev = 0
self.target_altitude = 0
