mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-15 11:03:57 -03:00
AP_Compass: merged compass changes from plane3.8
This commit is contained in:
parent
51e9091ca3
commit
2e11bef518
@ -5,6 +5,7 @@
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
|
||||
#include "AP_Compass_SITL.h"
|
||||
#include "AP_Compass_AK8963.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
#include "AP_Compass_BMM150.h"
|
||||
@ -17,9 +18,11 @@
|
||||
#include "AP_Compass_qflight.h"
|
||||
#include "AP_Compass_LIS3MDL.h"
|
||||
#include "AP_Compass_AK09916.h"
|
||||
#include "AP_Compass_QMC5883L.h"
|
||||
#if HAL_WITH_UAVCAN
|
||||
#include "AP_Compass_UAVCAN.h"
|
||||
#endif
|
||||
#include "AP_Compass_MMC3416.h"
|
||||
#include "AP_Compass.h"
|
||||
|
||||
extern AP_HAL::HAL& hal;
|
||||
@ -31,7 +34,7 @@ extern AP_HAL::HAL& hal;
|
||||
#endif
|
||||
|
||||
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
|
||||
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 600
|
||||
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 850
|
||||
#endif
|
||||
|
||||
const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
@ -41,7 +44,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass offsets in milligauss on the X axis
|
||||
// @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -49,7 +52,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass offsets in milligauss on the Y axis
|
||||
// @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -57,7 +60,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass offsets in milligauss on the Z axis
|
||||
// @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("OFS", 1, Compass, _state[0].offset, 0),
|
||||
@ -66,7 +69,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass declination
|
||||
// @Description: An angle to compensate between the true north and magnetic north
|
||||
// @Range: -3.142 3.142
|
||||
// @Units: Radians
|
||||
// @Units: rad
|
||||
// @Increment: 0.01
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("DEC", 2, Compass, _declination, 0),
|
||||
@ -101,25 +104,25 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
|
||||
// @Param: MOT_X
|
||||
// @DisplayName: Motor interference compensation for body frame X axis
|
||||
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT_Y
|
||||
// @DisplayName: Motor interference compensation for body frame Y axis
|
||||
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT_Z
|
||||
// @DisplayName: Motor interference compensation for body frame Z axis
|
||||
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MOT", 7, Compass, _state[0].motor_compensation, 0),
|
||||
@ -142,7 +145,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass2 offsets in milligauss on the X axis
|
||||
// @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -150,7 +153,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass2 offsets in milligauss on the Y axis
|
||||
// @Description: Offset to be added to compass2's y-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -158,32 +161,32 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass2 offsets in milligauss on the Z axis
|
||||
// @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("OFS2", 10, Compass, _state[1].offset, 0),
|
||||
|
||||
// @Param: MOT2_X
|
||||
// @DisplayName: Motor interference compensation to compass2 for body frame X axis
|
||||
// @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT2_Y
|
||||
// @DisplayName: Motor interference compensation to compass2 for body frame Y axis
|
||||
// @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT2_Z
|
||||
// @DisplayName: Motor interference compensation to compass2 for body frame Z axis
|
||||
// @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MOT2", 11, Compass, _state[1].motor_compensation, 0),
|
||||
@ -199,7 +202,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass3 offsets in milligauss on the X axis
|
||||
// @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -207,7 +210,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass3 offsets in milligauss on the Y axis
|
||||
// @Description: Offset to be added to compass3's y-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
@ -215,32 +218,32 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @DisplayName: Compass3 offsets in milligauss on the Z axis
|
||||
// @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
|
||||
// @Range: -400 400
|
||||
// @Units: milligauss
|
||||
// @Units: mGauss
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("OFS3", 13, Compass, _state[2].offset, 0),
|
||||
|
||||
// @Param: MOT3_X
|
||||
// @DisplayName: Motor interference compensation to compass3 for body frame X axis
|
||||
// @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT3_Y
|
||||
// @DisplayName: Motor interference compensation to compass3 for body frame Y axis
|
||||
// @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
|
||||
// @Param: MOT3_Z
|
||||
// @DisplayName: Motor interference compensation to compass3 for body frame Z axis
|
||||
// @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference
|
||||
// @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
|
||||
// @Range: -1000 1000
|
||||
// @Units: Offset per Amp or at Full Throttle
|
||||
// @Units: mGauss/A
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("MOT3", 14, Compass, _state[2].motor_compensation, 0),
|
||||
@ -417,6 +420,13 @@ const AP_Param::GroupInfo Compass::var_info[] = {
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
|
||||
|
||||
// @Param: TYPEMASK
|
||||
// @DisplayName: Compass disable driver type mask
|
||||
// @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
|
||||
// @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,10:QFLIGHT,11:UAVCAN,12:QMC5883
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
@ -490,6 +500,29 @@ bool Compass::_add_backend(AP_Compass_Backend *backend, const char *name, bool e
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
return true if a driver type is enabled
|
||||
*/
|
||||
bool Compass::_driver_enabled(enum DriverType driver_type)
|
||||
{
|
||||
uint32_t mask = (1U<<uint8_t(driver_type));
|
||||
return (mask & uint32_t(_driver_type_mask.get())) == 0;
|
||||
}
|
||||
|
||||
/*
|
||||
see if we already have probed a driver by bus type
|
||||
*/
|
||||
bool Compass::_have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const
|
||||
{
|
||||
uint32_t id = AP_HAL::Device::make_bus_id(bus_type, bus_num, address, devtype);
|
||||
for (uint8_t i=0; i<_compass_count; i++) {
|
||||
if (id == uint32_t(_state[i].dev_id.get())) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
detect available backends for this board
|
||||
*/
|
||||
@ -500,20 +533,32 @@ void Compass::_detect_backends(void)
|
||||
return;
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
|
||||
// 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(backend, name, external) \
|
||||
do { _add_backend(backend, name, external); \
|
||||
#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(AP_Compass_HIL::detect(*this), nullptr, false);
|
||||
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()) {
|
||||
case AP_BoardConfig::PX4_BOARD_PX4V1:
|
||||
@ -522,42 +567,77 @@ void Compass::_detect_backends(void)
|
||||
case AP_BoardConfig::PX4_BOARD_AUAV21:
|
||||
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
|
||||
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
|
||||
case AP_BoardConfig::PX4_BOARD_PIXRACER: {
|
||||
#if 0
|
||||
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
|
||||
#endif
|
||||
case AP_BoardConfig::PX4_BOARD_PIXRACER:{
|
||||
bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
|
||||
// external i2c bus
|
||||
ADD_BACKEND(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);
|
||||
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(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
|
||||
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);
|
||||
#if !HAL_MINIMIZE_FEATURES
|
||||
#if 0
|
||||
// lis3mdl - this is disabled for now due to an errata on pixhawk2 GPS unit, pending investigation
|
||||
ADD_BACKEND(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(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);
|
||||
#endif
|
||||
|
||||
// AK09916
|
||||
ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
true, ROTATION_YAW_270),
|
||||
//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_NONE),
|
||||
AP_Compass_AK09916::name, true);
|
||||
ADD_BACKEND(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);
|
||||
|
||||
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_NONE),
|
||||
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);
|
||||
|
||||
// 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(AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
|
||||
ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::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);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
@ -565,43 +645,53 @@ void Compass::_detect_backends(void)
|
||||
}
|
||||
switch (AP_BoardConfig::get_board_type()) {
|
||||
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
|
||||
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
|
||||
false, ROTATION_PITCH_180),
|
||||
AP_Compass_HMC5843::name, false);
|
||||
ADD_BACKEND(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)),
|
||||
AP_Compass_LSM303D::name, false);
|
||||
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(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
|
||||
AP_Compass_LSM303D::name, false);
|
||||
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(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
|
||||
AP_Compass_AK8963::name, false);
|
||||
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(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
|
||||
false, ROTATION_PITCH_180),
|
||||
AP_Compass_HMC5843::name, false);
|
||||
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
|
||||
AP_Compass_AK8963::name, false);
|
||||
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_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
|
||||
AP_Compass_AK8963::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(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180),
|
||||
AP_Compass_AK8963::name, false);
|
||||
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(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90),
|
||||
AP_Compass_AK8963::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_PH2SLIM:
|
||||
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_YAW_270),
|
||||
AP_Compass_AK8963::name, false);
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -609,29 +699,29 @@ void Compass::_detect_backends(void)
|
||||
}
|
||||
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
|
||||
ADD_BACKEND(AP_Compass_QURT::detect(*this), nullptr, false);
|
||||
ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QURT::detect(*this), nullptr, false);
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
|
||||
ADD_BACKEND(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(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")),
|
||||
AP_Compass_LSM303D::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_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(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
|
||||
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(AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::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(AP_Compass_QFLIGHT::detect(*this));
|
||||
ADD_BACKEND(DRIVER_QFLIGHT, AP_Compass_QFLIGHT::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
|
||||
ADD_BACKEND(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(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 1),
|
||||
AP_Compass_AK8963::name, true);
|
||||
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(AP_Compass_HMC5843::probe_mpu6000(*this),
|
||||
AP_Compass_HMC5843::name, false);
|
||||
ADD_BACKEND(AP_Compass_HMC5843::probe(*this,
|
||||
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",
|
||||
@ -641,67 +731,71 @@ void Compass::_detect_backends(void)
|
||||
true),
|
||||
AP_Compass_HMC5843::name, true);
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
|
||||
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180),
|
||||
AP_Compass_LSM9DS1::name, false);
|
||||
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(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_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(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(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_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(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(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_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(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(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
|
||||
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(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
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(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_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(AP_Compass_HMC5843::probe_mpu6000(*this),
|
||||
AP_Compass_HMC5843::name, false);
|
||||
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(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);
|
||||
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(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_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(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_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(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(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||
AP_Compass_AK8963::name, false);
|
||||
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
|
||||
AP_Compass_LSM9DS1::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)),
|
||||
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 ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr))
|
||||
{
|
||||
if((_backend_count < COMPASS_MAX_BACKEND) && (_compass_count < COMPASS_MAX_INSTANCES))
|
||||
{
|
||||
printf("Creating AP_Compass_UAVCAN\n\r");
|
||||
_backends[_backend_count] = new AP_Compass_UAVCAN(*this);
|
||||
_backend_count++;
|
||||
}
|
||||
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
|
||||
|
||||
|
@ -122,7 +122,7 @@ public:
|
||||
/*
|
||||
handle an incoming MAG_CAL command
|
||||
*/
|
||||
uint8_t handle_mag_cal_command(const mavlink_command_long_t &packet);
|
||||
MAV_RESULT handle_mag_cal_command(const mavlink_command_long_t &packet);
|
||||
|
||||
void send_mag_cal_progress(mavlink_channel_t chan);
|
||||
void send_mag_cal_report(mavlink_channel_t chan);
|
||||
@ -261,6 +261,9 @@ public:
|
||||
uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; }
|
||||
uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; }
|
||||
|
||||
uint32_t last_update_ms(void) const { return _state[get_primary()].last_update_ms; }
|
||||
uint32_t last_update_ms(uint8_t i) const { return _state[i].last_update_ms; }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// HIL variables
|
||||
@ -286,7 +289,7 @@ public:
|
||||
uint16_t get_offsets_max(void) const {
|
||||
return (uint16_t)_offset_max.get();
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
/// Register a new compas driver, allocating an instance number
|
||||
///
|
||||
@ -307,6 +310,9 @@ private:
|
||||
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
|
||||
bool _auto_reboot() { return _compass_cal_autoreboot; }
|
||||
|
||||
// see if we already have probed a driver by bus type
|
||||
bool _have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const;
|
||||
|
||||
|
||||
//keep track of which calibrators have been saved
|
||||
bool _cal_saved[COMPASS_MAX_INSTANCES];
|
||||
@ -317,6 +323,26 @@ private:
|
||||
bool _cal_complete_requires_reboot;
|
||||
bool _cal_has_run;
|
||||
|
||||
// enum of drivers for COMPASS_TYPEMASK
|
||||
enum DriverType {
|
||||
DRIVER_HMC5883 =0,
|
||||
DRIVER_LSM303D =1,
|
||||
DRIVER_AK8963 =2,
|
||||
DRIVER_BMM150 =3,
|
||||
DRIVER_LSM9DS1 =4,
|
||||
DRIVER_LIS3MDL =5,
|
||||
DRIVER_AK09916 =6,
|
||||
DRIVER_IST8310 =7,
|
||||
DRIVER_ICM20948 =8,
|
||||
DRIVER_MMC3416 =9,
|
||||
DRIVER_QFLIGHT =10,
|
||||
DRIVER_UAVCAN =11,
|
||||
DRIVER_QMC5883 =12,
|
||||
DRIVER_SITL =13,
|
||||
};
|
||||
|
||||
bool _driver_enabled(enum DriverType driver_type);
|
||||
|
||||
// backend objects
|
||||
AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
|
||||
uint8_t _backend_count;
|
||||
@ -388,11 +414,14 @@ private:
|
||||
} _state[COMPASS_MAX_INSTANCES];
|
||||
|
||||
AP_Int16 _offset_max;
|
||||
|
||||
|
||||
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
|
||||
|
||||
// if we want HIL only
|
||||
bool _hil_mode:1;
|
||||
|
||||
AP_Float _calibration_threshold;
|
||||
|
||||
// mask of driver types to not load. Bit positions match DEVTYPE_ in backend
|
||||
AP_Int32 _driver_type_mask;
|
||||
};
|
||||
|
@ -37,8 +37,17 @@
|
||||
#define REG_CNTL2 0x31
|
||||
#define REG_CNTL3 0x32
|
||||
|
||||
#define REG_ICM_WHOAMI 0x00
|
||||
#define REG_ICM_PWR_MGMT_1 0x06
|
||||
#define REG_ICM_INT_PIN_CFG 0x0f
|
||||
|
||||
#define ICM_WHOAMI_VAL 0xEA
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
/*
|
||||
probe for AK09916 directly on I2C
|
||||
*/
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
@ -47,7 +56,33 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), force_external, rotation);
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), nullptr,
|
||||
force_external,
|
||||
rotation, AK09916_I2C);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
probe for AK09916 connected via an ICM20948
|
||||
*/
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), std::move(dev_icm),
|
||||
force_external,
|
||||
rotation, AK09916_ICM20948_I2C);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
@ -58,10 +93,14 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
|
||||
|
||||
AP_Compass_AK09916::AP_Compass_AK09916(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev_icm,
|
||||
bool _force_external,
|
||||
enum Rotation _rotation)
|
||||
enum Rotation _rotation,
|
||||
enum bus_type _bus_type)
|
||||
: AP_Compass_Backend(compass)
|
||||
, bus_type(_bus_type)
|
||||
, dev(std::move(_dev))
|
||||
, dev_icm(std::move(_dev_icm))
|
||||
, force_external(_force_external)
|
||||
, rotation(_rotation)
|
||||
{
|
||||
@ -73,6 +112,26 @@ bool AP_Compass_AK09916::init()
|
||||
return false;
|
||||
}
|
||||
|
||||
if (bus_type == AK09916_ICM20948_I2C && dev_icm) {
|
||||
uint8_t rval;
|
||||
if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
|
||||
rval != ICM_WHOAMI_VAL) {
|
||||
// not an ICM_WHOAMI
|
||||
goto fail;
|
||||
}
|
||||
// see if ICM20948 is sleeping
|
||||
if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
|
||||
goto fail;
|
||||
}
|
||||
if (rval & 0x40) {
|
||||
// bring out of sleep mode, use internal oscillator
|
||||
dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00);
|
||||
hal.scheduler->delay(10);
|
||||
}
|
||||
// enable i2c bypass
|
||||
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
|
||||
}
|
||||
|
||||
uint16_t whoami;
|
||||
if (!dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2) ||
|
||||
whoami != 0x0948) {
|
||||
@ -98,7 +157,7 @@ bool AP_Compass_AK09916::init()
|
||||
set_external(compass_instance, true);
|
||||
}
|
||||
|
||||
dev->set_device_type(DEVTYPE_AK09916);
|
||||
dev->set_device_type(bus_type == AK09916_ICM20948_I2C?DEVTYPE_ICM20948:DEVTYPE_AK09916);
|
||||
set_dev_id(compass_instance, dev->get_bus_id());
|
||||
|
||||
// call timer() at 100Hz
|
||||
|
@ -26,6 +26,11 @@
|
||||
# define HAL_COMPASS_AK09916_I2C_ADDR 0x0C
|
||||
#endif
|
||||
|
||||
// the AK09916 can be connected via an ICM20948
|
||||
#ifndef HAL_COMPASS_ICM20948_I2C_ADDR
|
||||
# define HAL_COMPASS_ICM20948_I2C_ADDR 0x69
|
||||
#endif
|
||||
|
||||
class AP_Compass_AK09916 : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
@ -34,16 +39,32 @@ public:
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
// separate probe function for when behind a ICM20948 IMU
|
||||
static AP_Compass_Backend *probe_ICM20948(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
void read() override;
|
||||
|
||||
static constexpr const char *name = "AK09916";
|
||||
|
||||
private:
|
||||
AP_Compass_AK09916(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum bus_type {
|
||||
AK09916_I2C=0,
|
||||
AK09916_ICM20948_I2C,
|
||||
} bus_type;
|
||||
|
||||
AP_Compass_AK09916(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
enum Rotation rotation,
|
||||
enum bus_type bus_type);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm;
|
||||
|
||||
/**
|
||||
* Device periodic callback to read data from the sensor.
|
||||
|
@ -22,6 +22,7 @@
|
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define CHIP_ID_REG 0x40
|
||||
#define CHIP_ID_VAL 0x32
|
||||
@ -100,10 +101,27 @@ bool AP_Compass_BMM150::_load_trim_values()
|
||||
le16_t dig_z3;
|
||||
int8_t dig_xy2;
|
||||
uint8_t dig_xy1;
|
||||
} PACKED trim_registers;
|
||||
} PACKED trim_registers, trim_registers2;
|
||||
|
||||
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
|
||||
sizeof(trim_registers))) {
|
||||
// read the registers twice to confirm we have the right
|
||||
// values. There is no CRC in the registers and these values are
|
||||
// vital to correct operation
|
||||
int8_t tries = 4;
|
||||
while (tries--) {
|
||||
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
|
||||
sizeof(trim_registers))) {
|
||||
continue;
|
||||
}
|
||||
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,
|
||||
sizeof(trim_registers))) {
|
||||
continue;
|
||||
}
|
||||
if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (-1 == tries) {
|
||||
hal.console->printf("BMM150: Failed to load trim registers\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -132,27 +150,42 @@ bool AP_Compass_BMM150::init()
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Do a soft reset */
|
||||
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
}
|
||||
hal.scheduler->delay(2);
|
||||
// 10 retries for init
|
||||
_dev->set_retries(10);
|
||||
|
||||
/* Change power state from suspend mode to sleep mode */
|
||||
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
}
|
||||
hal.scheduler->delay(2);
|
||||
// use checked registers to cope with bus errors
|
||||
_dev->setup_checked_registers(4);
|
||||
|
||||
int8_t boot_tries = 4;
|
||||
while (boot_tries--) {
|
||||
/* Do a soft reset */
|
||||
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
|
||||
hal.scheduler->delay(2);
|
||||
if (!ret) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
/* Change power state from suspend mode to sleep mode */
|
||||
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
|
||||
hal.scheduler->delay(2);
|
||||
if (!ret) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
|
||||
if (!ret) {
|
||||
continue;
|
||||
}
|
||||
if (val == CHIP_ID_VAL) {
|
||||
// found it
|
||||
break;
|
||||
}
|
||||
if (boot_tries == 0) {
|
||||
hal.console->printf("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);
|
||||
}
|
||||
}
|
||||
if (val != CHIP_ID_VAL) {
|
||||
hal.console->printf("BMM150: Wrong id\n");
|
||||
goto fail;
|
||||
if (-1 == boot_tries) {
|
||||
goto bus_error;
|
||||
}
|
||||
|
||||
ret = _load_trim_values();
|
||||
@ -167,16 +200,16 @@ bool AP_Compass_BMM150::init()
|
||||
* - ODR = 20
|
||||
* But we are going to use 30Hz of ODR
|
||||
*/
|
||||
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2);
|
||||
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2, true);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
}
|
||||
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1);
|
||||
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1, true);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
}
|
||||
/* Change operation mode from sleep to normal and set ODR */
|
||||
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ);
|
||||
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ, true);
|
||||
if (!ret) {
|
||||
goto bus_error;
|
||||
}
|
||||
@ -189,14 +222,20 @@ bool AP_Compass_BMM150::init()
|
||||
_dev->set_device_type(DEVTYPE_BMM150);
|
||||
set_dev_id(_compass_instance, _dev->get_bus_id());
|
||||
|
||||
_perf_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "BMM150_err");
|
||||
|
||||
// 2 retries for run
|
||||
_dev->set_retries(2);
|
||||
|
||||
_dev->register_periodic_callback(MEASURE_TIME_USEC,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
|
||||
|
||||
_last_read_ms = AP_HAL::millis();
|
||||
|
||||
return true;
|
||||
|
||||
bus_error:
|
||||
hal.console->printf("BMM150: Bus communication error\n");
|
||||
fail:
|
||||
_dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
@ -226,15 +265,23 @@ int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t tx
|
||||
|
||||
int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
|
||||
{
|
||||
int32_t dividend = ((int32_t)(z - _dig.z4)) << 15;
|
||||
dividend -= (_dig.z3 * (rhall - _dig.xyz1)) >> 2;
|
||||
int32_t dividend = int32_t(z - _dig.z4) << 15;
|
||||
int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2);
|
||||
|
||||
int32_t divisor = ((int32_t)_dig.z1) * (rhall << 1);
|
||||
int32_t divisor = int32_t(_dig.z1) * (rhall << 1);
|
||||
divisor += 0x8000;
|
||||
divisor >>= 16;
|
||||
divisor += _dig.z2;
|
||||
|
||||
return constrain_int32(dividend / divisor, -0x8000, 0x8000);
|
||||
int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000);
|
||||
#if 0
|
||||
static uint8_t counter;
|
||||
if (counter++ == 0) {
|
||||
printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n",
|
||||
ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor);
|
||||
}
|
||||
#endif
|
||||
return ret;
|
||||
}
|
||||
|
||||
void AP_Compass_BMM150::_update()
|
||||
@ -246,10 +293,19 @@ void AP_Compass_BMM150::_update()
|
||||
|
||||
/* Checking data ready status */
|
||||
if (!ret || !(data[3] & 0x1)) {
|
||||
_dev->check_next_register();
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - _last_read_ms > 250) {
|
||||
// cope with power cycle to sensor
|
||||
_last_read_ms = now;
|
||||
_dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
|
||||
_dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
|
||||
hal.util->perf_count(_perf_err);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
const uint16_t rhall = le16toh(data[3] >> 2);
|
||||
const uint16_t rhall = le16toh(data[3]) >> 2;
|
||||
|
||||
Vector3f raw_field = Vector3f{
|
||||
(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
|
||||
@ -272,6 +328,8 @@ void AP_Compass_BMM150::_update()
|
||||
/* correct raw_field for known errors */
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
_last_read_ms = AP_HAL::millis();
|
||||
|
||||
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return;
|
||||
}
|
||||
@ -282,6 +340,8 @@ void AP_Compass_BMM150::_update()
|
||||
_accum_count = 5;
|
||||
}
|
||||
_sem->give();
|
||||
|
||||
_dev->check_next_register();
|
||||
}
|
||||
|
||||
void AP_Compass_BMM150::read()
|
||||
|
@ -66,4 +66,7 @@ private:
|
||||
int8_t xy2;
|
||||
uint16_t xyz1;
|
||||
} _dig;
|
||||
|
||||
uint32_t _last_read_ms;
|
||||
AP_HAL::Util::perf_counter_t _perf_err;
|
||||
};
|
||||
|
@ -59,6 +59,9 @@ public:
|
||||
DEVTYPE_LIS3MDL = 0x08,
|
||||
DEVTYPE_AK09916 = 0x09,
|
||||
DEVTYPE_IST8310 = 0x0A,
|
||||
DEVTYPE_ICM20948 = 0x0B,
|
||||
DEVTYPE_MMC3416 = 0x0C,
|
||||
DEVTYPE_QMC5883L = 0x0D,
|
||||
};
|
||||
|
||||
|
||||
|
@ -267,9 +267,9 @@ Compass::_get_cal_mask() const
|
||||
/*
|
||||
handle an incoming MAG_CAL command
|
||||
*/
|
||||
uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
|
||||
MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
|
||||
{
|
||||
uint8_t result = MAV_RESULT_FAILED;
|
||||
MAV_RESULT result = MAV_RESULT_FAILED;
|
||||
|
||||
switch (packet.command) {
|
||||
case MAV_CMD_DO_START_MAG_CAL: {
|
||||
|
@ -77,13 +77,14 @@ extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), rotation);
|
||||
AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), force_external, rotation);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
@ -94,10 +95,12 @@ AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
|
||||
|
||||
AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
: AP_Compass_Backend(compass)
|
||||
, _dev(std::move(dev))
|
||||
, _rotation(rotation)
|
||||
, _force_external(force_external)
|
||||
{
|
||||
}
|
||||
|
||||
@ -163,6 +166,10 @@ bool AP_Compass_IST8310::init()
|
||||
_dev->set_device_type(DEVTYPE_IST8310);
|
||||
set_dev_id(_instance, _dev->get_bus_id());
|
||||
|
||||
if (_force_external) {
|
||||
set_external(_instance, true);
|
||||
}
|
||||
|
||||
_periodic_handle = _dev->register_periodic_callback(SAMPLING_PERIOD_USEC,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void));
|
||||
|
||||
|
@ -1,4 +1,3 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
|
||||
*
|
||||
@ -25,11 +24,16 @@
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#ifndef HAL_COMPASS_IST8310_I2C_ADDR
|
||||
#define HAL_COMPASS_IST8310_I2C_ADDR 0x0E
|
||||
#endif
|
||||
|
||||
class AP_Compass_IST8310 : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
void read() override;
|
||||
@ -39,6 +43,7 @@ public:
|
||||
private:
|
||||
AP_Compass_IST8310(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
|
||||
void timer();
|
||||
@ -56,4 +61,5 @@ private:
|
||||
enum Rotation _rotation;
|
||||
uint8_t _instance;
|
||||
bool _ignore_next_sample;
|
||||
bool _force_external;
|
||||
};
|
||||
|
@ -49,7 +49,7 @@
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_Compass_Backend *AP_Compass_LIS3MDL::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
@ -98,7 +98,7 @@ bool AP_Compass_LIS3MDL::init()
|
||||
|
||||
dev->setup_checked_registers(5);
|
||||
|
||||
dev->write_register(ADDR_CTRL_REG1, 0x62, true); // 155Hz, UHP
|
||||
dev->write_register(ADDR_CTRL_REG1, 0xFC, true); // 80Hz, UHP
|
||||
dev->write_register(ADDR_CTRL_REG2, 0, true); // 4Ga range
|
||||
dev->write_register(ADDR_CTRL_REG3, 0, true); // continuous
|
||||
dev->write_register(ADDR_CTRL_REG4, 0x0C, true); // z-axis ultra high perf
|
||||
@ -124,7 +124,7 @@ bool AP_Compass_LIS3MDL::init()
|
||||
set_dev_id(compass_instance, dev->get_bus_id());
|
||||
|
||||
// call timer() at 155Hz
|
||||
dev->register_periodic_callback(1000000U/155U,
|
||||
dev->register_periodic_callback(1000000U/80U,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void));
|
||||
|
||||
return true;
|
||||
|
@ -16,7 +16,7 @@
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_HAL/Device.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "AP_Compass.h"
|
||||
@ -31,7 +31,7 @@ class AP_Compass_LIS3MDL : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
|
333
libraries/AP_Compass/AP_Compass_MMC3416.cpp
Normal file
333
libraries/AP_Compass/AP_Compass_MMC3416.cpp
Normal file
@ -0,0 +1,333 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Driver by Andrew Tridgell, Nov 2016
|
||||
*/
|
||||
#include "AP_Compass_MMC3416.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <utility>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <stdio.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
#define REG_PRODUCT_ID 0x20
|
||||
#define REG_XOUT_L 0x00
|
||||
#define REG_STATUS 0x06
|
||||
#define REG_CONTROL0 0x07
|
||||
#define REG_CONTROL1 0x08
|
||||
|
||||
// bits in REG_CONTROL0
|
||||
#define REG_CONTROL0_REFILL 0x80
|
||||
#define REG_CONTROL0_RESET 0x40
|
||||
#define REG_CONTROL0_SET 0x20
|
||||
#define REG_CONTROL0_NB 0x10
|
||||
#define REG_CONTROL0_TM 0x01
|
||||
|
||||
// datasheet says 50ms min for refill
|
||||
#define MIN_DELAY_SET_RESET 50
|
||||
|
||||
AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Compass_MMC3416 *sensor = new AP_Compass_MMC3416(compass, std::move(dev), force_external, rotation);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
AP_Compass_MMC3416::AP_Compass_MMC3416(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
|
||||
bool _force_external,
|
||||
enum Rotation _rotation)
|
||||
: AP_Compass_Backend(compass)
|
||||
, dev(std::move(_dev))
|
||||
, force_external(_force_external)
|
||||
, rotation(_rotation)
|
||||
{
|
||||
}
|
||||
|
||||
bool AP_Compass_MMC3416::init()
|
||||
{
|
||||
if (!dev->get_semaphore()->take(0)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
dev->set_retries(10);
|
||||
|
||||
uint8_t whoami;
|
||||
if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
|
||||
whoami != 0x06) {
|
||||
// not a MMC3416
|
||||
dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
// reset sensor
|
||||
dev->write_register(REG_CONTROL1, 0x80);
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
dev->write_register(REG_CONTROL0, 0x00); // single shot
|
||||
dev->write_register(REG_CONTROL1, 0x00); // 16 bit, 7.92ms
|
||||
|
||||
dev->get_semaphore()->give();
|
||||
|
||||
/* register the compass instance in the frontend */
|
||||
compass_instance = register_compass();
|
||||
|
||||
printf("Found a MMC3416 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
|
||||
|
||||
set_rotation(compass_instance, rotation);
|
||||
|
||||
if (force_external) {
|
||||
set_external(compass_instance, true);
|
||||
}
|
||||
|
||||
dev->set_device_type(DEVTYPE_MMC3416);
|
||||
set_dev_id(compass_instance, dev->get_bus_id());
|
||||
|
||||
dev->set_retries(1);
|
||||
|
||||
// call timer() at 100Hz
|
||||
dev->register_periodic_callback(10000,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, void));
|
||||
|
||||
// wait 250ms for the compass to make it's initial readings
|
||||
hal.scheduler->delay(250);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_Compass_MMC3416::timer()
|
||||
{
|
||||
const uint16_t measure_count_limit = 50;
|
||||
const uint16_t zero_offset = 32768; // 16 bit mode
|
||||
const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
|
||||
const float counts_to_milliGauss = 1.0e3f / sensitivity;
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_sample_ms > 500) {
|
||||
// seems to be stuck or on first sample, reset state machine
|
||||
state = STATE_REFILL1;
|
||||
last_sample_ms = now;
|
||||
}
|
||||
|
||||
/*
|
||||
we use the SET/RESET method to remove bridge offset every
|
||||
measure_count_limit measurements. This involves a fairly complex
|
||||
state machine, but means we are much less sensitive to
|
||||
temperature changes
|
||||
*/
|
||||
switch (state) {
|
||||
case STATE_REFILL1:
|
||||
if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
|
||||
state = STATE_REFILL1_WAIT;
|
||||
refill_start_ms = AP_HAL::millis();
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_REFILL1_WAIT: {
|
||||
uint8_t status;
|
||||
if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
|
||||
dev->read_registers(REG_STATUS, &status, 1) &&
|
||||
(status & 0x02) == 0) {
|
||||
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) ||
|
||||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
|
||||
state = STATE_REFILL1;
|
||||
} else {
|
||||
state = STATE_MEASURE_WAIT1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case STATE_MEASURE_WAIT1: {
|
||||
uint8_t status;
|
||||
if (dev->read_registers(REG_STATUS, &status, 1) && (status & 1)) {
|
||||
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
|
||||
state = STATE_REFILL1;
|
||||
break;
|
||||
}
|
||||
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
|
||||
state = STATE_REFILL1;
|
||||
} else {
|
||||
state = STATE_REFILL2_WAIT;
|
||||
refill_start_ms = AP_HAL::millis();
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case STATE_REFILL2_WAIT: {
|
||||
uint8_t status;
|
||||
if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
|
||||
dev->read_registers(REG_STATUS, &status, 1) &&
|
||||
(status & 0x02) == 0) {
|
||||
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) ||
|
||||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
|
||||
state = STATE_REFILL1;
|
||||
} else {
|
||||
state = STATE_MEASURE_WAIT2;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case STATE_MEASURE_WAIT2: {
|
||||
uint8_t status;
|
||||
if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
|
||||
break;
|
||||
}
|
||||
uint16_t data1[3];
|
||||
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
|
||||
state = STATE_REFILL1;
|
||||
break;
|
||||
}
|
||||
Vector3f field;
|
||||
|
||||
/*
|
||||
calculate field and offset
|
||||
*/
|
||||
Vector3f f1(float(data0[0]) - zero_offset,
|
||||
float(data0[1]) - zero_offset,
|
||||
float(data0[2]) - zero_offset);
|
||||
Vector3f f2(float(data1[0]) - zero_offset,
|
||||
float(data1[1]) - zero_offset,
|
||||
float(data1[2]) - zero_offset);
|
||||
field = (f1 - f2) * (counts_to_milliGauss / 2);
|
||||
Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2);
|
||||
if (!have_initial_offset) {
|
||||
offset = new_offset;
|
||||
have_initial_offset = true;
|
||||
} else {
|
||||
// low pass changes to the offset
|
||||
offset = offset * 0.95 + new_offset * 0.05;
|
||||
}
|
||||
|
||||
#if 0
|
||||
DataFlash_Class::instance()->Log_Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
|
||||
AP_HAL::micros64(),
|
||||
(double)new_offset.x,
|
||||
(double)new_offset.y,
|
||||
(double)new_offset.z,
|
||||
(double)offset.x,
|
||||
(double)offset.y,
|
||||
(double)offset.z);
|
||||
printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n",
|
||||
field.x, field.y, field.z,
|
||||
offset.x, offset.y, offset.z);
|
||||
#endif
|
||||
|
||||
accumulate_field(field);
|
||||
|
||||
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
|
||||
state = STATE_REFILL1;
|
||||
} else {
|
||||
state = STATE_MEASURE_WAIT3;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case STATE_MEASURE_WAIT3: {
|
||||
uint8_t status;
|
||||
if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
|
||||
break;
|
||||
}
|
||||
uint16_t data1[3];
|
||||
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
|
||||
state = STATE_REFILL1;
|
||||
break;
|
||||
}
|
||||
Vector3f field(float(data1[0]) - zero_offset,
|
||||
float(data1[1]) - zero_offset,
|
||||
float(data1[2]) - zero_offset);
|
||||
field *= -counts_to_milliGauss;
|
||||
field += offset;
|
||||
|
||||
accumulate_field(field);
|
||||
|
||||
// we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
|
||||
if (measure_count++ >= measure_count_limit) {
|
||||
measure_count = 0;
|
||||
state = STATE_REFILL1;
|
||||
} else {
|
||||
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
|
||||
state = STATE_REFILL1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
accumulate a field
|
||||
*/
|
||||
void AP_Compass_MMC3416::accumulate_field(Vector3f &field)
|
||||
{
|
||||
#if 0
|
||||
DataFlash_Class::instance()->Log_Write("MMC", "TimeUS,X,Y,Z", "Qfff",
|
||||
AP_HAL::micros64(),
|
||||
(double)field.x,
|
||||
(double)field.y,
|
||||
(double)field.z);
|
||||
#endif
|
||||
/* rotate raw_field from sensor frame to body frame */
|
||||
rotate_field(field, compass_instance);
|
||||
|
||||
/* publish raw_field (uncorrected point sample) for calibration use */
|
||||
publish_raw_field(field, AP_HAL::micros(), compass_instance);
|
||||
|
||||
/* correct raw_field for known errors */
|
||||
correct_field(field, compass_instance);
|
||||
|
||||
if (_sem->take(0)) {
|
||||
accum += field;
|
||||
accum_count++;
|
||||
_sem->give();
|
||||
}
|
||||
|
||||
last_sample_ms = AP_HAL::millis();
|
||||
}
|
||||
|
||||
void AP_Compass_MMC3416::read()
|
||||
{
|
||||
if (!_sem->take(0)) {
|
||||
return;
|
||||
}
|
||||
if (accum_count == 0) {
|
||||
_sem->give();
|
||||
return;
|
||||
}
|
||||
|
||||
accum /= accum_count;
|
||||
|
||||
publish_filtered_field(accum, compass_instance);
|
||||
|
||||
accum.zero();
|
||||
accum_count = 0;
|
||||
|
||||
_sem->give();
|
||||
}
|
77
libraries/AP_Compass/AP_Compass_MMC3416.h
Normal file
77
libraries/AP_Compass/AP_Compass_MMC3416.h
Normal file
@ -0,0 +1,77 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#ifndef HAL_COMPASS_MMC3416_I2C_ADDR
|
||||
# define HAL_COMPASS_MMC3416_I2C_ADDR 0x30
|
||||
#endif
|
||||
|
||||
class AP_Compass_MMC3416 : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
void read() override;
|
||||
|
||||
static constexpr const char *name = "MMC3416";
|
||||
|
||||
private:
|
||||
AP_Compass_MMC3416(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev;
|
||||
|
||||
enum {
|
||||
STATE_REFILL1,
|
||||
STATE_REFILL1_WAIT,
|
||||
STATE_MEASURE_WAIT1,
|
||||
STATE_REFILL2_WAIT,
|
||||
STATE_MEASURE_WAIT2,
|
||||
STATE_MEASURE_WAIT3,
|
||||
} state;
|
||||
|
||||
/**
|
||||
* Device periodic callback to read data from the sensor.
|
||||
*/
|
||||
bool init();
|
||||
void timer();
|
||||
void accumulate_field(Vector3f &field);
|
||||
|
||||
uint8_t compass_instance;
|
||||
Vector3f accum;
|
||||
uint16_t accum_count;
|
||||
bool force_external;
|
||||
Vector3f offset;
|
||||
uint16_t measure_count;
|
||||
bool have_initial_offset;
|
||||
uint32_t refill_start_ms;
|
||||
uint32_t last_sample_ms;
|
||||
|
||||
uint16_t data0[3];
|
||||
|
||||
enum Rotation rotation;
|
||||
};
|
231
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
Normal file
231
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
Normal file
@ -0,0 +1,231 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* Driver by RadioLink LjWang, Jun 2017
|
||||
* GPS compass module See<http://www.radiolink.com>
|
||||
*/
|
||||
#include "AP_Compass_QMC5883L.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <utility>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#define QMC5883L_REG_CONF1 0x09
|
||||
#define QMC5883L_REG_CONF2 0x0A
|
||||
|
||||
// data output rates for 5883L
|
||||
#define QMC5883L_ODR_10HZ (0x00 << 2)
|
||||
#define QMC5883L_ODR_50HZ (0x01 << 2)
|
||||
#define QMC5883L_ODR_100HZ (0x02 << 2)
|
||||
#define QMC5883L_ODR_200HZ (0x03 << 2)
|
||||
|
||||
// Sensor operation modes
|
||||
#define QMC5883L_MODE_STANDBY 0x00
|
||||
#define QMC5883L_MODE_CONTINUOUS 0x01
|
||||
|
||||
#define QMC5883L_RNG_2G (0x00 << 4)
|
||||
#define QMC5883L_RNG_8G (0x01 << 4)
|
||||
|
||||
#define QMC5883L_OSR_512 (0x00 << 6)
|
||||
#define QMC5883L_OSR_256 (0x01 << 6)
|
||||
#define QMC5883L_OSR_128 (0x10 << 6)
|
||||
#define QMC5883L_OSR_64 (0x11 << 6)
|
||||
|
||||
#define QMC5883L_RST 0x80
|
||||
|
||||
#define QMC5883L_REG_DATA_OUTPUT_X 0x00
|
||||
#define QMC5883L_REG_STATUS 0x06
|
||||
|
||||
#define QMC5883L_REG_ID 0x0D
|
||||
#define QMC5883_ID_VAL 0xFF
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),force_external,rotation);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
AP_Compass_QMC5883L::AP_Compass_QMC5883L(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
: AP_Compass_Backend(compass)
|
||||
, _dev(std::move(dev))
|
||||
, _rotation(rotation)
|
||||
, _force_external(force_external)
|
||||
{
|
||||
}
|
||||
|
||||
bool AP_Compass_QMC5883L::init()
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return false;
|
||||
}
|
||||
//must reset first
|
||||
_dev->write_register(QMC5883L_REG_CONF2,QMC5883L_RST);
|
||||
|
||||
_dev->set_retries(10);
|
||||
|
||||
uint8_t whoami;
|
||||
if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)||
|
||||
whoami != QMC5883_ID_VAL){
|
||||
// not an QMC5883L
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (!_dev->write_register(0x0B, 0x01)||
|
||||
!_dev->write_register(0x20, 0x40)||
|
||||
!_dev->write_register(0x21, 0x01)||
|
||||
!_dev->write_register(QMC5883L_REG_CONF1,
|
||||
QMC5883L_MODE_CONTINUOUS|
|
||||
QMC5883L_ODR_100HZ|
|
||||
QMC5883L_OSR_512|
|
||||
QMC5883L_RNG_8G)) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// lower retries for run
|
||||
_dev->set_retries(3);
|
||||
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
_instance = register_compass();
|
||||
|
||||
printf("%s found on bus %u id %u address 0x%02x\n", name,
|
||||
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
|
||||
|
||||
set_rotation(_instance, _rotation);
|
||||
|
||||
_dev->set_device_type(DEVTYPE_QMC5883L);
|
||||
set_dev_id(_instance, _dev->get_bus_id());
|
||||
|
||||
if (_force_external) {
|
||||
set_external(_instance, true);
|
||||
}
|
||||
|
||||
//Enable 100HZ
|
||||
_dev->register_periodic_callback(10000,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void));
|
||||
|
||||
return true;
|
||||
|
||||
fail:
|
||||
_dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_Compass_QMC5883L::timer()
|
||||
{
|
||||
struct PACKED {
|
||||
le16_t rx;
|
||||
le16_t ry;
|
||||
le16_t rz;
|
||||
} buffer;
|
||||
|
||||
const float range_scale = 1000.0f / 3000.0f;
|
||||
|
||||
uint8_t status;
|
||||
if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){
|
||||
return;
|
||||
}
|
||||
//new data is ready
|
||||
if (!(status & 0x04)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if(!_dev->read_registers(QMC5883L_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))){
|
||||
return ;
|
||||
}
|
||||
|
||||
uint32_t now = AP_HAL::micros();
|
||||
|
||||
auto x = -static_cast<int16_t>(le16toh(buffer.rx));
|
||||
auto y = static_cast<int16_t>(le16toh(buffer.ry));
|
||||
auto z = -static_cast<int16_t>(le16toh(buffer.rz));
|
||||
|
||||
#if 0
|
||||
printf("mag.x:%d\n",x);
|
||||
printf("mag.y:%d\n",y);
|
||||
printf("mag.z:%d\n",z);
|
||||
#endif
|
||||
|
||||
Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale };
|
||||
|
||||
// rotate to the desired orientation
|
||||
if (is_external(_instance)) {
|
||||
field.rotate(ROTATION_YAW_90);
|
||||
}
|
||||
|
||||
/* rotate raw_field from sensor frame to body frame */
|
||||
rotate_field(field, _instance);
|
||||
|
||||
/* publish raw_field (uncorrected point sample) for calibration use */
|
||||
publish_raw_field(field, now, _instance);
|
||||
|
||||
/* correct raw_field for known errors */
|
||||
correct_field(field, _instance);
|
||||
|
||||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
_accum += field;
|
||||
_accum_count++;
|
||||
if(_accum_count == 20){
|
||||
_accum.x /= 2;
|
||||
_accum.y /= 2;
|
||||
_accum.z /= 2;
|
||||
_accum_count = 10;
|
||||
}
|
||||
_sem->give();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Compass_QMC5883L::read()
|
||||
{
|
||||
if (!_sem->take_nonblocking()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_accum_count == 0) {
|
||||
_sem->give();
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3f field(_accum);
|
||||
field /= _accum_count;
|
||||
|
||||
publish_filtered_field(field, _instance);
|
||||
|
||||
_accum.zero();
|
||||
_accum_count = 0;
|
||||
|
||||
_sem->give();
|
||||
}
|
60
libraries/AP_Compass/AP_Compass_QMC5883L.h
Normal file
60
libraries/AP_Compass/AP_Compass_QMC5883L.h
Normal file
@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Emlid Ltd. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#ifndef HAL_COMPASS_QMC5883L_I2C_ADDR
|
||||
#define HAL_COMPASS_QMC5883L_I2C_ADDR 0x0D
|
||||
#endif
|
||||
|
||||
class AP_Compass_QMC5883L : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
void read() override;
|
||||
|
||||
static constexpr const char *name = "QMC5883L";
|
||||
|
||||
private:
|
||||
AP_Compass_QMC5883L(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
|
||||
void timer();
|
||||
bool init();
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
|
||||
Vector3f _accum = Vector3f();
|
||||
uint32_t _accum_count = 0;
|
||||
|
||||
enum Rotation _rotation;
|
||||
uint8_t _instance;
|
||||
bool _force_external:1;
|
||||
};
|
113
libraries/AP_Compass/AP_Compass_SITL.cpp
Normal file
113
libraries/AP_Compass/AP_Compass_SITL.cpp
Normal file
@ -0,0 +1,113 @@
|
||||
#include "AP_Compass_SITL.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
|
||||
_has_sample(false),
|
||||
AP_Compass_Backend(compass)
|
||||
{
|
||||
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
|
||||
if (_sitl != nullptr) {
|
||||
_compass._setup_earth_field();
|
||||
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
|
||||
_compass_instance[i] = register_compass();
|
||||
}
|
||||
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Compass_SITL::_timer()
|
||||
{
|
||||
// TODO: Refactor delay buffer with AP_Baro_SITL.
|
||||
|
||||
// Sampled at 100Hz
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if ((now - _last_sample_time) < 10) {
|
||||
return;
|
||||
}
|
||||
_last_sample_time = now;
|
||||
|
||||
// calculate sensor noise and add to 'truth' field in body frame
|
||||
// units are milli-Gauss
|
||||
Vector3f noise = rand_vec3f() * _sitl->mag_noise;
|
||||
Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
|
||||
|
||||
// add delay
|
||||
uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay.
|
||||
uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
|
||||
|
||||
// storing data from sensor to buffer
|
||||
if (now - last_store_time >= 10) { // store data every 10 ms.
|
||||
last_store_time = now;
|
||||
if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
|
||||
store_index = 0;
|
||||
}
|
||||
buffer[store_index].data = new_mag_data; // add data to current index
|
||||
buffer[store_index].time = last_store_time; // add time to current index
|
||||
store_index = store_index + 1; // increment index
|
||||
}
|
||||
|
||||
// return delayed measurement
|
||||
uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay
|
||||
// find data corresponding to delayed time in buffer
|
||||
for (uint8_t i=0; i<=buffer_length-1; i++) {
|
||||
// find difference between delayed time and time stamp in buffer
|
||||
uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time));
|
||||
// if this difference is smaller than last delta, store this time
|
||||
if (time_delta < best_time_delta) {
|
||||
best_index= i;
|
||||
best_time_delta = time_delta;
|
||||
}
|
||||
}
|
||||
if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error
|
||||
new_mag_data = buffer[best_index].data;
|
||||
}
|
||||
|
||||
new_mag_data -= _sitl->mag_ofs.get();
|
||||
|
||||
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
|
||||
rotate_field(new_mag_data, _compass_instance[i]);
|
||||
publish_raw_field(new_mag_data, AP_HAL::micros(), _compass_instance[i]);
|
||||
correct_field(new_mag_data, _compass_instance[i]);
|
||||
}
|
||||
|
||||
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return;
|
||||
}
|
||||
|
||||
_mag_accum += new_mag_data;
|
||||
_accum_count++;
|
||||
if (_accum_count == 10) {
|
||||
_mag_accum /= 2;
|
||||
_accum_count = 5;
|
||||
_has_sample = true;
|
||||
}
|
||||
_sem->give();
|
||||
}
|
||||
|
||||
void AP_Compass_SITL::read()
|
||||
{
|
||||
if (_sem->take_nonblocking()) {
|
||||
if (!_has_sample) {
|
||||
_sem->give();
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3f field(_mag_accum);
|
||||
field /= _accum_count;
|
||||
_mag_accum.zero();
|
||||
_accum_count = 0;
|
||||
|
||||
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
|
||||
publish_filtered_field(field, _compass_instance[i]);
|
||||
}
|
||||
|
||||
_has_sample = false;
|
||||
_sem->give();
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
43
libraries/AP_Compass/AP_Compass_SITL.h
Normal file
43
libraries/AP_Compass/AP_Compass_SITL.h
Normal file
@ -0,0 +1,43 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
#include <SITL/SITL.h>
|
||||
#include <AP_Math/vectorN.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Declination/AP_Declination.h>
|
||||
|
||||
#define SITL_NUM_COMPASSES 2
|
||||
|
||||
class AP_Compass_SITL : public AP_Compass_Backend {
|
||||
public:
|
||||
AP_Compass_SITL(Compass &);
|
||||
|
||||
void read(void);
|
||||
|
||||
private:
|
||||
uint8_t _compass_instance[SITL_NUM_COMPASSES];
|
||||
SITL::SITL *_sitl;
|
||||
|
||||
// delay buffer variables
|
||||
struct readings_compass {
|
||||
uint32_t time;
|
||||
Vector3f data;
|
||||
};
|
||||
uint8_t store_index;
|
||||
uint32_t last_store_time;
|
||||
static const uint8_t buffer_length = 50;
|
||||
VectorN<readings_compass,buffer_length> buffer;
|
||||
|
||||
void _timer();
|
||||
bool _has_sample;
|
||||
uint32_t _last_sample_time;
|
||||
|
||||
Vector3f _mag_accum;
|
||||
uint32_t _accum_count;
|
||||
|
||||
|
||||
};
|
||||
#endif // CONFIG_HAL_BOARD
|
Loading…
Reference in New Issue
Block a user