When running python code dronekit/pymavlink

Blue Robotics Software QGroundControl
, ,
1

I’m having difficulty in running my python code. Does anyone know why l lose communication everytime I run the code currently im using an Ardusub SITL simulator using the QGC. might be really helpful if you guys could help into solving this issue.

im trying to use a simple script to move the ROV foward and backwards:

from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
import math
#grab distance function
from DistanceFunc import get_distance_meters


# Connect to the vehicle (replace with the appropriate connection string)
connection_string = 'udp:127.0.0.1:14550'
vehicle = connect(connection_string, wait_ready=False)

# Arm and takeoff to a specified depth
def arm_and_takeoff(depth):
    print("Arming motors")
    vehicle.mode = VehicleMode("STABILIZE")  # Set mode to STABILIZE
    vehicle.armed = True  # Arm the vehicle

    while not vehicle.armed:
        print("Waiting for arming...")
        time.sleep(1)

    print("Taking off")
    vehicle.simple_takeoff(depth)  # Takeoff to the specified depth

    # Wait until the vehicle reaches the target depth
    while True:
        current_depth = vehicle.location.global_relative_frame.alt
        if current_depth >= depth * 0.95:  # Allow for a 5% margin
            print("Reached target depth")
            break
        time.sleep(1)

# Move forward for a given distance
def move_forward(distance):
    print("Moving forward")
    current_location = vehicle.location.global_relative_frame

    target_location = LocationGlobalRelative(
        current_location.lat + (distance / (111111.0 * math.cos(current_location.lat))),
        current_location.lon,
        current_location.alt
    )
    vehicle.simple_goto(target_location)

    # Wait until the vehicle reaches the target location
    while vehicle.mode.name == "GUIDED":
        remaining_distance = get_distance_meters(vehicle.location.global_frame, target_location)
        if remaining_distance <= 1:
            print("Reached target location")
            break
        time.sleep(1)

# Move backward for a given distance
def move_backward(distance):
    print("Moving backward")
    current_location = vehicle.location.global_relative_frame

    target_location = LocationGlobalRelative(
        current_location.lat - (distance / (111111.0 * math.cos(current_location.lat))),
        current_location.lon,
        current_location.alt
    )
    vehicle.simple_goto(target_location)

    # Wait until the vehicle reaches the target location
    while vehicle.mode.name == "GUIDED":
        remaining_distance = get_distance_meters(vehicle.location.global_frame, target_location)
        if remaining_distance <= 1:
            print("Reached target location")
            break
        time.sleep(1)

# Descend to a specified depth
def descend(depth):
    print("Descending")
    vehicle.simple_goto(vehicle.location.global_frame, depth)

    # Wait until the vehicle reaches the target depth
    while vehicle.mode.name == "GUIDED":
        current_depth = vehicle.location.global_relative_frame.alt
        if current_depth <= depth * 1.05:  # Allow for a 5% margin
            print("Reached target depth")
            break
        time.sleep(1)

# Ascend to the surface
def ascend():
    print("Ascending")
    vehicle.mode = VehicleMode("STABILIZE")  # Set mode to STABILIZE
    vehicle.simple_takeoff(0)  # Ascend to an altitude of 0

    # Wait until the vehicle reaches the surface
    while vehicle.location.global_relative_frame.alt > 0.2:
        time.sleep(1)

# Main program
if __name__ == '__main__':
    try:
        # Arm and takeoff to a depth of 5 meters
        arm_and_takeoff(5)

        # Move forward
        # Move forward
        move_forward(10)  # Move forward 10 meters

        # Move backward
        move_backward(5)  # Move backward 5 meters

        # Descend to a depth of 8 meters
        descend(8)

        # Ascend to the surface
        ascend()

        # Land and disarm the vehicle
        print("Landing")
        vehicle.mode = VehicleMode("LAND")
        while vehicle.armed:
            time.sleep(1)

        print("Disarming")
        vehicle.armed = False

    except KeyboardInterrupt:
        print("Keyboard interrupt detected. Stopping the script.")

    finally:
        # Close the connection to the vehicle
        vehicle.close()
2

Hi @GabrielWS, welcome to the forum :slight_smile:

I’m not familiar with dronekit, but a few pointers from my reading of your code:

  • your code seems to treat depth as a positive value, but is seemingly getting it directly from the vehicle’s altitude measurement, in which case depth below the surface should be a negative number (which may also be relevant for the input to the simple_takeoff function, if that’s designed for aerial vehicles)
  • ArduSub does not have a “LAND” flight mode
    • Perhaps “SURFACE” mode would be more appropriate there?
  • It doesn’t seem like your code is sending regular HEARTBEAT messages to the vehicle (unless dronekit does that automatically in a background thread?), so the vehicle may be automatically disarming due to the heartbeat failsafe (depending on how that’s configured)

That said, I wouldn’t expect any of those issues to cause a loss of communication - rather just no control. More generally I’m not sure how you could actually lose communication with a simulated vehicle unless you severed the link yourself, or did something that caused the simulation to crash.