水下巡检竞赛代码，树莓派控制飞控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 发送指令，调整水下机器人的位置。
通过以上代码和方法，我们就能基于树莓派和 Pixhawk 实现水下机器人的巡检功能，同时深入理解 MAVLink 协议在实际项目中的应用啦。希望大家在相关项目中能有所收获！
