diff --git a/README.md b/README.md index abf5a7be06898903861119d69365bad755cc9564..bdeb5387cf8577f0a2d615e07d0980073ea9e067 100644 --- a/README.md +++ b/README.md @@ -1,24 +1,4 @@ # yocto-embedded-tools #### 介绍 -This repo/branch summarizes the software and tools related to hieuler, serving oee yocto build. - -#### 使用说明 -Please reference to subdirectories. - -#### 参与贡献 - -1. Fork 本仓库 -2. 新建 Feat_xxx 分支 -3. 提交代码 -4. 新建 Pull Request - - -#### 特技 - -1. 使用 Readme\_XXX.md 来支持不同的语言,例如 Readme\_en.md, Readme\_zh.md -2. Gitee 官方博客 [blog.gitee.com](https://blog.gitee.com) -3. 你可以 [https://gitee.com/explore](https://gitee.com/explore) 这个地址来了解 Gitee 上的优秀开源项目 -4. [GVP](https://gitee.com/gvp) 全称是 Gitee 最有价值开源项目,是综合评定出的优秀开源项目 -5. Gitee 官方提供的使用手册 [https://gitee.com/help](https://gitee.com/help) -6. Gitee 封面人物是一档用来展示 Gitee 会员风采的栏目 [https://gitee.com/gitee-stars/](https://gitee.com/gitee-stars/) +代码放入hi_drone,文件名为openEuler_dronekit,具体使用说明参见其中的README diff --git a/hi-drone/README.md b/hi-drone/README.md deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/connect.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/connect.py new file mode 100644 index 0000000000000000000000000000000000000000..ba9f2fdf79b07eb0e110d07bf74d77aaa7ebec0c --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/connect.py @@ -0,0 +1,50 @@ +from dronekit import connect + +# Connect to the Vehicle (in this case a UDP endpoint) +vehicle = connect('/dev/ttyAMA4', wait_ready=True, baud=921600) + +# vehicle is an instance of the Vehicle class + +print ("Autopilot Firmware version: %s" % vehicle.version) + +print ("Autopilot capabilities (supports ftp): %s" % vehicle.capabilities.ftp) + +print ("Global Location: %s" % vehicle.location.global_frame) + +print ("Global Location (relative altitude): %s" % vehicle.location.global_relative_frame) + +print ("Local Location: %s" % vehicle.location.local_frame) #NED + +print ("Attitude: %s" % vehicle.attitude) + +print ("Velocity: %s" % vehicle.velocity) + +print ("GPS: %s" % vehicle.gps_0) + +print ("Groundspeed: %s" % vehicle.groundspeed) + +print ("Airspeed: %s" % vehicle.airspeed) + +print ("Gimbal status: %s" % vehicle.gimbal) + +print ("Battery: %s" % vehicle.battery) + +print ("EKF OK?: %s" % vehicle.ekf_ok) + +print ("Last Heartbeat: %s" % vehicle.last_heartbeat) + +print ("Rangefinder: %s" % vehicle.rangefinder) + +print ("Rangefinder distance: %s" % vehicle.rangefinder.distance) + +print ("Rangefinder voltage: %s" % vehicle.rangefinder.voltage) + +print ("Heading: %s" % vehicle.heading) + +print ("Is Armable?: %s" % vehicle.is_armable) + +print ("System status: %s" % vehicle.system_status.state) + +print ("Mode: %s" % vehicle.mode.name) # settable + +print ("Armed: %s" % vehicle.armed) # settable diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example1.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example1.py new file mode 100644 index 0000000000000000000000000000000000000000..68a83a06a8f438b9d47e4ec185565e5f7c33763e --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example1.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative + + +# 改为当前连接的pixhawk飞控的端口 +connection_string = '10.42.0.10:14550' +print('Connecting to vehicle on: %s' % connection_string) +# connect函数将会返回一个Vehicle类型的对象,即此处的vehicle +# 即可认为是无人机的主体,通过vehicle对象,我们可以直接控制无人机 +vehicle = connect(connection_string,wait_ready=True,baud=921600) + +# 定义arm_and_takeoff函数,使无人机解锁并起飞到目标高度 +# 参数aTargetAltitude即为目标高度,单位为米 +def arm_and_takeoff(aTargetAltitude): + # 进行起飞前检查 + print("Basic pre-arm checks") + # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 + # 是否初始化完毕。若以上检查通过,则会返回True + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + # 解锁无人机(电机将开始旋转) + print("Arming motors") + # 将无人机的飞行模式切换成"GUIDED"(一般建议在GUIDED模式下控制无人机) + vehicle.mode = VehicleMode("GUIDED") + # 通过设置vehicle.armed状态变量为True,解锁无人机 + vehicle.armed = True + + # 在无人机起飞之前,确认电机已经解锁 + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + # 发送起飞指令 + print("Taking off!") + # simple_takeoff将发送指令,使无人机起飞并上升到目标高度 + vehicle.simple_takeoff(aTargetAltitude) + + # 在无人机上升到目标高度之前,阻塞程序 + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # 当高度上升到目标高度的0.95倍时,即认为达到了目标高度,退出循环 + # vehicle.location.global_relative_frame.alt为相对于home点的高度 + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # 等待1s + time.sleep(1) + +# 调用上面声明的arm_and_takeoff函数,目标高度10m +arm_and_takeoff(10) + +# 设置在运动时,默认的空速为3m/s +print("Set default/target airspeed to 3") +# vehicle.airspeed变量可读可写,且读、写时的含义不同。 +# 读取时,为无人机的当前空速;写入时,设定无人机在执行航点任务时的默认速度 +vehicle.airspeed = 3 + +# 发送指令,让无人机前往第一个航点 +print("Going towards first point for 30 seconds ...") +# LocationGlobalRelative是一个类,它由经纬度(WGS84)和相对于home点的高度组成 +# 这条语句将创建一个位于南纬35.361354,东经149.165218,相对home点高20m的位置 +point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) +# simple_goto函数将位置发送给无人机,生成一个目标航点 +vehicle.simple_goto(point1) + +# simple_goto函数只发送指令,不判断有没有到达目标航点 +# 它可以被其他后续指令打断,此处延时30s,即让无人机朝向point1飞行30s +time.sleep(30) + +# 发送指令,让无人机前往第二个航点 +print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") +# 与之前类似,这条语句创建了另一个相对home高20m的点 +point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) +# simple_goto将目标航点发送给无人机,groundspeed=10设置飞行时的地速为10m/s +vehicle.simple_goto(point2, groundspeed=10) + +# 与之前一样,延时30s +time.sleep(30) + +# 发送"返航"指令 +print("Returning to Launch") +# 返航,只需将无人机的飞行模式切换成"RTL(Return to Launch)" +# 无人机会自动返回home点的正上方,之后自动降落 +vehicle.mode = VehicleMode("RTL") + +# 退出之前,清除vehicle对象 +print("Close vehicle object") +vehicle.close() + diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example2.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example2.py new file mode 100644 index 0000000000000000000000000000000000000000..210299fef061d4b45d1eb4ac9fd206ed698266ec --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example2.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative + + +# 改为当前连接的pixhawk飞控的端口 +connection_string = '10.42.0.10:14550' +print('Connecting to vehicle on: %s' % connection_string) +# connect函数将会返回一个Vehicle类型的对象,即此处的vehicle +# 即可认为是无人机的主体,通过vehicle对象,我们可以直接控制无人机 +vehicle = connect(connection_string,wait_ready=True,baud=921600) + +# 定义arm_and_takeoff函数,使无人机解锁并起飞到目标高度 +# 参数aTargetAltitude即为目标高度,单位为米 +def arm_and_takeoff(aTargetAltitude): + # 进行起飞前检查 + print("Basic pre-arm checks") + # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 + # 是否初始化完毕。若以上检查通过,则会返回True + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + # 解锁无人机(电机将开始旋转) + print("Arming motors") + # 将无人机的飞行模式切换成"GUIDED"(一般建议在GUIDED模式下控制无人机) + vehicle.mode = VehicleMode("GUIDED") + # 通过设置vehicle.armed状态变量为True,解锁无人机 + vehicle.armed = True + + # 在无人机起飞之前,确认电机已经解锁 + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + # 发送起飞指令 + print("Taking off!") + # simple_takeoff将发送指令,使无人机起飞并上升到目标高度 + vehicle.simple_takeoff(aTargetAltitude) + + # 在无人机上升到目标高度之前,阻塞程序 + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # 当高度上升到目标高度的0.95倍时,即认为达到了目标高度,退出循环 + # vehicle.location.global_relative_frame.alt为相对于home点的高度 + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # 等待1s + time.sleep(1) + +# 调用上面声明的arm_and_takeoff函数,目标高度10m +arm_and_takeoff(10) + +# 设置在运动时,默认的空速为3m/s +print("Set default/target airspeed to 3") +# vehicle.airspeed变量可读可写,且读、写时的含义不同。 +# 读取时,为无人机的当前空速;写入时,设定无人机在执行航点任务时的默认速度 +vehicle.airspeed = 3 + +# 发送指令,让无人机前往第一个航点 +print("Going towards first point for 30 seconds ...") +# LocationGlobalRelative是一个类,它由经纬度(WGS84)和相对于home点的高度组成 +# 这条语句将创建一个位于南纬35.361354,东经149.165218,相对home点高20m的位置 +point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) +# simple_goto函数将位置发送给无人机,生成一个目标航点 +vehicle.simple_goto(point1) + +# simple_goto函数只发送指令,不判断有没有到达目标航点 +# 它可以被其他后续指令打断,此处延时30s,即让无人机朝向point1飞行30s +time.sleep(30) + +# 发送指令,让无人机前往第二个航点 +print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") +# 与之前类似,这条语句创建了另一个相对home高20m的点 +point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) +# simple_goto将目标航点发送给无人机,groundspeed=10设置飞行时的地速为10m/s +vehicle.simple_goto(point2, groundspeed=10) + +# 与之前一样,延时30s +time.sleep(30) + +# 发送"返航"指令 +print("Returning to Launch") +# 返航,只需将无人机的飞行模式切换成"RTL(Return to Launch)" +# 无人机会自动返回home点的正上方,之后自动降落 +vehicle.mode = VehicleMode("RTL") + +# 退出之前,清除vehicle对象 +print("Close vehicle object") +vehicle.close() + diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example3.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example3.py new file mode 100644 index 0000000000000000000000000000000000000000..4253aedb6715f6a4b986da62431383033d5b6cbb --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example3.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative +from pymavlink import mavutil + +# 改为当前连接的pixhawk飞控的端口 +connection_string = '10.42.0.10:14550' +print('Connecting to vehicle on: %s' % connection_string) +# connect函数将会返回一个Vehicle类型的对象,即此处的vehicle +# 即可认为是无人机的主体,通过vehicle对象,我们可以直接控制无人机 +vehicle = connect(connection_string,wait_ready=True,baud=921600) + +# 定义arm_and_takeoff函数,使无人机解锁并起飞到目标高度 +# 参数aTargetAltitude即为目标高度,单位为米 +def arm_and_takeoff(aTargetAltitude): + # 进行起飞前检查 + print("Basic pre-arm checks") + # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 + # 是否初始化完毕。若以上检查通过,则会返回True + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + # 解锁无人机(电机将开始旋转) + print("Arming motors") + # 将无人机的飞行模式切换成"GUIDED"(一般建议在GUIDED模式下控制无人机) + vehicle.mode = VehicleMode("GUIDED") + # 通过设置vehicle.armed状态变量为True,解锁无人机 + vehicle.armed = True + + # 在无人机起飞之前,确认电机已经解锁 + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + # 发送起飞指令 + print("Taking off!") + # simple_takeoff将发送指令,使无人机起飞并上升到目标高度 + vehicle.simple_takeoff(aTargetAltitude) + + # 在无人机上升到目标高度之前,阻塞程序 + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # 当高度上升到目标高度的0.95倍时,即认为达到了目标高度,退出循环 + # vehicle.location.global_relative_frame.alt为相对于home点的高度 + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # 等待1s + time.sleep(1) + +# 调用上面声明的arm_and_takeoff函数,目标高度10m +arm_and_takeoff(10) + +# 设置在运动时,默认的空速为3m/s +print("Set default/target airspeed to 3") +# vehicle.airspeed变量可读可写,且读、写时的含义不同。 +# 读取时,为无人机的当前空速;写入时,设定无人机在执行航点任务时的默认速度 +vehicle.airspeed = 3 + +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) + +# 让无人机向左飞行,速度10m/s,飞行时间10秒,延迟(duration)10秒这样拐角更方正. +# x表示前后方向,y表示左右方向,z表示垂直高度方向 +velocity_x = 0 +velocity_y = -10 +velocity_z = 0 +duration = 10 +send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration) +time.sleep(10) + +#让无人机向前飞行,速度10m/s,飞行时间10秒 +velocity_x = 10 +velocity_y = 0 +velocity_z = 0 +duration = 10 +send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration) +time.sleep(10) + +#也可以用下面这种格式:让无人机向右飞行,速度10m/s,飞行时间10秒 +send_body_ned_velocity(0,10,0,10) +time.sleep(10) + +#最后让无人机向后飞行,速度10m/s,飞行时间10秒,这样无人机就飞出一个正方形 +send_body_ned_velocity(-10,0,0,10) +time.sleep(10) + +# 发送指令,让无人机前往第一个航点 +print("Going towards first point for 30 seconds ...") +# LocationGlobalRelative是一个类,它由经纬度(WGS84)和相对于home点的高度组成 +# 这条语句将创建一个位于南纬35.361354,东经149.165218,相对home点高20m的位置 +point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) +# simple_goto函数将位置发送给无人机,生成一个目标航点 +vehicle.simple_goto(point1) + +# simple_goto函数只发送指令,不判断有没有到达目标航点 +# 它可以被其他后续指令打断,此处延时30s,即让无人机朝向point1飞行30s +time.sleep(30) +# 发送"返航"指令 +print("Returning to Launch") +# 返航,只需将无人机的飞行模式切换成"RTL(Return to Launch)" +# 无人机会自动返回home点的正上方,之后自动降落 +vehicle.mode = VehicleMode("RTL") + +# 退出之前,清除vehicle对象 +print("Close vehicle object") +vehicle.close() + diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example4.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example4.py new file mode 100644 index 0000000000000000000000000000000000000000..7ea40c9a9f1b9011a0535670d93d0a65d609ecd4 --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example4.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative +from pymavlink import mavutil + +# 改为当前连接的pixhawk飞控的端口 +connection_string = '/dev/ttyAMA4' +print('Connecting to vehicle on: %s' % connection_string) +# connect函数将会返回一个Vehicle类型的对象,即此处的vehicle +# 即可认为是无人机的主体,通过vehicle对象,我们可以直接控制无人机 +vehicle = connect(connection_string,wait_ready=True,baud=921600) + +# 定义arm_and_takeoff函数,使无人机解锁并起飞到目标高度 +# 参数aTargetAltitude即为目标高度,单位为米 +def arm_and_takeoff(aTargetAltitude): + # 进行起飞前检查 + print("Basic pre-arm checks") + # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 + # 是否初始化完毕。若以上检查通过,则会返回True + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + # 解锁无人机(电机将开始旋转) + print("Arming motors") + # 将无人机的飞行模式切换成"GUIDED"(一般建议在GUIDED模式下控制无人机) + vehicle.mode = VehicleMode("GUIDED") + # 通过设置vehicle.armed状态变量为True,解锁无人机 + vehicle.armed = True + # 在无人机起飞之前,确认电机已经解锁 + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + # 发送起飞指令 + print("Taking off!") + # simple_takeoff将发送指令,使无人机起飞并上升到目标高度 + vehicle.simple_takeoff(aTargetAltitude) + + # 在无人机上升到目标高度之前,阻塞程序 + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # 当高度上升到目标高度的0.95倍时,即认为达到了目标高度,退出循环 + # vehicle.location.global_relative_frame.alt为相对于home点的高度 + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # 等待1s + time.sleep(1) + +# 调用上面声明的arm_and_takeoff函数,目标高度3m +arm_and_takeoff(3) + +# 设置在运动时,默认的空速为3m/s +print("Set default/target airspeed to 3") +# vehicle.airspeed变量可读可写,且读、写时的含义不同。 +# 读取时,为无人机的当前空速;写入时,设定无人机在执行航点任务时的默认速度 +vehicle.airspeed = 3 + +#定义方向控制 +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) + +# 让无人机向左飞行,速度0.2m/s,飞行时间(duration)10秒,延迟(sleep)2秒这样拐角更方正. +# x表示前后方向,y表示左右方向,z表示垂直高度方向 +velocity_x = 0 +velocity_y = -0.2 +velocity_z = 0 +duration = 10 +send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration) +time.sleep(2) + +#让无人机向前飞行,速度0.2m/s,飞行时间10秒 +velocity_x = 0.2 +velocity_y = 0 +velocity_z = 0 +duration = 10 +send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration) +time.sleep(2) + +#也可以用下面这种格式:让无人机向右飞行,速度0.2m/s,飞行时间10秒 +send_body_ned_velocity(0,0.2,0,10) +time.sleep(2) + +#最后让无人机向后飞行,速度0.2m/s,飞行时间5秒,这样无人机就飞出一个正方形 +send_body_ned_velocity(-0.2,0,0,10) +time.sleep(2) +# 发送"降落"指令 +print("Land") +# 降落,只需将无人机的飞行模式切换成"Land" +# 无人机会自动返回home点的正上方,之后自动降落 +vehicle.mode = VehicleMode("LAND") + +# 退出之前,清除vehicle对象 +print("Close vehicle object") +vehicle.close() diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example5.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example5.py new file mode 100644 index 0000000000000000000000000000000000000000..c2011cfda7c86685096d32ecdf93864f3b2c3d3f --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example5.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + + + +from __future__ import print_function +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative +from pymavlink import mavutil + +# 改为当前连接的pixhawk飞控的端口 +connection_string = '/dev/ttyAMA4' +print('Connecting to vehicle on: %s' % connection_string) +# connect函数将会返回一个Vehicle类型的对象,即此处的vehicle +# 即可认为是无人机的主体,通过vehicle对象,我们可以直接控制无人机 +vehicle = connect(connection_string,wait_ready=True,baud=921600) + +# 定义arm_and_takeoff函数,使无人机解锁并起飞到目标高度 +# 参数aTargetAltitude即为目标高度,单位为米 +def arm_and_takeoff(aTargetAltitude): + # 进行起飞前检查 + print("Basic pre-arm checks") + # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器 + # 是否初始化完毕。若以上检查通过,则会返回True + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + # 解锁无人机(电机将开始旋转) + print("Arming motors") + # 将无人机的飞行模式切换成"GUIDED"(一般建议在GUIDED模式下控制无人机) + vehicle.mode = VehicleMode("GUIDED") + # 通过设置vehicle.armed状态变量为True,解锁无人机 + vehicle.armed = True + # 在无人机起飞之前,确认电机已经解锁 + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + # 发送起飞指令 + print("Taking off!") + # simple_takeoff将发送指令,使无人机起飞并上升到目标高度 + vehicle.simple_takeoff(aTargetAltitude) + + # 在无人机上升到目标高度之前,阻塞程序 + while True: + print(" Altitude: ", vehicle.location.global_relative_frame.alt) + # 当高度上升到目标高度的0.95倍时,即认为达到了目标高度,退出循环 + # vehicle.location.global_relative_frame.alt为相对于home点的高度 + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # 等待1s + time.sleep(1) + +# 调用上面声明的arm_and_takeoff函数,目标高度3m +arm_and_takeoff(3) +# 设置在运动时,默认的空速为3m/s +print("Set default/target airspeed to 3") +# vehicle.airspeed变量可读可写,且读、写时的含义不同。 +# 读取时,为无人机的当前空速;写入时,设定无人机在执行航点任务时的默认速度 +vehicle.airspeed = 3 +time.sleep(5) +# 发送"降落"指令 +print("Land") +# 降落,只需将无人机的飞行模式切换成"Land" +# 无人机会自动返回home点的正上方,之后自动降落 +vehicle.mode = VehicleMode("LAND") + +# 退出之前,清除vehicle对象 +print("Close vehicle object") +vehicle.close() + diff --git a/hi-drone/openEuler_dronekit/Dronekit_Demo/example6.py b/hi-drone/openEuler_dronekit/Dronekit_Demo/example6.py new file mode 100644 index 0000000000000000000000000000000000000000..4009b8ada9b410716f45de8f795b604f0138c3e8 --- /dev/null +++ b/hi-drone/openEuler_dronekit/Dronekit_Demo/example6.py @@ -0,0 +1,100 @@ +""" +Simple script for take off and control with arrow keys +""" + + +import time +from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal +from pymavlink import mavutil +import Tkinter as tk + + +# +# connection_string = '10.42.0.1:14550' +print('Connecting...') +# vehicle = connect('10.42.0.1:14550',wait_ready=False,baud=921600) +vehicle = connect('/dev/ttyAMA4',wait_ready=False,baud=921600) +vehicle.wait_ready(True,raise_exception=False) + +# +gnd_speed = 0.2 # [m/s] + +# +def arm_and_takeoff(altitude): + + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + while not vehicle.armed: time.sleep(1) + + print("Taking Off") + vehicle.simple_takeoff(altitude) + + while True: + v_alt = vehicle.location.global_relative_frame.alt + print(">> Altitude = %.1f m"%v_alt) + if v_alt >= altitude * 0.95: + print("Target altitude reached") + break + time.sleep(1) + +# +def set_velocity_body(vehicle, vx, vy, vz): + """ Remember: vz is positive downward!!! + + + Bitmask to indicate which dimensions should be ignored by the vehicle + (a value of 0b0000000000000000 or 0b0000001000000000 indicates that + none of the setpoint dimensions should be ignored). Mapping: + bit 1: x, bit 2: y, bit 3: z, + bit 4: vx, bit 5: vy, bit 6: vz, + bit 7: ax, bit 8: ay, bit 9: + + + """ + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, + 0, 0, + mavutil.mavlink.MAV_FRAME_BODY_NED, + 0b0000111111000111, #-- BITMASK -> Consider only the velocities + 0, 0, 0, #-- POSITION + vx, vy, vz, #-- VELOCITY + 0, 0, 0, #-- ACCELERATIONS + 0, 0) + vehicle.send_mavlink(msg) + vehicle.flush() + + +# +def key(event): + if event.char == event.keysym: #-- standard keys + if event.keysym == 'r': + print("r pressed >> Set the vehicle to RTL") + vehicle.mode = VehicleMode("RTL") + elif event.keysym == 'l': + print("l pressed >> Set the vehicle to LAND") + vehicle.mode = VehicleMode("LAND") + + else: #-- non standard keys + if event.keysym == 'Up': + set_velocity_body(vehicle, gnd_speed, 0, 0) + elif event.keysym == 'Down': + set_velocity_body(vehicle,-gnd_speed, 0, 0) + elif event.keysym == 'Left': + set_velocity_body(vehicle, 0, -gnd_speed, 0) + elif event.keysym == 'Right': + set_velocity_body(vehicle, 0, gnd_speed, 0) + + +# +# +arm_and_takeoff(0) + +# +root = tk.Tk() +print(">> Control the drone with the arrow keys. Press r for RTL mode") +print(">> Control the drone with the arrow keys. Press l for LAND mode") +root.bind_all('', key) +root.mainloop() + diff --git a/hi-drone/openEuler_dronekit/README.md b/hi-drone/openEuler_dronekit/README.md new file mode 100644 index 0000000000000000000000000000000000000000..0ef88de38a3c54bb713b460a63d37811e8ff346e --- /dev/null +++ b/hi-drone/openEuler_dronekit/README.md @@ -0,0 +1,245 @@ +**一、部署ubuntu环境** + +按照openEuler官方部署教程,在ubuntu22.04上部署好相关openEuler环境 + +**二、下载项目代码** + +将本项目代码openEuler_dronekit通过运行脚本download_repos.sh来自动下载所需五个环境代码包 + +``` + +赋予脚本执行权限 + +chmod +x download_repos.sh + + +下载代码包 + +./download_repos.sh + + +``` + +然后将项目代码中的文件夹 + +1.dronekit-python + +2.monotonic + +3.python-future + +4.pymavlink + +5.pyserial + + +依次拷贝到拷贝到docker容器目录中 + +其命令类似如下 +``` +cp -rf dronekit-python /home/robot/openeuler/build_sd3403/build/hieulerpi1 + +``` +这里docker容器目录:/home/robot/openeuler/build_sd3403/build/hieulerpi1,是以我电脑为例,用户需要根据自己部署的docker容器目录做修改 + + +**三、编译项目代码** + +``` +执行下面命令则进入docker容器目录: + +cd /home/robot/openeuler/build_sd3403/build/hieulerpi1 + + + +执行下面命令,切换到openEuler交互环境,用户切换为openeuler: + +oebuild bitbake + +执行下面命令,加载SDK(注意.后面的空格) + +. /opt/openeuler/oecore-x86_64/environment-setup-aarch64-openeuler-linux + +依次编译上面第二步拷贝过来的文件夹,例如编译dronekit-python项目(禁止BUILD_TESTING): + +cd dronekit-python +colcon build --cmake-args -DBUILD_TESTING=False + +编译完成后,生成的可执行文件在install目录中 + +退出openEuler交叉编译环境 exit +``` + +**四、 下载** + +(1)由于有五个文件夹编译,会在各个文件夹内生成各自的install文件夹,为了避免混淆建议加前缀,例如dronekit-python文件夹生成的install,重命名加前缀变为dronkit_install,其余四个文件夹中install类似,然后将这些加前缀后的install文件夹压缩tar包后下载到开发板上。 + +压缩命令类似如下 + +``` +tar -czvf output_filename.tar.gz folder_to_compress +``` + + +这里我是部署在虚拟机中ubuntu22.04,所以要先把压缩包拷贝到windows下再使用scp命令拷贝到开发板中,这里用户根据自己的情况,方法不限,可以不像我用共享目录,可以用其他办法,只要最后将压缩文件拷贝出便可 + + +(2)确保开发板是正常开机状态并且已经刷写了openEuler系统,这一步可以参考 + +[openEuler使用手册](file:///C:/Users/29063/Desktop/%E6%AC%A7%E6%8B%89%E6%B4%BE/%E3%80%90%E6%98%93%E7%99%BE%E7%BA%B3%E3%80%91%E6%AC%A7%E6%8B%89%E6%B4%BE%20Euler%20Pi/%E3%80%90%E6%98%93%E7%99%BE%E7%BA%B3%E3%80%91%E6%AC%A7%E6%8B%89%E6%B4%BE%20Euler%20Pi/01.%20%E7%94%A8%E6%88%B7%E6%89%8B%E5%86%8C/01.%20%E4%BA%A7%E5%93%81%E4%BD%93%E9%AA%8C%E6%89%8B%E5%86%8C/%E6%AC%A7%E6%8B%89%E6%B4%BEEuler%20Pi%20%E5%BF%AB%E9%80%9F%E9%AA%8C%E8%AF%81%E6%89%8B%E5%86%8C.pdf)刷写 + +用windows终端ssh或者MobaXterm连接开发板,这个用户名和密码在开发板刷机时用户会被要求设置,这里以我板子为例默认网口IP为192.168.10.8,子网掩码为255.255.255.0,所以ssh root@192.168.10.8正常情况网线连接电脑,电脑与开发板同一网段10便可以输入密码连接上。 + + +(3)scp命令将五个压缩包依次拷贝到开发板中,命令类似下面 + +``` +scp -r dronkit_install.tar.gz root@192.168.10.8:/root +``` +!!!需要注意这里拷贝上面五个intall压缩包后,还需要拷贝Dronekit_Demo文件压缩包,文件位置同样在git项目openEuler_dronekit下,压缩方式类似,这个文件包中的程序为后面控制无人机的控制程序集合 + + +(4)开发板的openEuler环境中,修改目录名 + +在开发板中依次将五个install压缩包解压 + +解压命令类似如下 +``` +tar -xzvf filename.tar.gz +``` + +``` +在开发板的openEuler环境,五个解压后install文件夹类似操作,进入install目录,修改setup.sh: + +cd dronkit_install +vi setup.sh + +第十行为colcon的编译目录名, +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/openeuler/build/sd3403/eulercar/install +. +我们需要将install目录下的所有文件中,该目录名,替换为,开发板的工作目录名/root/install。类似如下 + +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/root/dronkit_install + +这里手动修改,点击i进入编辑模式,修改好后按Esc键退出编辑模式,然后输入:wq保存退出文件 + +``` + + + + **五、 运行**** + +先决条件是需要无人机搭载开发板,用户可以自己DIY无人机,确保无人机飞控为APM,将telem2口连接开发板的RTG排针插口,飞控中要设置telem2口模式offboard,并且波特率为921600,开发板中则需要将串口/dev/ttyAMA4赋权,操作方法见官方手册, 更改uart4复用关系: + +``` +bspmm 0x0102F0134 0x1201 +bspmm 0x0102F0138 0x1201 +``` + +[官方uart操作方法](https://gitee.com/HiEuler/externed_device_sample/tree/master/uart) + +用户也可以联系阿木实验室,使用阿木实验室无人机Z410_openEuler款直接使用,无需部署,到手即飞 + +[阿木实验室官网](https://www.amovlab.com/) + + +``` +实飞使用控制Demo前需要加载环境变量,依次加载前面修改的五个install的setup.sh + +source ~/dronkit_install/setup.sh +source ~/monotonic_install/setup.sh +source ~/future_install/setup.sh +source ~/pymavlink_install/setup.sh +source ~/pyserial_install/setup.sh +``` + +``` + +开发板中的Dronekit_Demo解压后进入 + +cd Dronekit_Demo + +ls可以看到多个py文件,这些都是连接控制无人机的程序 + +为了验证整个的软硬件链路是否正常配置,用户可以运行connect.py程序来看程序是否可以正常连接无人机飞控获取相关飞控信息 + +python connect.py + +正常情况下程序运行正常会如下显示: +sd3403 ~/Dronekit_Demo # python connect.py +CRITICAL:autopilot:PreArm: Throttle below Failsafe +CRITICAL:autopilot:PreArm: Hardware safety switch +Autopilot Firmware version: APM:Copter-4.1.0 +Autopilot capabilities (supports ftp): True +Global Location: LocationGlobal:lat=0.0,lon=0.0,alt=None +Global Location (relative altitude): LocationGlobalRelative:lat=0.0,lon=0.0,alt=-0.334 +Local Location: LocationLocal:north=None,east=None,down=None +Attitude: Attitude:pitch=0.003257771022617817,yaw=0.8752917051315308,roll=-0.01555346418172121 +Velocity: [0.0, 0.0, -0.01] +GPS: GPSInfo:fix=0,num_sat=0 +Groundspeed: 0.006987953092902899 +Airspeed: 0.0 +Gimbal status: Gimbal: pitch=None, roll=None, yaw=None +Battery: Battery:voltage=12.333,current=0.65,level=98 +EKF OK?: False +Last Heartbeat: 0.38181029199998306 +Rangefinder: Rangefinder: distance=None, voltage=None +Rangefinder distance: None +Rangefinder voltage: None +Heading: 50 +Is Armable?: False +System status: STANDBY +Mode: STABILIZE +Armed: False + +正常输出上面内容为开发板通过项目程序运行获取飞控相关信息,如固件版本,GPS信号,电池电量等等,上面正常显示代表软硬件链路正常运行,用户可以带上无人机准备户外飞行 + +``` + +操作无人机的方法因无人机不同而异,这里我以阿木实验室Z410无人机为例,用户可以参见Z410无人机使用手册来进行操作 + +[Z410无人机使用手册](https://wiki.amovlab.com/pdf/Z410-4B%e6%97%a0%e4%ba%ba%e6%9c%ba%e4%bd%bf%e7%94%a8%e6%89%8b%e5%86%8c.pdf) + + +在无人机进入GPS定点模式,且用户确认无人机相关传感器参数正常,可以正常定点飞行 + +这时候便可以远程进入openEuler,这里远程建议使用图数传模块,图数传移动端连接开发板网口,图数传地面端连接电脑网口,确保,和之前一样远程ssh连接进入开发板,同样的操作,先依次刷新环境变量,然后进入Demo文件夹,建议再次运行connect程序连接确认没有问题,确认ok之后便可以运行其他程序 + +``` + +source ~/dronkit_install/setup.sh +source ~/monotonic_install/setup.sh +source ~/future_install/setup.sh +source ~/pymavlink_install/setup.sh +source ~/pyserial_install/setup.sh + +cd Dronekit_Demo + +python connect.py + +``` + + +这里实飞程序推荐使用 example4.py,example5.py + + +``` + +python example5.py + +example5.py 控制无人机起飞到 3 米高度,然后悬停 5 秒,再自动降落 +``` + +``` + +python example4.py + + 在室外空旷处,此脚本控制无人机起飞到 3 米高度,然后飞出一个 2 米*2 米的正方形,再自动降落 +``` + +而 example1-3.py,为SITL仿真结合MP地面站运行的python示例,感兴趣的用户可以深度阅读Z410无人机使用手册,其中有详细讲解 + + +[Z410无人机使用手册](https://wiki.amovlab.com/pdf/Z410-4B%e6%97%a0%e4%ba%ba%e6%9c%ba%e4%bd%bf%e7%94%a8%e6%89%8b%e5%86%8c.pdf) + +!!!无人机飞行请谨慎注意,遵守相关法规法律,使用飞行中注意自身安全!!! diff --git a/hi-drone/openEuler_dronekit/download_repos.sh b/hi-drone/openEuler_dronekit/download_repos.sh new file mode 100755 index 0000000000000000000000000000000000000000..6b8cf43bcdf46b6cfaa9905cb2e52866ba4674a4 --- /dev/null +++ b/hi-drone/openEuler_dronekit/download_repos.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +# 定义要下载的代码包地址 +REPOS=( + "https://github.com/dronekit/dronekit-python.git" + "https://github.com/atdt/monotonic.git" + "https://github.com/PythonCharmers/python-future.git" + "https://github.com/ArduPilot/pymavlink.git" + "https://github.com/pyserial/pyserial.git" +) + +# 获取脚本所在目录 +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +# 进入脚本所在目录 +cd $SCRIPT_DIR + +# 依次克隆每个代码包 +for REPO in "${REPOS[@]}"; do + git clone $REPO +done +