mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_InertialSensor: add Robsense PhenixPro Devkit Board support
This commit is contained in:
parent
0e1ce2a7fd
commit
efbb030494
@ -21,6 +21,7 @@
|
||||
#include "AP_InertialSensor_QURT.h"
|
||||
#include "AP_InertialSensor_SITL.h"
|
||||
#include "AP_InertialSensor_qflight.h"
|
||||
#include "AP_InertialSensor_RST.h"
|
||||
|
||||
/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
|
||||
* Output is on the debug console. */
|
||||
@ -834,6 +835,17 @@ AP_InertialSensor::detect_backends(void)
|
||||
} else {
|
||||
hal.console->printf("aero: onboard IMU not detected\n");
|
||||
}
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_RST
|
||||
AP_InertialSensor_Backend *backend = AP_InertialSensor_RST::probe(*this, hal.spi->get_device(HAL_INS_RST_G_NAME),
|
||||
hal.spi->get_device(HAL_INS_RST_A_NAME),
|
||||
HAL_INS_DEFAULT_G_ROTATION,
|
||||
HAL_INS_DEFAULT_A_ROTATION);
|
||||
if (backend) {
|
||||
_add_backend(backend);
|
||||
hal.console->printf("RST: IMU detected\n");
|
||||
} else {
|
||||
hal.console->printf("RST: IMU not detected\n");
|
||||
}
|
||||
#else
|
||||
#error Unrecognised HAL_INS_TYPE setting
|
||||
#endif
|
||||
|
@ -88,9 +88,11 @@ public:
|
||||
DEVTYPE_ACC_BMA180 = 0x12,
|
||||
DEVTYPE_ACC_MPU6000 = 0x13,
|
||||
DEVTYPE_ACC_MPU9250 = 0x16,
|
||||
DEVTYPE_ACC_IIS328DQ = 0x17,
|
||||
DEVTYPE_GYR_MPU6000 = 0x21,
|
||||
DEVTYPE_GYR_L3GD20 = 0x22,
|
||||
DEVTYPE_GYR_MPU9250 = 0x24
|
||||
DEVTYPE_GYR_MPU9250 = 0x24,
|
||||
DEVTYPE_GYR_I3G4250D = 0x25,
|
||||
};
|
||||
|
||||
protected:
|
||||
|
436
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
Normal file
436
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
Normal file
@ -0,0 +1,436 @@
|
||||
/*
|
||||
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
|
63
libraries/AP_InertialSensor/AP_InertialSensor_RST.h
Normal file
63
libraries/AP_InertialSensor/AP_InertialSensor_RST.h
Normal file
@ -0,0 +1,63 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_HAL/SPIDevice.h>
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AP_InertialSensor_Backend.h"
|
||||
|
||||
class AP_InertialSensor_RST : public AP_InertialSensor_Backend
|
||||
{
|
||||
public:
|
||||
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_a,
|
||||
enum Rotation rotation_g);
|
||||
|
||||
virtual ~AP_InertialSensor_RST();
|
||||
|
||||
// probe the sensor on SPI bus
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
|
||||
enum Rotation rotation_a,
|
||||
enum Rotation rotation_g);
|
||||
|
||||
/* update accel and gyro state */
|
||||
bool update() override;
|
||||
|
||||
void start(void) override;
|
||||
|
||||
private:
|
||||
bool _init_sensor();
|
||||
bool _init_gyro();
|
||||
bool _init_accel();
|
||||
void gyro_measure();
|
||||
void accel_measure();
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_gyro;//i3g4250d
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev_accel;//iis328dq
|
||||
AP_HAL::Semaphore *_gyro_spi_sem;
|
||||
AP_HAL::Semaphore *_accel_spi_sem;
|
||||
|
||||
// gyro whoami
|
||||
uint8_t _gyro_whoami;
|
||||
// accel whoami
|
||||
uint8_t _accel_whoami;
|
||||
|
||||
float _gyro_scale;
|
||||
float _accel_scale;
|
||||
|
||||
// gyro and accel instances
|
||||
uint8_t _gyro_instance;
|
||||
uint8_t _accel_instance;
|
||||
enum Rotation _rotation_g;
|
||||
enum Rotation _rotation_a;
|
||||
};
|
||||
#endif
|
Loading…
Reference in New Issue
Block a user