2016-12-13 21:47:22 -04:00
|
|
|
/*
|
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
driver for all supported Invensense IMUs, including MPU6000, MPU9250
|
2017-03-01 20:52:03 -04:00
|
|
|
ICM-20608 and ICM-20602
|
2016-12-13 21:47:22 -04:00
|
|
|
*/
|
|
|
|
|
2015-08-16 16:06:41 -03:00
|
|
|
#include <assert.h>
|
2016-01-12 14:22:11 -04:00
|
|
|
#include <utility>
|
2016-11-08 20:33:05 -04:00
|
|
|
#include <stdio.h>
|
2015-08-16 16:06:41 -03:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2020-09-01 02:42:43 -03:00
|
|
|
#include <AP_InternalError/AP_InternalError.h>
|
2021-02-23 19:46:23 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2016-01-12 14:22:11 -04:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
#include "AP_InertialSensor_Invensense.h"
|
2011-11-12 23:20:25 -04:00
|
|
|
|
2012-10-11 21:27:19 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
2011-11-12 23:20:25 -04:00
|
|
|
|
2015-11-03 09:46:29 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
2015-08-15 19:52:30 -03:00
|
|
|
#include <AP_HAL_Linux/GPIO.h>
|
2015-08-17 23:10:05 -03:00
|
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
2016-12-16 13:49:20 -04:00
|
|
|
#define INVENSENSE_DRDY_PIN BBB_P8_14
|
2016-10-03 11:00:18 -03:00
|
|
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
|
2016-12-16 13:49:20 -04:00
|
|
|
#define INVENSENSE_EXT_SYNC_ENABLE 1
|
2016-08-31 01:56:27 -03:00
|
|
|
#endif
|
2014-07-05 05:19:08 -03:00
|
|
|
#endif
|
2016-08-31 01:56:27 -03:00
|
|
|
|
2018-01-16 22:55:24 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
|
|
// hal.console can be accessed from bus threads on ChibiOS
|
|
|
|
#define debug(fmt, args ...) do {hal.console->printf("MPU: " fmt "\n", ## args); } while(0)
|
|
|
|
#else
|
2016-12-13 21:47:22 -04:00
|
|
|
#define debug(fmt, args ...) do {printf("MPU: " fmt "\n", ## args); } while(0)
|
2018-01-16 22:55:24 -04:00
|
|
|
#endif
|
2016-11-21 01:49:16 -04:00
|
|
|
|
2016-08-31 01:56:27 -03:00
|
|
|
/*
|
|
|
|
EXT_SYNC allows for frame synchronisation with an external device
|
|
|
|
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
|
|
|
|
*/
|
2016-12-16 13:49:20 -04:00
|
|
|
#ifndef INVENSENSE_EXT_SYNC_ENABLE
|
|
|
|
#define INVENSENSE_EXT_SYNC_ENABLE 0
|
2014-07-06 23:03:57 -03:00
|
|
|
#endif
|
2014-07-05 05:19:08 -03:00
|
|
|
|
2021-02-11 00:01:38 -04:00
|
|
|
// code to debug unexpected register changes
|
|
|
|
#define INVENSENSE_DEBUG_REG_CHANGE 0
|
|
|
|
|
|
|
|
#if INVENSENSE_DEBUG_REG_CHANGE
|
|
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
#endif
|
|
|
|
|
2018-01-01 22:40:07 -04:00
|
|
|
#include "AP_InertialSensor_Invensense_registers.h"
|
2012-05-08 22:25:09 -03:00
|
|
|
|
2016-11-23 05:33:55 -04:00
|
|
|
#define MPU_SAMPLE_SIZE 14
|
2020-12-17 18:02:53 -04:00
|
|
|
#define MPU_FIFO_BUFFER_LEN 8
|
2015-06-15 09:58:30 -03:00
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
|
|
|
#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2012-11-05 00:27:03 -04:00
|
|
|
/*
|
2012-11-19 11:37:42 -04:00
|
|
|
* RM-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
|
2012-11-05 00:27:03 -04:00
|
|
|
* accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
|
|
|
|
*
|
|
|
|
* See note below about accel scaling of engineering sample MPU6k
|
|
|
|
* variants however
|
|
|
|
*/
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Invensense::AP_InertialSensor_Invensense(AP_InertialSensor &imu,
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
|
|
|
enum Rotation rotation)
|
2016-01-12 14:22:11 -04:00
|
|
|
: AP_InertialSensor_Backend(imu)
|
2016-11-09 01:16:52 -04:00
|
|
|
, _temp_filter(1000, 1)
|
2016-11-03 06:19:04 -03:00
|
|
|
, _rotation(rotation)
|
2016-11-23 03:02:51 -04:00
|
|
|
, _dev(std::move(dev))
|
2015-07-02 14:22:36 -03:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Invensense::~AP_InertialSensor_Invensense()
|
2015-07-02 14:22:36 -03:00
|
|
|
{
|
2016-11-08 20:33:05 -04:00
|
|
|
if (_fifo_buffer != nullptr) {
|
2018-01-09 17:43:32 -04:00
|
|
|
hal.util->free_type(_fifo_buffer, MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2015-10-02 15:02:16 -03:00
|
|
|
delete _auxiliary_bus;
|
2015-07-28 13:43:27 -03:00
|
|
|
}
|
2015-07-02 14:22:36 -03:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Backend *AP_InertialSensor_Invensense::probe(AP_InertialSensor &imu,
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
|
|
|
enum Rotation rotation)
|
2015-07-28 13:43:27 -03:00
|
|
|
{
|
2016-11-07 23:01:53 -04:00
|
|
|
if (!dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Invensense *sensor =
|
|
|
|
new AP_InertialSensor_Invensense(imu, std::move(dev), rotation);
|
2016-01-12 14:22:11 -04:00
|
|
|
if (!sensor || !sensor->_init()) {
|
|
|
|
delete sensor;
|
2015-07-28 13:43:27 -03:00
|
|
|
return nullptr;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2016-12-13 21:47:22 -04:00
|
|
|
if (sensor->_mpu_type == Invensense_MPU9250) {
|
|
|
|
sensor->_id = HAL_INS_MPU9250_I2C;
|
|
|
|
} else {
|
|
|
|
sensor->_id = HAL_INS_MPU60XX_I2C;
|
|
|
|
}
|
2015-07-02 14:22:36 -03:00
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
return sensor;
|
2015-07-28 13:43:27 -03:00
|
|
|
}
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Backend *AP_InertialSensor_Invensense::probe(AP_InertialSensor &imu,
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
|
|
|
enum Rotation rotation)
|
2015-07-28 13:43:27 -03:00
|
|
|
{
|
2016-11-07 23:51:27 -04:00
|
|
|
if (!dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_InertialSensor_Invensense *sensor;
|
2016-06-13 17:39:12 -03:00
|
|
|
|
|
|
|
dev->set_read_flag(0x80);
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
sensor = new AP_InertialSensor_Invensense(imu, std::move(dev), rotation);
|
2016-01-12 14:22:11 -04:00
|
|
|
if (!sensor || !sensor->_init()) {
|
2015-07-02 14:22:36 -03:00
|
|
|
delete sensor;
|
2015-07-28 13:43:27 -03:00
|
|
|
return nullptr;
|
2015-07-02 14:22:36 -03:00
|
|
|
}
|
2016-12-13 21:47:22 -04:00
|
|
|
if (sensor->_mpu_type == Invensense_MPU9250) {
|
|
|
|
sensor->_id = HAL_INS_MPU9250_SPI;
|
2017-02-06 19:47:39 -04:00
|
|
|
} else if (sensor->_mpu_type == Invensense_MPU6500) {
|
|
|
|
sensor->_id = HAL_INS_MPU6500;
|
2016-12-13 21:47:22 -04:00
|
|
|
} else {
|
|
|
|
sensor->_id = HAL_INS_MPU60XX_SPI;
|
|
|
|
}
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2015-07-02 14:22:36 -03:00
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_init()
|
2011-11-12 23:20:25 -04:00
|
|
|
{
|
2016-12-16 13:49:20 -04:00
|
|
|
#ifdef INVENSENSE_DRDY_PIN
|
|
|
|
_drdy_pin = hal.gpio->channel(INVENSENSE_DRDY_PIN);
|
2014-07-05 05:19:08 -03:00
|
|
|
_drdy_pin->mode(HAL_GPIO_INPUT);
|
|
|
|
#endif
|
2013-01-10 18:12:19 -04:00
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
bool success = _hardware_init();
|
2013-01-10 18:12:19 -04:00
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
return success;
|
|
|
|
}
|
|
|
|
|
2020-09-01 02:42:43 -03:00
|
|
|
void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
|
2015-08-05 13:29:35 -03:00
|
|
|
{
|
2020-09-01 02:42:43 -03:00
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
if (log_error &&
|
|
|
|
!hal.scheduler->in_expected_delay() &&
|
|
|
|
now - last_reset_ms < 10000) {
|
|
|
|
reset_count++;
|
|
|
|
if (reset_count == 10) {
|
|
|
|
// 10 resets, each happening within 10s, triggers an internal error
|
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::imu_reset);
|
|
|
|
reset_count = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
last_reset_ms = now;
|
|
|
|
|
2016-11-23 02:02:39 -04:00
|
|
|
uint8_t user_ctrl = _last_stat_user_ctrl;
|
|
|
|
user_ctrl &= ~(BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
|
2017-03-09 19:43:54 -04:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
|
|
|
_register_write(MPUREG_FIFO_EN, 0);
|
2016-11-09 04:54:53 -04:00
|
|
|
_register_write(MPUREG_USER_CTRL, user_ctrl);
|
|
|
|
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
|
|
|
|
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
|
2016-11-21 01:49:16 -04:00
|
|
|
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
|
2016-11-23 02:02:39 -04:00
|
|
|
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, true);
|
2016-11-21 01:49:16 -04:00
|
|
|
hal.scheduler->delay_microseconds(1);
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
2016-11-23 02:02:39 -04:00
|
|
|
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
|
2017-04-30 21:53:41 -03:00
|
|
|
|
|
|
|
notify_accel_fifo_reset(_accel_instance);
|
|
|
|
notify_gyro_fifo_reset(_gyro_instance);
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
|
2016-01-12 14:22:11 -04:00
|
|
|
{
|
2016-11-04 06:24:24 -03:00
|
|
|
return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_InertialSensor_Invensense::start()
|
2016-01-12 14:22:11 -04:00
|
|
|
{
|
2020-01-18 17:42:34 -04:00
|
|
|
_dev->get_semaphore()->take_blocking();
|
2015-08-05 13:29:35 -03:00
|
|
|
|
|
|
|
// initially run the bus at low speed
|
2016-01-12 14:22:11 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
2015-08-05 13:29:35 -03:00
|
|
|
|
|
|
|
// only used for wake-up in accelerometer only low power mode
|
|
|
|
_register_write(MPUREG_PWR_MGMT_2, 0x00);
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
2016-11-08 20:33:05 -04:00
|
|
|
// always use FIFO
|
2020-09-01 02:42:43 -03:00
|
|
|
_fifo_reset(false);
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
// grab the used instances
|
2016-12-13 21:47:22 -04:00
|
|
|
enum DevTypes gdev, adev;
|
|
|
|
switch (_mpu_type) {
|
|
|
|
case Invensense_MPU9250:
|
|
|
|
gdev = DEVTYPE_GYR_MPU9250;
|
|
|
|
adev = DEVTYPE_ACC_MPU9250;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
2016-12-13 21:47:22 -04:00
|
|
|
break;
|
2019-03-10 21:15:37 -03:00
|
|
|
case Invensense_ICM20602:
|
|
|
|
gdev = DEVTYPE_INS_ICM20602;
|
|
|
|
adev = DEVTYPE_INS_ICM20602;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
2019-03-10 21:15:37 -03:00
|
|
|
break;
|
2019-03-25 15:25:33 -03:00
|
|
|
case Invensense_ICM20601:
|
|
|
|
gdev = DEVTYPE_INS_ICM20601;
|
|
|
|
adev = DEVTYPE_INS_ICM20601;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
2019-03-25 15:25:33 -03:00
|
|
|
break;
|
2016-12-13 21:47:22 -04:00
|
|
|
case Invensense_ICM20608:
|
2021-02-23 19:46:23 -04:00
|
|
|
// unfortunately we don't have a separate ID for 20608, and we
|
|
|
|
// can't change this without breaking existing calibrations
|
2016-12-13 21:47:22 -04:00
|
|
|
gdev = DEVTYPE_GYR_MPU6000;
|
|
|
|
adev = DEVTYPE_ACC_MPU6000;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
2016-12-13 21:47:22 -04:00
|
|
|
break;
|
2017-03-09 19:43:54 -04:00
|
|
|
case Invensense_ICM20789:
|
|
|
|
gdev = DEVTYPE_INS_ICM20789;
|
|
|
|
adev = DEVTYPE_INS_ICM20789;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
2017-03-09 19:43:54 -04:00
|
|
|
break;
|
2018-04-10 08:08:52 -03:00
|
|
|
case Invensense_ICM20689:
|
|
|
|
gdev = DEVTYPE_INS_ICM20689;
|
|
|
|
adev = DEVTYPE_INS_ICM20689;
|
2021-02-23 19:46:23 -04:00
|
|
|
_enable_offset_checking = true;
|
|
|
|
break;
|
|
|
|
case Invensense_MPU6000:
|
|
|
|
case Invensense_MPU6500:
|
|
|
|
default:
|
|
|
|
gdev = DEVTYPE_GYR_MPU6000;
|
|
|
|
adev = DEVTYPE_ACC_MPU6000;
|
2018-04-10 08:08:52 -03:00
|
|
|
break;
|
2016-12-13 21:47:22 -04:00
|
|
|
}
|
2017-03-29 08:09:16 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
setup temperature sensitivity and offset. This varies
|
|
|
|
considerably between parts
|
|
|
|
*/
|
|
|
|
switch (_mpu_type) {
|
|
|
|
case Invensense_MPU9250:
|
2019-04-04 19:07:44 -03:00
|
|
|
temp_zero = 21.0f;
|
|
|
|
temp_sensitivity = 1.0f/340;
|
2017-03-29 08:09:16 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Invensense_MPU6000:
|
|
|
|
case Invensense_MPU6500:
|
2019-04-04 19:07:44 -03:00
|
|
|
temp_zero = 36.53f;
|
|
|
|
temp_sensitivity = 1.0f/340;
|
2017-03-29 08:09:16 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Invensense_ICM20608:
|
|
|
|
case Invensense_ICM20602:
|
2019-03-25 15:25:33 -03:00
|
|
|
case Invensense_ICM20601:
|
2019-04-04 19:07:44 -03:00
|
|
|
temp_zero = 25.0f;
|
|
|
|
temp_sensitivity = 1.0f/326.8f;
|
2017-03-29 08:09:16 -03:00
|
|
|
break;
|
2017-03-09 19:43:54 -04:00
|
|
|
|
|
|
|
case Invensense_ICM20789:
|
2019-04-04 19:07:44 -03:00
|
|
|
temp_zero = 25.0f;
|
|
|
|
temp_sensitivity = 0.003f;
|
2017-03-09 19:43:54 -04:00
|
|
|
break;
|
2018-04-10 08:08:52 -03:00
|
|
|
case Invensense_ICM20689:
|
2019-04-04 19:07:44 -03:00
|
|
|
temp_zero = 25.0f;
|
|
|
|
temp_sensitivity = 0.003f;
|
2018-04-10 08:08:52 -03:00
|
|
|
break;
|
2017-03-29 08:09:16 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
|
|
|
|
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
|
2016-11-21 01:49:16 -04:00
|
|
|
|
2016-11-08 23:07:31 -04:00
|
|
|
// setup ODR and on-sensor filtering
|
|
|
|
_set_filter_register();
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2018-01-17 03:17:33 -04:00
|
|
|
// update backend sample rate
|
2020-05-21 15:29:28 -03:00
|
|
|
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
|
|
|
|
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
|
2018-03-18 20:28:33 -03:00
|
|
|
|
|
|
|
// indicate what multiplier is appropriate for the sensors'
|
|
|
|
// readings to fit them into an int16_t:
|
|
|
|
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
|
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
// set sample rate to 1000Hz and apply a software filter
|
|
|
|
// In this configuration, the gyro sample rate is 8kHz
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(MPUREG_SMPLRT_DIV, 0, true);
|
2015-08-05 13:29:35 -03:00
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
// Gyro scale 2000º/s
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true);
|
2015-08-05 13:29:35 -03:00
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
|
|
|
// read the product ID rev c has 1/2 the sensitivity of rev d
|
2016-09-03 12:37:47 -03:00
|
|
|
uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
if (_mpu_type == Invensense_MPU6000 &&
|
2016-11-05 06:43:28 -03:00
|
|
|
((product_id == MPU6000ES_REV_C4) ||
|
|
|
|
(product_id == MPU6000ES_REV_C5) ||
|
|
|
|
(product_id == MPU6000_REV_C4) ||
|
|
|
|
(product_id == MPU6000_REV_C5))) {
|
2015-08-05 13:29:35 -03:00
|
|
|
// Accel scale 8g (4096 LSB/g)
|
|
|
|
// Rev C has different scaling than rev D
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
|
2016-08-08 12:38:11 -03:00
|
|
|
_accel_scale = GRAVITY_MSS / 4096.f;
|
2019-03-25 15:25:33 -03:00
|
|
|
_gyro_scale = (radians(1) / 16.4f);
|
|
|
|
} else if (_mpu_type == Invensense_ICM20601) {
|
|
|
|
// Accel scale 32g (4096 LSB/g)
|
|
|
|
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
|
|
|
|
_accel_scale = GRAVITY_MSS / 4096.f;
|
|
|
|
_gyro_scale = (radians(1) / 8.2f);
|
|
|
|
_clip_limit = 29.5f * GRAVITY_MSS;
|
2015-08-05 13:29:35 -03:00
|
|
|
} else {
|
2016-08-08 12:38:11 -03:00
|
|
|
// Accel scale 16g (2048 LSB/g)
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
|
2016-08-08 12:38:11 -03:00
|
|
|
_accel_scale = GRAVITY_MSS / 2048.f;
|
2019-03-25 15:25:33 -03:00
|
|
|
_gyro_scale = (radians(1) / 16.4f);
|
2015-08-05 13:29:35 -03:00
|
|
|
}
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
2019-03-25 15:25:33 -03:00
|
|
|
if (_mpu_type == Invensense_ICM20608 ||
|
|
|
|
_mpu_type == Invensense_ICM20602 ||
|
|
|
|
_mpu_type == Invensense_ICM20601) {
|
2016-11-05 06:43:28 -03:00
|
|
|
// this avoids a sensor bug, see description above
|
2019-03-25 15:25:33 -03:00
|
|
|
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
|
|
|
|
}
|
2016-11-05 06:43:28 -03:00
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
// configure interrupt to fire when new data arrives
|
|
|
|
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
2018-02-28 17:48:18 -04:00
|
|
|
// clear interrupt on any read, and hold the data ready pin high
|
|
|
|
// until we clear the interrupt. We don't do this for the 20789 as
|
|
|
|
// that sensor has already setup the appropriate config inside the
|
|
|
|
// baro driver.
|
|
|
|
if (_mpu_type != Invensense_ICM20789) {
|
|
|
|
uint8_t v = _register_read(MPUREG_INT_PIN_CFG) | BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN;
|
|
|
|
v &= BIT_BYPASS_EN;
|
|
|
|
_register_write(MPUREG_INT_PIN_CFG, v);
|
|
|
|
}
|
|
|
|
|
2021-02-23 19:46:23 -04:00
|
|
|
if (_enable_offset_checking) {
|
|
|
|
/*
|
|
|
|
there is a bug in at least the ICM-20602 where the
|
|
|
|
MPUREG_ACC_OFF_Y_H changes at runtime, which adds an offset
|
|
|
|
on the Y accelerometer. To prevent this we read the factory
|
|
|
|
cal values of the sensor at startup and write them back as
|
|
|
|
checked register values. Then we rely on the register
|
|
|
|
checking code to detect the change and fix it
|
|
|
|
*/
|
|
|
|
uint8_t regs[] = { MPUREG_ACC_OFF_X_H, MPUREG_ACC_OFF_X_L,
|
|
|
|
MPUREG_ACC_OFF_Y_H, MPUREG_ACC_OFF_Y_L,
|
|
|
|
MPUREG_ACC_OFF_Z_H, MPUREG_ACC_OFF_Z_L };
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
|
|
|
|
_register_write(regs[i], _register_read(regs[i]), true);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (_mpu_type == Invensense_ICM20602) {
|
|
|
|
/*
|
|
|
|
save y offset high separately for ICM20602 to quickly
|
|
|
|
recover from a change in this register. The ICM20602 has a
|
|
|
|
bug where every few hours it can change the factory Y offset
|
|
|
|
register, which leads to a sudden change in Y accelerometer
|
|
|
|
output
|
|
|
|
*/
|
|
|
|
_saved_y_ofs_high = _register_read(MPUREG_ACC_OFF_Y_H);
|
|
|
|
}
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
// now that we have initialised, we set the bus speed to high
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
_dev->get_semaphore()->give();
|
2013-01-10 18:12:19 -04:00
|
|
|
|
2016-11-03 06:19:04 -03:00
|
|
|
// setup sensor rotations from probe()
|
|
|
|
set_gyro_orientation(_gyro_instance, _rotation);
|
|
|
|
set_accel_orientation(_accel_instance, _rotation);
|
2016-11-08 20:33:05 -04:00
|
|
|
|
2018-04-09 23:16:45 -03:00
|
|
|
// setup scale factors for fifo data after downsampling
|
2020-05-21 15:29:28 -03:00
|
|
|
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
|
|
|
|
_fifo_gyro_scale = _gyro_scale / _gyro_fifo_downsample_rate;
|
2018-04-09 23:16:45 -03:00
|
|
|
|
2016-11-08 20:33:05 -04:00
|
|
|
// allocate fifo buffer
|
2018-01-09 17:43:32 -04:00
|
|
|
_fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
|
2016-11-08 20:33:05 -04:00
|
|
|
if (_fifo_buffer == nullptr) {
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
|
2020-05-21 15:29:28 -03:00
|
|
|
// start the timer process to read samples, using the fastest rate avilable
|
|
|
|
_dev->register_periodic_callback(1000000UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
|
2013-01-03 14:22:55 -04:00
|
|
|
}
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2020-05-21 15:29:28 -03:00
|
|
|
// get a startup banner to output to the GCS
|
|
|
|
bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) {
|
|
|
|
if (_fast_sampling) {
|
|
|
|
snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
|
2020-05-21 15:29:28 -03:00
|
|
|
_gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate / 1000.0, _gyro_backend_rate_hz / 1000.0);
|
2020-05-21 15:29:28 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
|
2014-10-14 01:48:33 -03:00
|
|
|
/*
|
2016-11-21 01:49:16 -04:00
|
|
|
publish any pending data
|
2014-10-14 01:48:33 -03:00
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::update()
|
2016-01-12 14:22:11 -04:00
|
|
|
{
|
2015-11-15 20:05:20 -04:00
|
|
|
update_accel(_accel_instance);
|
|
|
|
update_gyro(_gyro_instance);
|
2014-11-17 13:09:38 -04:00
|
|
|
|
2015-11-15 20:05:20 -04:00
|
|
|
_publish_temperature(_accel_instance, _temp_filtered);
|
2016-01-12 14:22:11 -04:00
|
|
|
|
2013-02-06 19:23:08 -04:00
|
|
|
return true;
|
2012-08-17 03:19:56 -03:00
|
|
|
}
|
2011-11-12 23:20:25 -04:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
/*
|
|
|
|
accumulate new samples
|
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_InertialSensor_Invensense::accumulate()
|
2016-11-21 01:49:16 -04:00
|
|
|
{
|
2016-11-23 05:33:55 -04:00
|
|
|
// nothing to do
|
2016-11-21 01:49:16 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AuxiliaryBus *AP_InertialSensor_Invensense::get_auxiliary_bus()
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
2016-01-12 14:22:11 -04:00
|
|
|
if (_auxiliary_bus) {
|
2015-10-02 15:02:16 -03:00
|
|
|
return _auxiliary_bus;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
if (_has_auxiliary_bus()) {
|
2016-12-13 21:47:22 -04:00
|
|
|
_auxiliary_bus = new AP_Invensense_AuxiliaryBus(*this, _dev->get_bus_id());
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2015-10-02 15:02:16 -03:00
|
|
|
return _auxiliary_bus;
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
/*
|
2016-12-13 21:47:22 -04:00
|
|
|
* Return true if the Invensense has new data available for reading.
|
2013-01-03 14:22:55 -04:00
|
|
|
*
|
|
|
|
* We use the data ready pin if it is available. Otherwise, read the
|
|
|
|
* status register.
|
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_data_ready()
|
2013-01-03 14:22:55 -04:00
|
|
|
{
|
|
|
|
if (_drdy_pin) {
|
|
|
|
return _drdy_pin->read() != 0;
|
2013-01-09 05:30:20 -04:00
|
|
|
}
|
2013-10-28 04:20:47 -03:00
|
|
|
uint8_t status = _register_read(MPUREG_INT_STATUS);
|
|
|
|
return (status & BIT_RAW_RDY_INT) != 0;
|
2013-01-03 14:22:55 -04:00
|
|
|
}
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
/*
|
2016-12-13 21:47:22 -04:00
|
|
|
* Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held
|
2013-01-03 14:22:55 -04:00
|
|
|
*/
|
2017-01-13 15:26:14 -04:00
|
|
|
void AP_InertialSensor_Invensense::_poll_data()
|
2013-01-03 14:22:55 -04:00
|
|
|
{
|
2016-11-08 20:33:05 -04:00
|
|
|
_read_fifo();
|
2021-02-11 00:01:38 -04:00
|
|
|
|
|
|
|
#if INVENSENSE_DEBUG_REG_CHANGE
|
2021-02-23 19:46:23 -04:00
|
|
|
_check_register_change();
|
|
|
|
#endif // INVENSENSE_DEBUG_REG_CHANGE
|
|
|
|
}
|
|
|
|
|
|
|
|
#if INVENSENSE_DEBUG_REG_CHANGE
|
|
|
|
/*
|
|
|
|
catch unexpected register changes on an IMU. This was used to
|
|
|
|
find the bug in the ICM-20602 that causes the Y accel offset
|
|
|
|
trim register to change value in flight
|
|
|
|
*/
|
|
|
|
void AP_InertialSensor_Invensense::_check_register_change(void)
|
|
|
|
{
|
2021-02-11 00:01:38 -04:00
|
|
|
if (_mpu_type != Invensense_ICM20602) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
static uint16_t counter;
|
|
|
|
if (++counter < 100) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
counter = 0;
|
|
|
|
static bool reg_init;
|
2021-02-23 19:46:23 -04:00
|
|
|
static uint8_t reg_value[0x80];
|
2021-02-11 00:01:38 -04:00
|
|
|
static uint8_t next_reg;
|
|
|
|
if (!reg_init) {
|
|
|
|
reg_init = true;
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(reg_value); i++) {
|
|
|
|
reg_value[i] = _register_read(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
bool skip = false;
|
|
|
|
if ((next_reg >= MPUREG_ACCEL_XOUT_H && next_reg <= MPUREG_GYRO_ZOUT_L) ||
|
|
|
|
next_reg == MPUREG_MEM_R_W || next_reg == MPUREG_FIFO_R_W ||
|
|
|
|
next_reg == MPUREG_INT_STATUS ||
|
|
|
|
next_reg == MPUREG_FIFO_COUNTH || next_reg == MPUREG_FIFO_COUNTL) {
|
|
|
|
skip = true;
|
|
|
|
}
|
|
|
|
if (!skip) {
|
|
|
|
uint8_t v = _register_read(next_reg);
|
|
|
|
if (v != reg_value[next_reg]) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "change[%02x] 0x%02x -> 0x%02x",
|
|
|
|
next_reg, reg_value[next_reg], v);
|
|
|
|
reg_value[next_reg] = v;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
next_reg = (next_reg+1) % ARRAY_SIZE(reg_value);
|
2013-01-03 14:22:55 -04:00
|
|
|
}
|
2021-02-23 19:46:23 -04:00
|
|
|
#endif // INVENSENSE_DEBUG_REG_CHANGE
|
2013-01-03 14:22:55 -04:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
|
2015-07-02 14:22:36 -03:00
|
|
|
{
|
2016-01-12 14:22:11 -04:00
|
|
|
for (uint8_t i = 0; i < n_samples; i++) {
|
2016-11-23 05:33:55 -04:00
|
|
|
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
2015-08-28 10:36:05 -03:00
|
|
|
Vector3f accel, gyro;
|
2016-08-31 01:56:27 -03:00
|
|
|
bool fsync_set = false;
|
2015-08-28 10:36:05 -03:00
|
|
|
|
2016-12-16 13:49:20 -04:00
|
|
|
#if INVENSENSE_EXT_SYNC_ENABLE
|
2016-08-31 01:56:27 -03:00
|
|
|
fsync_set = (int16_val(data, 2) & 1U) != 0;
|
|
|
|
#endif
|
|
|
|
|
2015-08-28 10:36:05 -03:00
|
|
|
accel = Vector3f(int16_val(data, 1),
|
|
|
|
int16_val(data, 0),
|
|
|
|
-int16_val(data, 2));
|
2016-08-08 12:38:11 -03:00
|
|
|
accel *= _accel_scale;
|
2015-08-28 10:36:05 -03:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
int16_t t2 = int16_val(data, 3);
|
2016-11-26 04:02:22 -04:00
|
|
|
if (!_check_raw_temp(t2)) {
|
2020-03-10 22:21:55 -03:00
|
|
|
if (!hal.scheduler->in_expected_delay()) {
|
|
|
|
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
|
|
|
}
|
2020-09-01 02:42:43 -03:00
|
|
|
_fifo_reset(true);
|
2016-11-21 01:49:16 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-03-29 08:09:16 -03:00
|
|
|
float temp = t2 * temp_sensitivity + temp_zero;
|
2016-11-09 01:16:52 -04:00
|
|
|
|
|
|
|
gyro = Vector3f(int16_val(data, 5),
|
|
|
|
int16_val(data, 4),
|
|
|
|
-int16_val(data, 6));
|
2019-03-25 15:25:33 -03:00
|
|
|
gyro *= _gyro_scale;
|
2014-10-16 19:24:08 -03:00
|
|
|
|
2015-08-28 10:36:05 -03:00
|
|
|
_rotate_and_correct_accel(_accel_instance, accel);
|
|
|
|
_rotate_and_correct_gyro(_gyro_instance, gyro);
|
|
|
|
|
2017-04-30 21:53:41 -03:00
|
|
|
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
|
2015-09-08 14:05:37 -03:00
|
|
|
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
|
2016-11-09 01:16:52 -04:00
|
|
|
|
|
|
|
_temp_filtered = _temp_filter.apply(temp);
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
return true;
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2015-08-27 16:05:13 -03:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
/*
|
|
|
|
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
|
|
|
|
|
|
|
|
To filter this we first apply a 1p low pass filter at 188Hz, then we
|
|
|
|
average over 8 samples to bring the data rate down to 1kHz. This
|
|
|
|
gives very good aliasing rejection at frequencies well above what
|
|
|
|
can be handled with 1kHz sample rates.
|
|
|
|
*/
|
2018-03-18 20:28:33 -03:00
|
|
|
bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
|
2016-11-08 20:33:05 -04:00
|
|
|
{
|
2016-11-21 01:49:16 -04:00
|
|
|
int32_t tsum = 0;
|
2019-03-11 09:56:40 -03:00
|
|
|
const int32_t unscaled_clip_limit = _clip_limit / _accel_scale;
|
2016-11-09 22:39:17 -04:00
|
|
|
bool clipped = false;
|
2016-11-21 01:49:16 -04:00
|
|
|
bool ret = true;
|
2016-11-09 22:39:17 -04:00
|
|
|
|
2016-11-08 20:33:05 -04:00
|
|
|
for (uint8_t i = 0; i < n_samples; i++) {
|
2016-11-23 05:33:55 -04:00
|
|
|
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
|
2016-11-21 01:49:16 -04:00
|
|
|
|
|
|
|
// use temperatue to detect FIFO corruption
|
|
|
|
int16_t t2 = int16_val(data, 3);
|
2016-11-26 04:02:22 -04:00
|
|
|
if (!_check_raw_temp(t2)) {
|
2020-03-10 22:21:55 -03:00
|
|
|
if (!hal.scheduler->in_expected_delay()) {
|
|
|
|
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
|
|
|
}
|
2020-09-01 02:42:43 -03:00
|
|
|
_fifo_reset(true);
|
2016-11-21 01:49:16 -04:00
|
|
|
ret = false;
|
|
|
|
break;
|
2016-11-09 22:39:17 -04:00
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
tsum += t2;
|
|
|
|
|
2020-05-21 15:29:28 -03:00
|
|
|
if (_accum.gyro_count % _gyro_to_accel_sample_ratio == 0) {
|
|
|
|
// accel data is at 4kHz or 1kHz
|
2016-11-21 01:49:16 -04:00
|
|
|
Vector3f a(int16_val(data, 1),
|
|
|
|
int16_val(data, 0),
|
|
|
|
-int16_val(data, 2));
|
2019-03-11 09:56:40 -03:00
|
|
|
if (fabsf(a.x) > unscaled_clip_limit ||
|
|
|
|
fabsf(a.y) > unscaled_clip_limit ||
|
|
|
|
fabsf(a.z) > unscaled_clip_limit) {
|
2016-11-21 01:49:16 -04:00
|
|
|
clipped = true;
|
|
|
|
}
|
|
|
|
_accum.accel += _accum.accel_filter.apply(a);
|
2020-05-21 15:29:28 -03:00
|
|
|
|
2018-03-07 04:09:15 -04:00
|
|
|
Vector3f a2 = a * _accel_scale;
|
2018-03-18 20:28:33 -03:00
|
|
|
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
|
2020-05-21 15:29:28 -03:00
|
|
|
|
|
|
|
_accum.accel_count++;
|
|
|
|
|
|
|
|
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
|
|
|
|
_accum.accel *= _fifo_accel_scale;
|
|
|
|
_rotate_and_correct_accel(_accel_instance, _accum.accel);
|
|
|
|
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
|
|
|
|
_accum.accel.zero();
|
|
|
|
_accum.accel_count = 0;
|
|
|
|
// we assume that the gyro rate is always >= and a multiple of the accel rate
|
|
|
|
_accum.gyro_count = 0;
|
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
}
|
|
|
|
|
2020-05-21 15:29:28 -03:00
|
|
|
_accum.gyro_count++;
|
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
Vector3f g(int16_val(data, 5),
|
2016-11-15 01:51:18 -04:00
|
|
|
int16_val(data, 4),
|
|
|
|
-int16_val(data, 6));
|
|
|
|
|
2019-03-25 15:25:33 -03:00
|
|
|
Vector3f g2 = g * _gyro_scale;
|
2018-03-18 20:28:33 -03:00
|
|
|
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
|
2018-03-07 04:09:15 -04:00
|
|
|
|
2020-07-23 17:14:42 -03:00
|
|
|
_accum.gyro += g;
|
2016-11-23 05:33:55 -04:00
|
|
|
|
2020-05-21 15:29:28 -03:00
|
|
|
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
|
2018-04-09 23:16:45 -03:00
|
|
|
_accum.gyro *= _fifo_gyro_scale;
|
2016-11-23 05:33:55 -04:00
|
|
|
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
|
|
|
|
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
|
|
|
|
_accum.gyro.zero();
|
|
|
|
}
|
2015-07-02 14:22:36 -03:00
|
|
|
}
|
2016-11-08 20:33:05 -04:00
|
|
|
|
2016-11-09 22:39:17 -04:00
|
|
|
if (clipped) {
|
|
|
|
increment_clip_count(_accel_instance);
|
|
|
|
}
|
2016-11-08 21:11:17 -04:00
|
|
|
|
2016-11-23 05:33:55 -04:00
|
|
|
if (ret) {
|
2017-03-29 08:09:16 -03:00
|
|
|
float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
|
2016-11-23 05:33:55 -04:00
|
|
|
_temp_filtered = _temp_filter.apply(temp);
|
2016-11-15 01:51:18 -04:00
|
|
|
}
|
2016-11-23 05:33:55 -04:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
return ret;
|
2016-11-09 04:53:55 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_InertialSensor_Invensense::_read_fifo()
|
2015-07-02 14:22:36 -03:00
|
|
|
{
|
|
|
|
uint8_t n_samples;
|
2016-01-12 14:22:11 -04:00
|
|
|
uint16_t bytes_read;
|
2016-11-08 20:33:05 -04:00
|
|
|
uint8_t *rx = _fifo_buffer;
|
2016-11-21 01:49:16 -04:00
|
|
|
bool need_reset = false;
|
2016-01-12 14:22:11 -04:00
|
|
|
|
|
|
|
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
|
2016-11-10 02:27:22 -04:00
|
|
|
goto check_registers;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
bytes_read = uint16_val(rx, 0);
|
2016-11-23 05:33:55 -04:00
|
|
|
n_samples = bytes_read / MPU_SAMPLE_SIZE;
|
2016-01-12 14:22:11 -04:00
|
|
|
|
|
|
|
if (n_samples == 0) {
|
|
|
|
/* Not enough data in FIFO */
|
2016-11-10 02:27:22 -04:00
|
|
|
goto check_registers;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-07-02 14:22:36 -03:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
/*
|
|
|
|
testing has shown that if we have more than 32 samples in the
|
|
|
|
FIFO then some of those samples will be corrupt. It always is
|
|
|
|
the ones at the end of the FIFO, so clear those with a reset
|
|
|
|
once we've read the first 24. Reading 24 gives us the normal
|
|
|
|
number of samples for fast sampling at 400Hz
|
2018-01-31 04:11:12 -04:00
|
|
|
|
|
|
|
On I2C with the much lower clock rates we need a lower threshold
|
|
|
|
or we may never catch up
|
2016-11-21 01:49:16 -04:00
|
|
|
*/
|
2018-01-31 04:11:12 -04:00
|
|
|
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) {
|
|
|
|
if (n_samples > 4) {
|
|
|
|
need_reset = true;
|
|
|
|
n_samples = 4;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (n_samples > 32) {
|
|
|
|
need_reset = true;
|
|
|
|
n_samples = 24;
|
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
}
|
|
|
|
|
2016-11-12 01:43:29 -04:00
|
|
|
while (n_samples > 0) {
|
2016-11-23 18:06:42 -04:00
|
|
|
uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
|
2016-11-26 19:39:55 -04:00
|
|
|
if (!_dev->set_chip_select(true)) {
|
|
|
|
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
|
|
|
|
goto check_registers;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// this ensures we keep things nicely setup for DMA
|
|
|
|
uint8_t reg = MPUREG_FIFO_R_W | 0x80;
|
|
|
|
if (!_dev->transfer(®, 1, nullptr, 0)) {
|
|
|
|
_dev->set_chip_select(false);
|
|
|
|
goto check_registers;
|
|
|
|
}
|
|
|
|
memset(rx, 0, n * MPU_SAMPLE_SIZE);
|
|
|
|
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
|
2020-03-10 22:21:55 -03:00
|
|
|
if (!hal.scheduler->in_expected_delay()) {
|
|
|
|
debug("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
|
|
|
|
}
|
2016-11-26 19:39:55 -04:00
|
|
|
_dev->set_chip_select(false);
|
|
|
|
goto check_registers;
|
|
|
|
}
|
|
|
|
_dev->set_chip_select(false);
|
2016-11-12 01:43:29 -04:00
|
|
|
}
|
2016-01-12 14:22:11 -04:00
|
|
|
|
2016-11-12 01:43:29 -04:00
|
|
|
if (_fast_sampling) {
|
2018-03-18 20:28:33 -03:00
|
|
|
if (!_accumulate_sensor_rate_sampling(rx, n)) {
|
2020-03-10 22:21:55 -03:00
|
|
|
if (!hal.scheduler->in_expected_delay()) {
|
|
|
|
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
|
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
break;
|
|
|
|
}
|
2016-11-12 01:43:29 -04:00
|
|
|
} else {
|
2016-11-26 04:02:22 -04:00
|
|
|
if (!_accumulate(rx, n)) {
|
2016-11-21 01:49:16 -04:00
|
|
|
break;
|
|
|
|
}
|
2016-11-12 01:43:29 -04:00
|
|
|
}
|
|
|
|
n_samples -= n;
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2016-11-09 04:53:55 -04:00
|
|
|
|
2016-11-21 01:49:16 -04:00
|
|
|
if (need_reset) {
|
2016-11-23 05:33:55 -04:00
|
|
|
//debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE);
|
2020-09-01 02:42:43 -03:00
|
|
|
_fifo_reset(false);
|
2016-11-18 21:53:25 -04:00
|
|
|
}
|
|
|
|
|
2016-11-10 02:27:22 -04:00
|
|
|
check_registers:
|
2016-11-25 04:55:13 -04:00
|
|
|
// check next register value for correctness
|
2021-02-23 19:46:23 -04:00
|
|
|
|
|
|
|
if (_mpu_type == Invensense_ICM20602) {
|
|
|
|
const uint8_t y_ofs = _register_read(MPUREG_ACC_OFF_Y_H);
|
|
|
|
if (y_ofs != _saved_y_ofs_high) {
|
|
|
|
/*
|
|
|
|
we check and restore the ICM20602 Y offset high register
|
|
|
|
on every update. We don't mark the IMU unhealthy when we
|
|
|
|
do this. This is a workaround for a bug in the ICM-20602
|
|
|
|
where this register can change in flight. We log these
|
|
|
|
events to help with log analysis, but don't shout at the
|
|
|
|
GCS to prevent possible flood
|
|
|
|
*/
|
|
|
|
AP::logger().Write_MessageF("ICM20602 yofs fix: %x %x", y_ofs, _saved_y_ofs_high);
|
|
|
|
_register_write(MPUREG_ACC_OFF_Y_H, _saved_y_ofs_high);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2016-11-25 04:55:13 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
2021-02-23 19:32:39 -04:00
|
|
|
AP_HAL::Device::checkreg reg;
|
|
|
|
if (!_dev->check_next_register(reg)) {
|
|
|
|
log_register_change(_dev->get_bus_id(), reg);
|
2016-11-25 04:55:13 -04:00
|
|
|
_inc_gyro_error_count(_gyro_instance);
|
|
|
|
_inc_accel_error_count(_accel_instance);
|
2016-11-10 02:27:22 -04:00
|
|
|
}
|
2016-11-25 04:55:13 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
2013-01-03 15:48:01 -04:00
|
|
|
}
|
2012-10-11 21:27:19 -03:00
|
|
|
|
2016-11-26 04:02:22 -04:00
|
|
|
/*
|
|
|
|
fetch temperature in order to detect FIFO sync errors
|
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_check_raw_temp(int16_t t2)
|
2016-11-26 04:02:22 -04:00
|
|
|
{
|
|
|
|
if (abs(t2 - _raw_temp) < 400) {
|
|
|
|
// cached copy OK
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
uint8_t trx[2];
|
|
|
|
if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
|
|
|
|
_raw_temp = int16_val(trx, 0);
|
|
|
|
}
|
2019-02-16 23:30:07 -04:00
|
|
|
return (abs(t2 - _raw_temp) < 800);
|
2016-11-26 04:02:22 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_block_read(uint8_t reg, uint8_t *buf,
|
2015-08-16 16:06:41 -03:00
|
|
|
uint32_t size)
|
|
|
|
{
|
2016-01-12 14:22:11 -04:00
|
|
|
return _dev->read_registers(reg, buf, size);
|
2015-08-16 16:06:41 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
uint8_t AP_InertialSensor_Invensense::_register_read(uint8_t reg)
|
2011-11-12 23:20:25 -04:00
|
|
|
{
|
2016-01-12 14:22:11 -04:00
|
|
|
uint8_t val = 0;
|
|
|
|
_dev->read_registers(reg, &val, 1);
|
2015-06-05 04:47:31 -03:00
|
|
|
return val;
|
2011-11-12 23:20:25 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_InertialSensor_Invensense::_register_write(uint8_t reg, uint8_t val, bool checked)
|
2011-11-12 23:20:25 -04:00
|
|
|
{
|
2016-11-10 02:27:22 -04:00
|
|
|
_dev->write_register(reg, val, checked);
|
2011-11-12 23:20:25 -04:00
|
|
|
}
|
|
|
|
|
2013-02-06 19:23:08 -04:00
|
|
|
/*
|
|
|
|
set the DLPF filter frequency. Assumes caller has taken semaphore
|
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_InertialSensor_Invensense::_set_filter_register(void)
|
2013-02-06 19:23:08 -04:00
|
|
|
{
|
2016-11-08 23:07:31 -04:00
|
|
|
uint8_t config;
|
2016-08-31 01:56:27 -03:00
|
|
|
|
2016-12-16 13:49:20 -04:00
|
|
|
#if INVENSENSE_EXT_SYNC_ENABLE
|
2016-08-31 01:56:27 -03:00
|
|
|
// add in EXT_SYNC bit if enabled
|
2016-11-08 23:07:31 -04:00
|
|
|
config = (MPUREG_CONFIG_EXT_SYNC_AZ << MPUREG_CONFIG_EXT_SYNC_SHIFT);
|
|
|
|
#else
|
|
|
|
config = 0;
|
2016-08-31 01:56:27 -03:00
|
|
|
#endif
|
|
|
|
|
2018-01-17 03:17:33 -04:00
|
|
|
// assume 1kHz sampling to start
|
2020-05-21 15:29:28 -03:00
|
|
|
_gyro_fifo_downsample_rate = _accel_fifo_downsample_rate = 1;
|
|
|
|
_gyro_to_accel_sample_ratio = 2;
|
|
|
|
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1000;
|
2018-01-17 03:17:33 -04:00
|
|
|
|
2016-11-15 01:51:18 -04:00
|
|
|
if (enable_fast_sampling(_accel_instance)) {
|
2020-05-21 15:29:28 -03:00
|
|
|
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
|
2016-11-21 01:49:16 -04:00
|
|
|
if (_fast_sampling) {
|
2020-05-21 15:29:28 -03:00
|
|
|
// constrain the gyro rate to be at least the loop rate
|
|
|
|
uint8_t loop_limit = 1;
|
|
|
|
if (get_loop_rate_hz() > 1000) {
|
|
|
|
loop_limit = 2;
|
|
|
|
}
|
|
|
|
if (get_loop_rate_hz() > 2000) {
|
|
|
|
loop_limit = 4;
|
2018-01-17 03:17:33 -04:00
|
|
|
}
|
2020-05-21 15:29:28 -03:00
|
|
|
// constrain the gyro rate to be a 2^N multiple
|
2020-05-21 15:29:28 -03:00
|
|
|
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
|
2020-05-21 15:29:28 -03:00
|
|
|
|
|
|
|
// calculate rate we will be giving gyro samples to the backend
|
2020-05-21 15:29:28 -03:00
|
|
|
_gyro_fifo_downsample_rate = 8 / fast_sampling_rate;
|
|
|
|
_gyro_backend_rate_hz *= fast_sampling_rate;
|
|
|
|
|
|
|
|
// calculate rate we will be giving accel samples to the backend
|
|
|
|
if (_mpu_type >= Invensense_MPU9250) {
|
|
|
|
_accel_fifo_downsample_rate = MAX(4 / fast_sampling_rate, 1);
|
|
|
|
_accel_backend_rate_hz *= MIN(fast_sampling_rate, 4);
|
|
|
|
} else {
|
|
|
|
_gyro_to_accel_sample_ratio = 8;
|
|
|
|
_accel_fifo_downsample_rate = 1;
|
|
|
|
_accum.accel_filter.set_cutoff_frequency(1000, 188);
|
|
|
|
}
|
2018-04-09 23:16:45 -03:00
|
|
|
|
2017-05-01 00:01:43 -03:00
|
|
|
// for logging purposes set the oversamping rate
|
2020-05-21 15:29:28 -03:00
|
|
|
_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
|
|
|
|
_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
|
2017-05-01 21:59:52 -03:00
|
|
|
|
2018-03-18 20:28:33 -03:00
|
|
|
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
|
|
|
|
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
|
|
|
|
|
2017-05-01 21:59:52 -03:00
|
|
|
/* set divider for internal sample rate to 0x1F when fast
|
|
|
|
sampling enabled. This reduces the impact of the slave
|
|
|
|
sensor on the sample rate. It ends up with around 75Hz
|
|
|
|
slave rate, and reduces the impact on the gyro and accel
|
|
|
|
sample rate, ending up with around 7760Hz gyro rate and
|
|
|
|
3880Hz accel rate
|
|
|
|
*/
|
|
|
|
_register_write(MPUREG_I2C_SLV4_CTRL, 0x1F);
|
2016-11-21 01:49:16 -04:00
|
|
|
}
|
2016-11-15 01:51:18 -04:00
|
|
|
}
|
2016-11-14 03:17:25 -04:00
|
|
|
|
|
|
|
if (_fast_sampling) {
|
2016-11-08 23:07:31 -04:00
|
|
|
// this gives us 8kHz sampling on gyros and 4kHz on accels
|
|
|
|
config |= BITS_DLPF_CFG_256HZ_NOLPF2;
|
|
|
|
} else {
|
|
|
|
// limit to 1kHz if not on SPI
|
|
|
|
config |= BITS_DLPF_CFG_188HZ;
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2016-11-21 01:49:16 -04:00
|
|
|
|
|
|
|
config |= MPUREG_CONFIG_FIFO_MODE_STOP;
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(MPUREG_CONFIG, config, true);
|
2016-11-08 20:33:05 -04:00
|
|
|
|
2019-03-25 15:25:33 -03:00
|
|
|
if (_mpu_type != Invensense_MPU6000) {
|
2016-11-08 23:07:31 -04:00
|
|
|
if (_fast_sampling) {
|
|
|
|
// setup for 4kHz accels
|
2016-11-10 02:27:22 -04:00
|
|
|
_register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_FCHOICE_B, true);
|
2016-11-08 23:07:31 -04:00
|
|
|
} else {
|
2018-04-10 08:08:52 -03:00
|
|
|
uint8_t fifo_size = (_mpu_type == Invensense_ICM20789 || _mpu_type == Invensense_ICM20689) ? 1:0;
|
2017-03-09 19:43:54 -04:00
|
|
|
_register_write(ICMREG_ACCEL_CONFIG2, ICM_ACC_DLPF_CFG_218HZ | (fifo_size<<6), true);
|
2016-11-08 23:07:31 -04:00
|
|
|
}
|
2016-11-08 20:33:05 -04:00
|
|
|
}
|
2013-02-06 19:23:08 -04:00
|
|
|
}
|
|
|
|
|
2016-11-05 06:43:28 -03:00
|
|
|
/*
|
2016-12-13 21:47:22 -04:00
|
|
|
check whoami for sensor type
|
2016-11-05 06:43:28 -03:00
|
|
|
*/
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_check_whoami(void)
|
2016-11-05 06:43:28 -03:00
|
|
|
{
|
|
|
|
uint8_t whoami = _register_read(MPUREG_WHOAMI);
|
|
|
|
switch (whoami) {
|
|
|
|
case MPU_WHOAMI_6000:
|
2016-12-13 21:47:22 -04:00
|
|
|
_mpu_type = Invensense_MPU6000;
|
2017-01-04 20:39:26 -04:00
|
|
|
return true;
|
2017-02-06 19:47:39 -04:00
|
|
|
case MPU_WHOAMI_6500:
|
|
|
|
_mpu_type = Invensense_MPU6500;
|
|
|
|
return true;
|
2016-12-13 21:47:22 -04:00
|
|
|
case MPU_WHOAMI_MPU9250:
|
|
|
|
case MPU_WHOAMI_MPU9255:
|
|
|
|
_mpu_type = Invensense_MPU9250;
|
2016-11-05 06:43:28 -03:00
|
|
|
return true;
|
2016-12-13 21:47:22 -04:00
|
|
|
case MPU_WHOAMI_20608:
|
|
|
|
_mpu_type = Invensense_ICM20608;
|
2016-11-05 06:43:28 -03:00
|
|
|
return true;
|
2017-03-01 20:52:03 -04:00
|
|
|
case MPU_WHOAMI_20602:
|
|
|
|
_mpu_type = Invensense_ICM20602;
|
|
|
|
return true;
|
2019-03-25 15:25:33 -03:00
|
|
|
case MPU_WHOAMI_20601:
|
|
|
|
_mpu_type = Invensense_ICM20601;
|
|
|
|
return true;
|
2017-03-09 19:43:54 -04:00
|
|
|
case MPU_WHOAMI_ICM20789:
|
2018-01-31 04:10:46 -04:00
|
|
|
case MPU_WHOAMI_ICM20789_R1:
|
2017-03-09 19:43:54 -04:00
|
|
|
_mpu_type = Invensense_ICM20789;
|
|
|
|
return true;
|
2018-04-10 08:08:52 -03:00
|
|
|
case MPU_WHOAMI_ICM20689:
|
|
|
|
_mpu_type = Invensense_ICM20689;
|
|
|
|
return true;
|
2016-11-05 06:43:28 -03:00
|
|
|
}
|
|
|
|
// not a value WHOAMI result
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2013-02-06 19:23:08 -04:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
bool AP_InertialSensor_Invensense::_hardware_init(void)
|
2011-11-12 23:20:25 -04:00
|
|
|
{
|
2020-01-18 17:42:34 -04:00
|
|
|
_dev->get_semaphore()->take_blocking();
|
2013-01-09 05:30:20 -04:00
|
|
|
|
2018-01-31 04:11:12 -04:00
|
|
|
// setup for register checking. We check much less often on I2C
|
|
|
|
// where the cost of the checks is higher
|
2021-02-23 19:46:23 -04:00
|
|
|
_dev->setup_checked_registers(13, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
|
2016-11-10 02:27:22 -04:00
|
|
|
|
2015-11-03 09:46:29 -04:00
|
|
|
// initially run the bus at low speed
|
2016-01-12 14:22:11 -04:00
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
2013-10-11 04:02:17 -03:00
|
|
|
|
2016-11-05 06:43:28 -03:00
|
|
|
if (!_check_whoami()) {
|
|
|
|
_dev->get_semaphore()->give();
|
|
|
|
return false;
|
|
|
|
}
|
2016-11-23 02:02:39 -04:00
|
|
|
|
2011-11-12 23:20:25 -04:00
|
|
|
// Chip reset
|
2012-12-27 06:28:41 -04:00
|
|
|
uint8_t tries;
|
2016-01-12 14:22:11 -04:00
|
|
|
for (tries = 0; tries < 5; tries++) {
|
2016-11-23 02:02:39 -04:00
|
|
|
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* First disable the master I2C to avoid hanging the slaves on the
|
|
|
|
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
|
|
|
|
* is used */
|
2016-11-23 02:02:39 -04:00
|
|
|
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
|
|
|
|
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
|
|
|
|
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
|
2015-08-16 16:23:24 -03:00
|
|
|
hal.scheduler->delay(10);
|
|
|
|
}
|
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
/* reset device */
|
2013-10-29 03:42:35 -03:00
|
|
|
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
|
2012-12-27 06:28:41 -04:00
|
|
|
hal.scheduler->delay(100);
|
|
|
|
|
2016-01-12 14:22:11 -04:00
|
|
|
/* bus-dependent initialization */
|
2016-11-04 06:24:24 -03:00
|
|
|
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
2016-01-12 14:22:11 -04:00
|
|
|
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
|
|
|
|
* done just after the device is reset) */
|
2016-11-23 02:02:39 -04:00
|
|
|
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
|
|
|
|
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-08-05 13:29:35 -03:00
|
|
|
|
2016-12-29 10:42:13 -04:00
|
|
|
/* bus-dependent initialization */
|
2018-01-01 21:50:00 -04:00
|
|
|
if ((_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) && (_mpu_type == Invensense_MPU9250 || _mpu_type == Invensense_ICM20789)) {
|
|
|
|
/* Enable I2C bypass to access internal device */
|
|
|
|
_register_write(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
|
2016-12-29 10:42:13 -04:00
|
|
|
}
|
|
|
|
|
2017-03-09 19:43:54 -04:00
|
|
|
|
2012-12-27 06:28:41 -04:00
|
|
|
// Wake up device and select GyroZ clock. Note that the
|
2016-12-13 21:47:22 -04:00
|
|
|
// Invensense starts up in sleep mode, and it can take some time
|
2012-12-27 06:28:41 -04:00
|
|
|
// for it to come out of sleep
|
2013-10-29 03:42:35 -03:00
|
|
|
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
|
2012-12-27 06:28:41 -04:00
|
|
|
hal.scheduler->delay(5);
|
|
|
|
|
|
|
|
// check it has woken up
|
2016-01-12 14:22:11 -04:00
|
|
|
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
|
2012-12-27 06:28:41 -04:00
|
|
|
break;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2015-07-02 14:22:36 -03:00
|
|
|
|
2015-08-05 13:29:35 -03:00
|
|
|
hal.scheduler->delay(10);
|
2016-01-12 14:22:11 -04:00
|
|
|
if (_data_ready()) {
|
2015-08-05 13:29:35 -03:00
|
|
|
break;
|
2016-01-12 14:22:11 -04:00
|
|
|
}
|
2012-12-27 06:28:41 -04:00
|
|
|
}
|
2016-01-12 14:22:11 -04:00
|
|
|
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
|
|
|
|
2012-12-27 06:28:41 -04:00
|
|
|
if (tries == 5) {
|
2017-01-21 00:39:09 -04:00
|
|
|
hal.console->printf("Failed to boot Invensense 5 times\n");
|
2018-02-06 02:44:22 -04:00
|
|
|
_dev->get_semaphore()->give();
|
2016-01-12 14:22:11 -04:00
|
|
|
return false;
|
2012-12-27 06:28:41 -04:00
|
|
|
}
|
2012-07-28 02:14:43 -03:00
|
|
|
|
2019-03-25 15:25:33 -03:00
|
|
|
if (_mpu_type == Invensense_ICM20608 ||
|
|
|
|
_mpu_type == Invensense_ICM20602 ||
|
|
|
|
_mpu_type == Invensense_ICM20601) {
|
2016-11-05 06:43:28 -03:00
|
|
|
// this avoids a sensor bug, see description above
|
2019-03-25 15:25:33 -03:00
|
|
|
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
|
|
|
|
}
|
2018-02-06 02:44:22 -04:00
|
|
|
_dev->get_semaphore()->give();
|
2016-11-05 06:43:28 -03:00
|
|
|
|
2013-01-10 18:12:19 -04:00
|
|
|
return true;
|
2011-11-12 23:20:25 -04:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_Invensense_AuxiliaryBusSlave::AP_Invensense_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
2015-08-16 16:23:24 -03:00
|
|
|
uint8_t instance)
|
|
|
|
: AuxiliaryBusSlave(bus, addr, instance)
|
2016-12-13 21:47:22 -04:00
|
|
|
, _mpu_addr(MPUREG_I2C_SLV0_ADDR + _instance * 3)
|
|
|
|
, _mpu_reg(_mpu_addr + 1)
|
|
|
|
, _mpu_ctrl(_mpu_addr + 2)
|
|
|
|
, _mpu_do(MPUREG_I2C_SLV0_DO + _instance)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
int AP_Invensense_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
2015-08-16 16:23:24 -03:00
|
|
|
uint8_t *out)
|
|
|
|
{
|
2016-12-13 21:47:22 -04:00
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
2015-08-16 16:23:24 -03:00
|
|
|
uint8_t addr;
|
|
|
|
|
|
|
|
/* Ensure the slave read/write is disabled before changing the registers */
|
2016-12-13 21:47:22 -04:00
|
|
|
backend._register_write(_mpu_ctrl, 0);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
if (out) {
|
2016-12-13 21:47:22 -04:00
|
|
|
backend._register_write(_mpu_do, *out);
|
2015-08-16 16:23:24 -03:00
|
|
|
addr = _addr;
|
|
|
|
} else {
|
|
|
|
addr = _addr | BIT_READ_FLAG;
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
backend._register_write(_mpu_addr, addr);
|
|
|
|
backend._register_write(_mpu_reg, reg);
|
|
|
|
backend._register_write(_mpu_ctrl, BIT_I2C_SLVX_EN | size);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
int AP_Invensense_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
2016-01-20 18:12:15 -04:00
|
|
|
uint8_t size)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
|
|
|
assert(buf);
|
|
|
|
|
|
|
|
if (_registered) {
|
2017-01-21 00:39:09 -04:00
|
|
|
hal.console->printf("Error: can't passthrough when slave is already configured\n");
|
2015-08-16 16:23:24 -03:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
int r = _set_passthrough(reg, size);
|
2016-01-20 18:12:15 -04:00
|
|
|
if (r < 0) {
|
2015-08-16 16:23:24 -03:00
|
|
|
return r;
|
2016-01-20 18:12:15 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* wait the value to be read from the slave and read it back */
|
|
|
|
hal.scheduler->delay(10);
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
2016-08-02 17:52:44 -03:00
|
|
|
if (!backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size)) {
|
|
|
|
return -1;
|
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* disable new reads */
|
2016-12-13 21:47:22 -04:00
|
|
|
backend._register_write(_mpu_ctrl, 0);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
return size;
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
int AP_Invensense_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
|
|
|
if (_registered) {
|
2017-01-21 00:39:09 -04:00
|
|
|
hal.console->printf("Error: can't passthrough when slave is already configured\n");
|
2015-08-16 16:23:24 -03:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
int r = _set_passthrough(reg, 1, &val);
|
2016-01-20 18:12:15 -04:00
|
|
|
if (r < 0) {
|
2015-08-16 16:23:24 -03:00
|
|
|
return r;
|
2016-01-20 18:12:15 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* wait the value to be written to the slave */
|
|
|
|
hal.scheduler->delay(10);
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* disable new writes */
|
2016-12-13 21:47:22 -04:00
|
|
|
backend._register_write(_mpu_ctrl, 0);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2016-03-03 17:22:26 -04:00
|
|
|
return 1;
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
int AP_Invensense_AuxiliaryBusSlave::read(uint8_t *buf)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
|
|
|
if (!_registered) {
|
2017-01-21 00:39:09 -04:00
|
|
|
hal.console->printf("Error: can't read before configuring slave\n");
|
2015-08-16 16:23:24 -03:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_bus.get_backend());
|
2016-03-03 17:22:26 -04:00
|
|
|
if (!backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size)) {
|
|
|
|
return -1;
|
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2016-03-03 17:22:26 -04:00
|
|
|
return _sample_size;
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
/* Invensense provides up to 5 slave devices, but the 5th is way too different to
|
2015-08-16 16:23:24 -03:00
|
|
|
* configure and is seldom used */
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_Invensense_AuxiliaryBus::AP_Invensense_AuxiliaryBus(AP_InertialSensor_Invensense &backend, uint32_t devid)
|
2016-11-04 06:24:24 -03:00
|
|
|
: AuxiliaryBus(backend, 4, devid)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_HAL::Semaphore *AP_Invensense_AuxiliaryBus::get_semaphore()
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
2016-12-13 21:47:22 -04:00
|
|
|
return static_cast<AP_InertialSensor_Invensense&>(_ins_backend)._dev->get_semaphore();
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AuxiliaryBusSlave *AP_Invensense_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
2016-12-13 21:47:22 -04:00
|
|
|
/* Enable slaves on Invensense if this is the first time */
|
2016-01-20 18:12:15 -04:00
|
|
|
if (_ext_sens_data == 0) {
|
2015-08-16 16:23:24 -03:00
|
|
|
_configure_slaves();
|
2016-01-20 18:12:15 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
return new AP_Invensense_AuxiliaryBusSlave(*this, addr, instance);
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
void AP_Invensense_AuxiliaryBus::_configure_slaves()
|
2015-08-16 16:23:24 -03:00
|
|
|
{
|
2016-12-13 21:47:22 -04:00
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_ins_backend);
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2017-03-09 19:43:54 -04:00
|
|
|
if (backend._mpu_type == AP_InertialSensor_Invensense::Invensense_ICM20789) {
|
|
|
|
// on 20789 we can't enable slaves if we want to be able to use the baro
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2020-01-18 17:42:34 -04:00
|
|
|
backend._dev->get_semaphore()->take_blocking();
|
|
|
|
|
2016-01-20 18:12:15 -04:00
|
|
|
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
|
2016-11-23 02:02:39 -04:00
|
|
|
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
|
|
|
|
backend._last_stat_user_ctrl |= BIT_USER_CTRL_I2C_MST_EN;
|
|
|
|
backend._register_write(MPUREG_USER_CTRL, backend._last_stat_user_ctrl);
|
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
|
|
|
/* stop condition between reads; clock at 400kHz */
|
|
|
|
backend._register_write(MPUREG_I2C_MST_CTRL,
|
|
|
|
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);
|
|
|
|
|
|
|
|
/* Hard-code divider for internal sample rate, 1 kHz, resulting in a
|
|
|
|
* sample rate of 100Hz */
|
|
|
|
backend._register_write(MPUREG_I2C_SLV4_CTRL, 9);
|
|
|
|
|
|
|
|
/* All slaves are subject to the sample rate */
|
|
|
|
backend._register_write(MPUREG_I2C_MST_DELAY_CTRL,
|
|
|
|
BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN |
|
|
|
|
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
|
2018-02-06 02:54:19 -04:00
|
|
|
|
|
|
|
backend._dev->get_semaphore()->give();
|
2015-08-16 16:23:24 -03:00
|
|
|
}
|
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
|
2015-08-16 16:23:24 -03:00
|
|
|
uint8_t reg, uint8_t size)
|
|
|
|
{
|
2016-01-20 18:12:15 -04:00
|
|
|
if (_ext_sens_data + size > MAX_EXT_SENS_DATA) {
|
2015-08-16 16:23:24 -03:00
|
|
|
return -1;
|
2016-01-20 18:12:15 -04:00
|
|
|
}
|
2015-08-16 16:23:24 -03:00
|
|
|
|
2016-12-13 21:47:22 -04:00
|
|
|
AP_Invensense_AuxiliaryBusSlave *mpu_slave =
|
|
|
|
static_cast<AP_Invensense_AuxiliaryBusSlave*>(slave);
|
2015-08-16 16:23:24 -03:00
|
|
|
mpu_slave->_set_passthrough(reg, size);
|
|
|
|
mpu_slave->_ext_sens_data = _ext_sens_data;
|
|
|
|
_ext_sens_data += size;
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
2017-08-02 13:08:09 -03:00
|
|
|
|
|
|
|
AP_HAL::Device::PeriodicHandle AP_Invensense_AuxiliaryBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
|
|
|
{
|
|
|
|
auto &backend = AP_InertialSensor_Invensense::from(_ins_backend);
|
|
|
|
return backend._dev->register_periodic_callback(period_usec, cb);
|
|
|
|
}
|