水下巡检竞赛代码，树莓派控制飞控stm32ros无线控制水下机器人控制水下机器人，只是实现巡检的功能，可以让你快速上手了解mvlink协议，前提得是pixhawk和树莓派，飞控树莓派，是针对巡检的代码，阈值纠偏 中心点纠偏，pix2.4.8 树莓派4b

最近参与水下巡检竞赛，研究了基于树莓派 4B 和 Pixhawk（版本 2.4.8）实现水下机器人巡检功能的代码，这里和大家分享下我的经验，希望能帮助你快速上手了解 MAVLink 协议。

整体架构

整个系统以树莓派作为主控单元，负责与外部通信、处理图像数据并向飞控 STM32（集成在 Pixhawk 中）发送指令。Pixhawk 则根据接收到的指令控制水下机器人的动作。

飞控与树莓派通信 - MAVLink 协议

MAVLink 协议是一种轻量级的消息协议，用于在飞行器组件之间进行通信。在树莓派端，我们可以使用 pymavlink 库来实现与 Pixhawk 的通信。

安装 pymavlink

pip install pymavlink

简单的连接代码示例

from pymavlink import mavutil

# 创建连接
master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')

# 等待心跳包，以确保连接成功
master.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (master.target_system, master.target_component))

在这段代码中，我们通过 mavutil.mavlinkconnection 建立了与 Pixhawk 的 UDP 连接。waitheartbeat 方法会阻塞程序，直到接收到来自 Pixhawk 的心跳包，表明连接成功。

巡检功能实现 - 阈值纠偏与中心点纠偏

在巡检过程中，我们需要根据摄像头获取的图像信息来调整水下机器人的位置，这就涉及到阈值纠偏和中心点纠偏。

阈值纠偏

假设我们通过图像识别获取到目标物体的颜色信息，以红色为例，我们可以设定一个颜色阈值范围来判断是否检测到目标。

import cv2
import numpy as np

# 读取图像
image = cv2.imread('underwater_image.jpg')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

# 定义红色的 HSV 阈值范围
lower_red = np.array([0, 100, 100])
upper_red = np.array([10, 255, 255])

# 根据阈值构建掩膜
mask = cv2.inRange(hsv, lower_red, upper_red)

# 对原图像和掩膜进行位运算
res = cv2.bitwise_and(image, image, mask=mask)

cv2.imshow('Original', image)
cv2.imshow('Mask', mask)
cv2.imshow('Result', res)
cv2.waitKey(0)
cv2.destroyAllWindows()

在这段代码里，我们将图像从 BGR 颜色空间转换到 HSV 颜色空间，然后通过 cv2.inRange 函数根据设定的阈值范围创建掩膜。这样就能筛选出图像中符合红色阈值范围的部分，以便后续判断是否需要纠偏。

中心点纠偏

当检测到目标后，我们要计算目标在图像中的中心点，并与图像中心进行比较，从而确定如何调整水下机器人的方向。

# 计算掩膜的矩
M = cv2.moments(mask)
if M["m00"]!= 0:
    cX = int(M["m10"] / M["m00"])
    cY = int(M["m01"] / M["m00"])
else:
    cX, cY = 0, 0

# 获取图像尺寸
height, width, _ = image.shape
image_center_x = width // 2
image_center_y = height // 2

# 计算偏差
delta_x = cX - image_center_x
delta_y = cY - image_center_y

print(f"X 方向偏差: {delta_x}, Y 方向偏差: {delta_y}")

这里通过 cv2.moments 函数计算掩膜的矩，进而得到目标的中心点坐标 cX 和 cY。然后获取图像的中心坐标，计算出与目标中心点的偏差 deltax 和 deltay。根据这些偏差值，我们就可以通过 MAVLink 协议向 Pixhawk 发送指令，调整水下机器人的位置。

水下巡检竞赛代码，树莓派控制飞控stm32ros无线控制水下机器人控制水下机器人，只是实现巡检的功能，可以让你快速上手了解mvlink协议，前提得是pixhawk和树莓派，飞控树莓派，是针对巡检的代码，阈值纠偏 中心点纠偏，pix2.4.8 树莓派4b

通过以上代码和方法，我们就能基于树莓派和 Pixhawk 实现水下机器人的巡检功能，同时深入理解 MAVLink 协议在实际项目中的应用啦。希望大家在相关项目中能有所收获！