使用esp32cam和树莓派制作简易图传遥控器（基于UDP）

目录

1. 小车端摄像头及wifi初始化

2. 建立UDP连接

3. 小车拍照并传输图像

4. 树莓派回传控制指令至esp32小车

---

我们最近想开发一款图传小车，先用最简单最易上手的硬件，来自AI-Thinker的esp32cam来做图像采集、发送端，并使用树莓派来接收并显示吧。

小车端使用了esp32cam，这个模块非常好用，集成了wifi和摄像头，初测wifi性能还不错，在办公室环境下传480x320p的图像很流畅，偶尔卡顿是因为被水泥墙遮挡。

接收端使用树莓派4B加一块3.5寸的GPIO屏，这边应该是LCD屏走的SPI接口的原因就比较拉跨了，刷新率基本每秒一帧吧，后续换成HDMI的屏幕应该会好一些。不过好歹系统搭起来了，以此文档记录一下。主程序如下：

#include 
#include "esp32_car.h"

void Camera_Initialization ();
void Wifi_setup();
void Streaming();

void setup() {

  Serial.begin(115200);
  Serial.setDebugOutput(true);
  delay(1000);

  Camera_Initialization ();
  Wifi_setup();
  Streaming();
}

void loop() {
}

具体实现分成以下几个部分：

1. 小车端摄像头及wifi初始化

这里我们使用的是esp32cam自带的ov2640摄像头，性能一般。小白如我最好选用AI Thinker官配的那个摄像头（短排线），因为我们更换了一个排线长度7.5cm的同样型号的ov2640摄像头以后就出现了各种莫名其妙的错误，如

• camera: Timeout waiting for VSYNC
• SCCB_Write(): SCCB_Write Failed addr:0x30, reg:0xe1, data:0x67, ret:263

网上搜到相关报错，基本是说自己配的摄像头质量太差，建议换回官配摄像头。但其实导致这些错误的根本原因是

1. 数据量太大（例程里使用的QVGA分辨率太高）；
2. 摄像头频率太高（XCLK默认20MHz）；
3. 主板带不动这么长的排线（信号经过太长的排线传输后损耗过大，以至达不到所需的电平阈值）。

知道了这些，解决起来就有头绪了，无非是降低摄像头分辨率frame_size，以及降低摄像头的时钟频率xclk_freq_hz。根据我们的实验结果，降低其中一个参数即可。时钟频率xclk这个参数比较微妙一些，我们测试最低也只能到5MHz。

config.frame_size = FRAMESIZE_HVGA;
config.xclk_freq_hz = 10000000;

实验测试时，我们还是会零星观察到之前提到的两个问题（VSYNC和SBBC）。为了提高程序的可靠性，我们加了一段代码，若摄像头初始化不成功，则过一秒重启系统。

while (esp_camera_init(&config) != ESP_OK) {
    Serial.println("Camera initalization failed");
    esp_deep_sleep(1000000); // Restart after 1 second
  }

摄像头初始化告一段落，接下来就是初始化wifi。道理很简单，就是创立一个AP热点wifi，取名esp32cam，到时候让树莓派终端连到这个wifi就可以了。

为了使用方便，我们为树莓派端代码加入了自动连接小车wifi的功能，这里需要先在树莓派使用pip3 install下载所需的两个python3的库，分别为pywifi和comtypes。树莓派连接wifi参考的是python pywifi模块——暴力破解wifi - komomon - 博客园

具体实现分为以下几个步骤：

• 获取无线网卡相关信息
• 断开现有wifi
• 连接esp32cam建立的wifi热点

import pywifi
from pywifi import *
import time

# Auto connect to esp32cam wifi
# First disconnect from the current wifi
wifi = pywifi.PyWiFi()
flag_exist_wifi = 0
for iface_i in range(len(wifi.interfaces())):
    iface = wifi.interfaces()[iface_i]
    if iface.status() == 4:
        flag_exist_wifi = 1
        break

if flag_exist_wifi:
    print("Found wifi already connected!")
    print(iface.name())
    print("Now discoonect")
    iface.disconnect()
else:
    print("No existing wifi, now connecting to new wifi")

# Then connect to esp32cam wifi
profile = pywifi.Profile()
profile.ssid = 'esp32cam'
profile = iface.add_network_profile(profile)
iface.connect(profile)

time.sleep(5) # Wait for 5 seconds to connect
if iface.status()==const.IFACE_CONNECTED:
    print("Connect to esp32cam")
else:
    print("Error in connection to esp32cam wifi!")

2. 建立UDP连接

