Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-04 15:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity
efbb030494
ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp

437 lines
13 KiB
C++
Raw Blame History

/*
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/>.
*/
/*
This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
This combination is available as a cheap 10DOF sensor on ebay
This sensor driver is an example only - it should not be used in any
serious autopilot as the latencies on I2C prevent good timing at
high sample rates. It is useful when doing an initial port of
ardupilot to a board where only I2C is available, and a cheap sensor
can be used.
Datasheets:
ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "AP_InertialSensor_RST.h"
#include <AP_Math/AP_Math.h>
#include <inttypes.h>
#include <utility>
#include <math.h>
#include <stdio.h>
const extern AP_HAL::HAL &hal;
#define ADDR_INCREMENT (1<<6)
/************************************iis328dq register addresses *******************************************/
#define ACCEL_WHO_AM_I 0x0F
#define ACCEL_WHO_I_AM 0x32
#define ACCEL_CTRL_REG1 0x20
/* keep lowpass low to avoid noise issues */
#define RATE_50HZ_LP_37HZ (0<<4) | (0<<3)
#define RATE_100HZ_LP_74HZ (0<<4) | (1<<3)
#define RATE_400HZ_LP_292HZ (1<<4) | (0<<3)
#define RATE_1000HZ_LP_780HZ (1<<4) | (1<<3)
#define ACCEL_CTRL_REG2 0x21
#define ACCEL_CTRL_REG3 0x22
#define ACCEL_CTRL_REG4 0x23
#define ACCEL_CTRL_REG5 0x24
#define ACCEL_HP_FILTER_RESETE 0x25
#define ACCEL_OUT_REFERENCE 0x26
#define ACCEL_STATUS_REG 0x27
#define ACCEL_OUT_X_L 0x28
#define ACCEL_OUT_X_H 0x29
#define ACCEL_OUT_Y_L 0x2A
#define ACCEL_OUT_Y_H 0x2B
#define ACCEL_OUT_Z_L 0x2C
#define ACCEL_OUT_Z_H 0x2D
#define ACCEL_INT1_CFG 0x30
#define ACCEL_INT1_SRC 0x31
#define ACCEL_INT1_TSH 0x32
#define ACCEL_INT1_DURATION 0x33
#define ACCEL_INT2_CFG 0x34
#define ACCEL_INT2_SRC 0x35
#define ACCEL_INT2_TSH 0x36
#define ACCEL_INT2_DURATION 0x37
/* Internal configuration values */
#define ACCEL_REG1_POWER_NORMAL ((0<<7) | (0<<6) | (1<<5))
#define ACCEL_REG1_Z_ENABLE (1<<2)
#define ACCEL_REG1_Y_ENABLE (1<<1)
#define ACCEL_REG1_X_ENABLE (1<<0)
#define ACCEL_REG4_BDU (1<<7)
#define ACCEL_REG4_BLE (1<<6)
#define ACCEL_REG4_FULL_SCALE_BITS ((1<<5) | (1<<4))
#define ACCEL_REG4_FULL_SCALE_2G ((0<<5) | (0<<4))
#define ACCEL_REG4_FULL_SCALE_4G ((0<<5) | (1<<4))
#define ACCEL_REG4_FULL_SCALE_8G ((1<<5) | (1<<4))
#define ACCEL_STATUS_ZYXOR (1<<7)
#define ACCEL_STATUS_ZOR (1<<6)
#define ACCEL_STATUS_YOR (1<<5)
#define ACCEL_STATUS_XOR (1<<4)
#define ACCEL_STATUS_ZYXDA (1<<3)
#define ACCEL_STATUS_ZDA (1<<2)
#define ACCEL_STATUS_YDA (1<<1)
#define ACCEL_STATUS_XDA (1<<0)
#define ACCEL_DEFAULT_RANGE_G 8
#define ACCEL_DEFAULT_RATE 1000
#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 780
#define ACCEL_ONE_G 9.80665f
/************************************i3g4250d register addresses *******************************************/
#define GYRO_WHO_AM_I 0x0F
#define GYRO_WHO_I_AM 0xD3
#define GYRO_CTRL_REG1 0x20
/* keep lowpass low to avoid noise issues */
#define RATE_100HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_200HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_200HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_200HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_400HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_400HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_400HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define RATE_400HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define RATE_800HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define RATE_800HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define RATE_800HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define RATE_800HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define GYRO_CTRL_REG2 0x21
#define GYRO_CTRL_REG3 0x22
#define GYRO_CTRL_REG4 0x23
#define RANGE_250DPS (0<<4)
#define RANGE_500DPS (1<<4)
#define RANGE_2000DPS (3<<4)
#define GYRO_CTRL_REG5 0x24
#define GYRO_REFERENCE 0x25
#define GYRO_OUT_TEMP 0x26
#define GYRO_STATUS_REG 0x27
#define GYRO_OUT_X_L 0x28
#define GYRO_OUT_X_H 0x29
#define GYRO_OUT_Y_L 0x2A
#define GYRO_OUT_Y_H 0x2B
#define GYRO_OUT_Z_L 0x2C
#define GYRO_OUT_Z_H 0x2D
#define GYRO_FIFO_CTRL_REG 0x2E
#define GYRO_FIFO_SRC_REG 0x2F
#define GYRO_INT1_CFG 0x30
#define GYRO_INT1_SRC 0x31
#define GYRO_INT1_TSH_XH 0x32
#define GYRO_INT1_TSH_XL 0x33
#define GYRO_INT1_TSH_YH 0x34
#define GYRO_INT1_TSH_YL 0x35
#define GYRO_INT1_TSH_ZH 0x36
#define GYRO_INT1_TSH_ZL 0x37
#define GYRO_INT1_DURATION 0x38
#define GYRO_LOW_ODR 0x39
/* Internal configuration values */
#define GYRO_REG1_POWER_NORMAL (1<<3)
#define GYRO_REG1_Z_ENABLE (1<<2)
#define GYRO_REG1_Y_ENABLE (1<<1)
#define GYRO_REG1_X_ENABLE (1<<0)
#define GYRO_REG4_BLE (1<<6)
#define GYRO_REG5_FIFO_ENABLE (1<<6)
#define GYRO_REG5_REBOOT_MEMORY (1<<7)
#define GYRO_STATUS_ZYXOR (1<<7)
#define GYRO_STATUS_ZOR (1<<6)
#define GYRO_STATUS_YOR (1<<5)
#define GYRO_STATUS_XOR (1<<4)
#define GYRO_STATUS_ZYXDA (1<<3)
#define GYRO_STATUS_ZDA (1<<2)
#define GYRO_STATUS_YDA (1<<1)
#define GYRO_STATUS_XDA (1<<0)
#define GYRO_FIFO_CTRL_BYPASS_MODE (0<<5)
#define GYRO_FIFO_CTRL_FIFO_MODE (1<<5)
#define GYRO_FIFO_CTRL_STREAM_MODE (1<<6)
#define GYRO_FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
#define GYRO_FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
#define GYRO_DEFAULT_RATE 800 //data output frequency
#define GYRO_DEFAULT_RANGE_DPS 2000
#define GYRO_DEFAULT_FILTER_FREQ 35
#define GYRO_TEMP_OFFSET_CELSIUS 40
// constructor
AP_InertialSensor_RST::AP_InertialSensor_RST(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_g,
enum Rotation rotation_a)
: AP_InertialSensor_Backend(imu)
, _dev_gyro(std::move(dev_gyro))
, _dev_accel(std::move(dev_accel))
, _rotation_g(rotation_g)
, _rotation_a(rotation_a)
{
}
AP_InertialSensor_RST::~AP_InertialSensor_RST()
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_RST::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
enum Rotation rotation_g,
enum Rotation rotation_a)
{
if (!dev_gyro && !dev_accel) {
return nullptr;
}
AP_InertialSensor_RST *sensor
= new AP_InertialSensor_RST(imu, std::move(dev_gyro), std::move(dev_accel), rotation_g, rotation_a);
if (!sensor || !sensor->_init_sensor()) {
delete sensor;
return nullptr;
}
return sensor;
}
/*
* init gyro
*/
bool AP_InertialSensor_RST::_init_gyro(void)
{
_gyro_spi_sem = _dev_gyro->get_semaphore();
if (!_gyro_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
// set flag for reading registers
_dev_gyro->set_read_flag(0x80);
_dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev_gyro->read_registers(GYRO_WHO_AM_I, &_gyro_whoami, sizeof(_gyro_whoami));
if (_gyro_whoami != GYRO_WHO_I_AM) {
hal.console->printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
printf("RST: unexpected gyro WHOAMI 0x%x\n", (unsigned)_gyro_whoami);
goto fail_whoami;
}
printf("detect i3g4250d\n");
//enter power-down mode first
_dev_gyro->write_register(GYRO_CTRL_REG1, 0);
hal.scheduler->delay(100);
_dev_gyro->write_register(GYRO_CTRL_REG1,
GYRO_REG1_POWER_NORMAL | GYRO_REG1_Z_ENABLE | GYRO_REG1_X_ENABLE | GYRO_REG1_Y_ENABLE | RATE_800HZ_LP_50HZ);
_dev_gyro->write_register(GYRO_CTRL_REG2, 0); /* disable high-pass filters */
_dev_gyro->write_register(GYRO_CTRL_REG3, 0x0); /* DRDY disable */
_dev_gyro->write_register(GYRO_CTRL_REG4, RANGE_2000DPS);
_dev_gyro->write_register(GYRO_CTRL_REG5, GYRO_REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
_dev_gyro->write_register(GYRO_FIFO_CTRL_REG, GYRO_FIFO_CTRL_BYPASS_MODE);
_gyro_scale = 70e-3f / 180.0f * M_PI;
hal.scheduler->delay(100);
_gyro_spi_sem->give();
return true;
fail_whoami:
_gyro_spi_sem->give();
return false;
}
/*
* init accel
*/
bool AP_InertialSensor_RST::_init_accel(void)
{
_accel_spi_sem = _dev_accel->get_semaphore();
if (!_accel_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev_accel->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev_accel->set_read_flag(0x80);
_dev_accel->read_registers(ACCEL_WHO_AM_I, &_accel_whoami, sizeof(_accel_whoami));
if (_accel_whoami != ACCEL_WHO_I_AM) {
hal.console->printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
printf("RST: unexpected accel WHOAMI 0x%x\n", (unsigned)_accel_whoami);
goto fail_whoami;
}
_dev_accel->write_register(ACCEL_CTRL_REG1,
ACCEL_REG1_POWER_NORMAL | ACCEL_REG1_Z_ENABLE | ACCEL_REG1_Y_ENABLE | ACCEL_REG1_X_ENABLE | RATE_1000HZ_LP_780HZ);
_dev_accel->write_register(ACCEL_CTRL_REG2, 0); /* disable high-pass filters */
_dev_accel->write_register(ACCEL_CTRL_REG3, 0x02); /* DRDY enable */
_dev_accel->write_register(ACCEL_CTRL_REG4, ACCEL_REG4_BDU | ACCEL_REG4_FULL_SCALE_8G);
_accel_scale = 0.244e-3f * ACCEL_ONE_G;
_accel_spi_sem->give();
return true;
fail_whoami:
_accel_spi_sem->give();
return false;
}
bool AP_InertialSensor_RST::_init_sensor(void)
{
if(!_init_gyro() || !_init_accel())
{
return false;
}
return true;
}
/*
startup the sensor
*/
void AP_InertialSensor_RST::start(void)
{
_gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D));
_accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ));
set_gyro_orientation(_gyro_instance, _rotation_g);
set_accel_orientation(_accel_instance, _rotation_a);
// start the timer process to read samples
_dev_gyro->register_periodic_callback(1150, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::gyro_measure, void));
_dev_accel->register_periodic_callback(800, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_RST::accel_measure, void));
}
/*
copy filtered data to the frontend
*/
bool AP_InertialSensor_RST::update(void)
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
return true;
}
// Accumulate values from gyros
void AP_InertialSensor_RST::gyro_measure(void)
{
Vector3f gyro;
uint8_t status = 0;
int16_t raw_data[3];
static uint64_t start = 0;
uint64_t now = 0;
static int count = 0;
_dev_gyro->read_registers(GYRO_STATUS_REG, &status, sizeof(status));
if((status & GYRO_STATUS_ZYXDA) == 0)
return;
count++;
now = AP_HAL::micros64();
if(now - start >= 1000000)
{
//printf("gyro count = %d\n", count);
count = 0;
start = now;
}
if (_dev_gyro->read_registers(GYRO_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data)))
{
gyro = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
gyro *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
}
}
// Accumulate values from accels
void AP_InertialSensor_RST::accel_measure(void)
{
Vector3f accel;
uint8_t status = 0;
int16_t raw_data[3];
static uint64_t start = 0;
uint64_t now = 0;
static int count = 0;
_dev_accel->read_registers(ACCEL_STATUS_REG, &status, sizeof(status));
if((status & ACCEL_STATUS_ZYXDA) == 0)
return;
count++;
now = AP_HAL::micros64();
if(now - start >= 1000000)
{
//printf("accel count = %d\n", count);
count = 0;
start = now;
}
if (_dev_accel->read_registers(ACCEL_OUT_X_L | ADDR_INCREMENT, (uint8_t *)raw_data, sizeof(raw_data)))
{
accel = Vector3f(raw_data[0], raw_data[1], raw_data[2]);
accel *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
}
}
#endif
Reference in New Issue View Git Blame Copy Permalink