Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: implement device IDs for compass drivers

This commit is contained in:
Andrew Tridgell 2016-11-04 20:24:53 +11:00
parent 64a5d3938c
commit de7913e8e9
9 changed files with 99 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.h
View File

@ -16,11 +16,9 @@
#define AP_COMPASS_TYPE_UNKNOWN 0x00
#define AP_COMPASS_TYPE_HIL 0x01
#define AP_COMPASS_TYPE_HMC5843 0x02
#define AP_COMPASS_TYPE_HMC5883L 0x03
#define AP_COMPASS_TYPE_PX4 0x04
#define AP_COMPASS_TYPE_VRBRAIN 0x05
#define AP_COMPASS_TYPE_AK8963_MPU9250 0x06
#define AP_COMPASS_TYPE_AK8963_I2C 0x07
#define AP_COMPASS_TYPE_AK8963 0x06
#define AP_COMPASS_TYPE_LSM303D 0x08
#define AP_COMPASS_TYPE_LSM9DS1 0x09
#define AP_COMPASS_TYPE_BMM150 0x0A

23
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -52,11 +52,9 @@ struct PACKED sample_regs {
extern const AP_HAL::HAL &hal;
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
uint32_t dev_id)
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus)
: AP_Compass_Backend(compass)
, _bus(bus)
, _dev_id(dev_id)
{
}
@ -73,7 +71,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
return nullptr;
}
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_I2C);
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -103,7 +101,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m
return nullptr;
}
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_MPU9250);
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -150,7 +148,9 @@ bool AP_Compass_AK8963::init()
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_dev_id(_compass_instance, _dev_id);
_bus->set_device_type(AP_COMPASS_TYPE_AK8963);
set_dev_id(_compass_instance, _bus->get_bus_id());
bus_sem->give();
@ -431,3 +431,14 @@ AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_Auxiliary::register_periodic_
return _bus->register_periodic_callback(period_usec, cb);
}
// set device type within a device class
void AP_AK8963_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
{
_bus->set_device_type(devtype);
}
// return 24 bit bus identifier
uint32_t AP_AK8963_BusDriver_Auxiliary::get_bus_id(void) const
{
return _bus->get_bus_id();
}

26
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -36,8 +36,7 @@ public:
void read() override;
private:
AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
uint32_t dev_id);
AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus);
void _make_factory_sensitivity_adjustment(Vector3f &field) const;
void _make_adc_sensitivity_adjustment(Vector3f &field) const;
@ -58,7 +57,6 @@ private:
float _mag_z_accum;
uint32_t _accum_count;
uint32_t _last_update_timestamp;
uint32_t _dev_id;
uint8_t _compass_instance;
bool _initialized;
@ -79,6 +77,12 @@ public:
virtual bool configure() { return true; }
virtual bool start_measurements() { return true; }
virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
// set device type within a device class
virtual void set_device_type(uint8_t devtype) = 0;
// return 24 bit bus identifier
virtual uint32_t get_bus_id(void) const = 0;
};
class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver
@ -93,6 +97,16 @@ public:
virtual AP_HAL::Semaphore *get_semaphore() override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
// set device type within a device class
void set_device_type(uint8_t devtype) override {
_dev->set_device_type(devtype);
}
// return 24 bit bus identifier
uint32_t get_bus_id(void) const override {
return _dev->get_bus_id();
}
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
};
@ -115,6 +129,12 @@ public:
bool configure();
bool start_measurements();
// set device type within a device class
void set_device_type(uint8_t devtype) override;
// return 24 bit bus identifier
uint32_t get_bus_id(void) const override;
private:
AuxiliaryBus *_bus;
AuxiliaryBusSlave *_slave;

4
libraries/AP_Compass/AP_Compass_BMM150.cpp
View File

@ -182,7 +182,9 @@ bool AP_Compass_BMM150::init()
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150);
_dev->set_device_type(AP_COMPASS_TYPE_BMM150);
set_dev_id(_compass_instance, _dev->get_bus_id());
_dev->register_periodic_callback(MEASURE_TIME_USEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, bool));

