树莓派3B读取PMW3901光流模块摘要

 

上图是实际输出数据，晚上室内暖色台灯光线效果，看上去还算比较干净，但实际使用还需要做滤波计算速度和补偿等。主要是光线问题，需要多尝试。 

 

不说废话，上代码。这里我使用的是第二个SPI1设备，可根据自己的实际情况修改为SPI0的。

树莓派3如何同时使用多个SPI外设，可以参考网上资料。管脚引用参考如下URL:

SPI at Raspberry Pi GPIO Pinout

#include <bcm2835.h>
#include <stdio.h>

#define __packed __attribute__((packed))
/**
 * 突发模式结构体。通过外部中断触发后使用该结构体读取完整数据。
 * 因为Raspberry Pi 3B(+)的外部中断是伪轮询方式，所以这里没用。
 */
typedef struct MotionBurst
{

    __packed union
    {
        uint8_t motion;
        __packed struct
        {
            uint8_t frameFrom0 : 1;
            uint8_t runMode : 2;
            uint8_t reserved1 : 1;
            uint8_t rawFrom0 : 1;
            uint8_t reserved2 : 2;
            uint8_t motionOccured : 1;
        };
    };

    uint8_t observation;
    int16_t deltaX;
    int16_t deltaY;

    uint8_t squal;

    uint8_t rawDataSum;
    uint8_t maxRawData;
    uint8_t minRawData;

    uint16_t shutter;
} MotionBurst;

MotionBurst motionBurst;
int16_t x = 0;
int16_t y = 0;
/**
 * SPI设备，写从设备，bit 7必须位0
 */
void registerWrite(uint8_t reg, uint8_t val)
{
    uint8_t buff[2] = {reg | 0x80, val};
    bcm2835_aux_spi_transfern((char *)buff, 2);
    // printf("%d::0x%02X 0x%02X\n", __LINE__, buff[0], buff[1]);
}
/**
 * SPI设备，读从设备，bit 7必须位1
 * 因为使用的是是spidev1.2，所以返回的数据第一个字节总是0xFF
 * Raspiberry 3B(+) 使用多个SPI从设备的管脚配置(pinout)可以参考以下官方URL：
 *  https://pinout.xyz/pinout/spi
 */
uint8_t registerRead(uint8_t reg)
{
    unsigned char buff[2] = {reg & 0x7F, 0x00};
    bcm2835_aux_spi_transfern((char *)buff, 2);
    // printf("%d::0x%02X 0x%02X\n", __LINE__, buff[0], buff[1]);
    return buff[1];
}
/**
 * 初始化PMW3901相关寄存器
 * 详细参考datasheet。网上很多pmw3901初始化代码不完整，主要是pmw3901模块的版本太多了。
*/
void init()
{
#ifndef NORMAL
    registerWrite(0x7F, 0x00);
    registerWrite(0x55, 0x01);
    registerWrite(0x50, 0x07);
    registerWrite(0x7F, 0x0E);
    registerWrite(0x43, 0x10);
    int i = 0;
    for (i = 0; i < 3; i++)
    {
        if (registerRead(0x47) != 0x08)
        {
            registerWrite(0x43, 0x10);
        }
        else
        {
            break;
        }
        if (i == 2)
        {
            exit(0);
        }
    }
    if ((registerRead(0x67) & 0x80) == 0x80)
        registerWrite(0x48, 0x04);
    else
        registerWrite(0x48, 0x02);
    registerWrite(0x7F, 0x00);
    registerWrite(0x51, 0x7B);
    registerWrite(0x50, 0x00);
    registerWrite(0x55, 0x00);
    registerWrite(0x7F, 0x0E);
    //
    uint8_t C1, C2;
    if (registerRead(0x73) == 0x00)
    {
        C1 = registerRead(0x70);
        C2 = registerRead(0x71);
        if (C1 <= 28)
            C1 += 14;
        else
            C1 += 11;
        if (C1 > 0x3F)
            C1 = 0x3F;

        C2 = ((unsigned short)C2 * 45) / 100;
        registerWrite(0x7F, 0x00);
        registerWrite(0x61, 0xAD);
        registerWrite(0x51, 0x70);
        registerWrite(0x7F, 0x0E);
        registerWrite(0x70, C1);
        registerWrite(0x71, C2);
    }
#endif
    registerWrite(0x7F, 0x00);
    registerWrite(0x61, 0xAD);
    registerWrite(0x7F, 0x03);
    registerWrite(0x40, 0x00);
    registerWrite(0x7F, 0x05);
    registerWrite(0x41, 0xB3);
    registerWrite(0x43, 0xF1);
    registerWrite(0x45, 0x14);
    registerWrite(0x5B, 0x32);
    registerWrite(0x5F, 0x34);
    registerWrite(0x7B, 0x08);
    registerWrite(0x7F, 0x06);
    registerWrite(0x44, 0x1B);
    registerWrite(0x40, 0xBF);
    registerWrite(0x4E, 0x3F);
    registerWrite(0x7F, 0x08);
    registerWrite(0x65, 0x20);
    registerWrite(0x6A, 0x18);
    registerWrite(0x7F, 0x09);
    registerWrite(0x4F, 0xAF);
    registerWrite(0x5F, 0x40);
    registerWrite(0x48, 0x80);
    registerWrite(0x49, 0x80);
    registerWrite(0x57, 0x77);
    registerWrite(0x60, 0x78);
    registerWrite(0x61, 0x78);
    registerWrite(0x62, 0x08);
    registerWrite(0x63, 0x50);
    registerWrite(0x7F, 0x0A);
    registerWrite(0x45, 0x60);
    registerWrite(0x7F, 0x00);
    registerWrite(0x4D, 0x11);
    registerWrite(0x55, 0x80);
    registerWrite(0x74, 0x1F);
    registerWrite(0x75, 0x1F);
    registerWrite(0x4A, 0x78);
    registerWrite(0x4B, 0x78);
    registerWrite(0x44, 0x08);
    registerWrite(0x45, 0x50);
    registerWrite(0x64, 0xFF);
    registerWrite(0x65, 0x1F);
    registerWrite(0x7F, 0x14);
    registerWrite(0x65, 0x60);
    registerWrite(0x66, 0x08);
    registerWrite(0x63, 0x78);
    registerWrite(0x7F, 0x15);
    registerWrite(0x48, 0x58);
    registerWrite(0x7F, 0x07);
    registerWrite(0x41, 0x0D);
    registerWrite(0x43, 0x14);
    registerWrite(0x4B, 0x0E);
    registerWrite(0x45, 0x0F);
    registerWrite(0x44, 0x42);
    registerWrite(0x4C, 0x80);
    registerWrite(0x7F, 0x10);
    registerWrite(0x5B, 0x02);
    registerWrite(0x7F, 0x07);
    registerWrite(0x40, 0x41);
    registerWrite(0x70, 0x00);

    bcm2835_delay(10);
    registerWrite(0x32, 0x44);
    registerWrite(0x7F, 0x07);
    registerWrite(0x40, 0x40);
    registerWrite(0x7F, 0x06);
    registerWrite(0x62, 0xf0);
    registerWrite(0x63, 0x00);
    registerWrite(0x7F, 0x0D);
    registerWrite(0x48, 0xC0);
    registerWrite(0x6F, 0xd5);
    registerWrite(0x7F, 0x00);
    registerWrite(0x5B, 0xa0);
    registerWrite(0x4E, 0xA8);
    registerWrite(0x5A, 0x50);
    registerWrite(0x40, 0x80);
}
int16_t deltaX = 0;
int16_t deltaY = 0;

