sn2_mission_v2.py

import math
from pymavlink import mavutil
import time


class mission_item:
    def __init__(self, l, current, x,y,z):
        self.seq = l
        self.frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
        self.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
        self.current = current
        self.auto = 1
        self.param1 = 0.0
        self.param2 = 2.00
        self.param3 = 20.00
        self.param4 = math.nan
        self.param5 = x
        self.param6 = y
        self.param7 = z
        self.mission_type = 0

def arm(the_connection):
    print("*-- Arming*")

    the_connection.mav.command_long_send(
            the_connection.target_system, 
            the_connection.target_component,
            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
            0,1,0,0,0,0,0,0
            )
    
    ack(the_connection, "COMMAND_ACK")

def takeoff(the_connection):
    print("*-- Takeoff Initiated*")

    the_connection.mav.command_long_send(
            the_connection.target_system, 
            the_connection.target_component,
            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
            0,0,0,0,math.nan,0,0,10)
    
    ack(the_connection, "COMMAND_ACK")

def upload_mission(the_connection, mission_items):
    n = len(mission_items)
    print("*-- Sending Message Out*")

    the_connection.mav.mission_count_send(
            the_connection.target_system, 
            the_connection.target_component,
            n,0)
    
    ack(the_connection, "MISSION_REQUEST")

    for waypoint in mission_items:
        print("*-- Creating a waypoint*")

        the_connection.mav.mission_item_send(
            the_connection.target_system, 
            the_connection.target_component,
            waypoint.seq,
            waypoint.frame,
            waypoint.command,
            waypoint.current,
            waypoint.auto,
            waypoint.param1,
            waypoint.param2,
            waypoint.param3,
            waypoint.param4,
            waypoint.param5,
            waypoint.param6,
            waypoint.param7,
            waypoint.mission_type
        )

    if waypoint != mission_items[n-1]:
        ack(the_connection, "MISSION_REQUEST")

    ack(the_connection, "MISSION_ACK")


def set_return(the_connection):
    print("*-- Set Return To Launch*")

    the_connection.mav.command_long_send(
            the_connection.target_system, 
            the_connection.target_component,
            mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
            0,0,0,0,0,0,0,0)
    
    ack(the_connection, "COMMAND_ACK")


def start_mission(the_connection):
    print("*-- Mission Start*")

    the_connection.mav.command_long_send(
            the_connection.target_system, 
            the_connection.target_component,
            mavutil.mavlink.MAV_CMD_MISSION_START,
            0,0,0,0,0,0,0,0)
    
    ack(the_connection, "COMMAND_ACK")



def ack(the_connection, keyword):
    print("-- Message Read " + \
          str(the_connection.recv_match(type=keyword, blocking=True))
          )
    
if __name__ == "__main__":
    print("-- Program Started")

    # the_connection = mavutil.mavlink_connection('COM7', 57600) #telemetry
    the_connection = mavutil.mavlink_connection('udpin:172.26.64.1:14550')

    # Wait for the first heartbeat 
    while the_connection.target_system == 0:
        print("-- Checking for heartbeat")
        the_connection.wait_heartbeat()
        print("Heartbeat from system (system %u component %u)" % \
            (the_connection.target_system,
             the_connection.target_component))
    
    mission_waypoints = []

    mission_waypoints.append(mission_item(0, 0, 7.5197185, 4.5203105, 15))
    mission_waypoints.append(mission_item(1, 0, 7.5194571, 4.5214718, 15)) 
    mission_waypoints.append(mission_item(2, 0, 7.5197185, 4.5203105, 15))
    # mission_waypoints.append(mission_item(1, 0, 7.5195278, 4.5211568, 15))
    # mission_waypoints.append(mission_item(2, 0, 7.5183386, 4.5264412, 5))

    upload_mission(the_connection, mission_waypoints)

    arm(the_connection)

    mode = 'GUIDED'

    # Get mode ID
    mode_id = the_connection.mode_mapping()[mode]
    print(mode_id)
    # Set new mode

    the_connection.mav.set_mode_send(
        the_connection.target_system,
        mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
        mode_id)


    # while True:
    #     msg = the_connection.recv_match(type ='HEARTBEAT', blocking = False)
    #     if msg:
    #         mode = mavutil.mode_string_v10(msg)
    #         # print(mode.split()[-1])


    #         if "STABILIZE" in mode:
    #             print("Checking mode")
    #             # Get mode ID
    #             mode_id = the_connection.mode_mapping()["GUIDED"]
    #             print(mode_id)
    #             # Set new mode

    #             the_connection.mav.set_mode_send(
    #                 the_connection.target_system,
    #                 mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
    #                 mode_id)
                
    #             print("Switched mode")
                    
    #             break

    while True:
        msg = the_connection.recv_match(type ='HEARTBEAT', blocking = False)
        if msg:
            mode = mavutil.mode_string_v10(msg)
            print(mode)

            if "GUIDED" in mode:
                print("Taking off")
                takeoff(the_connection)
                break

    while True:
        msg = the_connection.recv_match(type = "GLOBAL_POSITION_INT", blocking = False)
        if msg:
            if msg.relative_alt >= 9950:
                start_mission(the_connection)
                break

    while True:
        msg = the_connection.recv_match(type ='MISSION_CURRENT', blocking = False)
        # print(msg)
        if msg:
            print(msg.seq,msg.total)
            if msg.seq == msg.total:
                set_return(the_connection)
                break



    # time.sleep(5)

    # start_mission(the_connection)

    # # time.sleep(15)

    # # for mission_item in mission_waypoints:
    # #     print("-- Message Read " +
    # #             str(the_connection.recv_match(
    # #                 type="MISSION_ITEM_REACHED",
    # #                 condition= f"MISSION_ITEM_REACHED.seq == {mission_item.seq}",
    # #                 blocking=True)))
        
    # set_return(the_connection)

    # the_connection.mav.mission_set_current_send(the_connection.target_system, the_connection.target_component, 3)

# 172.26.64.1
# 14550