无人机编程donekit及通讯(三)——仿真

桦树无泪

已于 2022-05-07 14:49:39 修改

阅读量2.4k

点赞数 1

分类专栏：无人机协同文章标签： python 开发语言

于 2022-05-02 23:45:59 首次发布

本文链接：https://blog.csdn.net/HUASHUDEYANJING/article/details/124547086

版权

无人机协同专栏收录该内容

7 篇文章 7 订阅

订阅专栏

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)

起飞成功

python basic_template.py --connect 127.0.0.1:14550

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

在GUIDED模式下操控无人机就像你在手控制无人机一样，需要你不断地向无人机发送指令来操控无人机。而在AUTO模式下，你可以直接将你的飞行计划上传到无人机（也是通过MAVLink），这样无人机可以持续执行飞行计划，而不需要一直发送信息。所以AUTO模式非常适合你要让无人机执行一些自动化任务时。而GUIDED模式比较适合你需要让无人机根据环境来改变飞行状态等动态性较高的任务，比如精准降落等。

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 send_global_ned_velocity(vx, vy, vz):

无人机坐标系无人机正方向X 右Y 下Z send_local_ned_velocity(vx, vy, vz):

要在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/

桦树无泪

关注

1
点赞
踩
16

收藏

觉得还不错? 一键收藏
打赏
1
评论
无人机编程donekit及通讯(三)——仿真

1、启动SITL启动STLcd courseRoot/apm/ardupilot/sim_vehicle.py -v ArduCopter --console --map飞机起飞降落mode GUIDEDarm throttletakeoff 10mode LAND2、连接地面端cd /courseRoot/src./QGroundControl.AppImage 3、编写脚本打开SITL sim_vehicle.py -v ArduCopter可
复制链接

扫一扫