1 BlueROV概述
1.1 什么是ROV
维基百科
遥控潜水器（Remotely operated underwater vehicle，缩写ROV）是一个无人的水下航行器，以电缆连接到母船的人员操作。常搭载水下光源和照相机、摄影机、机械手臂、声纳等。因为具有机械手臂，所以又称为水下机器人。
百度百科
ROV，即遥控无人潜水器，是用于水下观察、检查和施工的水下机器人。
1.2 什么是BlueROV
国外一家公司开发的ROV产品，产品名字叫做BlueROV。
2 BlueROV结构介绍（以中型为例）
2.1 几个视图
下面先给出BlueROV的几个视图：
2.2 机械结构
如图，我大致将BlueROV机械结构分为了机架、电池仓、主控仓和其他功能模块四部分，接下来会对这几部分进行逐一介绍。
2.2.1 机架
Bluerov除去电池仓、主控仓和其他比如螺旋桨和灯等功能模块以外都是机架，机架机架主要起固定电池仓、主控仓和其他功能模块的作用。另外机架上面还可以固定后加的铅块、浮力块等物品用来调节使机体平衡。机架上部固定有浮力较大的浮体，为机体提供充足的浮力，保证机体在自然状态下或者电源耗尽的状态下能够自然浮出水面。
另外机架上面还预留了很多小孔，为开发者预留了很多接口，方便后续安装机械爪、声纳等拓展件。
2.2.2 电池仓
Bluerov机架上共固定有两个仓，其中下面的仓为电池仓。里面放置了一块18.0Ah的锂电池，从电池仓里面引出来一条线为控制仓供电。
2.2.3 主控仓
BlueROV机架上方固定的仓是主控仓。里面主要放置电源、电调、树莓派、Pixhawk、摄像头、载波模块等元器件，从控制仓里引出电源线和信号线用来为螺旋桨和灯提供能源和信号。
2.3 电气部件
2.3.1 连线
其中控制仓的接线图如下图：
电脑通过载波模块与树莓派相连；树莓派上通过USB-MicroUSB线与pixhawk相连，另外树莓派上还连接有5V6A电源为树莓派供电，一个摄像头负责获取图像；pixhawk上连接有相机云台、防水灯、电调-螺旋桨、5V6A电源、功率计、压强计。
下面对这些元器件进行逐一介绍。
2.3.2 主控仓元器件
1、树莓派
使用价格低廉的树莓派3B，用来控制Pixhawk，连接摄像头并传回图像信息。
2、电源
为树莓派3B和Pixhawk供电。
3、相机云台
用来控制相机上下运动，切换视角。
4、相机
感知外界环境，传回图像信息。
5、防水灯
照亮漆黑的海底（水底）环境，方便下水作业。
6、电调
电调有“电流控制”作用，电调内部电路有一套MOSFET管(“功率管”)。电流输入电调,内部电路接收来自接收机的信号，根据信号，把电流作出合适的“控制”,然后把“控制”后的电流输出到马达，从而控制电机的启停及转速。至于控制部分，控制信号控制电调在单位时间内开关的次数。针对电机不同，可分为有刷电调和无刷电调；有刷电调就是简单的直流输出；无刷电调就是把直流电转换成为3相交流电。电调输入是直流，可以接稳压电源，或者锂电池。
7、螺旋桨
推动ROV完成一系列运动，可以正转和反转，取决于输入的pwm控制信号。
8、功率计
及时检测电压电流等供电情况。
9、压强计
及时检测ROV所处的位置的水压，从而计算出当时的深度，使ROV能稳定在某一深度下。
10、Pixhawk
具有8个主输出通道和6个辅助输出通道，另外还有一个SB和一个RC输入通道。具有以下特征：
● 能够加载任何ArduPilot二进制固件文件（直升机，飞机，漫游者/船，潜艇）
● 包含用于连接多个外设的输入和输出连接
● 包含嵌入式 IMU、磁罗盘和陀螺仪，用于确定车辆的方向
● 能够保存车辆日志
3 软件控制
下面内容主要参考链接ardusub，部分内容来自链接文档，部分是个人经验总结。下方内容目的在于帮助初学者快速上手BlueROV的使用。
3.1 手柄控制
手柄控制需要搭配相应的控制软件QGRoundContral（下载链接：QGC下载），简称QGC，支持Windows，Ubuntu Linux等多种平台。Ubuntu安装QGC与Windows略有不同，安装方法详见QGroundControl Installation。软件安装以后需要手动配置以太网IP（192.168.2.1）才能实现连接，正常连接时左上角会出现“Ready To Fly”字样。其中部分电脑机型能正常连接但是不出现视频画面属正常情况，目前尚未找到解决方法。
软件安装好以后需要QGC连接ROV和手柄，打开“Vehicle Setup View”中的“Joystick”勾选“Enable joystick input”后，手柄此方能正常使用，手柄键位可自行摸索或者参考ardusub中QGroundControl一栏。
3.2 代码控制
3.2.1 环境配置
其本质是装一个pip包，详见链接Installation，这里仍然有几点需要说明。
Windows：
其中Windows下需要提前安装好Python3解释器，这里我推荐3.8左右版本的（高版本的似乎不支持mavproxy包，本人亲测，结果pip报错），Python3安装方法自行百度，
Ubuntu：
此时默认你已经安装好了ubuntu系统（18.04和20.04都可），因为这两个版本的ubuntu系统自带python3解释器，因此按照上述链接Installation教程一步步安装即可。
3.2.2 Pymavlink协议解读
Bluerov使用了开源的Pymavlink协议，Pymavlink协议是对Mavlink协议的Python封装，接下来是一些代码并对其进行介绍（注意标*的是重点）。
如果在学习过程中遇到问题可以访问生产BlueROV的bluerobotics公司搭建的论坛链接（友情提示：提问问题时用英文，尽量别出现中文字样，国外反华势力不容小觑），也欢迎大家访问我的个人主页交流讨论：robotmark。
1、Pixhawk直接与Linux系统电脑相连
# pixhawk通过串行连接到计算机(linux系统计算机)
from pymavlink import mavutil
master = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200) # 第一个参数是pix在linux操作系统中对应的文件名,第二个参数对应的是端口号
master.reboot_autopilot()
*2、在自己电脑上运行代码通过树莓派连接到pix
import time
from pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()while True:try:print(master.recv_match().to_dict())except:passtime.sleep(0.1)
如果出现以下信息就证明连接成功：
{'mavpackettype': 'AHRS2', 'roll': -0.11364290863275528, 'pitch': -0.02841472253203392, 'yaw': 2.0993032455444336, 'altitude': 0.0, 'lat': 0, 'lng': 0}
{'mavpackettype': 'AHRS3', 'roll': 0.025831475853919983, 'pitch': 0.006112074479460716, 'yaw': 2.1514968872070312, 'altitude': 0.0, 'lat': 0, 'lng': 0, 'v1': 0.0, 'v2': 0.0, 'v3': 0.0, 'v4': 0.0}
{'mavpackettype': 'VFR_HUD', 'airspeed': 0.0, 'groundspeed': 0.0, 'heading': 123, 'throttle': 0, 'alt': 3.129999876022339, 'climb': 3.2699999809265137}
{'mavpackettype': 'AHRS', 'omegaIx': 0.0014122836291790009, 'omegaIy': -0.022567369043827057, 'omegaIz': 0.02394154854118824, 'accel_weight': 0.0, 'renorm_val': 0.0, 'error_rp': 0.08894175291061401, 'error_yaw': 0.0990816056728363}
3、在树莓派上连接pix
import time
from pymavlink import mavutildef wait_conn():msg = Nonewhile not msg:master.mav.ping_send(int(time.time() * 1e6),0,0,0)msg = master.recv_match()time.sleep(0.5)master = mavutil.mavlink_connection('udpout:0.0.0.0:9000')
wait_conn()while True:try:print(master.recv_match().to_dict())except:passtime.sleep(0.1)
如果终端出现了以下输出，证明连接成功：
{'mavpackettype': 'AHRS2', 'roll': -0.11364290863275528, 'pitch': -0.02841472253203392, 'yaw': 2.0993032455444336, 'altitude': 0.0, 'lat': 0, 'lng': 0}
{'mavpackettype': 'AHRS3', 'roll': 0.025831475853919983, 'pitch': 0.006112074479460716, 'yaw': 2.1514968872070312, 'altitude': 0.0, 'lat': 0, 'lng': 0, 'v1': 0.0, 'v2': 0.0, 'v3': 0.0, 'v4': 0.0}
{'mavpackettype': 'VFR_HUD', 'airspeed': 0.0, 'groundspeed': 0.0, 'heading': 123, 'throttle': 0, 'alt': 3.129999876022339, 'climb': 3.2699999809265137}
{'mavpackettype': 'AHRS', 'omegaIx': 0.0014122836291790009, 'omegaIy': -0.022567369043827057, 'omegaIz': 0.02394154854118824, 'accel_weight': 0.0, 'renorm_val': 0.0, 'error_rp': 0.08894175291061401, 'error_yaw': 0.0990816056728363}
*4、锁上/解锁ROV
"""
用pymavlink协议去解锁和锁上飞行器
"""# 解锁
from pymavlink import mavutil
def arm():master = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200)master.wait_heartbeat()master.mav.command_long_send(master.target_system,master.target_component,mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,0,1, 0, 0, 0, 0, 0, 0)print("Waiting for the vehicle to arm")print('Armed!')# 上锁
def disarm():master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')master.wait_heartbeat()master.mav.command_long_send(master.target_system,master.target_component,mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,0,0, 0, 0, 0, 0, 0, 0)master.motors_disarmed_wait()
*5、更改飞行模式
import sys
from pymavlink import mavutil
import timemaster = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200)
master.wait_heartbeat()# try ['STABILIZE', 'ACRO', 'ALT_HOLD', 'AUTO', 'GUIDED', 'CIRCLE', 'SURFACE', 'POSHOLD', 'MANUAL']def change_mode(mode):# Check if mode is availableif mode not in master.mode_mapping():print('Unknown mode : {}'.format(mode))print('Try:', list(master.mode_mapping().keys()))sys.exit(1)# Get mode IDmode_id = master.mode_mapping()[mode]master.mav.set_mode_send(master.target_system,mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,mode_id)
*6、发送操作杆控制
import os
from pymavlink import mavutilimport time
master = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200)
master.wait_heartbeat()
def set_rc_channel_pwm(channel_id, pwm=1500):if channel_id < 1 or channel_id > 18:print("Channel does not exist.")returnrc_channel_values = [65535 for _ in range(18)]rc_channel_values[channel_id - 1] = pwmmaster.mav.rc_channels_override_send(master.target_system, # target_systemmaster.target_component, # target_component*rc_channel_values) # RC channel list, in microseconds.def static():set_rc_channel_pwm(1,1500) # 前后俯仰,中值1500set_rc_channel_pwm(2,1500) # 左右翻滚,中值1500set_rc_channel_pwm(3,1500) # 上浮下潜,中值1500set_rc_channel_pwm(4,1500) # 左右偏航,中值1500set_rc_channel_pwm(5,1500) # 前进后退,中值1500set_rc_channel_pwm(6,1500) # 左右横移,中值1500set_rc_channel_pwm(8,1100) # 左灯,中值1100set_rc_channel_pwm(9,1100) # 右灯,中值1100def aim(pwm1,pwm2,pwm3,pwm4):set_rc_channel_pwm(3,pwm1)#上浮下潜set_rc_channel_pwm(4,pwm2)#左拐右拐set_rc_channel_pwm(5,pwm3)#前进后退set_rc_channel_pwm(6,pwm4)#左右平移set_rc_channel_pwm(1,1500)set_rc_channel_pwm(2,1500)set_rc_channel_pwm(9, 1100)set_rc_channel_pwm(10, 1100)
7、发送手动控制
from pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()master.mav.manual_control_send(master.target_system,500,-500,250,500,0)buttons = 1 + 1 << 3 + 1 << 7
master.mav.manual_control_send(master.target_system,0,0,500,0,
buttons)
*8、读取所有参数
"""读出ROV所有参数并写入到param_data.log文件中"""
import time
import sysfrom pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()master.mav.param_request_list_send(master.target_system, master.target_component
)
file=open('param_data.log',mode='a',encoding="utf-8")
file.write('日志记录的时间: ')
file.write(time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()))
file.write('\n')
while True:#time.sleep(0.01)try:message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict()print('name: %s\tvalue: %d' % (message['param_id'],message['param_value']))file.write('name: %s\tvalue: %d' % (message['param_id'],message['param_value']))file.write('\n')except Exception as error:print(error)sys.exit(0)
9、读取和写入参数
import time
from pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()master.mav.param_request_read_send(master.target_system, master.target_component,b'SURFACE_DEPTH',-1
)message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict()
print('name: %s\tvalue: %d' %(message['param_id'].decode("utf-8"), message['param_value']))time.sleep(1)master.mav.param_set_send(master.target_system, master.target_component,b'SURFACE_DEPTH',-12,mavutil.mavlink.MAV_PARAM_TYPE_REAL32
)message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict()
print('name: %s\tvalue: %d' %(message['param_id'].decode("utf-8"), message['param_value']))time.sleep(1)master.mav.param_request_read_send(master.target_system, master.target_component,b'SURFACE_DEPTH',-1
)message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict()
print('name: %s\tvalue: %d' %(message['param_id'].decode("utf-8"), message['param_value']))
*10、对同一类型的消息进行过滤
from telnetlib import DO
from pymavlink import mavutil
import matplotlib.pyplot as plt
import timedef DONotFilter():"""不对信息进行过滤的函数"""master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')while True:msg = master.recv_match()if not msg:continueif msg: #会打印出所有msg# print("\n\n*****Got message: %s*****" % msg.get_type())print("Message: %s" % msg)def DOFilter_From_SingleType(msg_type): # msg_type是一个字符串类型的数据"""从单种类型的信息进行过滤"""master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')x_time=[]y_xacc=[]y_yacc=[]y_zacc=[]plt.ion()while True:msg = master.recv_match()#print(msg)if not msg:continueif msg.get_type() == msg_type:#print("\n\n*****Got message: %s*****" % msg.get_type()) #会打印出消息的类型#print("Message: %s" % msg) #会打印出进行上面一层过滤以后的所有msgprint(msg)x_time.append(time.time())y_xacc.append(msg.xacc)y_yacc.append(msg.yacc)y_zacc.append(msg.zacc)plt.clf() # 清除之前画的图plt.plot(x_time,y_xacc) # 画出当前 ax 列表和 ay 列表中的值的图形plt.plot(x_time,y_yacc)plt.plot(x_time,y_zacc)plt.xlabel('time')plt.ylabel('IMU')plt.pause(0.00000001) # 暂停一秒plt.ioff() # 关闭画图的窗口if __name__=='__main__':DOFilter_From_SingleType('RAW_IMU')# DO_NotFilter()
*11、对不同类型的消息进行过滤
"""用来接收不同类型的信息"""
from pymavlink import mavutil
import matplotlib.pyplot as pltmessage_types = {'VFR_HUD', 'RAW_IMU', 'ATTITUDE', 'VIBRATION'} # 把要获取消息的类型添加到字典中def handle_VFRHUD_message(msg):"""处理VFR_HUD类型的消息"""global VFRHUD_ParamsListgroundspeed, heading, alt = msg.groundspeed, msg.heading, msg.altVFRHUD_ParamsList=[groundspeed, heading, alt]# print('VFR_HUD', groundspeed, heading, alt)return VFRHUD_ParamsListdef handle_RAWIMU_message(msg):"""处理RAW_IMU类型的消息"""global RAWIMU_ParamsListxacc, yacc, zacc = msg.xacc, msg.yacc, msg.zacc# print('RAW_IMU', xacc, yacc, zacc)RAWIMU_ParamsList=[xacc, yacc, zacc]return RAWIMU_ParamsListdef handle_ATTITUDE_message(msg):"""处理ATTITUDE类型的消息"""global ATTITUDE_ParamsListroll, pitch, yaw, rollspeed, pitchspeed, yawspeed = msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed# print('ATTITUDE', roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)ATTITUDE_ParamsList=[roll, pitch, yaw, rollspeed, pitchspeed, yawspeed]return ATTITUDE_ParamsListdef handle_VIBRATION_message(msg):"""处理VIBRATION类型的消息"""global VIBRATION_ParamsListvibration_x, vibration_y, vibration_z = msg.vibration_x, msg.vibration_y, msg.vibration_z# print('VIBRATION', vibration_x, vibration_y, vibration_z)VIBRATION_ParamsList=[vibration_x, vibration_y, vibration_z]return VIBRATION_ParamsListmessage_handlers = {'VFR_HUD': handle_VFRHUD_message,'RAW_IMU': handle_RAWIMU_message,'ATTITUDE':handle_ATTITUDE_message,'VIBRATION':handle_VIBRATION_message
}# def handle_message(message):
# type_ = message.get_type()
# if type_ in message_handlers:
# message_handlers[type_](message)
# else:
# print(type_)# def DOFilter_From_DifferentType():
# """主函数"""
# # master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
# global master
# message=master.recv_match(type=message_types, blocking=True)
# type_=message.get_type()
# if not message:
# pass
# if type_ in message_handlers:
# # message_handlers[type_](message)
# if type_=='VFR_HUD':
# print(message_handlers[type_](message))
# VFRHUD_ParamsList=message_handlers[type_](message)
# if type_=='RAW_IMU':
# print(message_handlers[type_](message))
# RAWIMU_ParamsList=message_handlers[type_](message)
# if type_=='ATTITUDE':
# print(message_handlers[type_](message))
# ATTITUDE_ParamsList=message_handlers[type_](message)
# if type_=='VIBRATION':
# print(message_handlers[type_](message))
# VIBRATION_ParamsList=message_handlers[type_](message)def DOFilter_From_DifferentType():"""对不同类型的信息进行过滤"""# master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')# message_handlers[type_](message)global type_, msg, VFRHUD_ParamsList, RAWIMU_ParamsList, ATTITUDE_ParamsList, VIBRATION_ParamsListif type_=='VFR_HUD':# print(message_handlers[type_](msg))VFRHUD_ParamsList=message_handlers[type_](msg)if type_=='RAW_IMU':# print(message_handlers[type_](msg))RAWIMU_ParamsList=message_handlers[type_](msg)if type_=='ATTITUDE':# print(message_handlers[type_](msg))ATTITUDE_ParamsList=message_handlers[type_](msg)if type_=='VIBRATION':# print(message_handlers[type_](msg))VIBRATION_ParamsList=message_handlers[type_](msg)AllROV_ParamsList=[VFRHUD_ParamsList,RAWIMU_ParamsList,ATTITUDE_ParamsList,VIBRATION_ParamsList]print(AllROV_ParamsList)if __name__=='__main__':print(11111111111)master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')while 1:msg=master.recv_match(type=message_types, blocking=True)type_=msg.get_type()if type_ in message_handlers:DOFilter_From_DifferentType()else:continue
12、设置请求消息间隔
import time
from pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()def request_message_interval(message_id: int, frequency_hz: float):master.mav.command_long_send(master.target_system, master.target_component,mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,message_id,1e6 / frequency_hz,0, 0, 0, 0,0,)request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_AHRS2, 1)
request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 2)while True:try:print(master.recv_match().to_dict())except:passtime.sleep(0.1)
*13、控制相机云台转动至一定的角度
import time
from pymavlink import mavutilmaster = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()def look_at(tilt, roll=0, pan=0):master.mav.command_long_send(master.target_system,master.target_component,mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,1,tilt,roll,pan,0, 0, 0,mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)look_at(0,0,0) # 归中
*14、如果bluerov外加了机械爪，可以通过pix去直接控制机械爪的舵机
from pymavlink import mavutil
import timedef set_servo_pwm(servo_n, microseconds):master.mav.command_long_send(master.target_system, master.target_component,mavutil.mavlink.MAV_CMD_DO_SET_SERVO,0,servo_n + 8,microseconds,0,0,0,0,0)master = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200)
master.wait_heartbeat()def Ongripper():for us in range(1775, 1925, 25):set_servo_pwm(4, us)time.sleep(0.125)def Offgripper():for us in range(1900, 1600, -25):set_servo_pwm(4, us)time.sleep(0.125)if __name__ == "__main__":Offgripper()
15、高级舵机控制
from pymavlink import mavutilclass RawServoOutput:CMD_SET = mavutil.mavlink.MAV_CMD_DO_SET_SERVOdef __init__(self, master, instance, pwm_limits=(1100, 1500, 1900),set_default=True):self.master = masterself.instance = instanceself.min_us, self._current, self.max_us = pwm_limitsself._diff = self.max_us - self.min_usif set_default:self.set_direct(self._current)def set_direct(self, us):assert self.min_us <= us <= self.max_us, "Invalid input value."self.master.mav.command_long_send(self.master.target_system, self.master.target_component,self.CMD_SET,0,self.instance, us,0,0,0,0,0)self._current = usdef set_ratio(self, proportion):self.set_direct(proportion * self._diff + self.min_us)def inc(self, us=50):self.set_direct(max(self.max_us, self._current+us))def dec(self, us=50):self.set_direct(min(self.min_us, self._current-us))def set_min(self):self.set_ratio(0)def set_max(self):self.set_ratio(1)def center(self):self.set_ratio(0.5)class AuxServoOutput(RawServoOutput):def __init__(self, master, servo_n, main_outputs=8, **kwargs):super().__init__(master, servo_n+main_outputs, **kwargs)self._main_outputs = main_outputs@propertydef servo_n(self):return self.instance - self._main_outputsclass Gripper(AuxServoOutput):def __init__(self, master, servo_n, pwm_limits=(1100, 1100, 1500),**kwargs):super().__init__(master, servo_n, pwm_limits=pwm_limits, **kwargs)def open(self):self.set_max()def close(self):self.set_min()if __name__ == '__main__':from time import sleepautopilot = mavutil.mavlink_connection('udpin:0.0.0.0:14550')autopilot.wait_heartbeat()gripper = Gripper(autopilot, 1)for _ in range(3):gripper.open()sleep(2)gripper.close()sleep(1)
*16、设置目标深度和姿态（只在深度保持下才能用）
import time
import math
from pymavlink import mavutil
from pymavlink.quaternion import QuaternionBasedef set_target_attitude(roll, pitch, yaw):master.mav.set_attitude_target_send(int(1e3 * (time.time() - boot_time)),master.target_system, master.target_component,mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE,QuaternionBase([math.radians(angle) for angle in (roll, pitch, yaw)]),0, 0, 0, 0)master = mavutil.mavlink_connection("/dev/ttyACM0", baud=115200)
boot_time = time.time()
master.wait_heartbeat()master.arducopter_arm()
master.motors_armed_wait()DEPTH_HOLD = 'ALT_HOLD'
DEPTH_HOLD_MODE = master.mode_mapping()[DEPTH_HOLD]
while not master.wait_heartbeat().custom_mode == DEPTH_HOLD_MODE:master.set_mode(DEPTH_HOLD)roll_angle = pitch_angle = 0
for yaw_angle in range(0, 500, 10):set_target_attitude(roll_angle, pitch_angle, yaw_angle)time.sleep(1)for yaw_angle in range(500, 0, -30):set_target_attitude(roll_angle, pitch_angle, yaw_angle)time.sleep(1)master.arducopter_disarm()
master.motors_disarmed_wait()
另外也可以通过自己读出深度计的数据直接设置目标深度：
"""给ROV设置深度"""
import sys
import time
import matplotlib.pyplot as plt
from pymavlink import mavutil
from move.action import *
from pid_catalog.read import *
from move.arm_disarm import arm,disarm
from move.init import *class UD_PID():def __init__(self, dt, max, min, Kp, Kd, Ki):self.dt = dt # 循环时长self.max = max # 操作变量最大值self.min = min # 操作变量最小值self.Kp = Kp # 比例增益self.Kd = Kd # 积分增益self.Ki = Ki # 微分增益self.integral = 0 # 直到上一次的误差值self.pre_error = 0 # 上一次的误差值def calculate(self, setPoint, pv):# 其中 pv:process value 即过程值，error = setPoint - pv # 误差Pout = self.Kp * error # 比例项self.integral += error * self.dtIout = self.Ki * self.integral # 积分项derivative = (error - self.pre_error)/self.dtDout = self.Kd * derivative # 微分项output =1454+ Pout + Iout + Dout # 新的目标值if(output > self.max):output = self.maxelif(output < self.min):output = self.minself.pre_error = error # 保存本次误差，以供下次计算return output
ud_pid = UD_PID(0.1, 1494, 1414, 250, 100 , 100) x_pwm=[]
y_depth=[]# 传入预设的深度和布尔值(相当于一个开关的作用)，返回当前的深度
def set_target_depth(depth,Boolean):master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')plt.ion()while Boolean:msg = master.recv_match()if not msg:continueif msg.get_type() == 'VFR_HUD':print(msg.alt)a = ud_pid.calculate(depth,msg.alt)b = int(a)x_pwm.append(b)y_depth.append(msg.alt)plt.clf()plt.plot(x_pwm,y_depth)plt.pause(0.1)plt.ioff()print('b: ',b)throttle(b)return msg.altif __name__=='__main__': master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')master.wait_heartbeat()disarm()init()arm() #解锁 while 1:set_target_depth(-0.25,True)
通过指南针数据设置目标朝向：
"""根据地球磁场设置ROV的偏航角度"""
import sys
import timefrom pymavlink import mavutil
from move.action import *
from pid_catalog.read import *
from move.arm_disarm import arm,disarm
from move.init import *
from visual.door import *class ATTITUDE_PID():def __init__(self, dt, max, min, Kp, Kd, Ki):self.dt = dt # 循环时长self.max = max # 操作变量最大值self.min = min # 操作变量最小值self.Kp = Kp # 比例增益self.Kd = Kd # 积分增益self.Ki = Ki # 微分增益self.integral = 0 # 直到上一次的误差值self.pre_error = 0 # 上一次的误差值def calculate(self, setPoint, pv):# 其中 pv:process value 即过程值，error = setPoint - pv # 误差Pout = self.Kp * error # 比例项self.integral += error * self.dtIout = self.Ki * self.integral # 积分项derivative = (error - self.pre_error)/self.dtDout = self.Kd * derivative # 微分项output =1500+ Pout + Iout + Dout # 新的目标值if(output > self.max):output = self.maxelif(output < self.min):output = self.minself.pre_error = error # 保存本次误差，以供下次计算return output
attitude_pid = ATTITUDE_PID(0.1, 1540, 1460, 5, 0 , 0) # 这个是角度制的，正北方向是0°和360°，忘记是顺时针还是逆时针递增了
def set_target_attitude(attitude,Boolean):master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')while Boolean:msg = master.recv_match()if not msg:continueif msg.get_type() == 'VFR_HUD':print(msg.heading)a=attitude_pid.calculate(attitude,msg.heading)b=int(a)print('b: ',b)yaw(b)if __name__=='__main__':master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')master.wait_heartbeat()disarm()init()arm() #解锁set_target_attitude(360,True) #让ROV朝正北方向转
17、发送全球定位系统位置
import time
from pymavlink import mavutil
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()while True:time.sleep(0.2)master.mav.gps_input_send(0,0,(mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY),0,0,3,0,0,0,1,1,0,0,0,0,0,0,7)
18、附上我之前获取信息记录的日志文件msg.log，这刚好是一个周期，下次仍从VFR_HUD开始。
日志记录的时间为: 2022-05-10 14:51:51
VFR_HUD {airspeed : 0.0, groundspeed : 0.07441851496696472, heading : 342, throttle : 0, alt : -0.019999999552965164, climb : -0.0017754812724888325}
SERVO_OUTPUT_RAW {time_usec : 1032604820, port : 0, servo1_raw : 1500, servo2_raw : 1500, servo3_raw : 1500, servo4_raw : 1500, servo5_raw : 1500, servo6_raw : 1500, servo7_raw : 0, servo8_raw : 0, servo9_raw : 0, servo10_raw : 1100, servo11_raw : 0, servo12_raw : 0, servo13_raw : 0, servo14_raw : 0, servo15_raw : 0, servo16_raw : 0}
RC_CHANNELS {time_boot_ms : 1032607, chancount : 0, chan1_raw : 1500, chan2_raw : 1500, chan3_raw : 1500, chan4_raw : 1500, chan5_raw : 1500, chan6_raw : 1500, chan7_raw : 1500, chan8_raw : 1500, chan9_raw : 1100, chan10_raw : 1100, chan11_raw : 1100, chan12_raw : 0, chan13_raw : 0, chan14_raw : 0, chan15_raw : 0, chan16_raw : 0, chan17_raw : 0, chan18_raw : 0, rssi : 0}
RAW_IMU {time_usec : 1032607166, xacc : 25, yacc : -18, zacc : -996, xgyro : 0, ygyro : -4, zgyro : -36, xmag : 119, ymag : 161, zmag : 334, id : 0, temperature : 0}
SCALED_IMU2 {time_boot_ms : 1032607, xacc : -20, yacc : -39, zacc : -976, xgyro : -2, ygyro : 7, zgyro : 9, xmag : 0, ymag : 0, zmag : 0, temperature : 0}
SCALED_PRESSURE {time_boot_ms : 1032607, press_abs : 524.38818359375, press_diff : 0.0, temperature : 4625, temperature_press_diff : 0}
SCALED_PRESSURE2 {time_boot_ms : 1032607, press_abs : 1022.3999633789062, press_diff : 0.0, temperature : 2442, temperature_press_diff : 0}
SYSTEM_TIME {time_unix_usec : 0, time_boot_ms : 1032609}
AHRS {omegaIx : 0.0005717225722037256, omegaIy : 0.0045538474805653095, omegaIz : 0.04834114387631416, accel_weight : 0.0, renorm_val : 0.0, error_rp : 0.001656720181927085, error_yaw : 0.020652037113904953}
AHRS2 {roll : 0.022335169836878777, pitch : -0.034166205674409866, yaw : -0.8668319582939148, altitude : 0.0, lat : 0, lng : 0}
AHRS3 {roll : -0.004018092527985573, pitch : 0.014819731004536152, yaw : -0.29812362790107727, altitude : 0.0, lat : 0, lng : 0, v1 : 0.0, v2 : 0.0, v3 : 0.0, v4 : 0.0}
HWSTATUS {Vcc : 4935, I2Cerr : 0}
MOUNT_STATUS {target_system : 0, target_component : 0, pointing_a : 0, pointing_b : 0, pointing_c : 0, mount_mode : 0}
EKF_STATUS_REPORT {flags : 421, velocity_variance : 0.0, pos_horiz_variance : 0.014007089659571648, pos_vert_variance : 0.001747242291457951, compass_variance : 0.6115934252738953, terrain_alt_variance : 0.0, airspeed_variance : 0.0}
VIBRATION {time_usec : 1032615036, vibration_x : 0.025633109733462334, vibration_y : 0.03569412976503372, vibration_z : 0.024316098541021347, clipping_0 : 0, clipping_1 : 0, clipping_2 : 0}
BATTERY_STATUS {id : 0, battery_function : 0, type : 0, temperature : 32767, voltages : [16226, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535], current_battery : 69, current_consumed : 197, energy_consumed : 115, battery_remaining : -1, time_remaining : 0, charge_state : 0, voltages_ext : [0, 0, 0, 0], mode : 0, fault_bitmask : 0}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : CamTilt, value : 0.5}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : CamPan, value : 0.5}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : TetherTrn, value : 0.0}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : Lights1, value : 0.0}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : Lights2, value : 0.5}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : PilotGain, value : 0.5}
NAMED_VALUE_FLOAT {time_boot_ms : 1032615, name : InputHold, value : 0.0}
ATTITUDE {time_boot_ms : 1032701, roll : -0.004012368619441986, pitch : 0.014825592748820782, yaw : -0.2980099320411682, rollspeed : 0.000885059533175081, pitchspeed : 0.0003427495248615742, yawspeed : 0.012221965938806534}
GLOBAL_POSITION_INT {time_boot_ms : 1032701, lat : 0, lon : 0, alt : -20, relative_alt : -25, vx : 0, vy : 7, vz : 0, hdg : 34293}
SYS_STATUS {onboard_control_sensors_present : 35716111, onboard_control_sensors_enabled : 35691535, onboard_control_sensors_health : 36740111, load : 404, voltage_battery : 16234, current_battery : 69, battery_remaining : -1, drop_rate_comm : 0, errors_comm : 0, errors_count1 : 0, errors_count2 : 0, errors_count3 : 0, errors_count4 : 0}
POWER_STATUS {Vcc : 4933, Vservo : 4993, flags : 4}
MEMINFO {brkval : 0, freemem : 65535, freemem32 : 127776}
NAV_CONTROLLER_OUTPUT {nav_roll : -0.2298368662595749, nav_pitch : 0.8494492173194885, nav_bearing : -17, target_bearing : 0, wp_dist : 0, alt_error : 0.029539374634623528, aspd_error : 0.0, xtrack_error : 0.0}
MISSION_CURRENT {seq : 0}
3.2.3 如何使用Pymavlink来控制BlueROV运动（以巡线为例）
主函数思路：
1、首先需要将BlueROV解锁（比如在完成一些导包后调用前面的arm()函数使ROV解锁）
2、调整相机云台的朝向（巡线肯定是要向下看）
3、可以考虑创建两个线程以保证信息处理的实时性：
（1）视觉线程：
通过视觉代码把管道的中心点横坐标进行归一化，其中像素坐标系最左边是-1，最后边是1，中间是0，因此最终值 x∈[-1, 1]。
（2）动作线程：
其中动作包括两部分：
第一需要考虑到稳深控制，ROV本身的稳深模式“ALT_HOLD”不好用，可以自己读取深度计 并通过PID控制ROV的深度。
第二需要考虑到方向控制，可以用视觉返回的x通过PID计算出需要输出的pwm，进而对ROV的转向进行控制。
两个线程通过队列（queue）进行通信。
4、巡线结束后把机器人给锁上（使用前面封装好的disarm()函数锁上机器人）
**提示：**可以创建一个ROV类，把ROV需要用到的函数和参数写进去，面向对象。
4 工作过程
4.1 ROV下水前的准备
1.安装电池
2.测量电池仓和控制仓的气密性
3.安装通气螺栓并检查是否是否拧紧
4.检查螺旋桨附近是否有异物
4.2 ROV下水后进行观察
观察ROV是否出现了前后不平、左右不平的状况，如果出现这种状况，应该对ROV采用添加浮力块或者增加配重的方法对ROV进行调平，否则ROV可能会在运动的过程中出现运动不稳定的现象，不能够平稳运行。
4.3 ROV上岸前
将机器开回回收点，一定将机器锁上，禁止将手伸进ROV螺旋桨里面！！！
4.4 ROV上岸后
用毛巾把电池仓附近的水擦干，拧出通气螺栓，然后再拔出电池仓后盖并取出电池，电池快没电的话就对电池进行充电。
5 常见问题(目前想到的)
1、在自主代码调试过程中ROV在水中出现失控乱动的情况
首先不要慌张，安全要紧！！！如果是有缆调试的话“CTRL+C”结束进程，如果行不通的话请打开QGC，然后用QGC把机器锁上。
原理：机器只能受QGC或者代码一个进程控制，当QGC启动的时候，机器自动脱离代码的控制。
2、机器出现动力不足，无法正常运动
检查机器螺旋桨是否缠到异物以及电池是否快要没电了。
3、在调试代码的时候发现机器没有按照代码控制进行运动（代码确信正确），而是待在原地静止不动
1、如果之前打开过QGC的话，检查QGC是否关闭
2、很有可能是机器触发了故障保护机制，当机器接收信息的频率过低或者阻塞时，BlueROV会触发其自身的故障保护，防止ROV在不受人类控制下走丢。有兴趣的同学可参考我主页上的问题 I couldn’t connect to the ROV after resuming the thread。
![[黑马程序员Pandas教程]——Pandas缺失值处理](https://img-blog.csdnimg.cn/d4b27ff3208945a597e62d6cf760f064.png)