/**
 * 主动读取偏移信息
 * deltaX和deltaY为检测到2帧之间的象素差
 * x和y为当前累积位移像素
*/
void readMotion()
{
    uint8_t motion, shutter_up, squal;
    motion = registerRead(0x02);

    // shutter_lo = registerRead(0x0B);
    if ((motion & 0x80) == 0x80)
    {
        shutter_up = registerRead(0x0C);
        squal = registerRead(0x07);
        if (squal > 0x19 && shutter_up < 0x1F)
        {
            deltaX = ((int16_t)registerRead(0x04) << 8) | registerRead(0x03);
            deltaY = ((int16_t)registerRead(0x06) << 8) | registerRead(0x05);

            x += deltaX;
            y += deltaY;
        }
        else
        {
            deltaX = 0;
            deltaY = 0;
        }
    }
}
/**
 * Do not use this method on Raspberry Pi
*/
void readMotionBurst(MotionBurst *motion)
{

    bcm2835_aux_spi_transfern((char *)motion, sizeof(MotionBurst));
    printf("Size count:%d\n", sizeof(MotionBurst));
    for (int i = 0; i < sizeof(MotionBurst); i++)
    {
        printf("0x%02X ", *((uint8_t *)motion + i));
    }
    printf("\n");

    uint16_t realShutter = (motion->shutter >> 8) & 0x0FF;
    realShutter |= (motion->shutter & 0x0FF) << 8;
    motion->shutter = realShutter;
    if ((motion->squal < 25) && (motion->shutter >= 0x1F00)) // 数据质量不好舍弃
    {
        motion->deltaX = motion->deltaY = 0;
    }
    // x += motion->deltaX;
    // y += motion->deltaY;
}

int main(int argc, char **argv)
{

    if (!bcm2835_init())
    {
        printf("bcm2835_init failed. Are you running as root??\n");
        return 1;
    }

    if (!bcm2835_aux_spi_begin())
    {
        printf("bcm2835_aux_spi_begin failed. Are you running as root??\n");
        return 1;
    }
//寄存器复位
    registerWrite(0x3a, 0x5a);
    bcm2835_delay(100);

    uint8_t pid = registerRead(0x5F);
    printf("Product ID: 0x%02X\n", pid);
    init();
//清空数据
    registerWrite(0x02, 0x00);
    while (1)
    {
        readMotion();
        printf("x:%d,y:%d, deltaX:%d deltaY:%d\n", x, y, deltaX, deltaY);
        bcm2835_delay(100);
    }
    bcm2835_aux_spi_end();
    bcm2835_close();
    return 0;
}