Ardupilot程式之感測器LSM303d程式碼學習

魔城煙雨發表於2018-06-03

目錄

摘要

本文件主要記錄自己看ardupilot飛控程式碼——地磁感測器LSM303D的學習過程

1.原理圖學習

lsm303d
主要用到的引腳是:
PA5—-SPI1_SCK
PA6—-SPI1_MISO
PA7—-SPI1_MOSI
PB1—-MAG_DRDY
PB4—–ACCEL_DRDY
PC15—ACCEL_MAG_CS**
0


2.地磁各類和物件之間的關係

lsm303d

3.程式碼學習


(1)LSM303D怎麼註冊
(2)LSM303D資料怎麼更新
(3)LSM303D資料在哪裡被使用
(4)LSM303D資料怎麼傳送到地面站


(1)LSM303D怎麼註冊

1
2

3
分析其中的兩個重要函式
1。第一個函式bool Compass::init()
4

/****************************************************************************************************************
*函式原型:void Compass::_detect_backends(void)
*函式功能:識別地磁
*修改日期:2018-6-2
*備   注: detect available backends for this board
*****************************************************************************************************************/
void Compass::_detect_backends(void)
{

    if (_hil_mode)
    {
        _add_backend(AP_Compass_HIL::detect(*this), nullptr, false);
        return;
    }
//    printf("CONFIG_HAL_BOARD=%d\r\n",CONFIG_HAL_BOARD);//g.compass_enabled=1;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2)//CONFIG_HAL_BOARD=5
    {
        printf("CCC\r\n");
        // default to disabling LIS3MDL on pixhawk2 due to hardware issue
        _driver_type_mask.set_default(1U<<DRIVER_LIS3MDL);
    }
#endif

/*
  macro to add a backend with check for too many backends or compass
  instances. We don't try to start more than the maximum allowed
 */
#define ADD_BACKEND(driver_type, backend, name, external)   \
    do { if (_driver_enabled(driver_type)) { _add_backend(backend, name, external); } \
       if (_backend_count == COMPASS_MAX_BACKEND || \
           _compass_count == COMPASS_MAX_INSTANCES) { \
          return; \
        } \
    } while (0)

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL(*this), nullptr, false);
    return;
#endif

    ////////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
    ADD_BACKEND(DRIVER_SITL, AP_Compass_HIL::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
    switch (AP_BoardConfig::get_board_type())
    {
    printf("DDD\r\n");
    case AP_BoardConfig::PX4_BOARD_PX4V1:
    case AP_BoardConfig::PX4_BOARD_PIXHAWK:
    case AP_BoardConfig::PX4_BOARD_PHMINI:
    case AP_BoardConfig::PX4_BOARD_AUAV21:
    case AP_BoardConfig::PX4_BOARD_PH2SLIM:
    case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
    case AP_BoardConfig::PX4_BOARD_PIXRACER: 
    case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:{
        bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
        // external i2c bus
        ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
                                                              true, ROTATION_ROLL_180),
                    AP_Compass_HMC5843::name, true);
        // internal i2c bus
        ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
                                              both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
                    AP_Compass_HMC5843::name, both_i2c_external);

        //external i2c bus
        ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
                                                true,ROTATION_ROLL_180),
                    AP_Compass_QMC5883L::name, true);
        //internal i2c bus
        ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
                                                both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
                    AP_Compass_QMC5883L::name,both_i2c_external);

#if !HAL_MINIMIZE_FEATURES
        // AK09916 on ICM20948
        ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
                                                                        hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
                                                                        hal.i2c_mgr->get_device(1, HAL_COMPASS_ICM20948_I2C_ADDR),
                                                                        true, ROTATION_PITCH_180_YAW_90),
                     AP_Compass_AK09916::name, true);

        ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
                                                                        hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
                                                                        hal.i2c_mgr->get_device(0, HAL_COMPASS_ICM20948_I2C_ADDR),
                                                                        both_i2c_external, ROTATION_PITCH_180_YAW_90),
                     AP_Compass_AK09916::name, true);

        // lis3mdl
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR),
                                                              true, ROTATION_YAW_90),
                    AP_Compass_LIS3MDL::name, true);
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR),
                                                              both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
                    AP_Compass_LIS3MDL::name, both_i2c_external);

        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
                                                              true, ROTATION_YAW_90),
                    AP_Compass_LIS3MDL::name, true);
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
                                                              both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
                    AP_Compass_LIS3MDL::name, both_i2c_external);

        // AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
        if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 1, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
            ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
                                                                  true, ROTATION_YAW_270),
                    AP_Compass_AK09916::name, true);
        }
        if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 0, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
            ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
                                                                  both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
                        AP_Compass_AK09916::name, both_i2c_external);
        }

        // IST8310 on external and internal bus
        ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_IST8310_I2C_ADDR),
                                                              true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true);

        ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
                                                              both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
#endif // HAL_MINIMIZE_FEATURES
        }
        break;

    case AP_BoardConfig::PX4_BOARD_AEROFC:
