Browse Source

AP_Compass: detect compasses for minlure

Minlure has an onboard compass (HMC5883L) as slave of MPU-6000, but also
allows the use of an external HMC5883L compass, which should be
connected to the lure's I2C port.
master
Lucas De Marchi 9 years ago
parent
commit
68ae619448
  1. 13
      libraries/AP_Compass/AP_Compass.cpp
  2. 3
      libraries/AP_HAL/AP_HAL_Boards.h

13
libraries/AP_Compass/AP_Compass.cpp

@ -1,5 +1,8 @@ @@ -1,5 +1,8 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL_Linux/I2CDevice.h>
#endif
#include <AP_Vehicle/AP_Vehicle.h>
#include "AP_Compass_AK8963.h"
@ -477,6 +480,16 @@ void Compass::_detect_backends(void) @@ -477,6 +480,16 @@ void Compass::_detect_backends(void)
} else {
hal.console->printf("AK8953: External compass not detected\n");
}
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
_add_backend(AP_Compass_HMC5843::probe_mpu6000(*this));
_add_backend(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));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843

3
libraries/AP_HAL/AP_HAL_Boards.h

@ -260,8 +260,7 @@ @@ -260,8 +260,7 @@
#define HAL_BARO_MS5611_I2C_BUS 10
#define HAL_BARO_MS5611_I2C_ADDR 0x77
#define HAL_BARO_MS5611_USE_TIMER true
#define HAL_COMPASS_DEFAULT HAL_COMPASS_HMC5843_MPU6000
/* Internal compass */
/* Internal and external compasses */
#define HAL_COMPASS_HMC5843_I2C_ADDR 0x1E
#define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0"
#define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320

Loading…
Cancel
Save