STM32平台下获取Pixy2与物体之间的距离

想要获取Pixy2与物体之间的距离，你需要先测量或者得到以下几个关键数据：

• 需要进行测距物体的已知实际宽度W
• 物体与Pixy2之间的已知距离D
• 在上面距离下，Pixy2测量到的物体像素宽度P

PS：像素宽度 P指的是Pixy2视野里物体的宽度，就是你向Pixy2查询getBlocks()的颜色块的宽度Width

上面三个关键参数知道后，我们就可以计算出Pixy2相机的焦距F：

F = (P x D) / W

知道焦距后，我们设在物体与Pixy2相距不同距离情况下，Pixy2测得物体的像素宽度为pixy2_width（这个宽度可以向Pixy2查询获得）。那么Pixy2与物体之间的距离为：

Distance = (W x F) / pixy2_width

实例

首先测量物体与Pixy2固定距离时，Pixy2测到的像素宽度P

我这里用蓝色卡纸做了一个边长为20mm的正方形，也就是W=20，来看看用STM32查询Pixy2颜色块返回的信息。

主要看Width就可以了，Height有点小，是因为光线问题Pixy2测到的高度有点短，但是宽度是正确的，我们这里就选取像素宽度P=112。这个正方形卡纸与Pixy2之间的已知距离D=50，就像首图那样，两者相距50mm，那么我们就能得到焦距F：

F = (P x D) / W = (112 x 50) / 20 = 280

有了焦距F，我们就可以根据公式来编写检测Pixy2与物体间距离的程序了，如下：

int main(void)
 { 
    float  W = 20.0, Distance = 0.0;
    float  F = 280.0;  //焦距
    while(1)
    {
        //查询颜色块信息
        Pixy2_SendRecvPacket_getBlocks(recvBuf, PIXY2_SIGMAP, PIXY2_BLOCKS_NUMBER);
        pixy2_x=(recvBuf[8] | recvBuf[9] << 8);//获取X坐标
        pixy2_y=(recvBuf[10] | recvBuf[11] << 8);//获取Y坐标
        pixy2_width=(recvBuf[12] | recvBuf[13] << 8);//获取像素宽度
        pixy2_height=(recvBuf[14] | recvBuf[15] << 8);//获取像素高度

        if(pixy2_width<256)            
                Distance = (W * F) / (float)pixy2_width;//计算距离
    
        printf("Blocks: X:%d, Y:%d, Width:%d, Height:%d, Distance:%f0\r\n", 
                pixy2_x, pixy2_y, pixy2_width, pixy2_height, Distance); 
    }
}

这里只节选了测距的关键程序，能够完整表达整个测距的过程，下面会给出完整的project。我们把程序烧进STM32开发板，用串口调试助手看看行不行：

可以看出，效果还不错，实际应用中虽然只有几毫米的误差，但是也能够实现获取Pixy2与物体之间的距离的功能。你能够测到距离的精度，跟你计算的焦距有关，也就是说，W、D、P这三个参数足够精确的话，那么你最后能够测到的距离也应该足够精确。但是，Pixy2本身相机的测量误差，还有光线问题，也会影响你最终测到的距离精度。

Tip：如果你问我用高度来测量焦距F和距离Distance行吗？讲道理，是可以的，只要你把全部需要测量宽度的参数，换成测量高度，也是能够得出正确的距离，但是我还没试过:)

你可能需要的Support，STM32整个测距的工程文件：
链接：https://pan.baidu.com/s/12Kw0...
提取码：7py8

传送门：
https://www.instructables.com/id/Using-a-PixyCam-CMUcam5-to-Get-Distance-of-a-Known/

以上。