the below code is not working from pymavlink import mavutil import math import time…
- User
the below code is not working from pymavlink import mavutil import math import time
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): vx, vy, vz = calculate_velocity_components(wp, next_wp, speed) 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) def get_battery_status(self): msg = self.connection.recv_match(type='SYS_STATUS', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.battery_remain def get_rc_status(self): msg = self.connection.recv_match(type='RC_CHANNELS_RAW', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.chan8_raw def get_gps_status(self): msg = self.connection.recv_match(type='GPS_RAW_INT', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.satellites_visible, msg.ephclass 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 outputdistance = 5 # Distance in meters angle = 60 # Angle in degrees kp = 0.1 ki = 0.01 kd = 0.05 pid_limit = 0.0001 pid_lat = PIDController(kp, ki, kd, pid_limit) pid_lon = PIDController(kp, ki, kd, pid_limit)
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 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 check_safety(master_drone, follower_drone): # 1. Check battery levels master_battery = master_drone.get_battery_status() follower_battery = follower_drone.get_battery_status() if master_battery is not None and master_battery < 25 or follower_battery is not None and follower_battery < 25: print("Battery low!") return False
# 2. Check RC fail master_rc_status = master_drone.get_rc_status() follower_rc_status = follower_drone.get_rc_status() if master_rc_status is not None and master_rc_status < 1000 or follower_rc_status is not None and follower_rc_status < 1000: print("RC fail!") return False # 3. Check GPS fail master_gps_status = master_drone.get_gps_status() follower_gps_status = follower_drone.get_gps_status() if master_gps_status is not None and master_gps_status[0] < 6 or follower_gps_status is not None and follower_gps_status[0] < 6 or master_gps_status is not None and master_gps_status[1] > 200 or follower_gps_status is not None and follower_gps_status[1] > 200: print("GPS fail!") return False return Truewaypoints = [ (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10) ]
the_connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
master_drone = Drone(3, the_connection) follower_drone = Drone(2, the_connection)
Set mode to Guided and arm both drones
for drone in [master_drone, follower_drone]: drone.set_mode(4) drone.arm() drone.takeoff(10)
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
mode = the_connection.mode_mapping()[the_connection.flightmode] print(f"the drone is currectly at mode {mode}")
time_start = time.time() while mode == 4: if abort(): exit() # Keep checking safety safeguards if not check_safety(master_drone, follower_drone): break
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() if follower_position is None: 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) adjusted_follower_wp = ( follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2]) follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=3) else: mode_mapping = the_connection.mode_mapping() current_mode = the_connection.flightmode mode = mode_mapping[current_mode] time_start = time.time() time.sleep(0.1) continue break for drone in [master_drone, follower_drone]: drone.set_mode(6) drone.arm(False)the_connection.close()
- Assistant
The code seems to be correct in terms of syntax and logic. However, there may be a few issues that you need to verify.
-
Make sure that the drones you are using support the MAVLink protocol and are compatible with the pymavlink library.
-
Double-check your MAVLink connection settings. The code provided is using the following settings:
/dev/ttyUSB0for the port and57600for the baudrate. Ensure these settings are correct for your hardware setup. Adjust them accordingly. -
Verify the ArduPilot flight modes corresponding to the number in the
set_mode()function calls. In the provided code, the flight modes used are:
- 4: Guided mode is set when
drone.set_mode(4)is called. - 6: Return to Launch (RTL) mode is set when
drone.set_mode(6)is called.
Make sure these match the flight modes supported by your drone hardware.
-
You can try printing more status messages throughout the code to figure out which part exactly is not working as expected. For example, print the obtained positions, waypoint calculations, PID control adjustments, etc., to track the code execution flow.
-
Additionally, there might be dependency issues or bugs in the libraries being used. Ensure that you have installed the latest versions of the required packages.
-
Lastly, consider testing your hardware components (such as GPS and RC) separately to eliminate the possibility of a hardware malfunction.
Keep in mind that it's not always easy to troubleshoot code without additional context and information about the hardware setup. If none of the above suggestions help, consider providing more information about your hardware configuration, any specific error messages you've encountered, or clarifying which part of the code is not working as expected.
-
- User
give me the code with these print the obtained positions, waypoint calculations, PID control adjustments, etc., to track the code execution flow.
- Assistant
Here's the code with additional print statements for better tracking of the execution flow:
# ... class Drone: # ... 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( # ... def calculate_follower_coordinates(wp, distance, angle): # ... 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]) # ... while mode == 4: if abort(): exit() if not check_safety(master_drone, follower_drone): break 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: 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) else: # ... time_start = time.time() time.sleep(0.1) continue break # ...Remember that adding many print statements could slow down the execution of the code, so use them for debugging purposes only, and consider removing them when the code works as expected.
- User
is the below code correct? from pymavlink import mavutil import math import time
the_connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)
waypoints = [ (28.5861474, 77.3421320, 10), (28.5859040, 77.3420736, 10) ]
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) def get_battery_status(self): msg = self.connection.recv_match(type='SYS_STATUS', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.battery_remain def get_rc_status(self): msg = self.connection.recv_match(type='RC_CHANNELS_RAW', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.chan8_raw def get_gps_status(self): msg = self.connection.recv_match(type='GPS_RAW_INT', blocking=False) if msg and msg.get_srcSystem() == self.system_id: return msg.satellites_visible, msg.ephclass 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)
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 check_safety(master_drone, follower_drone): # 1. Check battery levels master_battery = master_drone.get_battery_status() follower_battery = follower_drone.get_battery_status() if master_battery is not None and master_battery < 25 or follower_battery is not None and follower_battery < 25: print("Battery low!") return False
# 2. Check RC fail master_rc_status = master_drone.get_rc_status() follower_rc_status = follower_drone.get_rc_status() if master_rc_status is not None and master_rc_status < 1000 or follower_rc_status is not None and follower_rc_status < 1000: print("RC fail!") return False # 3. Check GPS fail master_gps_status = master_drone.get_gps_status() follower_gps_status = follower_drone.get_gps_status() if master_gps_status is not None and master_gps_status[0] < 6 or follower_gps_status is not None and follower_gps_status[0] < 6 or master_gps_status is not None and master_gps_status[1] > 200 or follower_gps_status is not None and follower_gps_status[1] > 200: print("GPS fail!") return False return Truemaster_drone = Drone(3, the_connection) follower_drone = Drone(2, the_connection)
Set mode to Guided and arm both drones
for drone in [master_drone, follower_drone]: drone.set_mode(4) drone.arm() drone.takeoff(10)
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
mode = the_connection.mode_mapping()[the_connection.flightmode] print(f"the drone is currectly at mode {mode}")
time_start = time.time() while mode == 4: if abort(): exit() # Keep checking safety safeguards if not check_safety(master_drone, follower_drone): break
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: 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=2) else: mode_mapping = the_connection.mode_mapping() current_mode = the_connection.flightmode mode = mode_mapping[current_mode] time_start = time.time() time.sleep(0.1) continue break for drone in [master_drone, follower_drone]: drone.set_mode(6) drone.arm(False)the_connection.close()
- Assistant
Yes, the code you provided now includes the print statments I suggested previously:
- Printing current waypoint and the next waypoint
- Printing velocity components: vx, vy, vz
- Printing calculated follower coordinates
- Printing follower position
- Printing PID adjustments
- Printing adjusted follower waypoint
These print statements will help you track the execution flow and understand how the drone is performing, making it easier to debug any issues during flight. Please note that having many print statements could potentially slow down the execution of the code, so they should be used carefully and only for debugging purposes.