投稿
  1. 首页
  2. python

无人机编程donekit及通讯

code • 2022-5-6 • python • 阅读 31

无人机编程donekit及通讯,第1张

1、启动SITL 
启动STL
cd courseRoot/apm/ardupilot/
sim_vehicle.py -v ArduCopter --console --map

飞机起飞降落

mode GUIDED
arm throttle
takeoff 10
mode LAND
2、连接地面端 
cd /courseRoot/src
./QGroundControl.AppImage
3、连接内部STIL脚本
  • 打开SITL sim_vehicle.py -v ArduCopter

可以看到STIL提供两个对外接口 127.0.0.1：14550 和14551用来连接python脚本

  • python连接STIL脚本
from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException
import time
import socket
import exceptions
import math
import argparse
 
#####FUNCTIONS#####
 
def connectMyCopter():
 
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
    connection_string = args.connect
 
    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()
 
    vehicle = connect(connection_string, wait_ready=True)
 
    return vehicle
 
#####MAIN EXECUTABLE#####
 
vehicle = connectMyCopter()

python connection_template.py --connect 127.0.0.1:14550

出现上图说明连接成功

  • 起飞脚本
from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException
import time
import socket
import exceptions
import math
import argparse
 
def connectMyCopter():
 
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
 
    connection_string = args.connect
     
    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()
 
    vehicle = connect(connection_string, wait_ready=True)
 
    return vehicle
 
def arm_and_takeoff(targetHeight):
    while vehicle.is_armable!=True:
        print("Waiting for vehicle to become aramable")
        time.sleep(1)
    print("Vehicle is now armable")
 
    vehicle.mode = VehicleMode("GUIDED")
 
    while vehicle.mode!="GUIDED":
        print("Waiting for drone to enter GUIDED flight mode")
        time.sleep(1)
    print("Vehicle now in GUIDED MODE. Have fun!!")
 
    vehicle.armed = True
    while vehicle.armed==False:
        print("Waiting for drone to become armed")
        time.sleep(1)
    print("Look out! Virtual props are spinning!")
 
    vehicle.simple_takeoff(targetHeight) ##meters
 
    while True:
        print("Current Altitude:", vehicle.location.global_relative_frame.alt)
        if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
            break
        time.sleep(1)
    print("Target altitude reached!!")
    return None
 
vehicle = connectMyCopter()
arm_and_takeoff(10)

 起飞成功

4、 连接外部SITL脚本

写bash文件放到bin目录下  sudo chmod 777 launchSilt

#!/bin/bash
 
kill -9 $(ps -eF | grep QG | awk -F ' ' '{print $2}')>/dev/null 2>&1
kill -9 $(ps -eF | grep ardu | awk -F ' ' '{print $2}')>/dev/null 2>&1
kill -9 $(ps -eF | grep mav | awk -F ' ' '{print $2}')>/dev/null 2>&1
kill -9 $(ps -eF | grep apm | awk -F ' ' '{print $2}')>/dev/null 2>&1
 
 
#########
 
## Launch a Sitl drone
/home/zhao/courseRoot/apm/ardupilot/build/sitl/bin/arducopter -S -I0 --home 31.88763137,118.814176,13,0 --model "+" --speedup 1 --defaults $apm/ardupilot/Tools/autotest/default_params/copter.parm&
 
sleep 5
 
## Launch QGC
/home/zhao/courseRoot/src/QGroundControl.AppImage 2>/dev/null&
 
sleep 5
 
## Start MAVProxy
screen -dm mavproxy.py --master=tcp:127.0.0.1:5760 --out=127.0.0.1:14550 --out=127.0.0.1:5762
 
##Launch the dronekit-python script
/usr/bin/python "$1" --connect 127.0.0.1:5762
 
function finish {
    kill -9 $(ps -eF | grep QG | awk -F ' ' '{print $2}')>/dev/null 2>&1
    kill -9 $(ps -eF | grep ardu | awk -F ' ' '{print $2}')>/dev/null 2>&1
    kill -9 $(ps -eF | grep mav | awk -F ' ' '{print $2}')>/dev/null 2>&1
    kill -9 $(ps -eF | grep apm | awk -F ' ' '{print $2}')>/dev/null 2>&1
}
 