#ifdef HAL_COMPASS_IST8310_I2C_BUS
        ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
                                                              true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#endif
        break;
    default:
        break;
    }

 ////////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////////
    switch (AP_BoardConfig::get_board_type())
    {

    case AP_BoardConfig::PX4_BOARD_PIXHAWK:
         printf("FFF\r\n");
        ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
                                                              false, ROTATION_PITCH_180),
                    AP_Compass_HMC5843::name, false);
        ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)),
                    AP_Compass_LSM303D::name, false);
        break;

    case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
        ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
                    AP_Compass_LSM303D::name, false);
        // we run the AK8963 only on the 2nd MPU9250, which leaves the
        // first MPU9250 to run without disturbance at high rate
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
                    AP_Compass_AK8963::name, false);
        break;

    case AP_BoardConfig::PX4_BOARD_PIXRACER:
        ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
                                                              false, ROTATION_PITCH_180),
                    AP_Compass_HMC5843::name, false);
        // R15 has LIS3MDL on spi bus instead of HMC; same CS pin
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),
                                                              false, ROTATION_NONE),
                    AP_Compass_LIS3MDL::name, false);
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
                    AP_Compass_AK8963::name, false);
        break;

    case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
                    AP_Compass_AK8963::name, false);
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),
                                                              false, ROTATION_NONE),
                    AP_Compass_LIS3MDL::name, false);
        break;

#if 0
    case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
                    AP_Compass_AK8963::name, false);
        ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),
                                                              false, ROTATION_NONE),
                    AP_Compass_LIS3MDL::name, false);
        break;
#endif

    case AP_BoardConfig::PX4_BOARD_PHMINI:
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180),
                    AP_Compass_AK8963::name, false);
        break;

    case AP_BoardConfig::PX4_BOARD_AUAV21:
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
                    AP_Compass_AK8963::name, false);
        break;

    case AP_BoardConfig::PX4_BOARD_PH2SLIM:
        ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
                    AP_Compass_AK8963::name, false);
        break;

    default:
        break;
    }
////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
    ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QURT::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
    ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")),
                AP_Compass_LSM303D::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
                AP_Compass_HMC5843::name, false);
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT
    ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QFLIGHT::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 1),
                AP_Compass_AK8963::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this),
                AP_Compass_HMC5843::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this,
                     Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device(
                         { /* UEFI with lpss set to ACPI */
                           "platform/80860F41:05",
                           /* UEFI with lpss set to PCI */
                           "pci0000:00/0000:00:18.6" },
                         HAL_COMPASS_HMC5843_I2C_ADDR),
                     true),
                 AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
    ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180),
                AP_Compass_LSM9DS1::name, false);
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                 AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
                AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this),
                AP_Compass_HMC5843::name, false);
#elif  HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
                AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
                AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO
    ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
                AP_Compass_BMM150::name, false);
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
                AP_Compass_HMC5843::name, true);
    ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
                                                          true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
    ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
                AP_Compass_HMC5843::name, false);
    ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
                AP_Compass_AK8963::name, false);
    ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
                AP_Compass_LSM9DS1::name, false);
#else
    #error Unrecognised HAL_COMPASS_TYPE setting
#endif

#if HAL_WITH_UAVCAN
    if (_driver_enabled(DRIVER_UAVCAN)) {
        bool added;
        do {
            added = _add_backend(new AP_Compass_UAVCAN(*this), "UAVCAN", true);
            if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) {
                return;
            }
        } while (added);
    }
#endif

    if (_backend_count == 0 ||
        _compass_count == 0) {
        hal.console->printf("No Compass backends available\n");
    }
}

5
6

7

/****************************************************************************************************************
*函式原型:bool AP_Compass_LSM303D::_hardware_init()
*函式功能:地磁初始化函式
*修改日期:2018-6-2
*備   注:
*****************************************************************************************************************/
bool AP_Compass_LSM303D::_hardware_init()
{
    printf("YYY\r\n");
    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) 
    {
        AP_HAL::panic("LSM303D: Unable to get semaphore");
    }

    // initially run the bus at low speed
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);

    // Test WHOAMI
    uint8_t whoami = _register_read(ADDR_WHO_AM_I);
    if (whoami != WHO_I_AM) {
        hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
        goto fail_whoami;
    }

    uint8_t tries;
    for (tries = 0; tries < 5; tries++) {
        // ensure the chip doesn't interpret any other bus traffic as I2C
        _disable_i2c();

        /* enable mag */
        _reg7_expected = REG7_CONT_MODE_M;
        _register_write(ADDR_CTRL_REG7, _reg7_expected);
        _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M);

        // DRDY on MAG on INT2
        _register_write(ADDR_CTRL_REG4, 0x04);

        _mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
        _mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);

        hal.scheduler->delay(10);
        if (_data_ready()) {
            break;
        }
    }
    if (tries == 5) {
        hal.console->printf("Failed to boot LSM303D 5 times\n");
        goto fail_tries;
    }

    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
    _dev->get_semaphore()->give();

    return true;

fail_tries:
fail_whoami:
    _dev->get_semaphore()->give();
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
    return false;
}

(2)LSM303D資料怎麼更新

這裡我們更關心的是後面的定時器回撥函式

_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
8

9

10
11

12
重點分析座標系
13
當把飛控板按照上面這個座標系放置時,讀取的感測器資料是:
(1)x軸指向正北有最大值,z軸資料變化不大

13

(2)x軸指向正南有最小值,z軸資料變化不大
14

(3)y軸指向正南有最小值,z軸資料變化不大
15

(4)y軸指向正北有最大值,z軸資料變化不大
16



這裡寫圖片描述

還有更新的地方,但是沒有使用

SCHED_TASK(compass_accumulate, 100, 100), //地磁資料計算,執行週期是10ms
地磁資料
16
這裡寫圖片描述

總結:到這裡我們已經得到了原始資料:

這裡寫圖片描述

更新使用的資料

這裡寫圖片描述

(3)LSM303D資料在哪裡被使用

EKF學習在新增

(4)LSM303D資料怎麼傳送到地面站

這裡寫圖片描述

這裡寫圖片描述
mag = compass.get_field(0);
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }

相關文章