Ardupilot程序之传感器LSM303d代码学习
目录
- 目录
- 摘要
- 1.原理图学习
- 2.地磁各类和对象之间的关系
- 3.代码学习
- （2）LSM303D数据怎么更新
- 还有更新的地方，但是没有使用
- 总结:到这里我们已经得到了原始数据：
- 更新使用的数据
- （3）LSM303D数据在哪里被使用
- （4）LSM303D数据怎么传送到地面站
摘要
本文档主要记录自己看ardupilot飞控代码——地磁传感器LSM303D的学习过程
1.原理图学习
主要用到的引脚是：
PA5—-SPI1_SCK
PA6—-SPI1_MISO
PA7—-SPI1_MOSI
PB1—-MAG_DRDY
PB4—–ACCEL_DRDY
PC15—ACCEL_MAG_CS**
2.地磁各类和对象之间的关系
3.代码学习
（1）LSM303D怎么注册
（2）LSM303D数据怎么更新
（3）LSM303D数据在哪里被使用
（4）LSM303D数据怎么传送到地面站
（1）LSM303D怎么注册
分析其中的两个重要函数
1。第一个函数bool Compass::init()
/****************************************************************************************************************
*函数原型：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<#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");
}
} 
/****************************************************************************************************************
*函数原型：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));
重点分析坐标系
当把飞控板按照上面这个坐标系放置时，读取的传感器数据是：
（1）x轴指向正北有最大值，z轴数据变化不大
（2）x轴指向正南有最小值，z轴数据变化不大
（3）y轴指向正南有最小值，z轴数据变化不大
（4）y轴指向正北有最大值，z轴数据变化不大
还有更新的地方，但是没有使用
SCHED_TASK(compass_accumulate, 100, 100), //地磁数据计算，运行周期是10ms
总结:到这里我们已经得到了原始数据：
更新使用的数据
（3）LSM303D数据在哪里被使用
EKF学习在添加
（4）LSM303D数据怎么传送到地面站
mag = compass.get_field(0);
const Vector3f &get_field(uint8_t i) const { return _state[i].field; }