硬件:基于PX4的v5的硬件的修改,移除掉IO从控芯片
软件:PX4中v1.8.2版本
●基于BMI055传感器驱动的分析,BMI088是对BMI055的升级,顺道分析传感器的流程和部分Linux设备驱动开发的知识。
●贴过来的代码,都只是摘取部分。
●配合 ctrl+f 搜索查看代码更方便
包括编译部分、nuttx的芯片相关的驱动代码、传感器本身驱动部分,而“nuttx的芯片相关的驱动代码”又包括了SPI相关
(返回目录)
//路径和文件名:Firmware/cmake/configs/nuttx_px4fmu-v5_default.cmake:20
//屏蔽BMI055,增加BMI088驱动,在BMI055上升级
# drivers/imu/bmi055
drivers/imu/ccbmi088
(返回目录)
//路径和文件名:Firmware/ROMFS/px4fmu_common/init.d/rc.sensors:316
//关闭BMI055启动,BMI088启动分为加速度的启动和陀螺仪的启动
# Internal SPI bus BMI055 accel
#bmi055 -A -R 10 start
# ccbmi088 -A start
# Internal SPI bus BMI055 gyro
#bmi055 -G -R 10 start
# ccbmi088 -G start
(返回目录)
(返回:传感器驱动流程分析3.3.1)
具体流程图如下:
A. device::Device 所有物理驱动程序(I2C, SPI)的基类
namespace device
{
/*Fundamental base class for all physical drivers (I2C, SPI)*/
class __EXPORT /*A*/Device{
virtual int read(unsigned address, void *data, unsigned count) { return -ENODEV; }
virtual int write(unsigned address, void *data, unsigned count) { return -ENODEV; }
uint8_t get_device_bus() { return _device_id.devid_s.bus; }
...
}
}
B. device::CDev 任何字符设备的抽象类。
一般每个字符设备或者块设备都会在/dev目录(可以是任意目录,这样是为了统一)下对应一个设备文件。
linux用户层程序通过设备文件来使用驱动程序操作字符设备或块设备。
字符设备是3大类设备(字符设备、块设备、网络设备)中较简单的一类设备、其驱动程序中完成的主要工作是初始化、
添加和删除 struct cdev 结构体,申请和释放设备号,以及填充 struct file_operations 结构体中断的操作函数,实现
struct file_operations 结构体中的read()、write()和ioctl()等函数是驱动设计的主体工作
参考:linux中字符设备驱动
namespace device
{
class __EXPORT /*B*/CDev : public /*A*/ Device{
virtual int open(file_t *filep);
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen);
virtual int register_class_devname(const char *class_devname);
static const px4_file_operations_t fops; /*flag3 文件操作集合,读写相关等*/
...
}
}
C.device::SPI用于SPI上的字符设备的抽象类
namespace device __EXPORT
{
class __EXPORT /*C*/SPI : public /*B*/CDev{
virtual int init();
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
void set_frequency(uint32_t frequency) { _frequency = frequency; }
}
}
D.BMI055 最主要是构造函数
class /*D*/BMI055 : public /*C*/device::SPI{
BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,enum Rotation rotation);
...
}
E. BMI055_accel 和 F. BMI055_gyro 传感器驱动的具体实现
class /*E*/BMI055_accel : public /*D*/BMI055
BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
start();
measure();
...
}
class /*F*/BMI055_gyro : public /*D*/BMI055
BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
start();
measure();
...
}
(返回目录)
3.2.1 第一步
//路径:Firmware/src/drivers/boards/px4fmu-v5/init.c:260
//此函数应该是在boardctl()中被调用,本人也是一知半解
__EXPORT int board_app_initialize(uintptr_t arg) //第一步
{
VDD_3V3_SD_CARD_EN(true);
VDD_5V_PERIPH_EN(true);
...
int ret = stm32_spi_bus_initialize(); //第二步
...
}
3.2.1. 第二步
static struct spi_dev_s *spi_sensors;
//路径:Firmware/src/drivers/boards/px4fmu-v5/spi.c:142
__EXPORT int board_app_initialize(uintptr_t arg) // 第二步
{
/* Configure SPI-based devices */
spi_sensors = stm32_spibus_initialize(PX4_SPI_BUS_SENSORS); // 第三步
SPI_SETFREQUENCY(spi_sensors, 10000000);
SPI_SETBITS(spi_sensors, 8);
SPI_SETMODE(spi_sensors, SPIDEV_MODE3);
for (int cs = PX4_SENSORS_BUS_FIRST_CS; cs <= PX4_SENSORS_BUS_LAST_CS; cs++) {
SPI_SELECT(spi_sensors, cs, false); //(全局搜索flag2)
}
/* Get the SPI port for the BARO */
spi_baro = stm32_spibus_initialize(PX4_SPI_BUS_BARO);
...
/* Get the SPI port for the PX4_SPI_EXTERNAL1 */
spi_ext = stm32_spibus_initialize(PX4_SPI_BUS_EXTERNAL1);
...
/* Get the SPI port for the PX4_SPI_EXTERNAL2 */
spi_ext = stm32_spibus_initialize(PX4_SPI_BUS_EXTERNAL2);
...
}
3.2.3. 第三步 stm32_spibus_initialize
//路径:Firmware/platforms/nuttx/NuttX/nuttx/arch/arm/src/stm32f7/stm32_spi.c:1751
//@return: (FAR struct spi_dev_s *) priv
FAR struct /*A1*/spi_dev_s *stm32_spibus_initialize(int bus)
{
FAR struct /*B1*/stm32_spidev_s *priv = NULL; //第四步:SPI底层操作
irqstate_t flags = enter_critical_section(); //进入临界区
#ifdef CONFIG_STM32F7_SPI1
if (bus == 1)
{
/* Select SPI1 */
priv = /*B2*/&g_spi1dev; ///B2: g_spi1dev就代表了SPI1整个设备集合
/* Only configure if the bus is not already configured */
if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) //flag1(可全局搜flag1)
{
/* Configure SPI1 pins: SCK, MISO, and MOSI*/
/* GPIO配置 */
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI); //第五步:IO 配置 GPIO_SPI1_SCK GPIO_SPI1_MISO
/* Set up default configuration: Master, 8-bit, etc. */
spi_bus_initialize(priv); //第六步
}
}
else
#endif
#ifdef CONFIG_STM32F7_SPI2
if (bus == 2)
{
/* Select SPI2 */
...
leave_critical_section(flags); //离开临界区
return (FAR struct spi_dev_s *)priv;
}
3.2.4 第四步
①. spi 结构体相关综述,实例化对象 (A1 A2 B1 B2定义)
//路径:Firmware/platforms/nuttx/NuttX/nuttx/arch/arm/src/stm32f7/stm32_spi.c:229
//绑定SPI操作集合,就是绑定功能函数
static const struct /*A1*/ spi_ops_s /*A2*/g_sp1iops =
{
.lock = spi_lock,
/*#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s))*/
.select = stm32_spi1select, /*flag2 spi片选选择*/
/*#define SPI_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f))*/
.setfrequency = spi_setfrequency, /*spi设定频率*/
...
/*#define SPI_SNDBLOCK(d,b,l) ((d)->ops->exchange(d,b,0,l))*/
.exchange = spi_exchange,
...
};
//绑定SPI设备操作集合,包括了A2 SPI操作集合的实例,g_spi1dev就代表了SPI1整个设备集合
static struct /*B1*/ stm32_spidev_s /*B2*/g_spi1dev =
{
.spidev = /*A2*/{ &g_sp1iops },
.spibase = STM32_SPI1_BASE, //#define STM32_SPI1_BASE 0x40013000
/*(flag1)
uint16_t spi_getreg(FAR struct stm32_spidev_s *priv, uint8_t offset){
return getreg16(priv->spibase + offset); }
# define getreg16(a) (*(volatile uint16_t *)(a)) */
.spiclock = STM32_PCLK2_FREQUENCY,
...
}
②:A1 结构体定义
//@路径:v5/NuttX/nuttx/include/nuttx/spi/spi.h:519 (编译后的路径)
struct spi_ops_s
{
CODE int (*lock)(FAR struct spi_dev_s *dev, bool lock);
CODE void (*select)(FAR struct spi_dev_s *dev, uint32_t devid, bool selected);
CODE uint32_t (*setfrequency)(FAR struct spi_dev_s *dev, uint32_t frequency);
...
CODE void (*exchange)(FAR struct spi_dev_s *dev,
FAR const void *txbuffer, FAR void *rxbuffer, size_t nwords);
...
};
③:A1 结构体指针
//@路径:v5/NuttX/nuttx/include/nuttx/spi/spi.h:560 (编译后的路径)
struct spi_dev_s
{
FAR const struct spi_ops_s *ops;
};
④:B1 结构体定义
//路径:Firmware/platforms/nuttx/NuttX/nuttx/arch/arm/src/stm32f7/stm32_spi.c:143
struct stm32_spidev_s
{
struct spi_dev_s spidev; /* Externally visible part of the SPI interface */
uint32_t spibase; /* SPIn base address */
uint32_t spiclock; /* Clocking for the SPI module */
...
};
⑤: flag2 stm32_spi1select 片选选择 以片选为例,深入底层
//路径:Firmware/src/drivers/boards/px4fmu-v5/spi.c:242
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
{
/* SPI select is active low, so write !selected to select the device */
int sel = (int) devid;
ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_SENSORS);
/* Making sure the other peripherals are not selected */
for (size_t cs = 0; arraySize(spi1selects_gpio) > 1 && cs < arraySize(spi1selects_gpio); cs++) {
if (spi1selects_gpio[cs] != 0) {
stm32_gpiowrite(spi1selects_gpio[cs], 1); //④-5-1 spi1selects_gpio[cs]
}
}
uint32_t gpio = spi1selects_gpio[PX4_SPI_DEV_ID(sel)];
if (gpio) {
stm32_gpiowrite(gpio, !selected);
}
}
//路径:Firmware\src\drivers\boards\px4fmu-v5\spi.c
static const uint32_t spi1selects_gpio[] = PX4_SENSOR_BUS_CS_GPIO;
//路径:Firmware\src\drivers\boards\px4fmu-v5\board_config.h
#define PX4_SENSOR_BUS_CS_GPIO {GPIO_SPI1_CS1_ICM20689, GPIO_SPI1_CS2_ICM20602, GPIO_SPI1_CS3_BMI055_GYRO, GPIO_SPI1_CS4_BMI055_ACC}
#define PX4_SENSORS_BUS_FIRST_CS PX4_SPIDEV_ICM_20689
#define PX4_SENSORS_BUS_LAST_CS PX4_SPIDEV_BMI055_ACC
//路径:Firmware\src\drivers\boards\px4fmu-v5\board_config.h
#define GPIO_SPI1_CS1_ICM20689 /* PF2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN2)
#define GPIO_SPI1_CS2_ICM20602 /* PF3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN3)
#define GPIO_SPI1_CS3_BMI055_GYRO /* PF4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN4)
#define GPIO_SPI1_CS4_BMI055_ACC /* PG10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN10)
3.2.5 第五步: 根据自己硬件修改IO引脚定义
(返回4.1代码底层IO修改)
//路径: Firmware/platforms/nuttx/nuttx-configs/px4fmu-v5/include/board.h:390
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_3 /* PG9 */
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_3 /* PG11 */
3.2.6.第六步: SPI总线默认初始化,寄存器操作 (主、8位、SPI模式0等)
static void spi_bus_initialize(FAR struct stm32_spidev_s *priv)
{
...
/* Configure CR1 and CR2. Default configuration:
* Mode 0: CR1.CPHA=0 and CR1.CPOL=0
* Master: CR1.MSTR=1
* 8-bit: CR2.DS=7
* MSB tranmitted first: CR1.LSBFIRST=0
* Replace NSS with SSI & SSI=1: CR1.SSI=1 CR1.SSM=1 (prevents MODF error)
* Two lines full duplex: CR1.BIDIMODE=0 CR1.BIDIOIE=(Don't care) and CR1.RXONLY=0
*/
clrbits = SPI_CR1_CPHA | SPI_CR1_CPOL | SPI_CR1_BR_MASK | SPI_CR1_LSBFIRST |
SPI_CR1_RXONLY | SPI_CR1_BIDIOE | SPI_CR1_BIDIMODE;
setbits = SPI_CR1_MSTR | SPI_CR1_SSI | SPI_CR1_SSM;
spi_modifycr1(priv, setbits, clrbits);
clrbits = SPI_CR2_DS_MASK;
setbits = SPI_CR2_DS_8BIT | SPI_CR2_FRXTH; /* FRXTH must be high in 8-bit mode */
spi_modifycr2(priv, setbits, clrbits);
priv->frequency = 0;
priv->nbits = 8;
priv->mode = SPIDEV_MODE0;
/* Select a default frequency of approx. 400KHz */
spi_setfrequency((FAR struct spi_dev_s *)priv, 400000);
/* CRCPOLY configuration */
spi_putreg(priv, STM32_SPI_CRCPR_OFFSET, 7);
/* Initialize the SPI semaphore that enforces mutually exclusive access. */
...
/* Enable spi */
spi_modifycr1(priv, SPI_CR1_SPE, 0);
}
(返回目录)
#执行 bmi055 -A start 后:主要分三步:
void
start(bool external_bus, enum Rotation rotation, enum sensor_type sensor)
{
/*3.3.1 新建对象*/
BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int;
*g_dev_acc_ptr = new BMI055_accel(PX4_SPI_BUS_SENSORS, path_accel, PX4_SPIDEV_BMI055_ACC, rotation);
/*3.3.2 初始化*/
if (OK != (*g_dev_acc_ptr)->init()) {
goto fail_accel;
}
/*3.3.3 文件操作 */
fd_acc = open(path_accel, O_RDONLY);
if (fd_acc < 0) {
goto fail_accel;
}
if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail_accel;
}
close(fd_acc);
}
3.3.1 : 查看继承关系 3.1 类的继承流程 ,各个参数继承流程如下:
(返回:4.2 BMI088中修改传感器SPI总线相关)
/* 编号配合搜索更加
编号| 项目 | 原始值 | Value
-- |-------- | -------------------- | ---
Z1 | SPI总线 | PX4_SPI_BUS_SENSOR | 1
Z2 |设备路径 | path_accel | "/dev/bmi055_gyro"
Z3 |CS片选 | PX4_SPIDEV_BMI055_ACC | PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS,3) = 0x1000 0405
Z4 |旋转角度 | rotation | 0
Z5 |设备名字 | name | "BMI055_ACCEL"
Z6 |SPI模式 | SPIDEV_MODE3 | //CPOL=1 CHPHA=1
Z7 |SPI频率 | BMI055_BUS_SPEED | 10*1000*1000
*/
//E:
//路径:Firmware/src/drivers/imu/bmi055/bmi055_main.cpp:72
new BMI055_accel(/*Z1*/PX4_SPI_BUS_SENSORS,/*Z2*/path_accel,/*Z3*/PX4_SPIDEV_BMI055_ACC,/*Z4*/rotation);
//路径:Firmware/src/drivers/imu/bmi055/bmi055_accel.cpp:25
BMI055_accel::BMI055_accel(int/*Z1*/bus, const char/*Z2*/*path_accel, uint32_t/*Z3*/device,/*Z4*/enum Rotation rotation)
BMI055(/*Z5*/"BMI055_ACCEL",/*Z2*/path_accel,/*Z1*/bus,/*Z3*/device,/*Z6*/SPIDEV_MODE3,/*Z7*/BMI055_BUS_SPEED, /*Z4*/rotation),
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI055;
}
//D:
//路径:Firmware/src/drivers/imu/bmi055/bmi055_main.cpp:432
BMI055::BMI055(const char/*Z5*/*name, const char/*Z2*/*devname, int/*Z1*/bus, uint32_t/*Z3*/device, enum spi_mode_e/*Z6*/mode,uint32_t/*Z7*/frequency, enum Rotation/*Z4*/rotation):
/*C*/SPI(/*Z5*/name,/*Z2*/devname, /*Z1*/bus,/*Z3*/device,/*Z6*/mode,/*Z7*/frequency),
/*Z4*/_rotation(/*Z4*/rotation),
{
}
//C:
//路径:Firmware/src/lib/drivers/device/nuttx/SPI.cpp:60
SPI::SPI(const char/*Z5*/*name,
const char/*Z2*/*devname,
int/*Z1*/bus,
uint32_t/*Z3*/device,
/*Z6*/enum spi_mode_e mode,
/*Z7*/uint32_t frequency) :
// base class
CDev(/*Z5*/name, /*Z2*/devname),
/*Z3*/_device(device), //SPI_SELECT(_dev, _device, true); 片选 与“3.2.4”相关
/*Z6*/_mode(mode), //SPI_SETMODE(_dev, _mode); 设置模式 与“3.2.4”相关
/*Z7*/_frequency(frequency), //SPI_SETFREQUENCY(_dev, _frequency); 设置频率 与“3.2.4”相关
_dev(nullptr)
{
// fill in _device_id fields for a SPI device
_device_id.devid_s.bus_type = DeviceBusType_SPI;
/*Z1*/_device_id.devid_s.bus =/*Z1*/bus;
/*Z3*/_device_id.devid_s.address = (uint8_t)/*Z3*/device;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
}
//B:
//路径:Firmware/src/lib/drivers/device/CDev.cpp:50
CDev::CDev(const char/*Z5*/*name, const char/*Z2*/*devname) :
Device(/*Z5*/name),
/*Z2*/_devname(devname){
...
}
//A:
//路径:Firmware/src/lib/drivers/device/Device.hpp:191
Device(const char/*Z5*/*name) : /*Z5*/_name(name){
...
/*默认构造函数*/
}
3.3.2 BMI055_accel::init() 初始化
如修改总线、片选、总线频率、SPI模式、注册路径等:
(返回:4.3 传感器初始化)
//路径:Firmware/src/drivers/imu/bmi055/bmi055_accel.cpp:73
int BMI055_accel::init()
{
/*① 第一步 见下面分析*/
int ret = SPI::init(); //
/*② 第二步 BMI055传感器本身配置,包括带宽、量程、使能DRDY中断等,写BMI088的话,就再此初始化BMI088传感器*/
reset();
/*③ 第三步 这个不太懂,希望懂的人,能告知一二*/
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
/*④ 第四步:测量包括:传感器数据读取、低通滤波、角度旋转、数据大小端处理、归一化等,uORB发布*/
measure(false);
/*⑤ 第五步主题相关*/
sensor_accel_s arp;
_accel_reports->get(&arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
return OK;
}
① 第一步
//路径:Firmware/src/lib/drivers/device/nuttx/SPI.cpp:91
int
SPI::init()
{
_dev = px4_spibus_initialize(get_device_bus());
//#define px4_spibus_initialize(bus_num_1based) stm32_spibus_initialize(bus_num_1based)
//stm32_spibus_initialize() 见“3.2.3. 第三步(可搜索)” 就和底层完全联系起来了,返回的值代表了整个总线1,包括绑定了SPI的操作函数集合,SPI时间基准,SPI分频,SPIDMA相关
//全局搜get_device_bus()和_device_id.devid_s.bus发现就是总线1。
/*Z3*/SPI_SELECT(_dev, _device, false);
ret = probe();
/*_whoami寄存器相关,验证总线是否正常和传感器是否存在 */
ret = CDev::init();
//最重要是这个注册函数:ret = register_driver(/*Z2*/_devname, &fops, 0666, (void *)this); //flag3
/*向操作系统注册一个设备节点,定义一个驱动程序类bmi055并继承CDev类,来注册设备节点。CDev类是PX4程序中提供的一个C++基类,驱动程序需要继承,并重写open、close、read、write、ioctl、seek等方法。
比如fops中包括读操作,而bmi055::read 继承了CDev的虚函数,之后就可以用
sz = read(fd_acc, &a_report, sizeof(a_report)); 来调用bmi055::read()的函数(不知道说错了没,自己的理解)
●Linux中用insmod装载驱动程序,所以只需要指定函数入口,在装载时会自动执行入口函数,并生成设备节点.
●Nuttx中装载驱动程序不同于Linux可以在随时装载,而是写死在代码中的,即调用register_driver后完成驱动的装载。
*/
out:
return ret;
}
3.3.3 文件操作
...
fd_acc = open(path_accel, O_RDONLY);
/*在之前注册了驱动,在此打开设备,就能ioctl、read、write、了
传感器作为一个文件设备,操作步骤: open -> ioctl -> read -> close */
ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)
/*ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_DEFAULT_RATE)==>start()==>
BMI055_accel::measure_trampoline==>dev->measure();
是通过高精度定时器回调函数实现measure(), 有些精度不高的可以用工作者队列work_queue来循环
*/
close(fd_acc);
...
(返回目录)
如修改SPI底层IO:3.2.5 根据自己硬件修改IO引脚定义
如修改总线、片选、总线频率、SPI模式、注册路径等:3.3.1 各个参数继承流程
如带宽、测量量程、使能DRDY中断,还要注意读写函数(读有一个dummy_byte位):3.3.2 init() 初始化第二步BMI055传感器本身配置
调试打印我用warnx(); 或者PX4_INFO(); 其他调试也可以多利用“bmi055 test”
还可以在终端输入“bmi055 info”来获取传感器状态信息,包括读取原始数据所需要的时间,运行程序的次数,运行程序周期等。
在该目录下新建一个CMakeLists.txt文件 加入编译规则,参考着已有的编写就行了。
● Pixhawk原生固件PX4之MPU6000驱动分析
● PX4 MPU6000 驱动代码即启动过程分析 :设备驱动讲得比较仔细