ArduPilot添加新的惯导方案

环境：

        ArduRover4.2.2方案

        ChibIOS系统

        pixhawk1（PX4-V2）硬件环境

更换方案：

        使用MPU6000+ICM20689组合惯导替代原有的MPU6000+LSM303D+L3GD20方案

更换内容如下：

 一、hwdef.dat中添加icm20689芯片

#（fmuv3）
SPIDEV mpu6000        SPI1 DEVID4  MPU_CS       MODE3  2*MHZ  8*MHZ
SPIDEV icm20689       SPI1 DEVID1  GYRO_EXT_CS  MODE3  4*MHZ  8*MHZ
SPIDEV lsm9ds0_g      SPI1 DEVID1  GYRO_EXT_CS  MODE3 11*MHZ 11*MHZ 
SPIDEV lsm9ds0_am     SPI1 DEVID2  ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ 
#(pixhawk)
IMU Invensense SPI:mpu6000 ROTATION_ROLL_180
IMU LSM9DS0 SPI:lsm9ds0_g SPI:lsm9ds0_am ROTATION_ROLL_180 ROTATION_ROLL_180_YAW_270 ROTATION_PITCH_180
IMU Invensense SPI:icm20689 ROTATION_NONE

我的MPU6000在背面，ICM20689在正面，所以mpu6000反转180度

LSM303D和L3GD20使用LSM9DS0驱动，保留原有方案

二、libraries/AP_BoardConfig/board_drivers.cpp添加ICM20689支持

1、添加芯片ID识别

#define MPUREG_WHOAMI 0x75
#define MPU_WHOAMI_MPU60X0  0x68
#define MPU_WHOAMI_MPU9250  0x71
#define MPU_WHOAMI_ICM20608 0xaf
#define MPU_WHOAMI_ICM20602 0x12
#define MPU_WHOAMI_ICM20689 0x98

２、 硬件类型选择中添加芯片识别

    } else if ((spi_check_register("lsm9ds0_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) || 
                spi_check_register("icm20689", MPUREG_WHOAMI, MPU_WHOAMI_ICM20689)) &&
               (spi_check_register("mpu6000", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
                spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
                spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
                spi_check_register("mpu9250", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
 
        // classic or upgraded Pixhawk1
        state.board_type.set(PX4_BOARD_PIXHAWK);
        hal.console->printf("Detected Pixhawk\n");

添加完成

注释：

        APM中支持ICM20689芯片驱动，因此不需要另外添加，只需要在二1中添加一次芯片ID即可

        这个版本APM通过惯导方案选择匹配硬件类型，因此需要在二2中添加新的惯导方案。