前言：
首先分析PX4中MPU6000传感器驱动的注册，调用，实例的过程，先要理解一个事情。就是PX4是使用了一个类Linux操作系统的Nuttx操作系统。 也就是PX4上的驱动编写类似于Linux上的驱动编写。
软件层次图
所以我们分析的思路大概是
飞控程序如何调用MPU6000驱动获取数据 ==>Nuttx怎样注册的MPU6000设备驱动 ==> MPU6000驱动是又通过什么链接到了底层STM32驱动。
1.首先查看mpu6000驱动文件
class MPU6000 : public device::CDev
{
public:
MPU6000(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation,
int device_type);
virtual ~MPU6000();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
可以看到MPU6000公有继承于device::CDev。CDev是字符设备。
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
write : cdev_write,
seek : cdev_seek,
ioctl : cdev_ioctl,
poll : cdev_poll,
};
CDev::CDev(const char *name,
const char *devname,
int irq) :
// base class
Device(name, irq),
// public
// protected
_pub_blocked(false),
// private
_devname(devname),
_registered(false),
_max_pollwaiters(0),
_open_count(0),
_pollset(nullptr)
{
}
关于字符设备的一些定义，Linux操作系统认为一切都可以是文件。Nuttx类似，所以一个字符设备包含了向文件操作一样的读写等操作。暂时只分析到这，我们当前知道了MPU6000被注册为了字符设备,先分析stm32 SPI驱动是如何与设备进行连接并且调用的。
查看 mpu6000_spi.cpp
class MPU6000_SPI : public device::SPI
{
public:
MPU6000_SPI(int bus, spi_dev_e device, int device_type);
virtual ~MPU6000_SPI();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
int _device_type;
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned ®_speed_reg_out);
};
定义MPU6000_SPI类继承于 device::SPI ,初步思考是这个device是不是包含了固定的SPI基类，每一个使用SPI的设备继承于这个类并实现自己的一些操作？ 进入SPI来查看一下。
px4_v1.6.0/Firmware/src/drivers/device/spi.h
namespace device __EXPORT
{
/**
* Abstract class for character device on SPI
*/
#ifdef __PX4_NUTTX
class __EXPORT SPI : public CDev
#else
class __EXPORT SPI : public VDev
#endif
{
protected:
/**
* Constructor
*
* @param name Driver name
* @param devname Device node name
* @param bus SPI bus on which the device lives
* @param device Device handle (used by SPI_SELECT)
* @param mode SPI clock/data mode
* @param frequency SPI clock frequency
* @param irq Interrupt assigned to the device (or zero if none)
*/
SPI(const char *name,
const char *devname,
int bus,
enum spi_dev_e device,
enum spi_mode_e mode,
uint32_t frequency,
int irq = 0);
virtual ~SPI();
/**
* Locking modes supported by the driver.
*/
enum LockMode {
LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */
LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */
LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
};
virtual int init();
/**
* Check for the presence of the device on the bus.
*/
virtual int probe();
/**
* Perform a SPI transfer.
*
* If called from interrupt context, this interface does not lock
* the bus and may interfere with non-interrupt-context callers.
*
* Clients in a mixed interrupt/non-interrupt configuration must
* ensure appropriate interlocking.
*
* At least one of send or recv must be non-null.
*
* @param send Bytes to send to the device, or nullptr if
* no data is to be sent.
* @param recv Buffer for receiving bytes from the device,
* or nullptr if no bytes are to be received.
* @param len Number of bytes to transfer.
* @return OK if the exchange was successful, -errno
* otherwise.
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/**
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency);
/**
* Set the SPI bus locking mode
*
* This set the SPI locking mode. For devices competing with NuttX SPI
* drivers on a bus the right lock mode is LOCK_THREADS.
*
* @param mode Locking mode
*/
void set_lockmode(enum LockMode mode) { locking_mode = mode; }
LockMode locking_mode; /**< selected locking mode */
private:
enum spi_dev_e _device;
enum spi_mode_e _mode;
uint32_t _frequency;
struct spi_dev_s *_dev;
/* this class does not allow copying */
SPI(const SPI &);
SPI operator=(const SPI &);
protected:
int _bus;
int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
};
} // namespace device
看来确实是这样。 但是并没有找到其他什么关联到硬件驱动层的地方。在看看.c文件
px4_v1.6.0/Firmware/src/drivers/device/spi.c
int SPI::init()
{
int ret = OK;
/* attach to the spi bus */
if (_dev == nullptr) {
_dev = px4_spibus_initialize(_bus); /*这里有一个spibus初始化的操作*/
}
if (_dev == nullptr) {
DEVICE_DEBUG("failed to init SPI");
ret = -ENOENT;
goto out;
}
继续跟进。
px4_v1.6.0/Firmware/src/platforms/px4_micro_hal.h
110 define px4_spibus_initialize(port_1based) stm32_spibus_initialize(port_1based)
进入 stm32_spibus_initializ。
/home/amov/px4_v1.6.0/Firmware/NuttX/nuttx/arch/arm/src/stm32/stm32_spi.c
FAR struct spi_dev_s *stm32_spibus_initialize(int bus)
{
FAR struct stm32_spidev_s *priv = NULL;
irqstate_t flags = enter_critical_section();
#ifdef CONFIG_STM32_SPI1
if (bus == 1)
{
/* Select SPI1 */
priv = &g_spi1dev;
/* Only configure if the bus is not already configured */
if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0)
{
/* Configure SPI1 pins: SCK, MISO, and MOSI */
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
/* Set up default configuration: Master, 8-bit, etc. */
spi_bus_initialize(priv);
}
}
else
进入到/home/amov/px4_v1.6.0/Firmware/NuttX/nuttx/arch/arm/src/stm32/stm32_spi.c 好像能找到一丝与硬件驱动层的一些联系了。因为stm32_spi.c中的函数就是一些操作stm32单片机SPI总线的功能。这部分的引脚配置和硬件相关的在
/home/amov/px4_v1.6.0/Firmware/src/drivers/boards/px4fmu-v2/board_config.h中,修改这个位置就可以修改spi的引脚和配置。
根据模型选择对应芯片的片选信号。
stm32_spibus_initialize初始化一个g_spi1dev实例。 找到这部分查看一下。
#ifdef CONFIG_STM32_SPI1
static const struct spi_ops_s g_sp1iops =
{
.lock = spi_lock,
.select = stm32_spi1select,
.setfrequency = spi_setfrequency,
.setmode = spi_setmode,
.setbits = spi_setbits,
#ifdef CONFIG_SPI_HWFEATURES
.hwfeatures = spi_hwfeatures,
#endif
.status = stm32_spi1status,
#ifdef CONFIG_SPI_CMDDATA
.cmddata = stm32_spi1cmddata,
#endif
.send = spi_send,
#ifdef CONFIG_SPI_EXCHANGE
.exchange = spi_exchange,
#else
.sndblock = spi_sndblock,
.recvblock = spi_recvblock,
#endif
#ifdef CONFIG_SPI_CALLBACK
.registercallback = stm32_spi1register, /* Provided externally */
#else
.registercallback = 0, /* Not implemented */
#endif
};
static struct stm32_spidev_s g_spi1dev =
{
.spidev = { &g_sp1iops },
.spibase = STM32_SPI1_BASE,
.spiclock = STM32_PCLK2_FREQUENCY,
#ifdef CONFIG_STM32_SPI_INTERRUPTS
.spiirq = STM32_IRQ_SPI1,
#endif
#ifdef CONFIG_STM32_SPI_DMA
.rxch = DMACHAN_SPI1_RX,
.txch = DMACHAN_SPI1_TX,
#endif
};
#endif
g_sp1iops 是注册了一些函数指针实现了一个spi_ops_s实例，g_spi1dev注册了一些与stm32 spi硬件相关的地址和DMA的一些信息。 那么spi_ops_s在哪里呢？
/home/amov/px4_v1.6.0/Firmware/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* SPI select is active low, so write !selected to select the device */
switch (devid) {
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_ACCEL_MAG:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_BARO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_HMC:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_LIS:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_LIS, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
default:
break;
}
}
这部分模型由/home/amov/px4_v1.6.0/Firmware/src/drivers/mpu6000/mpu6000_spi.cpp
device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus)
根据脚本传来的总线信息进行选择。
/home/amov/px4_v1.6.0/Firmware/NuttX/nuttx/include/nuttx/spi/spi.h
struct spi_ops_s
{
CODE int (*lock)(FAR struct spi_dev_s *dev, bool lock);
CODE void (*select)(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
bool selected);
CODE uint32_t (*setfrequency)(FAR struct spi_dev_s *dev, uint32_t frequency);
#ifdef CONFIG_SPI_CS_DELAY_CONTROL
CODE int (*setdelay)(FAR struct spi_dev_s *dev, uint32_t a, uint32_t b,
uint32_t c);
#endif
CODE void (*setmode)(FAR struct spi_dev_s *dev, enum spi_mode_e mode);
CODE void (*setbits)(FAR struct spi_dev_s *dev, int nbits);
#ifdef CONFIG_SPI_HWFEATURES
CODE int (*hwfeatures)(FAR struct spi_dev_s *dev,
spi_hwfeatures_t features);
#endif
CODE uint8_t (*status)(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
#ifdef CONFIG_SPI_CMDDATA
CODE int (*cmddata)(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
bool cmd);
#endif
CODE uint16_t (*send)(FAR struct spi_dev_s *dev, uint16_t wd);
#ifdef CONFIG_SPI_EXCHANGE
CODE void (*exchange)(FAR struct spi_dev_s *dev,
FAR const void *txbuffer, FAR void *rxbuffer,
size_t nwords);
#else
CODE void (*sndblock)(FAR struct spi_dev_s *dev,
FAR const void *buffer, size_t nwords);
CODE void (*recvblock)(FAR struct spi_dev_s *dev, FAR void *buffer,
size_t nwords);
#endif
CODE int (*registercallback)(FAR struct spi_dev_s *dev,
spi_mediachange_t callback, void *arg);
};
从/px4_v1.6.0/Firmware/NuttX/nuttx/include/nuttx/spi/spi.h找到了定义。这里就是Nuttx系统对spi模型的定义。
在看下之前SPI中的这个发送函数。
px4_v1.6.0/Firmware/src/drivers/device/spi.c
SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
SPI_SELECT(_dev, _device, true);
/* do the transfer */
SPI_EXCHANGE(_dev, send, recv, len);
/* and clean up */
SPI_SELECT(_dev, _device, false);
return OK;
}
#define SPI_SETBITS(d,b) do { if ((d)->ops->setbits) (d)->ops->setbits(d,b); } while (0)
#define SPI_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f))
#define SPI_SETMODE(d,m) do { if ((d)->ops->setmode) (d)->ops->setmode(d,m); } while (0)
#ifdef CONFIG_SPI_EXCHANGE
# define SPI_EXCHANGE(d,t,r,l) ((d)->ops->exchange(d,t,r,l))
#endif
到现在为止，这device SPI模型是怎么连接到stm32 spi库的就很明显了。
1.device spi 定义了SPI设备的基本模型包含了初始化，发送接收，频率设置等，在被初始化的时候调用了 stm32_spibus_initialize。进入stm32 spi驱动层操作。
2.在stm32_spibus_initialize中实现了对spi初始化的所有操作，包括硬件配置，spi的功能性配置。
3.这些配置的的修改在/home/amov/px4_v1.6.0/Firmware/src/drivers/boards/px4fmu-v2/board_config.h
4.通过# define SPI_EXCHANGE(d,t,r,l) ((d)->ops->exchange(d,t,r,l)) 可以找到最底层的发送接受部分函数如下。
#if !defined(CONFIG_STM32_SPI_DMA) || defined(CONFIG_STM32_DMACAPABLE)
#if !defined(CONFIG_STM32_SPI_DMA)
static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
FAR void *rxbuffer, size_t nwords)
#else
static void spi_exchange_nodma(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
FAR void *rxbuffer, size_t nwords)
#endif
{
FAR struct stm32_spidev_s *priv = (FAR struct stm32_spidev_s *)dev;
DEBUGASSERT(priv && priv->spibase);
spiinfo("txbuffer=%p rxbuffer=%p nwords=%d\n", txbuffer, rxbuffer, nwords);
/* 8- or 16-bit mode? */
if (spi_16bitmode(priv))
{
/* 16-bit mode */
const uint16_t *src = (const uint16_t *)txbuffer;
uint16_t *dest = (uint16_t *)rxbuffer;
uint16_t word;
while (nwords-- > 0)
{
/* Get the next word to write. Is there a source buffer? */
if (src)
{
word = *src++;
}
else
{
word = 0xffff;
}
/* Exchange one word */
word = spi_send(dev, word);
/* Is there a buffer to receive the return value? */
if (dest)
{
*dest++ = word;
}
}
}
else
{
/* 8-bit mode */
const uint8_t *src = (const uint8_t *)txbuffer;
uint8_t *dest = (uint8_t *)rxbuffer;
uint8_t word;
while (nwords-- > 0)
{
/* Get the next word to write. Is there a source buffer? */
if (src)
{
word = *src++;
}
else
{
word = 0xff;
}
/* Exchange one word */
word = (uint8_t)spi_send(dev, (uint16_t)word);
/* Is there a buffer to receive the return value? */
if (dest)
{
*dest++ = word;
}
}
}
}
从device spi 初始化_dev = px4_spibus_initialize(_bus);函数中的参数_bus可以看出这就是不同的SPI绑定不同的设备。
MPU6000_SPI::MPU6000_SPI(int bus, spi_dev_e 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;
}
在MPU6000_SPI的构造函数中初始化SPI模型给予bus参数。
这部分
/home/amov/px4_v1.6.0/Firmware/src/drivers/mpu6000/mpu6000.cpp
int mpu6000_main(int argc, char *argv[])
{
enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = MPU_DEVICE_TYPE_MPU6000; /* 这里设置了设备类 */
int ch;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "T:XISsZzR:a:")) != EOF) {
switch (ch) {
case 'X':
busid = MPU6000_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MPU6000_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MPU6000_BUS_SPI_EXTERNAL1;
break;
case 's':
busid = MPU6000_BUS_SPI_INTERNAL1;
break;
case 'Z':
busid = MPU6000_BUS_SPI_EXTERNAL2;
break;
case 'z':
busid = MPU6000_BUS_SPI_INTERNAL2;
break;
case 'T':
device_type = atoi(optarg);
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
case 'a':
accel_range = atoi(optarg);
break;
default:
mpu6000::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
mpu6000::start(busid, rotation, accel_range, device_type);
}
if (!strcmp(verb, "stop")) {
mpu6000::stop(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
mpu6000::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
mpu6000::reset(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
mpu6000::info(busid);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
mpu6000::regdump(busid);
}
if (!strcmp(verb, "factorytest")) {
mpu6000::factorytest(busid);
}
if (!strcmp(verb, "testerror")) {
mpu6000::testerror(busid);
}
mpu6000::usage();
exit(1);
}
之后进入start可以看到started |= start_bus(bus_options[i], rotation, range, device_type);
start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type)
{
int fd = -1;
if (bus.dev != nullptr) {
warnx("%s SPI not available", bus.external ? "External" : "Internal");
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, bus.external);
if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new MPU6000(interface, bus.accelpath, bus.gyropath, rotation, device_type);
if (bus.dev == nullptr) {
delete interface;
return false;
}
if (OK != bus.dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
goto fail;
}
close(fd);
return true;
fail:
if (fd >= 0) {
close(fd);
}
if (bus.dev != nullptr) {
delete bus.dev;
bus.dev = nullptr;
}
return false;
}
mpu6000_main是由启动脚本rc.sensor而来。
# external MPU6K is rotated 180 degrees yaw
if mpu6000 -S -R 4 start
# FMUv2
if mpu6000 start
then
看见这个 -S -R 4参数了吗？ 不就对应上面mpu6000_main里面switch判断的不同参数了吗？
这样从一个传感器的使用初始化，总线配置。路线就找到了。
1.rc.sensor脚本给定了调用mpu6000_main需要的参数并启动。包括总线和摆放方向。
2.mpu6000_main中的start初始化了总线。
现在我们在反回去去看mpu6000真正的驱动实现，一个imu传感器的驱动应该包含有芯片初始化，配置，数据读取，数据处理，甚至是数据校准。我们回到mpu6000.c中。
/home/amov/px4_v1.6.0/Firmware/src/drivers/mpu6000/mpu6000.cpp
void test(enum MPU6000_BUS busid)
{
struct mpu6000_bus_option &bus = find_bus(busid);
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
int fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'mpu6000 start')", bus.accelpath);
}
/* get the driver */
int fd_gyro = open(bus.gyropath, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", bus.gyropath);
}
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) {
err(1, "reset to manual polling");
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / MPU6000_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* reset to default polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd);
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset(busid);
errx(0, "PASS");
}
看看这个测试代码，用于mpu6000芯片启动和数据输出的。当我们看到里面有
open
ioctl
read
close
reset
时候就知道了mpu6000被当成了文件，回想最前端mpu6000被注册成了字符设备。
int MPU6000::init()
{
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
/* probe again to get our settings that are based on the device type */
int ret = probe();
/* if probe failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
/* do init */
ret = CDev::init(); /* 在这里 ， 在这里，在这里，被注册为字符设备*/
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("CDev init failed");
return ret;
}
ret = -ENOMEM;
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
}
ret = -EIO;
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
/* 看这里，看这里，看这里 开启了orb公告*/
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation,
int device_type) :
CDev("MPU6000", path_accel), /*看这里，看这里，看这里*/
_interface(interface),
_device_type(device_type),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
/home/amov/px4_v1.6.0/Firmware/src/drivers/mpu6000/mpu6000.h
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
#define MPU_DEVICE_PATH_ACCEL1 "/dev/mpu6000_accel1"
#define MPU_DEVICE_PATH_GYRO1 "/dev/mpu6000_gyro1"
#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext"
#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext"
#define MPU_DEVICE_PATH_ACCEL_EXT1 "/dev/mpu6000_accel_ext1"
#define MPU_DEVICE_PATH_GYRO_EXT1 "/dev/mpu6000_gyro_ext1"
怎么被注册的注册设备路径名称都找到了。
在mpu6000_start（）中开启了定时采集
hrt_call_every(&_call,
1000,
_call_interval - MPU6000_TIMER_REDUCTION,
(hrt_callout)&MPU6000::measure_trampoline, this);
最终调用
int MPU6000::measure()
{
//printf("MPU6000::measure()\n");
if (_in_factory_test) {
// don't publish any data while in factory test mode
return OK;
}
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return OK;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
// sensor transfer at high clock speed
if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return -EIO;
}
check_registers();
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return OK;
}
memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
_got_duplicate = false;
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return -EIO;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return OK;
}
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
if (is_icm_device()) { // if it is an ICM20608
_last_temperature = (report.temp) / 326.8f + 25.0f;
} else { // If it is an MPU6000
_last_temperature = (report.temp) / 361.0f + 35.0f;
}
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
/* return device ID */
arb.device_id = _device_id.devid;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
/* return device ID */
grb.device_id = _gyro->_device_id.devid;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); /*看这里看这里 发布了数据*/
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);/*看这里看这里 发布了数据*/
}
/* stop measuring */
perf_end(_sample_perf);
return OK;
}
好吧最后画一张大一点总图贯串这些。
666
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
