class MyClient(MultirotorClient): def __init__(self, **kwargs): super(MyClient, self).__init__(**kwargs) self.client = MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True) self.client.armDisarm(True) #self.client.reset() # 실행 시 뒤에 코드가 무시될 수 있어 필요할 때만 주석 해제 self.client.takeoffAsync().join() self.roll = PIDDouble() self.pitch = PIDDouble() # Set PID Gain self.set_pid_gain() #Save Flight Data self.flight_data = pd.DataFrame() self.flight_data['altitude'] = 0 self.now = None # Drone Value self.angle = {'pitch': None, 'roll': None, 'yaw': None} self.rate = {'pitch': None, 'roll': None, 'yaw': None} self.altitude = None self.longitude = None self.latitude = None self.DT = 0.001 self.ccr = {1: None, 2: None, 3: None, 4: None, 'duration': 1} self.motor = {1: 0.60001, 2: 0.60001, 3: 0.6, 4: 0.6, 'duration': 1} def __del__(self): self.now = time.localtime() self.now = time.strftime('%d%H%M%S', self.now) # self.flight_data.to_csv(f'data/simulation_data_{self.now}') def run(self): while 1: start = time.time() # Read Sensor Data # IMU self.read_angle() self.read_rate() # Barometer self.read_altitude() # GPS self.read_gps() # Save Data self.flight_data['altitude'] = self.altitude # PID Control self.Double_Roll_Pitch_PID_Calculation(self.pitch, 0, self.angle['pitch'], self.rate['pitch'], self.DT) self.Double_Roll_Pitch_PID_Calculation(self.roll, 0, self.angle['roll'], self.rate['roll'], self.DT) # PWM self.ccr[ 1] = 84000 + 600 * 84 - self.pitch.IN.pid_result + self.roll.IN.pid_result self.ccr[ 2] = 84000 + 600 * 84 + self.pitch.IN.pid_result + self.roll.IN.pid_result self.ccr[ 3] = 84000 + 600 * 84 + self.pitch.IN.pid_result - self.roll.IN.pid_result self.ccr[ 4] = 84000 + 600 * 84 - self.pitch.IN.pid_result - self.roll.IN.pid_result for i in range(4): if self.ccr[i + 1] < 84000: self.ccr[i + 1] = 84000 elif self.ccr[i + 1] > 168000: self.ccr[i + 1] = 168000 self.ccr[i + 1] = (self.ccr[i + 1] - 84000) / 84000 # Write Motors self.client.moveByMotorPWMsAsync(self.ccr[4], self.ccr[2],\ self.ccr[1], self.ccr[3], self.ccr['duration']) # print('roll : ',self.angle['roll'],' pitch : ', self.angle['pitch'], ' yaw : ', self.angle['yaw']) self.DT = time.time() - start if keyboard.read_key() == 'q': break del self.client def read_angle(self): self.angle['pitch'], self.angle['roll'], self.angle['yaw'] = \ airsim.to_eularian_angles(self.client.getImuData('Imu', 'Drone1').orientation) self.angle['pitch'] *= -1 def read_rate(self): self.rate['roll'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.x_val self.rate['pitch'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.y_val self.rate['yaw'] = self.client.getImuData( 'Imu', 'Drone1').angular_velocity.z_val self.rate['pitch'] *= -1 def read_altitude(self): self.altitude = self.client.getBarometerData( barometer_name='Barometer', vehicle_name='Drone1').altitude def read_gps(self): self.longitude = self.client.getGpsData( 'Gps', 'Drone1').gnss.geo_point.longitude self.latitude = self.client.getGpsData( 'Gps', 'Drone1').gnss.geo_point.latitude def set_pid_gain(self): self.pitch.IN.kp = 104 self.pitch.IN.ki = 0 self.pitch.IN.kd = 7 self.pitch.OUT.kp = 14 self.pitch.OUT.ki = 0 self.pitch.OUT.kd = 3 self.roll.IN.kp = 104 self.roll.IN.ki = 0 self.roll.IN.kd = 7 self.roll.OUT.kp = 14 self.roll.OUT.ki = 0 self.roll.OUT.kd = 3 def Double_Roll_Pitch_PID_Calculation( self, axis: PIDDouble, set_point_angle: float, angle: float, # BNO080 Rotation Angle rate: float, # ICM-20602 Angular Rate DT: float # loop time ): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# # P calculation axis.OUT.reference = set_point_angle axis.OUT.meas_value = angle axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # I calculation, DT need to be given in every loop axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 2000 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D calculation OUTER_DERIV_FILT_ENABLE = True axis.OUT.error_deriv = -rate # Define derivative of outer loop (rate = ICM-20602 Angular Rate) if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = rate # ICM-20602 angular rate # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop def Single_Yaw_Heading_PID_Calculation( axis: PIDSingle, set_point_angle: float, angle: float, # BNO080 Rotation Angle rate: float, # ICM-20602 Angular Rate DT: float # loop time ): # *********** Single PID Begin (Yaw Angular Position) *************# axis.reference = set_point_angle # Set point of yaw heading @ yaw stick is center. axis.meas_value = angle # Current BNO080_Yaw angle @ yaw stick is center. # P Calculation axis.error = axis.reference - axis.meas_value # Define error of yaw angle control if (axis.error > 180): axis.error -= 360 elif (axis.error < -180): axis.error += 360 axis.p_result = axis.error * axis.kp # Calculate P result of yaw angle control # I Calculation axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw angle control axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw angle control # D Calculation axis.error_deriv = -rate # Define differentiation of yaw angle control axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw angle control axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw angle contro def Single_Yaw_Rate_PID_Calculation( axis: PIDSingle, set_point_rate: float, rate: float, # ICM-20602 Angular Rate DT: float): # *********** Single PID Begin (Yaw Angular Rate Control) *************# axis.reference = set_point_rate # Set point of yaw heading @ yaw stick is not center. axis.meas_value = rate # Current ICM20602.gyro_z @ yaw stick is not center. # P calculation axis.error = axis.reference - axis.meas_value # Define error of yaw rate control axis.p_result = axis.error * axis.kp # Calculate P result of yaw rate control # I calculation axis.error_sum = axis.error_sum + axis.error * DT # Define summation of yaw rate control axis.i_result = axis.error_sum * axis.ki # Calculate I result of yaw rate control # D calculation axis.error_deriv = -( axis.meas_value - axis.meas_value_prev ) / DT # Define differentiation of yaw rate control axis.meas_value_prev = axis.meas_value # Refresh value_prev to the latest value axis.d_result = axis.error_deriv * axis.kd # Calculate D result of yaw rate control axis.pid_result = axis.p_result + axis.i_result + axis.d_result # Calculate PID result of yaw control # *******************************************************************# def Double_Altitude_PID_Calculation(axis: PIDDouble, set_point_altitude: float, current_altitude: float, DT: float): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# axis.OUT.reference = set_point_altitude # Set point of outer PID control axis.OUT.meas_value = current_altitude # Actual Altitude from Fusion # P Calculation axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop # I Calculation axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 500 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D Calculation axis.OUT.error_deriv = -(axis.OUT.meas_value - axis.OUT.meas_value_prev) / DT axis.OUT.meas_value_prev = axis.OUT.meas_value OUTER_DERIV_FILT_ENABLE = True if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = -axis.OUT.error_deriv_filt # ICM-20602 angular rate # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop if axis.IN.pid_result < -2100: axis.IN.pid_result = -2100 if axis.IN.pid_result > 16800: axis.IN.pid_result = 16800 def Double_GPS_PID_Calculation(axis: PIDDouble, set_point_gps: float, gps: float, DT: float): # *********** Double PID Outer Begin (Roll and Pitch Angular Position Control) *************# axis.OUT.reference = set_point_gps # Set point of outer PID control axis.OUT.meas_value = gps # P calculation axis.OUT.error = axis.OUT.reference - axis.OUT.meas_value # Define error of outer loop axis.OUT.p_result = axis.OUT.error * axis.OUT.kp # Calculate P result of outer loop # I calculation axis.OUT.error_sum = axis.OUT.error_sum + axis.OUT.error * DT # Define summation of outer loop OUT_ERR_SUM_MAX = 500 OUT_ERR_SUM_MIN = -OUT_ERR_SUM_MAX if axis.OUT.error_sum > OUT_ERR_SUM_MAX: axis.OUT.error_sum = OUT_ERR_SUM_MAX elif axis.OUT.error_sum < OUT_ERR_SUM_MIN: axis.OUT.error_sum = OUT_ERR_SUM_MIN axis.OUT.i_result = axis.OUT.error_sum * axis.OUT.ki # Calculate I result of outer loop # D calculation axis.OUT.error_deriv = -(axis.OUT.meas_value - axis.OUT.meas_value_prev ) / DT # Define derivative of outer loop axis.OUT.meas_value_prev = axis.OUT.meas_value OUTER_DERIV_FILT_ENABLE = True if not OUTER_DERIV_FILT_ENABLE: axis.OUT.d_result = axis.OUT.error_deriv * axis.OUT.kd # Calculate D result of outer loop else: axis.OUT.error_deriv_filt = axis.OUT.error_deriv_filt * 0.4 + axis.OUT.error_deriv * 0.6 # filter for derivative axis.OUT.d_result = axis.OUT.error_deriv_filt * axis.OUT.kd # Calculate D result of inner loop axis.OUT.pid_result = axis.OUT.p_result + axis.OUT.i_result + axis.OUT.d_result # Calculate PID result of outer loop # ************ Double PID Inner Begin (Roll and Pitch Angular Rate Control) **************# axis.IN.reference = axis.OUT.pid_result # Set point of inner PID control is the PID result of outer loop (for double PID control) axis.IN.meas_value = -axis.OUT.error_deriv # P calculation axis.IN.error = axis.IN.reference - axis.IN.meas_value # Define error of inner loop axis.IN.p_result = axis.IN.error * axis.IN.kp # Calculate P result of inner loop # I calculation axis.IN.error_sum = axis.IN.error_sum + axis.IN.error * DT # Define summation of inner loop IN_ERR_SUM_MAX = 500 IN_ERR_SUM_MIN = -IN_ERR_SUM_MAX if axis.IN.error_sum > IN_ERR_SUM_MAX: axis.IN.error_sum = IN_ERR_SUM_MAX elif axis.IN.error_sum < IN_ERR_SUM_MIN: axis.IN.error_sum = IN_ERR_SUM_MIN axis.IN.i_result = axis.IN.error_sum * axis.IN.ki # Calculate I result of inner loop # D calculation axis.IN.error_deriv = -(axis.IN.meas_value - axis.IN.meas_value_prev ) / DT # Define derivative of inner loop axis.IN.meas_value_prev = axis.IN.meas_value # Refresh value_prev to the latest value INNER_DERIV_FILT_ENABLE = True if not INNER_DERIV_FILT_ENABLE: axis.IN.d_result = axis.IN.error_deriv * axis.IN.kd # Calculate D result of inner loop else: axis.IN.error_deriv_filt = axis.IN.error_deriv_filt * 0.5 + axis.IN.error_deriv * 0.5 # filter for derivative axis.IN.d_result = axis.IN.error_deriv_filt * axis.IN.kd # Calculate D result of inner loop axis.IN.pid_result = axis.IN.p_result + axis.IN.i_result + axis.IN.d_result # Calculate PID result of inner loop
class Drone(Thread): def __init__(self, telemetry: Telemetry): super(Drone, self).__init__() self.daemon = True self._exit = False self.telemetry = telemetry self.client: Optional[MultirotorClient] = None self.connect() self.controller = Controller(self.client, self.telemetry) self.collision_type = CollisionType.NONE def run(self): while not self._exit: try: self._process() except BufferError: pass except KeyboardInterrupt: self.shutdown() def _process(self): self.update_telemetry() if self.is_collision(): self.process_collision() elif self.telemetry.collision_mode: self.stop_collision() else: self.controller.check_progress() time.sleep(0.1) def shutdown(self): self._exit = True self.client.enableApiControl(False) self.client.reset() def connect(self): self.client = MultirotorClient() self.client.confirmConnection() self.client.enableApiControl(True) self.client.getBarometerData() # region TELEMETRY def update_telemetry(self): multirotor_state = self.client.getMultirotorState() self.telemetry.landed_state = multirotor_state.landed_state self.telemetry.ned_position = multirotor_state.kinematics_estimated.position self.telemetry.linear_velocity = multirotor_state.kinematics_estimated.linear_velocity self.telemetry.gps_position = self.client.getGpsData().gnss.geo_point self.telemetry.gps_home = self.client.getHomeGeoPoint() self.telemetry.quaternion = self.client.simGetVehiclePose().orientation self.get_front_camera_image() self.get_bottom_camera_image() def get_bottom_camera_image(self): self.client.simSetCameraOrientation('0', Quaternionr(0.0, -0.7, 0.0, 0.5)) bottom_camera_data = self.client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, pixels_as_float=True) ]) self.telemetry.terrain_collision.process_image(bottom_camera_data[0]) def get_front_camera_image(self): self.client.simSetCameraOrientation('0', Quaternionr(0.0, 0.0, 0.0, 1.0)) front_camera_data = self.client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, pixels_as_float=True) ]) self.telemetry.wall_collision.process_image(front_camera_data[0]) # endregion # region COLLISION def is_collision(self): collision = (self.telemetry.wall_collision.collision or self.telemetry.terrain_collision.collision) return collision def process_collision(self): wall_collision = self.telemetry.wall_collision.collision terrain_collision = self.telemetry.terrain_collision.collision if wall_collision: if terrain_collision: self.wall_collision() self.collision_type = CollisionType.BOTH else: self.wall_collision() self.collision_type = CollisionType.WALL else: if terrain_collision: self.terrain_collision() self.collision_type = CollisionType.TERRAIN else: self.collision_type = CollisionType.NONE def wall_collision(self): if (self.collision_type != CollisionType.WALL and self.collision_type != CollisionType.BOTH): self.telemetry.collision_mode = True self.client.moveToZAsync(-500, settings.COLLISION_SPEED) def terrain_collision(self): if self.collision_type != CollisionType.TERRAIN: if self.telemetry.ned_position.z_val < -8: self.telemetry.collision_mode = True target = self.telemetry.target_position actual = self.telemetry.ned_position self.client.moveToPositionAsync(target.x_val, target.y_val, actual.z_val - 1, settings.COLLISION_SPEED) def stop_collision(self): self.controller.send_position() self.telemetry.collision_mode = False