is the below code correct? from pymavlink import mavutil import math import time…
- User
is the below code correct? from pymavlink import mavutil import math import time
the_connection = mavutil.mavlink_connection('COM14', baud=57600) the_connection.wait_heartbeat()
msg = the_connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True) master_waypoint = (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)
waypoints = [ master_waypoint, (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10), # Repeat the first waypoint to make the drone return to its starting point ] distance = 5 # Distance in meters angle = 60 # Angle in degrees kp = 0.1 ki = 0.01 kd = 0.05 pid_limit = 0.0001
class Drone:
def __init__(self, system_id, connection): self.system_id = system_id self.connection = connection def set_mode(self, mode): self.connection.mav.set_mode_send( self.system_id, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode ) def arm(self, arm=True): self.connection.mav.command_long_send(self.system_id, self.connection.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, int(arm), 0, 0, 0, 0, 0, 0) def takeoff(self, altitude): self.connection.mav.command_long_send(self.system_id, self.connection.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude) def send_waypoint(self, wp, next_wp, speed): # Print wp and next_wp print("Current waypoint: {} | Next waypoint: {}".format(wp, next_wp)) vx, vy, vz = calculate_velocity_components(wp, next_wp, speed) # Print velocity components print("Velocity components: vx={}, vy={}, vz={}".format(vx, vy, vz)) self.connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message( 10, self.system_id, self.connection.target_component, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, int(0b110111111000), int(wp[0] * 10 ** 7), int(wp[1] * 10 ** 7), wp[2], vx, vy, vz, 0, 0, 0, 0, 0 # Set vx, vy, and vz from calculated components )) def get_position(self): self.connection.mav.request_data_stream_send( self.system_id, self.connection.target_component, mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1, 1) while True: msg = self.connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True) if msg.get_srcSystem() == self.system_id: return (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3) # master_waypoint = (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3) # # waypoints.insert(0, master_waypoint) # return master_waypointclass PIDController:
def __init__(self, kp, ki, kd, limit): self.kp = kp self.ki = ki self.kd = kd self.limit = limit self.prev_error = 0 self.integral = 0 def update(self, error, dt): derivative = (error - self.prev_error) / dt self.integral += error * dt self.integral = max(min(self.integral, self.limit), -self.limit) # Clamp the integral term output = self.kp * error + self.ki * self.integral + self.kd * derivative self.prev_error = error return outputpid_lat = PIDController(kp, ki, kd, pid_limit) pid_lon = PIDController(kp, ki, kd, pid_limit) master_drone = Drone(2, the_connection) follower_drone = Drone(3, the_connection)
def calculate_follower_coordinates(wp, distance, angle): earth_radius = 6371000.0 # in meters latitude_change = (180 * distance * math.cos(math.radians(angle))) / (math.pi * earth_radius) longitude_change = (180 * distance * math.sin(math.radians(angle))) / ( math.pi * earth_radius * math.cos(math.radians(wp[0]))) new_latitude = wp[0] + latitude_change new_longitude = wp[1] + longitude_change print("Calculated follower coordinates: lat={}, lon={}, alt={}".format(new_latitude, new_longitude, wp[2])) return (new_latitude, new_longitude, wp[2])
def calculate_velocity_components(current_wp, next_wp, speed): dx = next_wp[0] - current_wp[0] dy = next_wp[1] - current_wp[1] dz = next_wp[2] - current_wp[2] dx2 = dx ** 2 dy2 = dy ** 2 dz2 = dz ** 2 distance = math.sqrt(dx2 + dy2 + dz2) vx = (dx / distance) * speed vy = (dy / distance) * speed vz = (dz / distance) * speed return vx, vy, vz
def abort(): print("Type 'abort' to return to Launch and disarm motors.") start_time = time.monotonic() while time.monotonic() - start_time < 7: user_input = input("Time left: {} seconds \n".format(int(7 - (time.monotonic() - start_time)))) if user_input.lower() == "abort": print("Returning to Launch and disarming motors…") for drone in [master_drone, follower_drone]: drone.set_mode(6) # RTL mode drone.arm(False) # Disarm motors return True print("7 seconds have passed. Proceeding with waypoint task...") return False
Set mode to Guided and arm both drones
for drone in [master_drone, follower_drone]: drone.set_mode(4) drone.arm() drone.takeoff(10)
to check the curent mode --------------------------------------
Print the current flight mode
mode = the_connection.mode_mapping()[the_connection.flightmode] print("Current mode:", mode)
mode =4
print(f"the drone is currectly at mode {mode}")
time_start = time.time() while mode == 4: if abort(): exit()
# main loop if time.time() - time_start >= 1: for index, master_wp in enumerate(waypoints[:-1]): next_wp = waypoints[index + 1] master_drone.send_waypoint(master_wp, next_wp, speed=3) follower_position = master_drone.get_position() # Print follower position print("Follower position: {}".format(follower_position)) if follower_position is None: for drone in [master_drone, follower_drone]: drone.set_mode(6) drone.arm(False) break follower_wp = calculate_follower_coordinates(follower_position, distance, angle) dt = time.time() - time_start pid_lat_output = pid_lat.update(follower_wp[0] - follower_position[0], dt) pid_lon_output = pid_lon.update(follower_wp[1] - follower_position[1], dt) # Print PID output adjustments print("PID adjustments: lat={}, lon={}".format(pid_lat_output, pid_lon_output)) adjusted_follower_wp = ( follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2]) # Print adjusted follower waypoint print("Adjusted follower waypoint: {}".format(adjusted_follower_wp)) follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=3) if abort(): exit() # mode = the_connection.mode_mapping()[the_connection.flightmode] time.sleep(8) break # set the mode to rtl and disarms the drone for drone in [master_drone, follower_drone]: drone.set_mode(6) drone.arm(False)the_connection.close()
- Assistant
The code seems to be a script for controlling and following waypoints using two drones, receiving their positions, and PID controllers for performing a following behavior accurately. Here are a few suggestions and notes:
-
Double-check the initial waypoints provided in the
waypointslist. Make sure their latitude and longitude are correct. -
The script is using the
COM14serial port to communicate with the drone. Make sure that this port is correct and available on your system. -
Correct the indentation in the line:
mode = the_connection.mode_mapping()[the_connection.flightmode]The line should be indented one level to the right to match the
while mode == 4:loop.- The script has repetitive code for switching to RTL mode and disarming the motors. You can create a function for this purpose:
def return_to_launch_and_disarm(drones): for drone in drones: drone.set_mode(6) # RTL mode drone.arm(False) # Disarm motors # Replace repetitive code with this function call.-
Make sure you have the necessary libraries installed (pymavlink).
-
Ensure that the
kp,ki,kd, andpid_limitvalues are properly tuned for your specific application and drone types. -
Test the script in a safe environment to avoid potential damages or injuries.
Otherwise, the script appears to be correct and functional.
-