PX4 MPU6000 驱动代码即启动过程分析

一、启动的过程分析
#以SPI总线的mpu6000 启动过程为例
#执行 mpu6000 start 后：
1   start(MPU6000_BUS_ALL, ROTATION_NONE, MPU6000_ACCEL_DEFAULT_RANGE_G, MPU_DEVICE_TYPE_MPU6000);
2、    start_bus(bus_options[i], rotation, range, device_type);
3、   interface = bus.interface_constructor(bus.busnum, device_type, bus.external);
4、      interface->init();         # device_type = MPU_DEVICE_TYPE_MPU6000
5、      dev = new MPU6000(interface, bus.accelpath, bus.gyropath, rotation, device_type);
6、   dev->init();
7、   open(bus.accelpath, O_RDONLY);
8、   ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
9、   ioctl(fd, ACCELIOCSRANGE, range);
10、   close(fd);
11、   return;
12、return;

二、启动过程中执行的一些函数过程及功能分析

interface生成过程，即device::Device *_interface MPU6000_SPI(int bus, int device_type, bool external_bus)函数解析
     #入参:1，6000，false
MPU6000_SPI _interface(int bus, int device_type, bool external_bus)
    int cs = SPIDEV_NONE(0);
    cs = PX4_SPIDEV_MPU;
    #interface 是一个MPU6000_SPI对象指针
    interface = new MPU6000_SPI(bus,  cs, device_type);
return interface;

继承关系:Device ---> CDev ---> SPI ---> MPU6000_SPI

class MPU6000_SPI 构造函数解析:   device === > cs 对应于该SPI设备的片选信号
MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type):SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED),
_device_type(device_type)
{
_device_id.devid_s.devtype =  DRV_ACC_DEVTYPE_MPU6000;
}
    功能:用设备类型，总线地址，片选信号地址构造了一个 interface对象(启动过程中的第三点)，在构造MPU6000对象时要使用这个参数。

class MPU6000_SPI::init()函数分析:
int MPU6000_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
return OK;
}

int SPI::init()
{
int ret = OK;
_dev = px4_spibus_initialize(get_device_bus());
/* deselect device to ensure high to low transition of pin select */
SPI_SELECT(_dev, _device, false);
/* call the probe function to check whether the device is present */
ret = probe();
if (ret != OK) {
DEVICE_DEBUG("probe failed");
goto out;
}
/* do base class init, which will create the device node, etc. */
//由代码注释可知 "dev/mpu6000_accel" 这样的设备节点是由基类CDev::init()函数注册产生的。
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("cdev init failed");
goto out;
}
/* tell the workd where we are */
DEVICE_LOG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000);
//这一句话是pixhawk在启动时串口输出的log,怎么样，是不是很像很像。
//MPU6000 on SPI bus 1 at 4 (1000 KHz) <------- 这一句是启动时打印的log
out:
return ret;
}


//根据上面的注释，我们进而分析基类的init()函数,果然可以发现register_driver(_devname, &fops, 0666, (void *)this);这一句话，这是用于注册驱动程序的。
//Linux中用insmod装载驱动程序,所以只需要指定函数入口，在装载时会自动执行入口函数，并生成设备节点.
//Nuttx中装载驱动程序不同于Linux可以在随时装载，而是写死在代码中的，即调用register_driver后完成驱动的装载。
int CDev::init()
{
DEVICE_DEBUG("CDev::init");
// base class init first
int ret = Device::init();
if (ret != PX4_OK) {
goto out;
}
// now register the driver
if (_devname != nullptr) { //<-----这里进行判断，是否要装载驱动
ret = register_driver(_devname, &fops, 0666, (void *)this); //<-----就是这里装载的
if (ret != PX4_OK) {
goto out;
}
_registered = true;
}
out:
return ret;
}


_devname是在构造子类对象时传入的 "dev/XXX" 设备节点路径。分析到这里之后，只需要确定fops对象，即可确定出对应设备的驱动程序了。接下来，我们要确定的是，在何时注册驱动程序并生成节点的。因为构造MPU6000_SPI对象时初始化的_devname = nullptr,所以在interface->init()个步骤里没有注册驱动程序。所以我们接下来进行MPU6000类构造函数和MPU600::init() 函数的分析，应该就能确定出fops对象的初始化和注册驱动程序的调用。

class MPU6000 构造函数解析:
//入参:MPU6000_SPI对象指针,"/dev/mpu6000_accel","/dev/mpu6000_gyro",NONE,MPU_DEVICE_TYPE_MPU6000
dev = new MPU6000(interface, bus.accelpath, bus.gyropath, rotation, device_type);
class MPU6000{
    ...
    MPU6000_gyro *_gyro;
    ...
};

