import cv2 as cv import robotpy_apriltag as apriltag from pymavlink import mavutil import time import math class DroneController: def __init__(self, connection_string=f"/dev/serial0", baud=57600): """ Inizializza la connessione al drone connection_string: stringa di connessione (UDP, TCP, serial, etc.) """ self.master = mavutil.mavlink_connection(connection_string, baud=baud) self.master.wait_heartbeat() print("Connessione stabilita con il drone") # Abilita il controllo di posizione self.master.arducopter_arm() self.master.motors_armed_wait() print("Drone armato") def get_current_position(self): """ Ottiene la posizione corrente del drone """ msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5) if msg: return { 'lat': msg.lat / 1e7, 'lon': msg.lon / 1e7, 'alt': msg.alt / 1000.0, 'relative_alt': msg.relative_alt / 1000.0 } return None def get_current_attitude(self): """ Ottiene l'orientamento corrente del drone """ msg = self.master.recv_match(type='ATTITUDE', blocking=True, timeout=5) if msg: return { 'roll': math.degrees(msg.roll), 'pitch': math.degrees(msg.pitch), 'yaw': math.degrees(msg.yaw) } return None def Direction(self, x): """ Ruota il drone rispetto all'asse verticale (yaw) x: angolo in gradi da ruotare (positivo = senso orario, negativo = antiorario) """ try: # Ottieni l'orientamento corrente current_attitude = self.get_current_attitude() if not current_attitude: print("Errore: impossibile ottenere l'orientamento corrente") return False # Calcola il nuovo yaw current_yaw = current_attitude['yaw'] new_yaw = (current_yaw + x) % 360 print(f"Rotazione di {x}° - Yaw corrente: {current_yaw:.2f}° -> Nuovo yaw: {new_yaw:.2f}°") # Invia comando di rotazione self.master.mav.set_attitude_target_send( 0, # time_boot_ms self.master.target_system, self.master.target_component, 0b00000100, # type_mask (ignora roll, pitch, thrust - controlla solo yaw) mavutil.mavlink.mavlink_quaternion_from_euler(0, 0, math.radians(new_yaw)), 0, # roll rate 0, # pitch rate math.radians(x/2), # yaw rate (velocità di rotazione) 0 # thrust ) # Attendi che la rotazione sia completata time.sleep(abs(x) / 30.0) # Stima del tempo basata sulla velocità di rotazione print(f"Rotazione completata") return True except Exception as e: print(f"Errore durante la rotazione: {e}") return False def Delta(self, z=0, x=0, y=0): """ Sposta il drone delle coordinate specificate (movimento relativo)\n x: spostamento sinistra/destra (metri, positivo = destra)\n y: spostamento su/giù (metri, positivo = giù)\n z: spostamento in avanti/indietro (metri, positivo = avanti) """ y = -y # Inverti l'asse y per allinearlo alla convenzione di MAVLink (positivo = su) try: # Ottieni posizione corrente current_pos = self.get_current_position() if not current_pos: print("Errore: impossibile ottenere la posizione corrente") return False print(f"Spostamento richiesto: x={z}m, y={x}m, z={y}m") print(f"Posizione corrente: lat={current_pos['lat']:.7f}, lon={current_pos['lon']:.7f}, alt={current_pos['relative_alt']:.2f}m") # Calcola le nuove coordinate # Conversione approssimativa: 1 grado di latitudine ≈ 111320 metri # 1 grado di longitudine ≈ 111320 * cos(latitudine) metri lat_offset = z / 111320.0 lon_offset = x / (111320.0 * math.cos(math.radians(current_pos['lat']))) new_lat = current_pos['lat'] + lat_offset new_lon = current_pos['lon'] + lon_offset new_alt = current_pos['relative_alt'] + y print(f"Nuova posizione target: lat={new_lat:.7f}, lon={new_lon:.7f}, alt={new_alt:.2f}m") # Invia comando di movimento self.master.mav.set_position_target_global_int_send( 0, # time_boot_ms self.master.target_system, self.master.target_component, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0b0000111111111000, # type_mask (posizione e yaw) int(new_lat * 1e7), # lat_int int(new_lon * 1e7), # lon_int new_alt, # alt 0, 0, 0, # vx, vy, vz 0, 0, 0, # afx, afy, afz 0, 0 # yaw, yaw_rate ) # Attendi che il movimento sia completato distance = math.sqrt(z*z + x*x + y*y) estimated_time = distance / 2.0 # Stima del tempo basata sulla distanza time.sleep(estimated_time) print(f"Movimento completato") return True except Exception as e: print(f"Errore durante il movimento: {e}") return False def set_mode(self, mode): """ Imposta la modalità di volo del drone """ mode_id = self.master.mode_mapping()[mode] self.master.mav.set_mode_send( self.master.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id ) print(f"Modalità impostata su: {mode}") def takeoff(self, altitude=10): """ Decollo del drone """ self.set_mode('GUIDED') time.sleep(1) self.master.mav.command_long_send( self.master.target_system, self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude ) print(f"Decollo a {altitude}m iniziato") def land(self): """ Atterraggio del drone """ self.master.mav.command_long_send( self.master.target_system, self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0, 0, 0 ) print("Atterraggio iniziato") def emergency_stop(self): """ Arresto di emergenza """ self.master.arducopter_disarm() print("ARRESTO DI EMERGENZA - MOTORI DISARMATI") # Crea il detector detector = apriltag.AprilTagDetector() detector.clearFamilies() detector.addFamily("tag36h11") # Parametri della camera fx = 285.79 # distanza focale x fy = 293.63 # distanza focale y cx = 164.24 # punto principale x cy = 130.10 # punto principale y # Grandezza dell'AprilTag in metri tag_size = 0.033 # Crea il pose estimator pose_estimator = apriltag.AprilTagPoseEstimator( apriltag.AprilTagPoseEstimator.Config( tagSize=tag_size, fx=fx, fy=fy, cx=cx, cy=cy ) ) # Stream video STREAM_URL = "http://192.168.4.1/stream" capture = cv.VideoCapture(STREAM_URL) capture.set(cv.CAP_PROP_FPS, 30) if not capture.isOpened(): print("Impossibile aprire lo stream") exit() # Controller del drone try: """# Crea l'istanza del controller drone = DroneController("udp:192.168.4.4") time.sleep(2) # Decollo a 1.5m da terra drone.takeoff(1.5) time.sleep(2)""" last = apriltag.AprilTagDetection.Point() # Loop principale while True: ret, frame = capture.read() if not ret: continue image = cv.cvtColor(frame, cv.COLOR_BGR2GRAY) # Trova i tag detections = detector.detect(image) # Disegna i contorni del tag for detection in detections: corners = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) corners = detection.getCorners(corners) corner_points = [ (int(corners[0]), int(corners[1])), # Angolo 1 (int(corners[2]), int(corners[3])), # Angolo 2 (int(corners[4]), int(corners[5])), # Angolo 3 (int(corners[6]), int(corners[7])) # Angolo 4 ] for i in range(4): pt1 = corner_points[i] pt2 = corner_points[(i+1) % 4] cv.line(frame, pt1, pt2, (0, 255, 0), 2) center = detection.getCenter() if abs(center.x - last.x) > .35: continue if abs(center.y - last.y) > .35: continue cv.circle(frame, (int(center.x), int(center.y)), 5, (0, 0, 255), -1) pose = pose_estimator.estimate(detection) # Ottieni rotazione e traslazione translation = pose.translation() rotation = pose.rotation() print(f"Translation (x, y, z): [{translation.x}, {translation.y}, {translation.z}]") """drone.Delta(translation[0], translation[1], translation[2])""" print(f"Rotation (x, y, z): [{rotation.x}, {rotation.y}, {rotation.z-.10}]") """drone.Direction(math.radians(rotation[0]))""" cv.imshow("AprilTag Detection", frame) key = cv.waitKey(33) if key == 27: print("Uscita manuale") break except KeyboardInterrupt: print("\nInterruzione da tastiera") """drone.land()""" except Exception as e: print(f"Errore: {e}") if 'drone' in locals(): """drone.emergency_stop()""" # Chiudi tutto capture.release() cv.destroyAllWindows()