AP_Compass: add and use AP_COMPASS_HMC5843_ENABLED

This commit is contained in:
Peter Barker 2023-01-30 10:33:08 +11:00 committed by Andrew Tridgell
parent dd139a3a63
commit ff99ff34c7
5 changed files with 31 additions and 10 deletions

View File

@ -1065,7 +1065,7 @@ void Compass::_probe_external_i2c_compasses(void)
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
#endif
#if AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED || defined(HAL_USE_I2C_MAG_HMC5843)
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
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));
}
}
#endif
#endif
#endif // !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
#endif // AP_COMPASS_HMC5843_ENABLED
#if AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED || defined(HAL_USE_I2C_MAG_QMC5883L)
//external i2c bus
@ -1325,6 +1325,7 @@ void Compass::_detect_backends(void)
break;
case AP_BoardConfig::VRX_BOARD_BRAIN54:
case AP_BoardConfig::VRX_BOARD_BRAIN51: {
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180));
@ -1332,6 +1333,7 @@ void Compass::_detect_backends(void)
// internal i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
false, ROTATION_YAW_270));
#endif // AP_COMPASS_HMC5843_ENABLED
}
break;
@ -1340,9 +1342,11 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::VRX_BOARD_CORE10:
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
case AP_BoardConfig::VRX_BOARD_UBRAIN52: {
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180));
#endif // AP_COMPASS_HMC5843_ENABLED
}
break;
@ -1351,8 +1355,10 @@ void Compass::_detect_backends(void)
}
switch (AP_BoardConfig::get_board_type()) {
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),
false, ROTATION_PITCH_180));
#endif
#if AP_COMPASS_LSM303D_ENABLED
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE));
#endif
@ -1403,8 +1409,10 @@ void Compass::_detect_backends(void)
break;
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),
false, ROTATION_YAW_90));
#endif
#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));
#endif

View File

@ -414,7 +414,9 @@ private:
// enum of drivers for COMPASS_TYPEMASK
enum DriverType {
#if AP_COMPASS_HMC5843_ENABLED
DRIVER_HMC5843 =0,
#endif
#if AP_COMPASS_LSM303D_ENABLED
DRIVER_LSM303D =1,
#endif

View File

@ -21,9 +21,9 @@
* 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 <utility>
@ -31,8 +31,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_Compass_HMC5843.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_InertialSensor/AP_InertialSensor.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();
}
#endif
#endif // AP_COMPASS_HMC5843_ENABLED

View File

@ -1,11 +1,17 @@
#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_HAL/AP_HAL.h>
#include <AP_HAL/Device.h>
#include <AP_Math/AP_Math.h>
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
class AuxiliaryBus;
@ -147,3 +153,5 @@ private:
AuxiliaryBusSlave *_slave;
bool _started;
};
#endif // AP_COMPASS_HMC5843_ENABLED

View File

@ -34,6 +34,10 @@
#define AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED AP_COMPASS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_COMPASS_HMC5843_ENABLED
#define AP_COMPASS_HMC5843_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_COMPASS_IST8308_ENABLED
#define AP_COMPASS_IST8308_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
#endif