小车端通过udp.begin函数来创建一个UDP连接，需要知道小车IP（由于之前我们为小车创立了AP wifi，其IP默认是192.168.4.1），以及小车端口号CAR_UDP_PORT（我们使用宏定义其为1122）。也就是说，我们事先是知道小车的IP和端口号的，但是，我们并不知道树莓派的IP地址。

// Start a udp service
  udp.begin(myIP, CAR_UDP_PORT);

若我们想往树莓派发送数据，就必须自动获取树莓派的IP。这需要树莓派在连上小车的wifi后像小车的IP和端口发送一条信息，这样小车通过监听这个端口号就能知道树莓派的IP了。这里调用的是udp.remoteIP()和udp.remotePort()这两个函数，具体实现如下：

// Once the controller sends a string to me and then I can get his IP
void GetRemoteIP() {

  uint8_t rBuff[256]; //UDP receive buffer

  // Listen at "CAR_UDP_PORT"
  while (1) {

    // Check to see if UDP packet has come to me
    int len = udp.parsePacket();

    if (len > 0)
    {
      // Read the incoming string to rBuff
      len = udp.read(rBuff, len);
      Serial.write(rBuff, len);

      //Get remote ip
      toAddress = udp.remoteIP();
      toPort = udp.remotePort();
      Serial.print("\n Remote IP address: ");
      Serial.print(toAddress);
      Serial.printf(":%d\n", toPort);

      break;
    }
  }
}

对应的，树莓派端需要给小车发一条信息，来让小车监听到。在发之前，树莓派首先需要知道自己的IP地址，这个可以通新建立一个socket至任何合法的IP地址（无所谓能不能ping通，如8.8.8.8即可）并根据返回的getsockname()函数得到。接着是树莓派将它自己的IP绑定至一个socket套接字usoc。这里要注意两点，一是这里的套接字s只是为了得到树莓派的IP地址而创建的；真正用来和esp32cam沟通的套接字是后面发信息用的这个usoc。二是通过建立socket连接的途径查找本地IP时必须等足够的时间（如10秒）让wifi连接好，否则会提示address not reachable的错误。

import socket

# Get wlan0 IP address
time.sleep(10) # Wait long enough for wifi to get ready
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.connect(("8.8.8.8", 80))
IP_controller = (s.getsockname()[0])
print(IP_controller)
s.close()

usoc = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) #UDP socket
usoc.bind((IP_controller, 6000)) #IP & port

# Send a message to car in order for car to get my IP
usoc.sendto(b'from pi', ('192.168.4.1', 1122))

3. 小车拍照并传输图像

小车的代码主要参考了官方例程里的CameraWebServer和B站的大牛UP主“技术宅物语”的这个教程ESP32加Python实现无线视频传输，技术宅开源。

大概的思路就是使用esp32开启一个UDP服务，将摄像头采集的画面一帧一帧先打成UDP包传到遥控器树莓派端，树莓派收到UDP包后再还原出一帧一帧的图片，然后使用opencv显示出来。参考视频里的UP主提到他没有使用esp32cam因为摄像头有些问题，其实是他发完图忘记调用esp_camera_fb_return(fb)来清摄像头缓存了。亲测他这个代码实现是可以兼容esp32cam的。

// Take a picture and UDP send to remote port
void Streaming() {

  camera_fb_t * fb = NULL;
  esp_err_t res = ESP_OK;
  uint8_t rBuff[256]; //UDP receive buffer

  // Start a udp service
  udp.begin(myIP, CAR_UDP_PORT);

  // Get remote controller IP address
  GetRemoteIP();

  int fid = 0; // Frame ID
  while (1)
  {
    // Capture a picture
    fb = esp_camera_fb_get();

    // Send the image
    if (fb)
    {
      udp_send_chunk((uint8_t *)fb->buf, fb->len, ++fid);
      esp_camera_fb_return(fb);
      fb = NULL;
    }
    //    delay(100);
  }
}

树莓派端使用opencv将拿到的frame转成jpg再全屏显示到LCD屏上。LCD屏的驱动参考在这里：3.5inch RPi Display - LCD wiki

