mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: LSM303D: cleanup driver
Make it similar to other compass drivers, including method names.
This commit is contained in:
Lucas De Marchi
parent
20a4a42cb3
commit
5164fbb81b
|
|
@ -441,7 +441,7 @@ void Compass::_detect_backends(void)
|
|||
_add_backend(AP_Compass_QURT::detect(*this));
|
||||
#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)));
|
||||
_add_backend(AP_Compass_LSM303D::detect_spi(*this));
|
||||
_add_backend(AP_Compass_LSM303D::probe(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
|
||||
// detect_mpu9250() failed will cause panic if no actual mpu9250 backend,
|
||||
// in BH, only one compass should be detected
|
||||
|
|
|
|||
|
|
@ -14,12 +14,12 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "AP_Compass_LSM303D.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#include <AP_HAL_Linux/GPIO.h>
|
||||
|
|
@ -147,47 +147,25 @@ extern const AP_HAL::HAL& hal;
|
|||
|
||||
#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
|
||||
|
||||
|
||||
#define INT_CTRL_M 0x12
|
||||
#define INT_SRC_M 0x13
|
||||
|
||||
/* default values for this device */
|
||||
#define LSM303D_ACCEL_DEFAULT_RANGE_G 8
|
||||
#define LSM303D_ACCEL_DEFAULT_RATE 800
|
||||
#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
|
||||
#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
|
||||
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
|
||||
#define LSM303D_MAG_DEFAULT_RATE 100
|
||||
|
||||
#define LSM303D_DEBUG 0
|
||||
#if LSM303D_DEBUG
|
||||
#include <stdio.h>
|
||||
#define error(...) fprintf(stderr, __VA_ARGS__)
|
||||
#define debug(...) hal.console->printf(__VA_ARGS__)
|
||||
#define ASSERT(x) assert(x)
|
||||
#else
|
||||
#define error(...)
|
||||
#define debug(...)
|
||||
#define ASSERT(x)
|
||||
#endif
|
||||
AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass)
|
||||
: AP_Compass_Backend(compass)
|
||||
{
|
||||
}
|
||||
|
||||
// constructor
|
||||
AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass):
|
||||
AP_Compass_Backend(compass)
|
||||
{}
|
||||
|
||||
// detect the sensor
|
||||
AP_Compass_Backend *AP_Compass_LSM303D::detect_spi(Compass &compass)
|
||||
AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass)
|
||||
{
|
||||
AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass);
|
||||
if (sensor == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
if (!sensor->init()) {
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return NULL;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
|
@ -239,9 +217,7 @@ bool AP_Compass_LSM303D::_data_ready()
|
|||
bool AP_Compass_LSM303D::_read_raw()
|
||||
{
|
||||
if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) {
|
||||
hal.console->println(
|
||||
"LSM303D _read_data_transaction_accel: _reg7_expected unexpected");
|
||||
// reset();
|
||||
hal.console->println("LSM303D _read_data_transaction_accel: _reg7_expected unexpected");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -282,13 +258,12 @@ bool AP_Compass_LSM303D::_read_raw()
|
|||
return true;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
bool
|
||||
AP_Compass_LSM303D::init()
|
||||
bool AP_Compass_LSM303D::init()
|
||||
{
|
||||
// TODO: support users without data ready pin
|
||||
if (LSM303D_DRDY_M_PIN < 0)
|
||||
if (LSM303D_DRDY_M_PIN < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
|
||||
|
|
@ -325,13 +300,12 @@ AP_Compass_LSM303D::init()
|
|||
}
|
||||
} while (1);
|
||||
|
||||
_scaling[0] = 1.0;
|
||||
_scaling[1] = 1.0;
|
||||
_scaling[2] = 1.0;
|
||||
_initialised = true;
|
||||
|
||||
/* register the compass instance in the frontend */
|
||||
_compass_instance = register_compass();
|
||||
set_dev_id(_compass_instance, get_dev_id());
|
||||
set_dev_id(_compass_instance, AP_COMPASS_TYPE_LSM303D);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||
set_external(_compass_instance, false);
|
||||
#endif
|
||||
|
|
@ -340,19 +314,13 @@ AP_Compass_LSM303D::init()
|
|||
|
||||
_spi_sem->give();
|
||||
hal.scheduler->resume_timer_procs();
|
||||
_initialised = true;
|
||||
|
||||
return _initialised;
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t AP_Compass_LSM303D::get_dev_id()
|
||||
bool AP_Compass_LSM303D::_hardware_init()
|
||||
{
|
||||
return AP_COMPASS_TYPE_LSM303D;
|
||||
}
|
||||
|
||||
bool AP_Compass_LSM303D::_hardware_init(void)
|
||||
{
|
||||
if (!_spi_sem->take(100)) {
|
||||
if (!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
AP_HAL::panic("LSM303D: Unable to get semaphore");
|
||||
}
|
||||
|
||||
|
|
@ -371,8 +339,6 @@ bool AP_Compass_LSM303D::_hardware_init(void)
|
|||
_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
|
||||
_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
|
||||
|
||||
// TODO: Software filtering
|
||||
|
||||
// now that we have initialised, we set the SPI bus speed to high
|
||||
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
|
||||
_spi_sem->give();
|
||||
|
|
@ -382,6 +348,9 @@ bool AP_Compass_LSM303D::_hardware_init(void)
|
|||
|
||||
void AP_Compass_LSM303D::_update()
|
||||
{
|
||||
uint32_t time_us = AP_HAL::micros();
|
||||
Vector3f raw_field;
|
||||
|
||||
if (AP_HAL::micros() - _last_update_timestamp < 10000) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -390,53 +359,42 @@ void AP_Compass_LSM303D::_update()
|
|||
return;
|
||||
}
|
||||
|
||||
_collect_samples();
|
||||
if (!_read_raw()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
|
||||
|
||||
// rotate raw_field from sensor frame to body frame
|
||||
rotate_field(raw_field, _compass_instance);
|
||||
|
||||
// publish raw_field (uncorrected point sample) for calibration use
|
||||
publish_raw_field(raw_field, time_us, _compass_instance);
|
||||
|
||||
// correct raw_field for known errors
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
_mag_x_accum += raw_field.x;
|
||||
_mag_y_accum += raw_field.y;
|
||||
_mag_z_accum += raw_field.z;
|
||||
_accum_count++;
|
||||
if (_accum_count == 10) {
|
||||
_mag_x_accum /= 2;
|
||||
_mag_y_accum /= 2;
|
||||
_mag_z_accum /= 2;
|
||||
_accum_count = 5;
|
||||
}
|
||||
|
||||
_last_update_timestamp = AP_HAL::micros();
|
||||
|
||||
fail:
|
||||
_spi_sem->give();
|
||||
}
|
||||
|
||||
void AP_Compass_LSM303D::_collect_samples()
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_read_raw()) {
|
||||
error("_read_raw() failed\n");
|
||||
} else {
|
||||
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
|
||||
uint32_t time_us = AP_HAL::micros();
|
||||
|
||||
// rotate raw_field from sensor frame to body frame
|
||||
rotate_field(raw_field, _compass_instance);
|
||||
|
||||
// publish raw_field (uncorrected point sample) for _scaling use
|
||||
publish_raw_field(raw_field, time_us, _compass_instance);
|
||||
|
||||
// correct raw_field for known errors
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
_mag_x_accum += raw_field.x;
|
||||
_mag_y_accum += raw_field.y;
|
||||
_mag_z_accum += raw_field.z;
|
||||
_accum_count++;
|
||||
if (_accum_count == 10) {
|
||||
_mag_x_accum /= 2;
|
||||
_mag_y_accum /= 2;
|
||||
_mag_z_accum /= 2;
|
||||
_accum_count = 5;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
void AP_Compass_LSM303D::read()
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -446,9 +404,7 @@ void AP_Compass_LSM303D::read()
|
|||
}
|
||||
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
Vector3f field(_mag_x_accum * _scaling[0],
|
||||
_mag_y_accum * _scaling[1],
|
||||
_mag_z_accum * _scaling[2]);
|
||||
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
|
||||
field /= _accum_count;
|
||||
|
||||
_accum_count = 0;
|
||||
|
|
@ -458,7 +414,7 @@ void AP_Compass_LSM303D::read()
|
|||
publish_filtered_field(field, _compass_instance);
|
||||
}
|
||||
|
||||
void AP_Compass_LSM303D::_disable_i2c(void)
|
||||
void AP_Compass_LSM303D::_disable_i2c()
|
||||
{
|
||||
// TODO: use the register names
|
||||
uint8_t a = _register_read(0x02);
|
||||
|
|
@ -471,68 +427,65 @@ void AP_Compass_LSM303D::_disable_i2c(void)
|
|||
_register_write(0x02, (0xE7 & a));
|
||||
}
|
||||
|
||||
uint8_t AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga)
|
||||
bool AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga)
|
||||
{
|
||||
uint8_t setbits = 0;
|
||||
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
|
||||
float new_scale_ga_digit = 0.0f;
|
||||
|
||||
if (max_ga == 0)
|
||||
if (max_ga == 0) {
|
||||
max_ga = 12;
|
||||
}
|
||||
|
||||
if (max_ga <= 2) {
|
||||
_mag_range_ga = 2;
|
||||
setbits |= REG6_FULL_SCALE_2GA_M;
|
||||
new_scale_ga_digit = 0.080f;
|
||||
|
||||
} else if (max_ga <= 4) {
|
||||
_mag_range_ga = 4;
|
||||
setbits |= REG6_FULL_SCALE_4GA_M;
|
||||
new_scale_ga_digit = 0.160f;
|
||||
|
||||
} else if (max_ga <= 8) {
|
||||
_mag_range_ga = 8;
|
||||
setbits |= REG6_FULL_SCALE_8GA_M;
|
||||
new_scale_ga_digit = 0.320f;
|
||||
|
||||
} else if (max_ga <= 12) {
|
||||
_mag_range_ga = 12;
|
||||
setbits |= REG6_FULL_SCALE_12GA_M;
|
||||
new_scale_ga_digit = 0.479f;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
_mag_range_scale = new_scale_ga_digit;
|
||||
_register_modify(ADDR_CTRL_REG6, clearbits, setbits);
|
||||
return 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency)
|
||||
bool AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency)
|
||||
{
|
||||
uint8_t setbits = 0;
|
||||
uint8_t clearbits = REG5_RATE_BITS_M;
|
||||
|
||||
if (frequency == 0)
|
||||
if (frequency == 0) {
|
||||
frequency = 100;
|
||||
}
|
||||
|
||||
if (frequency <= 25) {
|
||||
setbits |= REG5_RATE_25HZ_M;
|
||||
_mag_samplerate = 25;
|
||||
|
||||
} else if (frequency <= 50) {
|
||||
setbits |= REG5_RATE_50HZ_M;
|
||||
_mag_samplerate = 50;
|
||||
|
||||
} else if (frequency <= 100) {
|
||||
setbits |= REG5_RATE_100HZ_M;
|
||||
_mag_samplerate = 100;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
_register_modify(ADDR_CTRL_REG5, clearbits, setbits);
|
||||
return 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,49 +10,44 @@
|
|||
|
||||
class AP_Compass_LSM303D : public AP_Compass_Backend
|
||||
{
|
||||
private:
|
||||
bool _read_raw(void);
|
||||
uint8_t _register_read( uint8_t reg );
|
||||
void _register_write( uint8_t reg, uint8_t val );
|
||||
void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits);
|
||||
bool _data_ready();
|
||||
bool _hardware_init(void);
|
||||
void _collect_samples();
|
||||
void _update();
|
||||
void _disable_i2c(void);
|
||||
uint8_t _mag_set_range(uint8_t max_ga);
|
||||
uint8_t _mag_set_samplerate(uint16_t frequency);
|
||||
|
||||
AP_HAL::SPIDeviceDriver *_spi = nullptr;
|
||||
AP_HAL::Semaphore *_spi_sem = nullptr;
|
||||
AP_HAL::DigitalSource *_drdy_pin_m = nullptr;
|
||||
float _scaling[3] = { 0 };
|
||||
bool _initialised = false;
|
||||
|
||||
int16_t _mag_x = 0;
|
||||
int16_t _mag_y = 0;
|
||||
int16_t _mag_z = 0;
|
||||
float _mag_x_accum = 0.0f;
|
||||
float _mag_y_accum = 0.0f;
|
||||
float _mag_z_accum = 0.0f;
|
||||
uint32_t _last_accum_time = 0;
|
||||
uint32_t _last_update_timestamp = 0;
|
||||
uint8_t _accum_count = 0;
|
||||
|
||||
uint8_t _compass_instance = 0;
|
||||
uint8_t _product_id = 0;
|
||||
|
||||
uint8_t _mag_range_ga = 0;
|
||||
uint8_t _mag_samplerate = 0;
|
||||
uint8_t _reg7_expected = 0;
|
||||
float _mag_range_scale = 0.0f;
|
||||
|
||||
public:
|
||||
AP_Compass_LSM303D(Compass &compass);
|
||||
bool init(void);
|
||||
void read(void);
|
||||
uint32_t get_dev_id();
|
||||
static AP_Compass_Backend *probe(Compass &compass);
|
||||
|
||||
// detect the sensor
|
||||
static AP_Compass_Backend *detect_spi(Compass &compass);
|
||||
bool init() override;
|
||||
void read() override;
|
||||
|
||||
private:
|
||||
AP_Compass_LSM303D(Compass &compass);
|
||||
|
||||
bool _read_raw();
|
||||
uint8_t _register_read(uint8_t reg);
|
||||
void _register_write(uint8_t reg, uint8_t val);
|
||||
void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits);
|
||||
bool _data_ready();
|
||||
bool _hardware_init();
|
||||
void _update();
|
||||
void _disable_i2c();
|
||||
bool _mag_set_range(uint8_t max_ga);
|
||||
bool _mag_set_samplerate(uint16_t frequency);
|
||||
|
||||
AP_HAL::SPIDeviceDriver *_spi;
|
||||
AP_HAL::Semaphore *_spi_sem;
|
||||
AP_HAL::DigitalSource *_drdy_pin_m;
|
||||
|
||||
float _mag_range_scale;
|
||||
float _mag_x_accum;
|
||||
float _mag_y_accum;
|
||||
float _mag_z_accum;
|
||||
uint32_t _last_update_timestamp;
|
||||
int16_t _mag_x;
|
||||
int16_t _mag_y;
|
||||
int16_t _mag_z;
|
||||
uint8_t _accum_count;
|
||||
|
||||
uint8_t _compass_instance;
|
||||
bool _initialised;
|
||||
|
||||
uint8_t _mag_range_ga;
|
||||
uint8_t _mag_samplerate;
|
||||
uint8_t _reg7_expected;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue