AP_InertialSensor: added driver for ADIS16470

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -23,6 +23,7 @@
 #include "AP_InertialSensor_BMI055.h"
 #include "AP_InertialSensor_BMI088.h"
 #include "AP_InertialSensor_Invensensev2.h"
+#include "AP_InertialSensor_ADIS1647x.h"
 
 /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
  * Output is on the debug console. */

libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp

@@ -0,0 +1,321 @@
+/*
+ * This file 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 file 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 <utility>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+#include "AP_InertialSensor_ADIS1647x.h"
+
+#define BACKEND_SAMPLE_RATE 2000
+
+/*
+  device registers
+ */
+#define REG_PROD_ID  0x72
+#define  PROD_ID_16470     0x4056
+#define  PROD_ID_16477     0x405d
+
+#define REG_GLOB_CMD 0x68
+#define  GLOB_CMD_SW_RESET 0x80
+
+#define REG_RANG_MDL 0x5E // 16477 only
+
+#define REG_DATA_CNTR 0x22
+
+/*
+  timings
+ */
+#define T_STALL_US   20U
+#define T_RESET_MS   250U
+
+#define TIMING_DEBUG 0
+#if TIMING_DEBUG
+#define DEBUG_SET_PIN(n,v) hal.gpio->write(55-n, v)
+#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(55-n)
+#else
+#define DEBUG_SET_PIN(n,v)
+#define DEBUG_TOGGLE_PIN(n)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+AP_InertialSensor_ADIS1647x::AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
+                                                   AP_HAL::OwnPtr<AP_HAL::Device> _dev,
+                                                   enum Rotation _rotation)
+    : AP_InertialSensor_Backend(imu)
+    , dev(std::move(_dev))
+    , rotation(_rotation)
+{
+}
+
+AP_InertialSensor_Backend *
+AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
+                                AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                enum Rotation rotation)
+{
+    if (!dev) {
+        return nullptr;
+    }
+    auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation);
+
+    if (!sensor) {
+        return nullptr;
+    }
+
+    if (!sensor->init()) {
+        delete sensor;
+        return nullptr;
+    }
+
+    return sensor;
+}
+
+void AP_InertialSensor_ADIS1647x::start()
+{
+    accel_instance = _imu.register_accel(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));
+    gyro_instance = _imu.register_gyro(BACKEND_SAMPLE_RATE,   dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));
+
+    // setup sensor rotations from probe()
+    set_gyro_orientation(gyro_instance, rotation);
+    set_accel_orientation(accel_instance, rotation);
+
+    /*
+      as the sensor does not have a FIFO we need to jump through some
+      hoops to ensure we don't lose any samples. This creates a thread
+      to do the capture, running at very high priority
+     */
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_ADIS1647x::loop, void),
+                                      "ADIS1647x",
+                                      1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
+        AP_HAL::panic("Failed to create ADIS1647x thread");
+    }
+}
+
+/*
+  check product ID
+ */
+bool AP_InertialSensor_ADIS1647x::check_product_id(void)
+{
+    uint16_t prod_id = read_reg16(REG_PROD_ID);
+    switch (prod_id) {
+    case PROD_ID_16470:
+        // can do up to 40G
+        accel_scale = 1.25 * GRAVITY_MSS * 0.001;
+        _clip_limit = 39.5f * GRAVITY_MSS;
+        gyro_scale = radians(0.1);
+        return true;
+
+    case PROD_ID_16477:
+        // can do up to 40G
+        accel_scale = 1.25 * GRAVITY_MSS * 0.001;
+        _clip_limit = 39.5f * GRAVITY_MSS;
+        // RANG_MDL register used for gyro range
+        uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
+        switch ((rang_mdl >> 2) & 3) {
+        case 0:
+            gyro_scale = radians(1.0/160);
+            break;
+        case 1:
+            gyro_scale = radians(1.0/40);
+            break;
+        case 3:
+            gyro_scale = radians(1.0/10);
+            break;
+        default:
+            return false;
+        }
+        return true;
+    }
+    return false;
+}
+
+
+bool AP_InertialSensor_ADIS1647x::init()
+{
+    WITH_SEMAPHORE(dev->get_semaphore());
+    if (!check_product_id()) {
+        return false;
+    }
+
+    // perform software reset
+    write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
+    hal.scheduler->delay(T_RESET_MS);
+
+    // re-check after reset
+    if (!check_product_id()) {
+        return false;
+    }
+
+    // we leave all config registers at defaults
+
+#if TIMING_DEBUG
+    // useful for debugging scheduling of transfers
+    hal.gpio->pinMode(52, HAL_GPIO_OUTPUT);
+    hal.gpio->pinMode(53, HAL_GPIO_OUTPUT);
+    hal.gpio->pinMode(54, HAL_GPIO_OUTPUT);
+    hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
+#endif
+
+    // we need to use low speed for burst transfers
+    dev->set_speed(AP_HAL::Device::SPEED_LOW);
+
+    return true;
+}
+
+
+/*
+  read a 16 bit register value
+ */
+uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const
+{
+    uint8_t req[2] = {regnum, 0};
+    uint8_t reply[2] {};
+    dev->transfer(req, sizeof(req), nullptr, 0);
+    hal.scheduler->delay_microseconds(T_STALL_US);
+    dev->transfer(nullptr, 0, reply, sizeof(reply));
+    uint16_t ret = (reply[0]<<8U) | reply[1];
+    return ret;
+}
+
+
+/*
+  write a 16 bit register value
+ */
+void AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value) const
+{
+    uint8_t req[2];
+    req[0] = (regnum | 0x80);
+    req[1] = value & 0xFF;
+    dev->transfer(req, sizeof(req), nullptr, 0);
+    hal.scheduler->delay_microseconds(T_STALL_US);
+
+    req[0] = ((regnum+1) | 0x80);
+    req[1] = (value>>8) & 0xFF;
+    dev->transfer(req, sizeof(req), nullptr, 0);
+    hal.scheduler->delay_microseconds(T_STALL_US);
+}
+
+/*
+  loop to read the sensor
+ */
+void AP_InertialSensor_ADIS1647x::read_sensor(void)
+{
+    struct adis_data {
+        uint8_t cmd[2];
+        uint16_t diag_stat;
+        int16_t  gx;
+        int16_t  gy;
+        int16_t  gz;
+        int16_t  ax;
+        int16_t  ay;
+        int16_t  az;
+        int16_t  temp;
+        uint16_t counter;
+        uint8_t  pad;
+        uint8_t  checksum;
+    } data {};
+
+    uint64_t sample_start_us = AP_HAL::micros64();
+    do {
+        WITH_SEMAPHORE(dev->get_semaphore());
+        data.cmd[0] = REG_GLOB_CMD;
+        DEBUG_SET_PIN(2, 1);
+        if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
+            break;
+        }
+        DEBUG_SET_PIN(2, 0);
+    } while (be16toh(data.counter) == last_counter);
+
+    DEBUG_SET_PIN(1, 1);
+
+    /*
+      check the 8 bit checksum of the packet
+     */
+    uint8_t sum = 0;
+    const uint8_t *b = (const uint8_t *)&data.diag_stat;
+    for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
+        sum += b[i];
+    }
+    if (sum != data.checksum) {
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        DEBUG_TOGGLE_PIN(3);
+        // corrupt data
+        return;
+    }
+
+    /*
+      check if we have lost a sample
+     */
+    uint16_t counter = be16toh(data.counter);
+    if (done_first_read && uint16_t(last_counter+1) != counter) {
+        DEBUG_TOGGLE_PIN(3);
+    }
+    done_first_read = true;
+    last_counter = counter;
+
+    Vector3f accel{float(int16_t(be16toh(data.ax))), float(int16_t(be16toh(data.ay))), float(int16_t(be16toh(data.az)))};
+    Vector3f gyro{float(int16_t(be16toh(data.gx))), float(int16_t(be16toh(data.gy))), float(int16_t(be16toh(data.gz)))};
+
+    accel *= accel_scale;
+    gyro *= gyro_scale;
+
+    _rotate_and_correct_accel(accel_instance, accel);
+    _notify_new_accel_raw_sample(accel_instance, accel, sample_start_us);
+
+    _rotate_and_correct_gyro(gyro_instance, gyro);
+    _notify_new_gyro_raw_sample(gyro_instance, gyro, sample_start_us);
+
+    /*
+      publish average temperature at 20Hz
+     */
+    temp_sum += float(int16_t(be16toh(data.temp))*0.1);
+    temp_count++;
+
+    if (temp_count == 100) {
+        _publish_temperature(accel_instance, temp_sum/temp_count);
+        temp_sum = 0;
+        temp_count = 0;
+    }
+    DEBUG_SET_PIN(1, 0);
+}
+
+/*
+  sensor read loop
+ */
+void AP_InertialSensor_ADIS1647x::loop(void)
+{
+    while (true) {
+        uint32_t tstart = AP_HAL::micros();
+        // we deliberately set the period a bit fast to ensure we
+        // don't lose a sample
+        const uint32_t period_us = 480;
+        read_sensor();
+        uint32_t dt = AP_HAL::micros() - tstart;
+        if (dt < period_us) {
+            hal.scheduler->delay_microseconds(period_us - dt);
+        }
+    }
+}
+
+bool AP_InertialSensor_ADIS1647x::update()
+{
+    update_accel(accel_instance);
+    update_gyro(gyro_instance);
+    return true;
+}

libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h

@@ -0,0 +1,70 @@
+/*
+ * This file 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 file 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/>.
+ */
+/*
+  the ADIS1647x is unusual as it uses 16 bit registers. It also needs
+  to run as the only sensor on the SPI bus for good performance
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+class AP_InertialSensor_ADIS1647x : public AP_InertialSensor_Backend {
+public:
+    static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
+                                            AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                            enum Rotation rotation);
+
+    /**
+     * Configure the sensors and start reading routine.
+     */
+    void start() override;
+    bool update() override;
+
+private:
+    AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
+                                AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                enum Rotation rotation);
+
+    /*
+      initialise driver
+     */
+    bool init();
+    void read_sensor(void);
+    void loop(void);
+    bool check_product_id();
+
+    // read a 16 bit register
+    uint16_t read_reg16(uint8_t regnum) const;
+
+    // write a 16 bit register
+    void write_reg16(uint8_t regnum, uint16_t value) const;
+    
+    AP_HAL::OwnPtr<AP_HAL::Device> dev;
+
+    uint8_t accel_instance;
+    uint8_t gyro_instance;
+    enum Rotation rotation;
+
+    uint16_t last_counter;
+    bool done_first_read;
+    float temp_sum;
+    uint8_t temp_count;
+
+    float accel_scale;
+    float gyro_scale;
+};

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -108,6 +108,7 @@ public:
         DEVTYPE_INS_ICM20649 = 0x2E,
         DEVTYPE_INS_ICM20602 = 0x2F,
         DEVTYPE_INS_ICM20601 = 0x30,
+        DEVTYPE_INS_ADIS1647X = 0x31,
     };
 
 protected: