DroneKit-python入门教程教程-SITL仿真-控制飞行
DroneKit-python入门教程教程-示例-控制飞行
说明：
- 介绍如何使用dronekit实现控制飞控 上/下/前/后/左/右 或是 上/下/东/西/南/北
ardupilot平台
- 代码如下：
from pymavlink import mavutil
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_BODY_NED, # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
0b0000111111000111, # type_mask
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # m/s
0, 0, 0, # x, y, z acceleration
0, 0)
for x in range(0,duration):
vehicle.send_mavlink(msg)
time.sleep(1)
connection_string = '127.0.0.1:14540' # Edit to suit your needs.
takeoff_alt = 10
vehicle = connect(connection_string, wait_ready=True)
while not vehicle.is_armable:
time.sleep(1)
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed:
print('Waiting for arming...')
time.sleep(1)
vehicle.simple_takeoff(takeoff_alt) # Take off to target altitude
while True:
print('Altitude: %d' % vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt >= takeoff_alt * 0.95:
print('REACHED TARGET ALTITUDE')
break
time.sleep(1)
# This is the command to move the copter 5 m/s forward for 10 sec.
velocity_x = 0
velocity_y = 5
velocity_z = 0
duration = 10
send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration)
# backwards at 5 m/s for 10 sec.
velocity_x = 0
velocity_y = -5
velocity_z = 0
duration = 10
send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration)
vehicle.mode = VehicleMode("LAND")
- 通过调用send_body_ned_velocity方法，可以实现不同方向的指定运动
PX4平台
- 官网给的dronekit控制px4示例代码做了修改，由c编写的程序通过socket连接dronekit的python程序，发送指令向上/下/东/西/南/北移动，python程序接收后会执行相应操作
- C代码
//
// main.cpp
// connect_python
//
// Created by Qiucheng LIN on 2020/1/8.
// Copyright © 2020 Qiucheng LIN. All rights reserved.
//
#include <iostream>
#include <unistd.h>//Linux系统下网络通讯的头文件集合
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <stdarg.h>
#include <fcntl.h>
int main(int argc, const char * argv[]) {
// insert code here...
int client_sockfd;//客户端套接字
struct sockaddr_in my_addr; //服务器网络地址结构体
socklen_t sin_size;
memset(&my_addr,0,sizeof(my_addr)); //数据初始化--清零
my_addr.sin_family=AF_INET; //设置为IP通信
my_addr.sin_addr.s_addr=inet_addr("127.0.0.1");//服务器IP地址--允许连接到所有本地地址上
my_addr.sin_port=htons(7777); //服务器端口号
/*创建服务器端套接字--IPv4协议，面向连接通信，TCP协议*/
if((client_sockfd=socket(PF_INET,SOCK_STREAM,0))<0)
{
perror("socket error");
return 1;
}
/*将套接字绑定到服务器的网络地址上*/
if(bind(client_sockfd,(struct sockaddr *)&my_addr,sizeof(struct sockaddr))<0)
{
perror("bind error");
return 1;
}
/*监听连接请求--监听队列长度为5
if(listen(server_sockfd,5)<0)
{
perror("listen error");
return 1;
}; */
sin_size=sizeof(struct sockaddr_in);
sockaddr_in addrConServer;
addrConServer.sin_family = AF_INET;
addrConServer.sin_port = htons(8888);
addrConServer.sin_addr.s_addr = inet_addr("127.0.0.1");
if(connect(client_sockfd, (sockaddr *)&addrConServer, sin_size)<0){
perror("connect error");
return 1;
}
/*等待客户端连接请求到达
if((client_sockfd=accept(server_sockfd,(struct sockaddr *)&remote_addr,&sin_size))<0)
{
perror("accept error");
return 1;
}*/
// char buf[3]={'a','c','k'};
char recvbuf[100];
//printf("accept client %s\n",inet_ntoa(remote_addr.sin_addr));
//send(client_sockfd, buf, sizeof(buf), 0);
long n=recv(client_sockfd,recvbuf,sizeof(recvbuf),0);
recvbuf[n]='\0';
std::cout<<recvbuf<<" "<<n<<std::endl;
char flag;
printf("takeoff?(y/n)");
std::cin>>flag;
if(flag=='y'||flag=='Y')
send(client_sockfd, &flag, sizeof(flag), 0);
else return 0;
char cmd;
while(true){
printf("input comander:");
std::cin>>cmd;
printf("cmd=%c\n",cmd);
switch (cmd) {
case '1':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '2':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '3':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '4':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '5':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '6':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
case '7':
send(client_sockfd, &cmd, sizeof(cmd), 0);
break;
default:
printf("invalid commander/n");
break;
}
if(cmd==7) break;
}
close(client_sockfd);
return 0;
}
- python程序
# Import DroneKit-Python
from dronekit import connect, Command, LocationGlobal
from pymavlink import mavutil
import time, sys, argparse, math, socket
################################################################################################
# Settings
################################################################################################
connection_string = '127.0.0.1:14540'
MAV_MODE_AUTO = 4
# https://github.com/PX4/Firmware/blob/master/Tools/mavlink_px4.py
# Parse connection argument
parser = argparse.ArgumentParser()
parser.add_argument("-c", "--connect", help="connection string")
args = parser.parse_args()
if args.connect:
connection_string = args.connect
################################################################################################
# Init
################################################################################################
# Connect to the Vehicle
print "Connecting"
vehicle = connect(connection_string, wait_ready=True)
def PX4setMode(mavMode):
vehicle._master.mav.command_long_send(vehicle._master.target_system, vehicle._master.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
mavMode,
0, 0, 0, 0, 0, 0)
def get_location_offset_meters(original_location, dNorth, dEast, alt):
"""
Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the
specified `original_location`. The returned Location adds the entered `alt` value to the altitude of the `original_location`.
The function is useful when you want to move the vehicle around specifying locations relative to
the current vehicle position.
The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.
For more information see:
http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters
"""
earth_radius=6378137.0 #Radius of "spherical" earth
#Coordinate offsets in radians
dLat = dNorth/earth_radius
dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))
#New position in decimal degrees
newlat = original_location.lat + (dLat * 180/math.pi)
newlon = original_location.lon + (dLon * 180/math.pi)
return LocationGlobal(newlat, newlon,original_location.alt+alt)
################################################################################################
# Listeners
################################################################################################
home_position_set = False
#Create a message listener for home position fix
@vehicle.on_message('HOME_POSITION')
def listener(self, name, home_position):
global home_position_set
home_position_set = True
################################################################################################
# Start mission example
################################################################################################
s = socket.socket() # create socket
host = '127.0.0.1'
port = 8888 # portnumber
s.bind((host,port))
s.listen(5)
c,addr = s.accept()
# wait for a home position lock
while not home_position_set:
print "Waiting for home position..."
time.sleep(1)
c.send("Already!".encode())
# Display basic vehicle state
print " Type: %s" % vehicle._vehicle_type
print " Armed: %s" % vehicle.armed
print " System status: %s" % vehicle.system_status.state
print " GPS: %s" % vehicle.gps_0
print " Alt: %s" % vehicle.location.global_relative_frame.alt
# Change to AUTO mode
PX4setMode(MAV_MODE_AUTO)
time.sleep(1)
# Load commands
cmds = vehicle.commands
cmds.clear()
home = vehicle.location.global_relative_frame
"""
# move 10 meters north
wp = get_location_offset_meters(wp, 10, 0, 0);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# move 10 meters east
wp = get_location_offset_meters(wp, 0, 10, 0);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# move 10 meters south
wp = get_location_offset_meters(wp, -10, 0, 0);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# move 10 meters west
wp = get_location_offset_meters(wp, 0, -10, 0);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# land
wp = get_location_offset_meters(home, 0, 0, 10);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# Upload mission
cmds.upload()
time.sleep(2)
"""
print("wait for takeoff!")
c.recv(1).decode()
print("taking off now!")
# Arm vehicle
vehicle.armed = True
# takeoff to 10 meters
wp = get_location_offset_meters(home, 0, 0, 10);
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
cmds.upload()
time.sleep(2)
vehicle.commands.next
vehicle.commands.next
while 1:
print("alt= %f" %wp.alt)
print("lot= %f" %wp.lon)
print("lat= %f" %wp.lat)
cmd_num=c.recv(1).decode()
print("receive commander %c\n" %cmd_num)
#cmds.clear()
if cmd_num=='1': #up
wp = get_location_offset_meters(wp, 0, 0, 10)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='2': #down
wp = get_location_offset_meters(wp, 0, 0, -10)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='3': #north
wp = get_location_offset_meters(wp, 10, 0, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='4': #east
wp = get_location_offset_meters(wp, 0, 10, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='5': #south
wp = get_location_offset_meters(wp, -10, 0, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='6': #west
wp = get_location_offset_meters(wp, 0, -10, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt)
elif cmd_num=='7':
break
cmds.add(cmd)
cmds.upload()
time.sleep(2)
vehicle.commands.next
vehicle.commands.next
print("moving now!")
time.sleep(1)
"""# monitor mission execution
nextwaypoint = vehicle.commands.next
print "next " nextwaypoint
while nextwaypoint < len(vehicle.commands):
if vehicle.commands.next > nextwaypoint:
display_seq = vehicle.commands.next+1
print "Moving to waypoint %s" % display_seq
nextwaypoint = vehicle.commands.next
time.sleep(1)
# wait for the vehicle to land
while vehicle.commands.next > 0:
time.sleep(1)
"""
# Disarm vehicle
vehicle.armed = False
time.sleep(1)
# Close vehicle object before exiting script
vehicle.close()
time.sleep(1)
- 测试方式：
- 由于目前条件有限，我还只在模拟器上测试了一下
- 首先要运行jmavsim仿真程序，然后运行dronekit python程序，最后运行c程序
- 通过在c程序中输入命令，操作仿真器中的无人机移动。
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号