nparr = np.frombuffer(jpgBuff, dtype=np.uint8)
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
out_win = "esp32cam"
cv2.namedWindow(out_win, cv2.WINDOW_NORMAL)
cv2.setWindowProperty(out_win, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
cv2.imshow(out_win, image)

有时候，python3会报错不能import cv2库。可以参考这个帖子树莓派python3安装cv2_树莓派3b使用pip和apt安装Python3 Opencv_weixin_39594312的博客-CSDN博客

来安装。具体方法归纳如下：

sudo apt-get install python3-numpy
Sudo pip3 install opencv-python
Sudo apt-get install libatlas-base-dev
Sudo pip3 install numpy --upgrade

打好了“图传”的框架，我们就可以开始正式使用UDP传输了。小车端按照下图将摄像头拍摄到的frame打包成UDP包，这里包长是packetlen=1000。注意这里我们为了让程序看起来简洁一些，没有采用上述的“高位在前”的方式来存储“帧号、帧体积、标准包长度、此包长度”这些量；相反，我们直接用memcpy函数使用“低位在前”的方式将这些参数写入数据包头中。

void udp_send_chunk(uint8_t* frame, size_t len, size_t frameCount)
{
  uint8_t txBuffer[1024] = {0};
  size_t frameId = frameCount;
  size_t frameSize = len;

  int packetCount = 0;
  int packetId = 1;
  int packetLen = 1000;
  int packetSize = 0;

  if (frameSize == 0)
  {
    Serial.printf("Send buffer len=0.\r\n");
    return;
  }

  packetCount = frameSize / packetLen + ((frameSize % packetLen) == 0 ? 0 : 1);

  size_t sendOffset = 0;
  while (sendOffset < frameSize)
  {

    packetSize = ((sendOffset + packetLen) > frameSize) ? (frameSize - sendOffset) : (packetLen);

    //Header
    txBuffer[0] = 0x12;
    memcpy(&txBuffer[1], &frameId, 4);
    memcpy(&txBuffer[5], &frameSize, 4);
    txBuffer[9] = packetCount;
    txBuffer[10] = packetId;
    memcpy(&txBuffer[11], &packetLen, 2);
    memcpy(&txBuffer[13], &packetSize, 2);

    // Data
    memcpy(&txBuffer[15], frame + sendOffset, packetSize);

    //UDP send packet
    udp.beginPacket(toAddress, toPort);
    udp.write((const  uint8_t *)txBuffer, 15 + packetSize);
    udp.endPacket();

    //Set send offset to next position
    sendOffset += packetSize;
    packetId++;
  }
}

 对应的，树莓派端需要将收到的包拼接成frame图片。

    udpbuff, address = usoc.recvfrom(10240)
	
	frameId = (udpbuff[4] << 24) + (udpbuff[3] << 16) + (udpbuff[2] << 8) + udpbuff[1]
	frameSize = (udpbuff[8] << 24) + (udpbuff[7] << 16) + (udpbuff[6] << 8) + udpbuff[5]
	packetId = udpbuff[10]
	packetSize = (udpbuff[14] << 8) + udpbuff[13]

	if frameIdNow != frameId:
		frameIdNow = frameId
		frameSizeNow = frameSize
		packetCount = udpbuff[9]
		packetLen = (udpbuff[12] << 8) + udpbuff[11]
		frameSizeOk = 0
		packetIdNow = 0
		jpgBuff = bytes('', 'utf-8')
			
	if (packetId <= packetCount) and (packetId > packetIdNow):
		if packetSize == (len(udpbuff)-15):
			if (packetSize == packetLen) or (packetId == packetCount):
				jpgBuff = jpgBuff + udpbuff[15:]
				frameSizeOk = frameSizeOk + len(udpbuff) - 15

4. 树莓派回传控制指令至esp32小车

其实，我们可以在小车一帧一帧发送图片的while循环里监听udp的receive buffer，达到接受树莓派发来的指令的目的。如接收到树莓派发来的“F”指令后，esp32小车会向串口打印“Car moving forward”。除此以外，树莓派还可以定时向小车端发送“heartbear”心跳信息，让小车端时刻明晰网络情况，检查树莓派遥控器是否掉线，进而进行一些例如急停的处理。树莓派端具体实现为：

usoc.sendto(b'heartbeat', ('192.168.4.1', 1122))

小车端的具体实现为：

    int len = udp.parsePacket();
    if (len > 0)
    {
      len = udp.read(rBuff, len);
      //      Serial.write(rBuff, len);

      char rBuff_short[len + 1] = {0};
      strncpy(rBuff_short, (char *)rBuff, len);
      rBuff_short[len + 1] = '\0';

      if (!strcmp(rBuff_short, "heartbeat")) {
        Serial.print("heartbeat ");
      }

      if (!strcmp(rBuff_short, "F")) {
        Serial.print("Car moving forward!");
      }

      if (!strcmp(rBuff_short, "q")) {
        Serial.println("End streaming");
        break;
      }
    }