基于Pymavlink协议的BlueROV开发

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的锂电池，从电池仓里面引出来一条线为控制仓供电。
BlueROV电池

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、磁罗盘和陀螺仪，用于确定车辆的方向
● 能够保存车辆日志
Pixhawk

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一栏。
QGC

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 mavutil

master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
master.wait_heartbeat()

while True:
    try:
        print(master.recv_match().to_dict())
    except:
        pass
    time.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 mavutil

def wait_conn():
    msg = None
    while 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:
        pass
    time.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 time

master = 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 available
    if mode not in master.mode_mapping():
        print('Unknown mode : {}'.format(mode))
        print('Try:', list(master.mode_mapping().keys()))
        sys.exit(1)

    # Get mode ID
    mode_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 mavutil

import 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.")
        return

    rc_channel_values = [65535 for _ in range(18)]
    rc_channel_values[channel_id - 1] = pwm
    master.mav.rc_channels_override_send(
        master.target_system,                # target_system
        master.target_component,             # target_component
        *rc_channel_values)                  # RC channel list, in microseconds.

def static():
    set_rc_channel_pwm(1,1500)  # 前后俯仰,中值1500
    set_rc_channel_pwm(2,1500)  # 左右翻滚,中值1500
    set_rc_channel_pwm(3,1500)  # 上浮下潜,中值1500
    set_rc_channel_pwm(4,1500)  # 左右偏航,中值1500
    set_rc_channel_pwm(5,1500)  # 前进后退,中值1500
    set_rc_channel_pwm(6,1500)  # 左右横移,中值1500
    set_rc_channel_pwm(8,1100)  # 左灯,中值1100
    set_rc_channel_pwm(9,1100)  # 右灯,中值1100

def 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 mavutil

master = 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 sys

from pymavlink import mavutil

master = 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 mavutil

master = 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 time

def DONotFilter():
    """不对信息进行过滤的函数"""
    master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
    while True:
        msg = master.recv_match()
        if not msg:
            continue
        if 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:
            continue
        if msg.get_type() == msg_type:
            #print("\n\n*****Got message: %s*****" % msg.get_type()) #会打印出消息的类型
            #print("Message: %s" % msg)  #会打印出进行上面一层过滤以后的所有msg
            print(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 plt

message_types = {'VFR_HUD', 'RAW_IMU', 'ATTITUDE', 'VIBRATION'}  # 把要获取消息的类型添加到字典中

def handle_VFRHUD_message(msg):
    """处理VFR_HUD类型的消息"""
    global VFRHUD_ParamsList
    groundspeed, heading, alt = msg.groundspeed, msg.heading, msg.alt
    VFRHUD_ParamsList=[groundspeed, heading, alt]
    # print('VFR_HUD', groundspeed, heading, alt)
    return VFRHUD_ParamsList

def handle_RAWIMU_message(msg):
    """处理RAW_IMU类型的消息"""
    global RAWIMU_ParamsList
    xacc, yacc, zacc = msg.xacc, msg.yacc, msg.zacc
    # print('RAW_IMU', xacc, yacc, zacc)
    RAWIMU_ParamsList=[xacc, yacc, zacc]
    return RAWIMU_ParamsList

def handle_ATTITUDE_message(msg):
    """处理ATTITUDE类型的消息"""
    global ATTITUDE_ParamsList
    roll, 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_ParamsList

def handle_VIBRATION_message(msg):
    """处理VIBRATION类型的消息"""
    global VIBRATION_ParamsList
    vibration_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_ParamsList

message_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_ParamsList
    if 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 mavutil

master = 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:
        pass
    time.sleep(0.1)

*13、控制相机云台转动至一定的角度

import time
from pymavlink import mavutil

master = 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 time

def 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 mavutil

class RawServoOutput:

    CMD_SET = mavutil.mavlink.MAV_CMD_DO_SET_SERVO

    def __init__(self, master, instance, pwm_limits=(1100, 1500, 1900),
                 set_default=True):
        self.master     = master
        self.instance   = instance
        self.min_us, self._current, self.max_us = pwm_limits
        self._diff      = self.max_us - self.min_us

        if 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 = us

    def 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

    @property
    def servo_n(self):
        return self.instance - self._main_outputs

class 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 sleep

    autopilot = 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 QuaternionBase

def 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.dt
        Iout = self.Ki * self.integral  # 积分项
        derivative = (error - self.pre_error)/self.dt
        Dout = self.Kd * derivative     # 微分项
 
        output =1454+ Pout + Iout + Dout     # 新的目标值
 
        if(output > self.max):
            output = self.max
        elif(output < self.min):
            output = self.min

        self.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:
            continue
        if 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.alt

if __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 time

from 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.dt
        Iout = self.Ki * self.integral  # 积分项
        derivative = (error - self.pre_error)/self.dt
        Dout = self.Kd * derivative     # 微分项
 
        output =1500+ Pout + Iout + Dout     # 新的目标值
 
        if(output > self.max):
            output = self.max
        elif(output < self.min):
            output = self.min

        self.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:
            continue
        if 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。