mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 05:58:30 -04:00
AP_Compass: add and use AP_COMPASS_HMC5843_ENABLED
This commit is contained in:
parent
dd139a3a63
commit
ff99ff34c7
@ -1065,7 +1065,7 @@ void Compass::_probe_external_i2c_compasses(void)
|
|||||||
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
|
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
|
||||||
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
|
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
|
||||||
#endif
|
#endif
|
||||||
#if AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED || defined(HAL_USE_I2C_MAG_HMC5843)
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
// external i2c bus
|
// external i2c bus
|
||||||
FOREACH_I2C_EXTERNAL(i) {
|
FOREACH_I2C_EXTERNAL(i) {
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||||
@ -1081,8 +1081,8 @@ void Compass::_probe_external_i2c_compasses(void)
|
|||||||
all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270));
|
all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif // !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
|
||||||
#endif
|
#endif // AP_COMPASS_HMC5843_ENABLED
|
||||||
|
|
||||||
#if AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED || defined(HAL_USE_I2C_MAG_QMC5883L)
|
#if AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED || defined(HAL_USE_I2C_MAG_QMC5883L)
|
||||||
//external i2c bus
|
//external i2c bus
|
||||||
@ -1325,6 +1325,7 @@ void Compass::_detect_backends(void)
|
|||||||
break;
|
break;
|
||||||
case AP_BoardConfig::VRX_BOARD_BRAIN54:
|
case AP_BoardConfig::VRX_BOARD_BRAIN54:
|
||||||
case AP_BoardConfig::VRX_BOARD_BRAIN51: {
|
case AP_BoardConfig::VRX_BOARD_BRAIN51: {
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
// external i2c bus
|
// external i2c bus
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||||
true, ROTATION_ROLL_180));
|
true, ROTATION_ROLL_180));
|
||||||
@ -1332,6 +1333,7 @@ void Compass::_detect_backends(void)
|
|||||||
// internal i2c bus
|
// internal i2c bus
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||||
false, ROTATION_YAW_270));
|
false, ROTATION_YAW_270));
|
||||||
|
#endif // AP_COMPASS_HMC5843_ENABLED
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@ -1340,9 +1342,11 @@ void Compass::_detect_backends(void)
|
|||||||
case AP_BoardConfig::VRX_BOARD_CORE10:
|
case AP_BoardConfig::VRX_BOARD_CORE10:
|
||||||
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
|
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
|
||||||
case AP_BoardConfig::VRX_BOARD_UBRAIN52: {
|
case AP_BoardConfig::VRX_BOARD_UBRAIN52: {
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
// external i2c bus
|
// external i2c bus
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||||
true, ROTATION_ROLL_180));
|
true, ROTATION_ROLL_180));
|
||||||
|
#endif // AP_COMPASS_HMC5843_ENABLED
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@ -1351,8 +1355,10 @@ void Compass::_detect_backends(void)
|
|||||||
}
|
}
|
||||||
switch (AP_BoardConfig::get_board_type()) {
|
switch (AP_BoardConfig::get_board_type()) {
|
||||||
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
|
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
|
||||||
false, ROTATION_PITCH_180));
|
false, ROTATION_PITCH_180));
|
||||||
|
#endif
|
||||||
#if AP_COMPASS_LSM303D_ENABLED
|
#if AP_COMPASS_LSM303D_ENABLED
|
||||||
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE));
|
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE));
|
||||||
#endif
|
#endif
|
||||||
@ -1403,8 +1409,10 @@ void Compass::_detect_backends(void)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
|
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||||
false, ROTATION_YAW_90));
|
false, ROTATION_YAW_90));
|
||||||
|
#endif
|
||||||
#if AP_COMPASS_LSM303D_ENABLED
|
#if AP_COMPASS_LSM303D_ENABLED
|
||||||
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270));
|
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270));
|
||||||
#endif
|
#endif
|
||||||
|
@ -414,7 +414,9 @@ private:
|
|||||||
|
|
||||||
// enum of drivers for COMPASS_TYPEMASK
|
// enum of drivers for COMPASS_TYPEMASK
|
||||||
enum DriverType {
|
enum DriverType {
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
DRIVER_HMC5843 =0,
|
DRIVER_HMC5843 =0,
|
||||||
|
#endif
|
||||||
#if AP_COMPASS_LSM303D_ENABLED
|
#if AP_COMPASS_LSM303D_ENABLED
|
||||||
DRIVER_LSM303D =1,
|
DRIVER_LSM303D =1,
|
||||||
#endif
|
#endif
|
||||||
|
@ -21,9 +21,9 @@
|
|||||||
* Sensor is initialized in Continuos mode (10Hz)
|
* Sensor is initialized in Continuos mode (10Hz)
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include "AP_Compass_HMC5843.h"
|
||||||
|
|
||||||
#ifdef HAL_COMPASS_HMC5843_I2C_ADDR
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
|
|
||||||
#include <assert.h>
|
#include <assert.h>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
@ -31,8 +31,7 @@
|
|||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_HAL/utility/sparse-endian.h>
|
#include <AP_HAL/utility/sparse-endian.h>
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include "AP_Compass_HMC5843.h"
|
|
||||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||||
#include <AP_InertialSensor/AuxiliaryBus.h>
|
#include <AP_InertialSensor/AuxiliaryBus.h>
|
||||||
|
|
||||||
@ -587,4 +586,4 @@ uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
|
|||||||
return _bus->get_bus_id();
|
return _bus->get_bus_id();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif // AP_COMPASS_HMC5843_ENABLED
|
||||||
|
@ -1,11 +1,17 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AP_Compass_config.h"
|
||||||
|
|
||||||
|
#if AP_COMPASS_HMC5843_ENABLED
|
||||||
|
|
||||||
|
#ifndef HAL_COMPASS_HMC5843_I2C_ADDR
|
||||||
|
#define HAL_COMPASS_HMC5843_I2C_ADDR 0x1E
|
||||||
|
#endif
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_HAL/Device.h>
|
#include <AP_HAL/Device.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
|
||||||
|
|
||||||
#include "AP_Compass.h"
|
|
||||||
#include "AP_Compass_Backend.h"
|
#include "AP_Compass_Backend.h"
|
||||||
|
|
||||||
class AuxiliaryBus;
|
class AuxiliaryBus;
|
||||||
@ -147,3 +153,5 @@ private:
|
|||||||
AuxiliaryBusSlave *_slave;
|
AuxiliaryBusSlave *_slave;
|
||||||
bool _started;
|
bool _started;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_COMPASS_HMC5843_ENABLED
|
||||||
|
@ -34,6 +34,10 @@
|
|||||||
#define AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED AP_COMPASS_BACKEND_DEFAULT_ENABLED
|
#define AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED AP_COMPASS_BACKEND_DEFAULT_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_COMPASS_HMC5843_ENABLED
|
||||||
|
#define AP_COMPASS_HMC5843_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef AP_COMPASS_IST8308_ENABLED
|
#ifndef AP_COMPASS_IST8308_ENABLED
|
||||||
#define AP_COMPASS_IST8308_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
|
#define AP_COMPASS_IST8308_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
Loading…
Reference in New Issue
Block a user