/* * 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" /* device registers */ #define REG_PROD_ID 0x72 #define PROD_ID_16470 0x4056 #define PROD_ID_16477 0x405d #define PROD_ID_16507 0x407b #define REG_GLOB_CMD 0x68 #define GLOB_CMD_SW_RESET 0x80 #define REG_RANG_MDL 0x5E // 16477 only #define REG_DATA_CNTR 0x22 #define REG_MSC_CTRL 0x60 # define REG_MSC_CTRL_BURST32 0x200 # define REG_MSC_CTRL_BURSTSEL 0x100 # define REG_MSC_CTRL_GCOMP 0x080 # define REG_MSC_CTRL_PCOMP 0x040 # define REG_MSC_CTRL_SENSBW 0x010 # define REG_MSC_CTRL_DRPOL 0x001 #define REG_DEC_RATE 0x64 # define REG_DEC_RATE_2000Hz 0 # define REG_DEC_RATE_1000Hz 1 # define REG_DEC_RATE_666Hz 2 # define REG_DEC_RATE_500Hz 3 # define REG_DEC_RATE_400Hz 4 #define REG_FILT_CTRL 0x5c /* timings */ #define T_STALL_US 20U #define T_RESET_MS 500U #define TIMING_DEBUG 0 #if TIMING_DEBUG #define DEBUG_SET_PIN(n,v) hal.gpio->write(52+n, v) #define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(52+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, uint8_t drdy_gpio) : AP_InertialSensor_Backend(imu) , dev(std::move(_dev)) , rotation(_rotation) , drdy_pin(drdy_gpio) { } AP_InertialSensor_Backend * AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum Rotation rotation, uint8_t drdy_gpio) { if (!dev) { return nullptr; } auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation, drdy_gpio); if (!sensor) { return nullptr; } if (!sensor->init()) { delete sensor; return nullptr; } return sensor; } void AP_InertialSensor_ADIS1647x::start() { if (!_imu.register_accel(accel_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) || !_imu.register_gyro(gyro_instance, expected_sample_rate_hz, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) { return; } // 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(uint16_t &prod_id) { prod_id = read_reg16(REG_PROD_ID); switch (prod_id) { case PROD_ID_16470: // can do up to 40G opmode = OpMode::Basic; accel_scale = 1.25 * GRAVITY_MSS * 0.001; _clip_limit = 39.5f * GRAVITY_MSS; gyro_scale = radians(0.1); expected_sample_rate_hz = 2000; return true; case PROD_ID_16477: { // can do up to 40G opmode = OpMode::Basic; accel_scale = 1.25 * GRAVITY_MSS * 0.001; _clip_limit = 39.5f * GRAVITY_MSS; expected_sample_rate_hz = 2000; // 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; } case PROD_ID_16507: { opmode = OpMode::Delta32; expected_sample_rate_hz = 1200; accel_scale = 392.0 / 2097152000.0; dvel_scale = 400.0 / 0x7FFFFFFF; _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(125) / 0x4E200000; dangle_scale = radians(360.0 / 0x7FFFFFFF); break; case 1: gyro_scale = radians(500) / 0x4E200000; dangle_scale = radians(720.0 / 0x7FFFFFFF); break; case 3: gyro_scale = radians(2000) / 0x4E200000; dangle_scale = radians(2160.0 / 0x7FFFFFFF); break; default: return false; } if (opmode == OpMode::Basic) { accel_scale *= 0x10000; gyro_scale *= 0x10000; } return true; } } return false; } bool AP_InertialSensor_ADIS1647x::init() { WITH_SEMAPHORE(dev->get_semaphore()); uint8_t tries = 10; uint16_t prod_id = 0; do { // perform software reset write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET); hal.scheduler->delay(100); } while (!check_product_id(prod_id) && --tries); if (tries == 0) { return false; } // bring rate down if (expected_sample_rate_hz < 450) { if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_400Hz, true)) { return false; } } else if (expected_sample_rate_hz < 600) { if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_500Hz, true)) { return false; } } else if (expected_sample_rate_hz < 700) { if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_666Hz, true)) { return false; } } else if (expected_sample_rate_hz < 1500) { if (!write_reg16(REG_DEC_RATE, REG_DEC_RATE_1000Hz, true)) { return false; } } if (!write_reg16(REG_FILT_CTRL, 0, true)) { return false; } // choose burst type and compensation uint16_t msc_ctrl = REG_MSC_CTRL_GCOMP | REG_MSC_CTRL_PCOMP | REG_MSC_CTRL_DRPOL; if (opmode == OpMode::Delta32) { msc_ctrl |= REG_MSC_CTRL_BURST32 | REG_MSC_CTRL_BURSTSEL; } else if (opmode == OpMode::AG32) { msc_ctrl |= REG_MSC_CTRL_BURST32; } if (!write_reg16(REG_MSC_CTRL, msc_ctrl, true)) { return true; } #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 */ bool AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value, bool confirm) const { const uint8_t retries = 16; for (uint8_t i=0; i<retries; i++) { 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); if (!confirm || read_reg16(regnum) == value) { return true; } } return false; } /* read the sensor using 16 bit burst transfer of gyro/accel data */ void AP_InertialSensor_ADIS1647x::read_sensor16(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 {}; 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); _rotate_and_correct_gyro(gyro_instance, gyro); _notify_new_gyro_raw_sample(gyro_instance, gyro); /* 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); } /* read the sensor using 32 bit burst transfer of accel/gyro */ void AP_InertialSensor_ADIS1647x::read_sensor32(void) { struct adis_data { uint8_t cmd[2]; uint16_t diag_stat; uint16_t gx_low; uint16_t gx_high; uint16_t gy_low; uint16_t gy_high; uint16_t gz_low; uint16_t gz_high; uint16_t ax_low; uint16_t ax_high; uint16_t ay_low; uint16_t ay_high; uint16_t az_low; uint16_t az_high; uint16_t temp; uint16_t counter; uint8_t pad; uint8_t checksum; } data {}; 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(accel_scale*int32_t(be16toh(data.ax_low) | (be16toh(data.ax_high)<<16))), -float(accel_scale*int32_t(be16toh(data.ay_low) | (be16toh(data.ay_high)<<16))), -float(accel_scale*int32_t(be16toh(data.az_low) | (be16toh(data.az_high)<<16)))}; Vector3f gyro{float(gyro_scale*int32_t(be16toh(data.gx_low) | (be16toh(data.gx_high)<<16))), -float(gyro_scale*int32_t(be16toh(data.gy_low) | (be16toh(data.gy_high)<<16))), -float(gyro_scale*int32_t(be16toh(data.gz_low) | (be16toh(data.gz_high)<<16)))}; _rotate_and_correct_accel(accel_instance, accel); _notify_new_accel_raw_sample(accel_instance, accel); _rotate_and_correct_gyro(gyro_instance, gyro); _notify_new_gyro_raw_sample(gyro_instance, gyro); /* 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); } /* read the sensor using 32 bit burst transfer of delta-angle/delta-velocity */ void AP_InertialSensor_ADIS1647x::read_sensor32_delta(void) { struct adis_data { uint8_t cmd[2]; uint16_t diag_stat; uint16_t dax_low; uint16_t dax_high; uint16_t day_low; uint16_t day_high; uint16_t daz_low; uint16_t daz_high; uint16_t dvx_low; uint16_t dvx_high; uint16_t dvy_low; uint16_t dvy_high; uint16_t dvz_low; uint16_t dvz_high; uint16_t temp; uint16_t counter; uint8_t pad; uint8_t checksum; } data {}; 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 dvel{float(dvel_scale*int32_t(be16toh(data.dvx_low) | (be16toh(data.dvx_high)<<16))), -float(dvel_scale*int32_t(be16toh(data.dvy_low) | (be16toh(data.dvy_high)<<16))), -float(dvel_scale*int32_t(be16toh(data.dvz_low) | (be16toh(data.dvz_high)<<16)))}; Vector3f dangle{float(dangle_scale*int32_t(be16toh(data.dax_low) | (be16toh(data.dax_high)<<16))), -float(dangle_scale*int32_t(be16toh(data.day_low) | (be16toh(data.day_high)<<16))), -float(dangle_scale*int32_t(be16toh(data.daz_low) | (be16toh(data.daz_high)<<16)))}; // compensate for clock errors, see "DELTA ANGLES" in datasheet dangle *= expected_sample_rate_hz / _gyro_raw_sample_rate(gyro_instance); dvel *= expected_sample_rate_hz / _accel_raw_sample_rate(gyro_instance); _notify_new_delta_velocity(accel_instance, dvel); _notify_new_delta_angle(gyro_instance, dangle); /* 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 = (1000000UL / expected_sample_rate_hz) - 20U; bool wait_ok = false; if (drdy_pin != 0) { // when we have a DRDY pin then wait for it to go high DEBUG_SET_PIN(0, 1); wait_ok = hal.gpio->wait_pin(drdy_pin, AP_HAL::GPIO::INTERRUPT_RISING, 2100); DEBUG_SET_PIN(0, 0); } if (opmode == OpMode::Delta32) { read_sensor32_delta(); } else if (opmode == OpMode::AG32) { read_sensor32(); } else { read_sensor16(); } uint32_t dt = AP_HAL::micros() - tstart; if (dt < period_us) { uint32_t wait_us = period_us - dt; if (!wait_ok || wait_us > period_us/2) { DEBUG_SET_PIN(3, 1); hal.scheduler->delay_microseconds(wait_us); DEBUG_SET_PIN(3, 0); } } } } bool AP_InertialSensor_ADIS1647x::update() { update_accel(accel_instance); update_gyro(gyro_instance); return true; }