构造函数代码节选:
    case MPU_DEVICE_TYPE_MPU6000:
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
/* Prime _gyro with parents devid. */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
break;


由此可见: class MPU6000 包含一个 MPU6000_gyro类型的指针,在构造MPU6000对象时,也会同时实例化出一个MPU6000_gyro对象，并在初始化列表中初始化_gyro指针,_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;两句可以看出 MPU6000对象负责 /dev/mpu6000_accel 设备节点的读写控制,_gyro指针负责对 /dev/mpu6000_gyro设备节点的读写控制，从而完整的使用MPU6000提供的加速度、角速度数据。


跑题了，为了分析出调用的驱动程序在哪，我们要分析出fops对象在哪初始化的,和何时在哪注册的驱动程序。下面是MPU6000构造函数分析(删减掉不需要的部分)

MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation,
int device_type) :
CDev("MPU6000", path_accel),//在这里构造了一个CDev对象 名字是"MPU6000" , 设备路径是 "/dev/mpu6000_accel"
_interface(interface),       //这里初始化了_interface
_device_type(device_type),       //这里初始化了_device_type
_gyro(new MPU6000_gyro(this, path_gyro)), //这里构造了一个MPU6000_gyro对象,并用它初始化了_gyro
   #解析：{
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :

CDev("MPU6000_gyro", path),    

 //这样的话，在构造MPU6000_gyro对象的同时就构造了一个CDev对象，名字:"MPU6000_gyro" 路径:"/dev/mpu6000_gyro"

_parent(parent),    
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
   }
_rotation(rotation), //初始化了芯片朝向
...
{
...
}
这样的话class MPU6000的构造函数做的事情就很清楚了，它初始化了一些值，并构造了一个MPU6000_gyro对象，在这个过程中产生了两个CDev对象，名字分别为"MPU6000"和"MPU6000_gyro",路径分别为"/dev/mpu6000_accel"和"/dev/mpu6000_gyro"。在看CDev的构造函数只做了 int ret = px4_sem_init(&_lock, 0, 1); 这一件事情，看名字猜测是信号量相关设置，并没有驱动注册相关的操作，所以驱动的注册应该就在MPU6000::init() 这个函数中了，接下来解析MPU6000::init()到底做了什么事情。
 
MPU6000::init()函数解析:
首先分析执行流程：
MPU6000::init()
    /* probe again to get our settings that are based on the device type */
    ret = probe();
    ret = CDev::init();   <--------- 这里应该是做"/dev/mpu6000_accel"驱动的注册
    // set software low pass filter for controllers
    //软件低通滤波相关设置，暂时不深究，我们的目标是找到驱动程序，这样才知道改电路板需要做哪些设置和修改。
    param_t accel_cut_ph = param_find("IMU_ACCEL_CU/TOFF");
    param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
    /* do CDev init for the gyro device node, keep it optional */
    ret = _gyro->init();  <--------- 这里应该是做"/dev/mpu6000_gyro" 驱动的注册
    _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
    measure();
    ...将测量的数据通过uORB通信总线广播出去
return;

还是那个问题，注册的驱动函数到底是哪些，在哪里。记得前面提到过CDev类的init()函数里执行了ret = register_driver(_devname, &fops, 0666, (void *)this); 传入了&fops这个文件操作对象指针，跳转到定义发现fops是CDev对象的一个成员，其初始化值如下所示(代码路径位于:/src/Firmware/src/drivers/device/nuttx/cdev_platform.cpp中)。初始化的语法有点奇怪，注释里说用了CNU C的扩展语法，有兴趣可以百度一下，据说内核里很多都用到了GNU C的新特性。
const struct file_operations device::CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
write : cdev_write,
seek : cdev_seek,
ioctl : cdev_ioctl,
poll : cdev_poll
};


这样依赖调用过程也就明了了。总结一下,继承CDev(字符设备-char device 缩写)的类公用一个fops 文件操作对象,这个fops对象里函数指针的值被初始化为cdev_open、cdev_close etc。所以用CDev::init()注册驱动后，应用层调用open(xxx,xxx,xxx)、read(xxx,xxx,xxx)...等标准的文件操作接口会最终调用到驱动层的cdev_open(xxx,xxx,xxx)、cdev_read(xxx,xxx,xxx)函数。那
问题来了,因为用的是一个fops,所以对"/dev/mpu6000_accel"和"/dev/mpu6000_gyro"的read、write操作会映射到同一个cdev_read()、cdev_write()函数,这样的话同一套函数怎么实现对不同设备的不同操作呢。我们去看cdev_xxxx()的函数实现，就可以发现，它是通过C++的多态实现的，下面以cdev_open()函数为例解析一下,代码如下
(源代码位于:/src/Firmware/src/drivers/device/nuttx/cdev_platform.cpp)


