From d772289b0d3de78c2a6b4fa16c6652a6ebd80d9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 5 Apr 2020 17:19:00 +1000 Subject: [PATCH] AP_InertialSensor: added driver for ADIS16470 --- .../AP_InertialSensor/AP_InertialSensor.cpp | 1 + .../AP_InertialSensor_ADIS1647x.cpp | 321 ++++++++++++++++++ .../AP_InertialSensor_ADIS1647x.h | 70 ++++ .../AP_InertialSensor_Backend.h | 1 + 4 files changed, 393 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 18dd8967df..be35f6cce6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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. */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp new file mode 100644 index 0000000000..5f49226a44 --- /dev/null +++ b/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 . + */ + +#include +#include +#include +#include + +#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 _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 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; idelay_microseconds(period_us - dt); + } + } +} + +bool AP_InertialSensor_ADIS1647x::update() +{ + update_accel(accel_instance); + update_gyro(gyro_instance); + return true; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h new file mode 100644 index 0000000000..1ac6b86bcd --- /dev/null +++ b/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 . + */ +/* + 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 + +#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 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 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 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; +}; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 5f93526a92..8317d56ae1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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: