Pixhawk原生固件PX4之添外置传感器MPU6500

欢迎交流~ 个人 Gitter 交流平台，点击直达：

成功的在Pixhawk上添加了一个自定义的传感器MPU6500.

Pixhawk飞控板上空余出一个SPI4接口

提示： 多出来的GPIO_EXT引脚可以作为片选引脚，再接一个传感器。

PX4驱动文件中已经写好了mpu6500的驱动程序，只是没有使用。整体与mpu6000.cpp类似，但是一些细节部分还未统一。总之PX4的传感器驱动部分一直在改。

这里是基于Firmware 1.5.2

修改位置

Firmware/src/drivers/boards/px4fmu-v2/board_config.h

/*Fantasy modified begins */
/* SPI4 off */
#define GPIO_SPI4_SCK_OFF   (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN2)  // PE2
#define GPIO_SPI4_MISO_OFF  (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN5)  // PE5
#define GPIO_SPI4_MOSI_OFF  (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN6)  // PE6
/*Fantasy modified ends */

/* SPI4 chip selects off Fantasy add*/
#define GPIO_SPI_CS_EXT0_OFF        (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTE|GPIO_PIN4) // PE4
/*Fantasy modified ends */

Firmware/src/drivers/boards/px4fmu-v2/px4fmu2_init.c

__EXPORT int nsh_archinitialize(void)
{
  ...
    spi4 = px4_spibus_initialize(4);

    /* Default SPI4 to 1MHz and de-assert the known chip selects. */
    SPI_SETFREQUENCY(spi4, 10000000);
    SPI_SETBITS(spi4, 8);
    SPI_SETMODE(spi4, SPIDEV_MODE3);
    /* Fantasy modified begins */
    // SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
    // SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
    SPI_SELECT(spi4, PX4_SPIDEV_EXT_MPU, false);
    // 此处不改也可，只是为了强调SPI_SELECT()这个函数的作用
    /* Fantasy modified ends */

Firmware/src/drivers/mpu6500/mpu6500.cpp

int
mpu6500_main(int argc, char *argv[])
{
    /* Fantasy modified begins*/
    //bool external_bus = false;
    bool external_bus = true; //因为这里使用的是外部SPI
    /* Fantasy modified ends*/
  ...

Firmware/ROMFS/px4fmu_common/init.d/rc.sensors

#!nsh
#
# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers.
#
...
    else
        # FMUv2
        if mpu6000 start
        then
        fi

        if mpu9250 start
        then
        fi

        if l3gd20 start
        then
        fi

        if lsm303d start
        then
        fi
    fi
fi

Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake

...
    drivers/pwm_input
    drivers/camera_trigger
    drivers/bst
    #drivers/snapdragon_rc_pwm
    drivers/lis3mdl
    drivers/mpu6500
...

随便在某宝买了一款MPU6500的传感器模块

接上就好使

实际测试

确定好使。

总结与疑问：

• 配置好端口驱动注册到系统就可以采用基本的文件操作指令了
• 之前听说传感器都是采用的中断方式读取，这里笔者并没有接MPU6500传感器模块的中断引脚。

/* 
 * A simple data read app
 */
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <nuttx/config.h>
#include <drivers/drv_hrt.h>
#include <sys/types.h>
#include <stdint.h>
#include <sys/time.h>
#include <px4_time.h>
#include <string.h>
#include <nuttx/rtc.h>

#include <stdio.h>
#include <errno.h>
#include <uORB/uORB.h>

#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>



__EXPORT int fantasy_test_main(int argc, char *argv[]);

int fantasy_test_main(int argc, char *argv[])
{
    PX4_INFO("Hello Sky!");

    /* subscribe to sensor_combined topic */
    int accel_sub_fd = orb_subscribe(ORB_ID(sensor_accel_mpu6500));
    int  gyro_sub_fd = orb_subscribe(ORB_ID(sensor_gyro_mpu6500));
    /* limit the update rate to 10 Hz */
    orb_set_interval(accel_sub_fd, 100);
    orb_set_interval(gyro_sub_fd,100);

    /* one could wait for multiple topics with this technique, just using one here */
    px4_pollfd_struct_t fds[] = {
        { .fd = accel_sub_fd,   .events = POLLIN },
        { .fd = gyro_sub_fd,    .events = POLLIN },

    };

    int error_counter = 0;

    for (int i = 0; i < 10; i++) {
        /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
        int poll_ret = px4_poll(fds, 2, 1000);

        /* handle the poll result */
        if (poll_ret == 0) {
            /* this means none of our providers is giving us data */
            PX4_ERR("Got no data within a second");

        } else if (poll_ret < 0) {
            /* this is seriously bad - should be an emergency */
            if (error_counter < 10 || error_counter % 50 == 0) {
                /* use a counter to prevent flooding (and slowing us down) */
                PX4_ERR("ERROR return value from poll(): %d", poll_ret);
            }

            error_counter++;

        } else {

            if (fds[0].revents & POLLIN) {
                /* obtained data for the first file descriptor */
                struct sensor_accel_s acc_raw;
                /* copy sensors raw data into local buffer */
                orb_copy(ORB_ID(sensor_accel_mpu6500), accel_sub_fd, &acc_raw);
                PX4_INFO("Accelerometer:\t %8.4f \t%8.4f \t%8.4f ",
                     (double)acc_raw.x_raw,
                     (double)acc_raw.y_raw,
                     (double)acc_raw.z_raw);
            }


            if (fds[1].revents & POLLIN) {
                /* obtained data for the first file descriptor */
                struct sensor_gyro_s gyro_raw;
                /* copy sensors raw data into local buffer */
                orb_copy(ORB_ID(sensor_gyro_mpu6500), gyro_sub_fd, &gyro_raw);
                PX4_INFO("Gyroscope:\t %8.4f \t%8.4f \t%8.4f ",
                     (double)gyro_raw.x_raw,
                     (double)gyro_raw.y_raw,
                     (double)gyro_raw.z_raw);
            }

        }
    }

    PX4_INFO("exiting");

    return 0;
}

接下来在QGC添加自定义消息。

哈

                                          By Fantasy