static int
cdev_open(file_t *filp)
{
device::CDev *cdev = (device::CDev *)(filp->f_inode->i_private);
return cdev->open(filp);
}


分析该函数的实现我们可以知道,在调用cdev_open时,通过filep获得了一个字符设备指针cdev,然后cdev->open(filep) 也就是调用了CDev::open()函数，在CDev类的定义中,我们可以看到CDev::open()是一个虚函数(简单点说就是用父类指针调用虚函数时,子类有重新实现该虚函数则调用子类实现的，若子类没实现则调用父类的)。所以，如果MPU6000类有重新实现open()函数，那么在open("/dev/mpu6000_accel")时dev->open()调用到的就是MPU6000类实现的open()函数,否则的话调用到的就是CDev类实现的open()函数。这样的话就利用了C++多态的特性实现了用一个fops注册多个设备驱动程序，并能根据对不同设备的操作调用到不同的函数。那么分析到这，调用过程就明了了。总结一下。


在调用CDev::init()时用CDev类的fops对象和不同的设备路径名(dev_path)完成了驱动的注册。注册驱动后,调用标准的open()、write() etc 文件操作接口后会通过内核走到字符设备驱动层的cdev_open()、cdev_write()函数,在cdev_xxx中通过传入的文件名，找到对应的设备对象的地址（指针）,再通过该指针调用到对应的open()、write()函数,利用C++的多态特性调用到不同的函数。以上就是PX4上某个设备的启动过程，和设备驱动调用过程。如果要分析设备驱动到底做了什么，那么就去分析CDev类的open()、read()、write() etc 函数或者其子类重实现的open()、read()、write()函数 即可知道该动作做了什么事情。 


三、验证分析:
写了一个驱动程序,class MyTestCdev,继承自CDev类,声明如下:
#define DEV_NAME "wTestDev"
#define DEV_PATH "/dev/wTestDev"

class MyTestCdev : public device::CDev
{
public:
MyTestCdev(const char *name,const char *devname);
~MyTestCdev();

virtual int open(file_t *filep); 
virtual int close(file_t *filep);
};


因为只是用作测试,所以声明的很简单，继承自CDev类，并重新实现了open和close函数,open、close函数的定义如下,
int MyTestCdev::open(file_t * filep)
{
printf("MyTestDev::open() function has been called. filep:%p\n",filep);
return 0;
}


int MyTestCdev::close(file_t * filep)
{
printf("MyTestDev::close() function has been called. filep:%p\n",filep);
return 0;

}
内容很简单，就打印两个log。接下来是主函数,主函数的内容如下:
int wtest_dev_main(int argc,char* argv[])
{
device::CDev* test = new MyTestCdev(DEV_NAME,DEV_PATH); //regist a new device
test -> init();


open(DEV_PATH,O_RDONLY);

return 0;
}


首先实例化了一个MyTestCdev类对象,并且调用了父类也就是CDev::init()函数注册了驱动程序,设备的路径是"/dev/wTestDev"，并打开了该设备。包含的头文件参考PX4内部已经实现的驱动包含的头文件就行了，
接下来将代码编译少些进pixhawk中,运行wtest_dev程序观察现象。运行wtest_dev程序后，可以看到串口控制台打印出了"MyTestDev::open() function has been called. filep:20021380"一段话，也就是我们在open函数中打印的那一句。进入到/dev目录下，ls,发现多了wTestDev这么一个设备节点，所以证明了我们前面的分析是正确的。


顺道提一下，在PX4中增加自己应用的几个步骤:
1、在你想添加的类型目录下新建一个文件夹，例如我想添加一个驱动,那么就在Firmware/src/driver目录下添加一个自己的文件夹.
2、在自己的文件夹下面新建源文件 编写代码。
3、在该目录下新建一个CMakeLists.txt文件 加入编译规则，参考着已有的编写就行了。
4、在cmake/configs 目录下的配置文件中增加要编译的模块,例如pixracer就在nuttx_px4fmu-v4_default.cmake中增加要的模块。
5、make px4fmu-vx_default upload 将程序编译烧写进板子。
6、连上串口，就可以在串口控制台敲指令运行自己编写的代码了。