mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
20a4a42cb3
These are not used. The only place where raw fields are used are in the compass calibrator and we don't need to store them. Additionally remove duplicated documentation about the meaning of the functions to avoid them getting out of sync.
539 lines
15 KiB
C++
539 lines
15 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
This program 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 program 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/>.
|
|
*/
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "AP_Compass_LSM303D.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
#include <AP_HAL_Linux/GPIO.h>
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
|
#define LSM303D_DRDY_M_PIN RPI_GPIO_27
|
|
#endif
|
|
#endif
|
|
|
|
#ifndef LSM303D_DRDY_M_PIN
|
|
#define LSM303D_DRDY_M_PIN -1
|
|
#endif
|
|
|
|
/* SPI protocol address bits */
|
|
#define DIR_READ (1<<7)
|
|
#define DIR_WRITE (0<<7)
|
|
#define ADDR_INCREMENT (1<<6)
|
|
|
|
/* register addresses: A: accel, M: mag, T: temp */
|
|
#define ADDR_WHO_AM_I 0x0F
|
|
#define WHO_I_AM 0x49
|
|
|
|
#define ADDR_OUT_TEMP_L 0x05
|
|
#define ADDR_OUT_TEMP_H 0x06
|
|
#define ADDR_STATUS_M 0x07
|
|
#define ADDR_OUT_X_L_M 0x08
|
|
#define ADDR_OUT_X_H_M 0x09
|
|
#define ADDR_OUT_Y_L_M 0x0A
|
|
#define ADDR_OUT_Y_H_M 0x0B
|
|
#define ADDR_OUT_Z_L_M 0x0C
|
|
#define ADDR_OUT_Z_H_M 0x0D
|
|
|
|
#define ADDR_INT_CTRL_M 0x12
|
|
#define ADDR_INT_SRC_M 0x13
|
|
#define ADDR_REFERENCE_X 0x1c
|
|
#define ADDR_REFERENCE_Y 0x1d
|
|
#define ADDR_REFERENCE_Z 0x1e
|
|
|
|
#define ADDR_STATUS_A 0x27
|
|
#define ADDR_OUT_X_L_A 0x28
|
|
#define ADDR_OUT_X_H_A 0x29
|
|
#define ADDR_OUT_Y_L_A 0x2A
|
|
#define ADDR_OUT_Y_H_A 0x2B
|
|
#define ADDR_OUT_Z_L_A 0x2C
|
|
#define ADDR_OUT_Z_H_A 0x2D
|
|
|
|
#define ADDR_CTRL_REG0 0x1F
|
|
#define ADDR_CTRL_REG1 0x20
|
|
#define ADDR_CTRL_REG2 0x21
|
|
#define ADDR_CTRL_REG3 0x22
|
|
#define ADDR_CTRL_REG4 0x23
|
|
#define ADDR_CTRL_REG5 0x24
|
|
#define ADDR_CTRL_REG6 0x25
|
|
#define ADDR_CTRL_REG7 0x26
|
|
|
|
#define ADDR_FIFO_CTRL 0x2e
|
|
#define ADDR_FIFO_SRC 0x2f
|
|
|
|
#define ADDR_IG_CFG1 0x30
|
|
#define ADDR_IG_SRC1 0x31
|
|
#define ADDR_IG_THS1 0x32
|
|
#define ADDR_IG_DUR1 0x33
|
|
#define ADDR_IG_CFG2 0x34
|
|
#define ADDR_IG_SRC2 0x35
|
|
#define ADDR_IG_THS2 0x36
|
|
#define ADDR_IG_DUR2 0x37
|
|
#define ADDR_CLICK_CFG 0x38
|
|
#define ADDR_CLICK_SRC 0x39
|
|
#define ADDR_CLICK_THS 0x3a
|
|
#define ADDR_TIME_LIMIT 0x3b
|
|
#define ADDR_TIME_LATENCY 0x3c
|
|
#define ADDR_TIME_WINDOW 0x3d
|
|
#define ADDR_ACT_THS 0x3e
|
|
#define ADDR_ACT_DUR 0x3f
|
|
|
|
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
|
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
|
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
|
#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
|
|
#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4))
|
|
#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4))
|
|
#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
|
#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
|
#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
|
#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
|
|
#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
|
#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
|
|
|
#define REG1_BDU_UPDATE (1<<3)
|
|
#define REG1_Z_ENABLE_A (1<<2)
|
|
#define REG1_Y_ENABLE_A (1<<1)
|
|
#define REG1_X_ENABLE_A (1<<0)
|
|
|
|
#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
|
|
#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
|
|
#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
|
|
#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
|
|
#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
|
|
|
|
#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
|
|
#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
|
|
#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
|
|
#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
|
|
#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3))
|
|
#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3))
|
|
|
|
#define REG5_ENABLE_T (1<<7)
|
|
|
|
#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
|
|
#define REG5_RES_LOW_M ((0<<6) | (0<<5))
|
|
|
|
#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
|
|
#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
|
|
#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
|
|
#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
|
|
#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2))
|
|
#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2))
|
|
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
|
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
|
|
|
#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5))
|
|
#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5))
|
|
#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5))
|
|
#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5))
|
|
#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5))
|
|
|
|
#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_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
|
|
|
|
// 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_LSM303D *sensor = new AP_Compass_LSM303D(compass);
|
|
if (sensor == NULL) {
|
|
return NULL;
|
|
}
|
|
if (!sensor->init()) {
|
|
delete sensor;
|
|
return NULL;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
uint8_t AP_Compass_LSM303D::_register_read(uint8_t reg)
|
|
{
|
|
uint8_t addr = reg | 0x80; // Set most significant bit
|
|
|
|
uint8_t tx[2];
|
|
uint8_t rx[2];
|
|
|
|
tx[0] = addr;
|
|
tx[1] = 0;
|
|
_spi->transaction(tx, rx, 2);
|
|
|
|
return rx[1];
|
|
}
|
|
|
|
void AP_Compass_LSM303D::_register_write(uint8_t reg, uint8_t val)
|
|
{
|
|
uint8_t tx[2];
|
|
uint8_t rx[2];
|
|
|
|
tx[0] = reg;
|
|
tx[1] = val;
|
|
_spi->transaction(tx, rx, 2);
|
|
}
|
|
|
|
void AP_Compass_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits)
|
|
{
|
|
uint8_t val;
|
|
|
|
val = _register_read(reg);
|
|
val &= ~clearbits;
|
|
val |= setbits;
|
|
_register_write(reg, val);
|
|
}
|
|
|
|
/**
|
|
* Return true if the LSM303D has new data available for both the mag and
|
|
* the accels.
|
|
*/
|
|
bool AP_Compass_LSM303D::_data_ready()
|
|
{
|
|
return (_drdy_pin_m->read()) != 0;
|
|
}
|
|
|
|
|
|
// Read Sensor data
|
|
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();
|
|
return false;
|
|
}
|
|
|
|
if (!_data_ready()) {
|
|
return false;
|
|
}
|
|
|
|
struct PACKED {
|
|
uint8_t cmd;
|
|
uint8_t status;
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
} raw_mag_report_tx;
|
|
|
|
struct PACKED {
|
|
uint8_t cmd;
|
|
uint8_t status;
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
} raw_mag_report_rx;
|
|
|
|
/* fetch data from the sensor */
|
|
memset(&raw_mag_report_tx, 0, sizeof(raw_mag_report_tx));
|
|
memset(&raw_mag_report_rx, 0, sizeof(raw_mag_report_rx));
|
|
raw_mag_report_tx.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT;
|
|
_spi->transaction((uint8_t *)&raw_mag_report_tx, (uint8_t *)&raw_mag_report_rx, sizeof(raw_mag_report_tx));
|
|
|
|
_mag_x = raw_mag_report_rx.x;
|
|
_mag_y = raw_mag_report_rx.y;
|
|
_mag_z = raw_mag_report_rx.z;
|
|
|
|
if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
|
bool
|
|
AP_Compass_LSM303D::init()
|
|
{
|
|
// TODO: support users without data ready pin
|
|
if (LSM303D_DRDY_M_PIN < 0)
|
|
return false;
|
|
|
|
hal.scheduler->suspend_timer_procs();
|
|
|
|
_spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM);
|
|
_spi_sem = _spi->get_semaphore();
|
|
|
|
_drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
|
|
_drdy_pin_m->mode(HAL_GPIO_INPUT);
|
|
|
|
// Test WHOAMI
|
|
uint8_t whoami = _register_read(ADDR_WHO_AM_I);
|
|
if (whoami != WHO_I_AM) {
|
|
hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
|
|
AP_HAL::panic("LSM303D: bad WHOAMI");
|
|
}
|
|
|
|
uint8_t tries = 0;
|
|
do {
|
|
// TODO: don't try to init 25 times
|
|
bool success = _hardware_init();
|
|
if (success) {
|
|
hal.scheduler->delay(5+2);
|
|
if (!_spi_sem->take(100)) {
|
|
AP_HAL::panic("LSM303D: Unable to get semaphore");
|
|
}
|
|
if (_data_ready()) {
|
|
_spi_sem->give();
|
|
break;
|
|
}
|
|
_spi_sem->give();
|
|
}
|
|
if (tries++ > 5) {
|
|
AP_HAL::panic("PANIC: failed to boot LSM303D 5 times");
|
|
}
|
|
} while (1);
|
|
|
|
_scaling[0] = 1.0;
|
|
_scaling[1] = 1.0;
|
|
_scaling[2] = 1.0;
|
|
|
|
/* register the compass instance in the frontend */
|
|
_compass_instance = register_compass();
|
|
set_dev_id(_compass_instance, get_dev_id());
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
|
set_external(_compass_instance, false);
|
|
#endif
|
|
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void));
|
|
|
|
_spi_sem->give();
|
|
hal.scheduler->resume_timer_procs();
|
|
_initialised = true;
|
|
|
|
return _initialised;
|
|
}
|
|
|
|
uint32_t AP_Compass_LSM303D::get_dev_id()
|
|
{
|
|
return AP_COMPASS_TYPE_LSM303D;
|
|
}
|
|
|
|
bool AP_Compass_LSM303D::_hardware_init(void)
|
|
{
|
|
if (!_spi_sem->take(100)) {
|
|
AP_HAL::panic("LSM303D: Unable to get semaphore");
|
|
}
|
|
|
|
// initially run the bus at low speed
|
|
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
|
|
|
|
// ensure the chip doesn't interpret any other bus traffic as I2C
|
|
_disable_i2c();
|
|
|
|
/* enable mag */
|
|
_reg7_expected = REG7_CONT_MODE_M;
|
|
_register_write(ADDR_CTRL_REG7, _reg7_expected);
|
|
_register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
|
|
_register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
|
|
|
|
_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();
|
|
|
|
return true;
|
|
}
|
|
|
|
void AP_Compass_LSM303D::_update()
|
|
{
|
|
if (AP_HAL::micros() - _last_update_timestamp < 10000) {
|
|
return;
|
|
}
|
|
|
|
if (!_spi_sem->take_nonblocking()) {
|
|
return;
|
|
}
|
|
|
|
_collect_samples();
|
|
|
|
_last_update_timestamp = AP_HAL::micros();
|
|
_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;
|
|
}
|
|
|
|
if (_accum_count == 0) {
|
|
/* We're not ready to publish*/
|
|
return;
|
|
}
|
|
|
|
hal.scheduler->suspend_timer_procs();
|
|
Vector3f field(_mag_x_accum * _scaling[0],
|
|
_mag_y_accum * _scaling[1],
|
|
_mag_z_accum * _scaling[2]);
|
|
field /= _accum_count;
|
|
|
|
_accum_count = 0;
|
|
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
|
|
hal.scheduler->resume_timer_procs();
|
|
|
|
publish_filtered_field(field, _compass_instance);
|
|
}
|
|
|
|
void AP_Compass_LSM303D::_disable_i2c(void)
|
|
{
|
|
// TODO: use the register names
|
|
uint8_t a = _register_read(0x02);
|
|
_register_write(0x02, (0x10 | a));
|
|
a = _register_read(0x02);
|
|
_register_write(0x02, (0xF7 & a));
|
|
a = _register_read(0x15);
|
|
_register_write(0x15, (0x80 | a));
|
|
a = _register_read(0x02);
|
|
_register_write(0x02, (0xE7 & a));
|
|
}
|
|
|
|
uint8_t 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)
|
|
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;
|
|
}
|
|
|
|
_mag_range_scale = new_scale_ga_digit;
|
|
_register_modify(ADDR_CTRL_REG6, clearbits, setbits);
|
|
return 0;
|
|
}
|
|
|
|
uint8_t AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency)
|
|
{
|
|
uint8_t setbits = 0;
|
|
uint8_t clearbits = REG5_RATE_BITS_M;
|
|
|
|
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;
|
|
}
|
|
|
|
_register_modify(ADDR_CTRL_REG5, clearbits, setbits);
|
|
return 0;
|
|
}
|