trap finish EXIT
------------------------------保存并退出
chmod +x launchSitl
sudo cp launchSitl /usr/local/bin/launchSitl

launchSitl basic_template.py

 launchSitl auto_mission.py

from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException, Command
import time
import socket
import exceptions
import math
import argparse
from pymavlink import mavutil
 
def connectMyCopter():
 
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
 
    connection_string = args.connect
     
    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()
 
    vehicle = connect(connection_string, wait_ready=True)
 
    return vehicle
 
def arm_and_takeoff(targetHeight):
    while vehicle.is_armable!=True:
        print("Waiting for vehicle to become aramable")
        time.sleep(1)
    print("Vehicle is now armable")
 
    vehicle.mode = VehicleMode("GUIDED")
 
    while vehicle.mode!="GUIDED":
        print("Waiting for drone to enter GUIDED flight mode")
        time.sleep(1)
    print("Vehicle now in GUIDED MODE. Have fun!!")
 
    vehicle.armed = True
    while vehicle.armed==False:
        print("Waiting for drone to become armed")
        time.sleep(1)
    print("Look out! Virtual props are spinning!")
 
    vehicle.simple_takeoff(targetHeight) ##meters
 
    while True:
        print("Current Altitude:", vehicle.location.global_relative_frame.alt)
        if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
            break
        time.sleep(1)
    print("Target altitude reached!!")
    return None
 
vehicle = connectMyCopter()
 
 
wphome = vehicle.location.global_relative_frame
 
cmd1 = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,wphome.lat,wphome.lon,wphome.alt)
cmd2 = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,31.88663137,118.812176,25)
cmd3 = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,31.88563137,118.812876,25)
cmd4 = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,0,0,0,0,0,0,0,0,0)
 
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
 
cmds.clear()
 
cmds.add(cmd1)
cmds.add(cmd2)
cmds.add(cmd3)
cmds.add(cmd4)
 
vehicle.commands.upload()
 
arm_and_takeoff(10)
 
print("after arm and takeoff")
vehicle.mode = VehicleMode("AUTO")
while vehicle.mode != "AUTO":
    time.sleep(.2)
 
while vehicle.location.global_relative_frame.alt > 2:
    print("Drone is executing mission, but we can still run code")
    time.sleep(2)

5、 控制飞行速度

全球坐标系北是正X，东是正Y   下是正Z

无人机坐标系 无人机正方向X  右Y  下Z

要在GUIDED模式下对无人机发送自定义的MAVLink指令，对于多数MAVLink指令，生成相应指令的函数为“小写指令名”+“_encode”

位掩码是一种用来定义状态的一串二进制数字，通过与目标数字串进行逻辑运算来达到控制状态的目的。位掩码可以占用很少资源实现丰富的状态标识。这里给出了三种位掩码，我们需要用表示速度的那个，也就是0b110111000111.

from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException, Command
import time
import socket
import exceptions
import math
import argparse
from pymavlink import mavutil
 
def connectMyCopter():
 
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
 
    connection_string = args.connect
     
    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()
 
    vehicle = connect(connection_string, wait_ready=True)
 
    return vehicle
 
def arm_and_takeoff(targetHeight):
    while vehicle.is_armable!=True:
        print("Waiting for vehicle to become aramable")
        time.sleep(1)
    print("Vehicle is now armable")
 
    vehicle.mode = VehicleMode("GUIDED")
 
    while vehicle.mode!="GUIDED":
        print("Waiting for drone to enter GUIDED flight mode")
        time.sleep(1)
    print("Vehicle now in GUIDED MODE. Have fun!!")
 
    vehicle.armed = True
    while vehicle.armed==False:
        print("Waiting for drone to become armed")
        time.sleep(1)
    print("Look out! Virtual props are spinning!")
 
    vehicle.simple_takeoff(targetHeight) ##meters
 
    while True:
        print("Current Altitude:", vehicle.location.global_relative_frame.alt)
        if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
            break
        time.sleep(1)
    print("Target altitude reached!!")
    return None
 
def send_global_ned_velocity(vx, vy, vz):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,0,0,mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED,
        0b110111000111,
        0,0,0,
        vx,vy,vz,
        0,0,0,
        0,0)
    vehicle.send_mavlink(msg)
    vehicle.flush()
 
def send_local_ned_velocity(vx, vy, vz):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,0,0,mavutil.mavlink.MAV_FRAME_LOCAL_NED,
        0b110111000111,
        0,0,0,
        vx,vy,vz,
        0,0,0,
        0,0)
    vehicle.send_mavlink(msg)
    vehicle.flush()
 
vehicle = connectMyCopter()
arm_and_takeoff(10)
time.sleep(2)
 
i=0
while i<=5:
    send_global_ned_velocity(5,0,0)
    time.sleep(1)
    print("Moving TRUE NORTH")
    i += 1
 
time.sleep(2)
 
i=0
while i<=5:
    send_global_ned_velocity(0,-5,0)
    time.sleep(1)
    print("Moving TRUE WEST")
    i += 1
 
time.sleep(2)
 
i=0
while i<=5:
    send_local_ned_velocity(5,0,0)
    time.sleep(1)
    print("Moving NORTH relative to front of drone")
    i += 1
 
time.sleep(2)
 
i=0
while i<=5:
    send_local_ned_velocity(0,-5,0)
    time.sleep(1)
    print("Moving NORTH relative to front of drone")
    i += 1
 
time.sleep(2)
 
i=0
while i<=5:
    send_local_ned_velocity(0,0,5)
    time.sleep(1)
    print("Moving DOWN")
    i += 1
 
time.sleep(2)
 
i=0
while i<=5:
    send_global_ned_velocity(0,0,-5)
    time.sleep(1)
    print("Moving UP")
    i += 1
 
while True:
    time.sleep(1)
6、 控制飞行方向 
from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException, Command
import time
import socket
import exceptions
import math
import argparse
from pymavlink import mavutil
 
def connectMyCopter():
 
    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()
 
    connection_string = args.connect
     
    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()
 
    vehicle = connect(connection_string, wait_ready=True)
 
    return vehicle
 
def arm_and_takeoff(targetHeight):
    while vehicle.is_armable!=True:
        print("Waiting for vehicle to become aramable")
        time.sleep(1)
    print("Vehicle is now armable")
 
    vehicle.mode = VehicleMode("GUIDED")
 
    while vehicle.mode!="GUIDED":
        print("Waiting for drone to enter GUIDED flight mode")
        time.sleep(1)
    print("Vehicle now in GUIDED MODE. Have fun!!")
 
    vehicle.armed = True
    while vehicle.armed==False:
        print("Waiting for drone to become armed")
        time.sleep(1)
    print("Look out! Virtual props are spinning!")
 
    vehicle.simple_takeoff(targetHeight) ##meters
 
    while True:
        print("Current Altitude:", vehicle.location.global_relative_frame.alt)
        if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
            break
        time.sleep(1)
    print("Target altitude reached!!")
    return None
 
def condition_yaw(degrees, relative):
    if relative:
        is_relative = 1
    else:
        is_relative = 0
    msg = vehicle.message_factory.command_long_encode(
        0,0,
        mavutil.mavlink.MAV_CMD_CONDITION_YAW,
        0,
        degrees,
        0,1,
        is_relative,
        0,0,0)
    vehicle.send_mavlink(msg)
    vehicle.flush()
 
vehicle = connectMyCopter()
arm_and_takeoff(10)
condition_yaw(30,1)
time.sleep(7)
condition_yaw(0,0)
time.sleep(7)
condition_yaw(270,0)
while True:
    time.sleep(1)

 参考

http://www.lxshaw.com/tech/drone/2021/04/13/%e6%97%a0%e4%ba%ba%e6%9c%ba%e7%bc%96%e7%a8%8b%e5%85%a5%e9%97%a8%ef%bc%88%e4%b9%9d%ef%bc%89%ef%bc%9agazebo%e7%ae%80%e4%bb%8b%e3%80%81%e5%ae%89%e8%a3%85%e4%b8%8e%e6%b5%8b%e8%af%95/

欢迎分享，转载请注明来源：内存溢出

原文地址: http://outofmemory.cn/langs/798459.html

赞 (0)
打赏 微信扫一扫 微信扫一扫 支付宝扫一扫 支付宝扫一扫
code code 管理员组
0 0
上一篇 2022-05-06
下一篇 2022-05-06

发表评论

登录后才能评论

评论列表（0条）

保存