zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: added driver for ADIS16470

zr-v5.1
Andrew Tridgell 5 years ago
parent
d43fc591c3
commit
d772289b0d
4 changed files with 393 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 321
      libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
  3. 70
      libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
  4. 1
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

1
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -23,6 +23,7 @@ @@ -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. */

321
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
Unescape View File

@ -0,0 +1,321 @@ @@ -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;
}

70
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
Unescape View File

@ -0,0 +1,70 @@ @@ -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;
};

1
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Unescape View File

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

Write Preview
Loading…
Cancel
Save