diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 090318759b..1e48a6e997 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 4c8fff7442..d0cd5e52d5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp new file mode 100644 index 0000000000..636438d591 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp @@ -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 . + */ +/* + 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 + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#include "AP_InertialSensor_RST.h" +#include + +#include +#include +#include +#include + +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 dev_gyro, + AP_HAL::OwnPtr 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 dev_gyro, + AP_HAL::OwnPtr 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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h new file mode 100644 index 0000000000..3cc7f3327a --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h @@ -0,0 +1,63 @@ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include +#include +#include +#include + +#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 dev_gyro, + AP_HAL::OwnPtr 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 dev_gyro, + AP_HAL::OwnPtr 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 _dev_gyro;//i3g4250d + AP_HAL::OwnPtr _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