AP_InertialSensor: add Robsense PhenixPro Devkit Board support

7 years ago · efbb030494
4 changed files with 514 additions and 1 deletions
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@ -21,6 +21,7 @@
				@@ -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)
				@@ -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
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@ -88,9 +88,11 @@ public:
				@@ -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:
--- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
@ -0,0 +1,436 @@
				@@ -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
--- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.h
@ -0,0 +1,63 @@
				@@ -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