Skip to content
TheChatArchiveThe Chat Archive
gpt-4-0314· openaiHugging Face Suzhen/CodeChat

is the below code correct? from pymavlink import mavutil import math import time class…

  1. User

    is the below code correct? 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)
    

    class 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 output
    

    distance = 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

    waypoints = [ (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()

    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()

  2. Assistant

    The provided code appears to be mostly correct. It initializes a Drone class, sets up some functions for calculating follower waypoints, velocity components, and PID control, and moves through a set of waypoints in the waypoints list. However, there are some potential issues and improvements that could be made:

    1. In the abort() function, when checking user input for aborting, it only takes the user input at every second. It might be better to use a non-blocking input and check continuously for user input.

    2. Make sure you have a proper signal handler to handle any keyboard interrupts (Ctrl+C) while your code is running. This will allow you to safely exit the program and ensure the drones are in a safe state (disarmed, RTL mode).

    3. In the calculate_follower_coordinates function, make sure the angle is given in the desired frame (North, East, Down or body frame). If it's a relative angle, then you might need to add the drone's heading angle to the input angle.

    4. The main loop at the end runs the waypoint task only when the drone mode is 4 (Guided mode). If the drone mode changes for any reason, you might need to handle it correctly to ensure safety.

    5. When no more waypoints are left, the mode should be set to RTL (Return to Launch) and the motors disarmed, but the loop should break out whenever follower_position is None. Instead, consider handling the case where the follower_position is not available.

    Lastly, I would recommend testing this code thoroughly in a simulator before attempting any real-world flights. You can use a simulator like Gazebo with ArduPilot SITL (Software In The Loop) to test your code in a safe and controlled environment.

  3. User

    in the above code, please add the following things

    1. add mission achnop=ledgement
    2. add safety features
    3. add rc fail check
    4. add battery check
    5. add gps fail check

    all the baove in the main loop. the main loop should work only of the abopve conditions are met

  4. Assistant

    Here is the modified code with the requested functionalities:

    # (Imports and class definitions)
    
    class Drone:
        # (Existing class definition)
    
        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.eph
    
    # constants and other code
    
    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 True
    
    # main code
    
    while mode == 4:
        if abort():
            exit()
    
        # Keep checking safety safeguards
        if not check_safety(master_drone, follower_drone):
            break
    
        # (rest of the while loop)
    
    for drone in [master_drone, follower_drone]:
        drone.set_mode(6)  # RTL mode
        drone.arm(False)   # Disarm motors
    
    the_connection.close()
    

    The above code adds the following functionalities:

    1. Mission acknowledgement: Since the code does not use autonomous mission waypoints like with MAV_CMD_MISSION_START, explicit mission acknowledgement is not required.

    2. Safety features: The function check_safety checks the status of battery, RC, and GPS issues.

    3. RC fail check: The get_rc_status method checks radio channel 8 for fail conditions. You can change the channel number based on your desired configuration.

    4. Battery check: The get_battery_status method checks the remaining battery percentage for both drones. If remaining battery is less than 25%, it triggers a safety issue.

    5. GPS fail check: The get_gps_status method checks the number of visible satellites and GPS horizontal dilution of precision (HDOP). If less than 6 satellites are visible or the HDOP is greater than 200, it triggers a safety issue.

    The main loop will only work if all safety conditions are met. If any safety checks fail, the drones will enter RTL mode and disarm motors.