Ardupilot程式之感測器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<<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");
}
}
/****************************************************************************************************************
*函式原型：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; }
相關文章
- Ardupilot之感測器mpu6000流程學習（mpu6000）
- 機器學習&惡意程式碼靜態檢測機器學習
- 利用深度學習和機器學習預測股票市場（附程式碼）深度學習機器學習
- Laravel核心程式碼學習 -- 控制器Laravel
- Python學習之路10-測試程式碼Python
- [機器學習] 低程式碼機器學習工具PyCaret庫使用指北機器學習
- Laravel核心程式碼學習 -- 服務提供器Laravel
- python-機器學習程式碼總結Python機器學習
- GET程式碼學習
- What-If 工具：無需寫程式碼，即可測試機器學習模型機器學習模型
- 用Python進行機器學習（附程式碼、學習資源）Python機器學習
- 機器學習 — AdaBoost演算法（手稿+程式碼）機器學習演算法
- 機器學習去除馬賽克案例（程式碼）機器學習
- 基於Sklearn機器學習程式碼實戰機器學習
- Ardupilot實現同一套程式碼支援不同的飛控板原理
- 機器學習_K近鄰Python程式碼詳解機器學習Python
- 初學python之感悟Python
- C++學習筆記，知識點+程式碼測試C++筆記
- Python學習筆記—程式碼Python筆記
- Laravel核心程式碼學習 — 路由Laravel路由
- 學習VIORB程式碼記錄ORB
- Laravel核心程式碼學習 -- 路由Laravel路由
- ROS_DWA--程式碼學習ROS
- 深度學習程式碼積累深度學習
- 【機器學習】--xgboost初始之程式碼實現分類機器學習
- 利用機器學習進行惡意程式碼分類機器學習
- Laravel核心程式碼學習--HTTP核心LaravelHTTP
- Laravel核心程式碼學習 -- Database QueryBuilderLaravelDatabaseUI
- 如何學習以太坊的程式碼
- 前端學習程式碼目錄存放前端
- Java學習之程式碼優化Java優化
- 對比學習 ——simsiam 程式碼解析。
- 《TensorFlow 機器學習方案手冊》（附 pdf 和完整程式碼）機器學習
- metarank: 推薦排名類的低程式碼機器學習工具機器學習
- 關於PHP_CodeSniffer程式碼檢測引數學習記錄PHP
- Laravel核心程式碼學習 — 模型關聯底層程式碼實現Laravel模型
- Laravel核心程式碼學習 -- 模型關聯底層程式碼實現Laravel模型
- 測試程式碼