30
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -180,7 +180,9 @@ bool AP_Compass_HMC5843::init()
read();
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
_bus->set_device_type(AP_COMPASS_TYPE_HMC5843);
set_dev_id(_compass_instance, _bus->get_bus_id());
if (_force_external) {
set_external(_compass_instance, true);
@ -209,7 +211,7 @@ bool AP_Compass_HMC5843::_timer()
return true;
}
uint32_t tnow = AP_HAL::micros();
uint32_t tnow = AP_HAL::micros();
// the _mag_N values are in the range -2048 to 2047, so we can
// accumulate up to 15 of them in an int16_t. Let's make it 14
@ -221,8 +223,7 @@ bool AP_Compass_HMC5843::_timer()
raw_field *= _gain_scale;
// rotate to the desired orientation
if (is_external(_compass_instance) &&
_product_id == AP_COMPASS_TYPE_HMC5883L) {
if (is_external(_compass_instance) && _is_hmc5883L) {
raw_field.rotate(ROTATION_YAW_90);
}
@ -246,7 +247,6 @@ bool AP_Compass_HMC5843::_timer()
_mag_z_accum /= 2;
_accum_count = 7;
}
_last_accum_time = tnow;
_sem->give();
}
@ -325,7 +325,7 @@ bool AP_Compass_HMC5843::_read_sample()
ry = be16toh(val.ry);
rz = be16toh(val.rz);
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
if (_is_hmc5883L) {
std::swap(ry, rz);
}
@ -355,11 +355,11 @@ bool AP_Compass_HMC5843::_detect_version()
if (_base_config == try_config) {
/* a 5883L supports the sample averaging config */
_product_id = AP_COMPASS_TYPE_HMC5883L;
_is_hmc5883L = true;
_gain_config = HMC5883L_GAIN_1_30_GA;
_gain_scale = (1.0f / 1090) * 1000;
} else if (_base_config == (HMC5843_OPMODE_NORMAL | HMC5843_OSR_75HZ)) {
_product_id = AP_COMPASS_TYPE_HMC5843;
_is_hmc5883L = false;
_gain_config = HMC5843_GAIN_1_00_GA;
_gain_scale = (1.0f / 1300) * 1000;
} else {
@ -378,7 +378,7 @@ bool AP_Compass_HMC5843::_calibrate()
int numAttempts = 0, good_count = 0;
bool success = false;
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
if (_is_hmc5883L) {
calibration_gain = HMC5883L_GAIN_2_50_GA;
/*
* note that the HMC5883 datasheet gives the x and y expected
@ -589,4 +589,16 @@ AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_Auxiliary::register_periodic
return _bus->register_periodic_callback(period_usec, cb);
}
// set device type within a device class
void AP_HMC5843_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
{
_bus->set_device_type(devtype);
}
// return 24 bit bus identifier
uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
{
return _bus->get_bus_id();
}
#endif

29
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -46,7 +46,6 @@ private:
float _scaling[3];
float _gain_scale;
uint32_t _last_accum_time;
// when unhealthy the millis() value to retry at
uint32_t _retry_time;
@ -62,10 +61,10 @@ private:
uint8_t _base_config;
uint8_t _compass_instance;
uint8_t _gain_config;
uint8_t _product_id;
bool _initialised;
bool _force_external;
bool _is_hmc5883L:1;
bool _initialised:1;
bool _force_external:1;
};
class AP_HMC5843_BusDriver
@ -83,6 +82,12 @@ public:
virtual bool start_measurements() { return true; }
virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
// set device type within a device class
virtual void set_device_type(uint8_t devtype) = 0;
// return 24 bit bus identifier
virtual uint32_t get_bus_id(void) const = 0;
};
class AP_HMC5843_BusDriver_HALDevice : public AP_HMC5843_BusDriver
@ -97,6 +102,16 @@ public:
AP_HAL::Semaphore *get_semaphore() override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
// set device type within a device class
void set_device_type(uint8_t devtype) override {
_dev->set_device_type(devtype);
}
// return 24 bit bus identifier
uint32_t get_bus_id(void) const override {
return _dev->get_bus_id();
}
private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
@ -119,6 +134,12 @@ public:
bool start_measurements() override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
// set device type within a device class
void set_device_type(uint8_t devtype) override;
// return 24 bit bus identifier
uint32_t get_bus_id(void) const override;
private:
AuxiliaryBus *_bus;

4
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -268,7 +268,9 @@ bool AP_Compass_LSM303D::init()
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_dev_id(_compass_instance, AP_COMPASS_TYPE_LSM303D);
_dev->set_device_type(AP_COMPASS_TYPE_LSM303D);
set_dev_id(_compass_instance, _dev->get_bus_id());
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
// FIXME: wrong way to force internal compass

9
libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
View File

@ -60,7 +60,7 @@ extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_LSM9DS1::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev)
{
AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev), AP_COMPASS_TYPE_LSM9DS1);
AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev));
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
@ -94,7 +94,9 @@ bool AP_Compass_LSM9DS1::init()
}
_compass_instance = register_compass();
set_dev_id(_compass_instance, _dev_id);
_dev->set_device_type(AP_COMPASS_TYPE_LSM9DS1);
set_dev_id(_compass_instance, _dev->get_bus_id());
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, bool));
@ -230,10 +232,9 @@ bool AP_Compass_LSM9DS1::_set_scale(void)
return true;
}
AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev, uint32_t dev_id)
AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, _dev_id(dev_id)
{
}

3
libraries/AP_Compass/AP_Compass_LSM9DS1.h
View File

@ -22,7 +22,7 @@ public:
virtual ~AP_Compass_LSM9DS1() {}
private:
AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev, uint32_t dev_id);
AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
bool _check_id(void);
bool _configure(void);
bool _set_scale(void);
@ -35,7 +35,6 @@ private:
void _dump_registers();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
uint32_t _dev_id;
uint8_t _compass_instance;
float _scaling;
float _mag_x_accum;