2021-12-28 13:43:48 -04:00
|
|
|
/*
|
|
|
|
* 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_HAL/utility/sparse-endian.h>
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
|
|
|
|
#include "AP_InertialSensor_BMI270.h"
|
|
|
|
|
|
|
|
//#define BMI270_DEBUG
|
|
|
|
|
|
|
|
// BMI270 registers (not the complete list)
|
|
|
|
enum BMI270Register : uint8_t {
|
|
|
|
BMI270_REG_CHIP_ID = 0x00,
|
|
|
|
BMI270_REG_ERR_REG = 0x02,
|
|
|
|
BMI270_REG_STATUS = 0x03,
|
|
|
|
BMI270_REG_ACC_DATA_X_LSB = 0x0C,
|
|
|
|
BMI270_REG_GYR_DATA_X_LSB = 0x12,
|
|
|
|
BMI270_REG_SENSORTIME_0 = 0x18,
|
|
|
|
BMI270_REG_SENSORTIME_1 = 0x19,
|
|
|
|
BMI270_REG_SENSORTIME_2 = 0x1A,
|
|
|
|
BMI270_REG_EVENT = 0x1B,
|
|
|
|
BMI270_REG_INT_STATUS_0 = 0x1C,
|
|
|
|
BMI270_REG_INT_STATUS_1 = 0x1D,
|
|
|
|
BMI270_REG_INTERNAL_STATUS = 0x21,
|
|
|
|
BMI270_REG_TEMPERATURE_LSB = 0x22,
|
|
|
|
BMI270_REG_TEMPERATURE_MSB = 0x23,
|
|
|
|
BMI270_REG_FIFO_LENGTH_LSB = 0x24,
|
|
|
|
BMI270_REG_FIFO_LENGTH_MSB = 0x25,
|
|
|
|
BMI270_REG_FIFO_DATA = 0x26,
|
|
|
|
BMI270_REG_ACC_CONF = 0x40,
|
|
|
|
BMI270_REG_ACC_RANGE = 0x41,
|
|
|
|
BMI270_REG_GYRO_CONF = 0x42,
|
|
|
|
BMI270_REG_GYRO_RANGE = 0x43,
|
|
|
|
BMI270_REG_AUX_CONF = 0x44,
|
|
|
|
BMI270_REG_FIFO_DOWNS = 0x45,
|
|
|
|
BMI270_REG_FIFO_WTM_0 = 0x46,
|
|
|
|
BMI270_REG_FIFO_WTM_1 = 0x47,
|
|
|
|
BMI270_REG_FIFO_CONFIG_0 = 0x48,
|
|
|
|
BMI270_REG_FIFO_CONFIG_1 = 0x49,
|
|
|
|
BMI270_REG_SATURATION = 0x4A,
|
|
|
|
BMI270_REG_INT1_IO_CTRL = 0x53,
|
|
|
|
BMI270_REG_INT2_IO_CTRL = 0x54,
|
|
|
|
BMI270_REG_INT_LATCH = 0x55,
|
|
|
|
BMI270_REG_INT1_MAP_FEAT = 0x56,
|
|
|
|
BMI270_REG_INT2_MAP_FEAT = 0x57,
|
|
|
|
BMI270_REG_INT_MAP_DATA = 0x58,
|
|
|
|
BMI270_REG_INIT_CTRL = 0x59,
|
|
|
|
BMI270_REG_INIT_DATA = 0x5E,
|
|
|
|
BMI270_REG_ACC_SELF_TEST = 0x6D,
|
|
|
|
BMI270_REG_GYR_SELF_TEST_AXES = 0x6E,
|
|
|
|
BMI270_REG_PWR_CONF = 0x7C,
|
|
|
|
BMI270_REG_PWR_CTRL = 0x7D,
|
|
|
|
BMI270_REG_CMD = 0x7E,
|
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
* The following device config microcode has the following copyright:
|
|
|
|
*
|
|
|
|
* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
|
|
|
|
*
|
|
|
|
* BSD-3-Clause
|
|
|
|
*
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
* modification, are permitted provided that the following conditions are met:
|
|
|
|
*
|
|
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
*
|
|
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
|
|
* documentation and/or other materials provided with the distribution.
|
|
|
|
*
|
|
|
|
* 3. Neither the name of the copyright holder nor the names of its
|
|
|
|
* contributors may be used to endorse or promote products derived from
|
|
|
|
* this software without specific prior written permission.
|
|
|
|
*
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
|
|
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
|
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
|
|
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
|
|
|
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
|
|
|
|
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*/
|
|
|
|
const uint8_t AP_InertialSensor_BMI270::maximum_fifo_config_file[] = { BMI270_REG_INIT_DATA,
|
|
|
|
0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00,
|
|
|
|
0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5,
|
|
|
|
0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22,
|
|
|
|
0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
|
|
|
|
0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00,
|
|
|
|
0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00,
|
|
|
|
0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb,
|
|
|
|
0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5,
|
|
|
|
0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41,
|
|
|
|
0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24,
|
|
|
|
0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94,
|
|
|
|
0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
|
|
|
|
0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20,
|
|
|
|
0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08,
|
|
|
|
0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0,
|
|
|
|
0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84,
|
|
|
|
0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61,
|
|
|
|
0xf5, 0xeb, 0x2c, 0xe1, 0x6f
|
|
|
|
};
|
|
|
|
/*
|
|
|
|
* End of Bosch microcode copyright
|
|
|
|
*/
|
|
|
|
|
|
|
|
#define BMI270_CHIP_ID 0x24
|
|
|
|
#define BMI270_CMD_SOFTRESET 0xB6
|
|
|
|
#define BMI270_CMD_FIFOFLUSH 0xB0
|
|
|
|
#define BMI270_FIFO_ACC_EN 0x40
|
|
|
|
#define BMI270_FIFO_GYR_EN 0x80
|
|
|
|
#define BMI270_BACKEND_SAMPLE_RATE 1600
|
|
|
|
|
|
|
|
/* Datasheet says that the device powers up and soft resets in 2s, so waiting for
|
|
|
|
* 5ms before initialization is enough. */
|
|
|
|
#define BMI270_POWERUP_DELAY_MSEC 5
|
|
|
|
|
|
|
|
/* This number of samples should provide only one read burst operation on the
|
|
|
|
* FIFO most of the time (99.99%). */
|
|
|
|
#define BMI270_MAX_FIFO_SAMPLES 8
|
|
|
|
|
|
|
|
#define BMI270_HARDWARE_INIT_MAX_TRIES 5
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
AP_InertialSensor_BMI270::AP_InertialSensor_BMI270(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_BMI270::probe(AP_InertialSensor &imu,
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
|
|
|
enum Rotation rotation)
|
|
|
|
{
|
|
|
|
if (!dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
auto sensor = new AP_InertialSensor_BMI270(imu, std::move(dev), rotation);
|
|
|
|
|
|
|
|
if (!sensor) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!sensor->init()) {
|
|
|
|
delete sensor;
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
|
|
|
AP_InertialSensor_Backend *
|
|
|
|
AP_InertialSensor_BMI270::probe(AP_InertialSensor &imu,
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
|
|
|
enum Rotation rotation)
|
|
|
|
{
|
|
|
|
if (!dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
auto sensor = new AP_InertialSensor_BMI270(imu, std::move(dev), rotation);
|
|
|
|
|
|
|
|
if (!sensor) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!sensor->init()) {
|
|
|
|
delete sensor;
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::start()
|
|
|
|
{
|
|
|
|
_dev->get_semaphore()->take_blocking();
|
|
|
|
|
|
|
|
configure_accel();
|
|
|
|
|
|
|
|
configure_gyro();
|
|
|
|
|
|
|
|
configure_fifo();
|
|
|
|
|
|
|
|
_dev->get_semaphore()->give();
|
|
|
|
|
|
|
|
// using headerless mode so gyro and accel ODRs must match
|
|
|
|
if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||
|
|
|
|
!_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// setup sensor rotations from probe()
|
|
|
|
set_gyro_orientation(_gyro_instance, _rotation);
|
|
|
|
set_accel_orientation(_accel_instance, _rotation);
|
|
|
|
|
|
|
|
/* Call _poll_data() at 1600Hz */
|
|
|
|
_dev->register_periodic_callback(1000000UL / BMI270_BACKEND_SAMPLE_RATE,
|
|
|
|
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::poll_data, void));
|
|
|
|
}
|
|
|
|
|
|
|
|
bool AP_InertialSensor_BMI270::update()
|
|
|
|
{
|
|
|
|
update_accel(_accel_instance);
|
|
|
|
update_gyro(_gyro_instance);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
read from registers, special SPI handling needed
|
|
|
|
*/
|
|
|
|
bool AP_InertialSensor_BMI270::read_registers(uint8_t reg, uint8_t *data, uint8_t len)
|
|
|
|
{
|
|
|
|
// for SPI we need to discard the first returned byte. See
|
|
|
|
// datasheet for explanation
|
|
|
|
uint8_t b[len+2];
|
|
|
|
b[0] = reg | 0x80;
|
|
|
|
memset(&b[1], 0, len+1);
|
|
|
|
if (!_dev->transfer(b, len+2, b, len+2)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
memcpy(data, &b[2], len);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
write registers with retries. The SPI sensor may take
|
|
|
|
several tries to correctly write a register
|
|
|
|
*/
|
|
|
|
bool AP_InertialSensor_BMI270::write_register(uint8_t reg, uint8_t v)
|
|
|
|
{
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
|
|
|
_dev->write_register(reg, v);
|
|
|
|
uint8_t v2 = 0;
|
|
|
|
if (read_registers(reg, &v2, 1) && v2 == v) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::check_err_reg()
|
|
|
|
{
|
|
|
|
#ifdef BMI270_DEBUG
|
|
|
|
uint8_t err = 0;
|
|
|
|
read_registers(BMI270_REG_ERR_REG, &err, 1);
|
|
|
|
|
|
|
|
if (err) {
|
|
|
|
if ((err & 1) == 1) {
|
|
|
|
uint8_t status = 0;
|
|
|
|
read_registers(BMI270_REG_INTERNAL_STATUS, &status, 1);
|
|
|
|
switch (status & 0xF) {
|
|
|
|
case 0:
|
|
|
|
AP_HAL::panic("BMI270: not_init\n");
|
|
|
|
break;
|
|
|
|
case 2:
|
|
|
|
AP_HAL::panic("BMI270: init_err\n");
|
|
|
|
break;
|
|
|
|
case 3:
|
|
|
|
AP_HAL::panic("BMI270: drv_err\n");
|
|
|
|
break;
|
|
|
|
case 4:
|
|
|
|
AP_HAL::panic("BMI270: sns_stop\n");
|
|
|
|
break;
|
|
|
|
case 5:
|
|
|
|
AP_HAL::panic("BMI270: nvm_error\n");
|
|
|
|
break;
|
|
|
|
case 6:
|
|
|
|
AP_HAL::panic("BMI270: start_up_error\n");
|
|
|
|
break;
|
|
|
|
case 7:
|
|
|
|
AP_HAL::panic("BMI270: compat_error\n");
|
|
|
|
break;
|
|
|
|
case 1: // init ok
|
|
|
|
if ((status>>5 & 1) == 1) {
|
|
|
|
AP_HAL::panic("BMI270: axes_remap_error\n");
|
|
|
|
} else if ((status>>6 & 1) == 1) {
|
|
|
|
AP_HAL::panic("BMI270: odr_50hz_error\n");
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
} else if ((err>>6 & 1) == 1) {
|
|
|
|
AP_HAL::panic("BMI270: fifo_err\n");
|
|
|
|
} else if ((err>>7 & 1) == 1) {
|
|
|
|
AP_HAL::panic("BMI270: aux_err\n");
|
|
|
|
} else {
|
|
|
|
AP_HAL::panic("BMI270: internal error detected %d\n", err>>1 & 0xF);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::configure_accel()
|
|
|
|
{
|
|
|
|
// set acc in high performance mode with normal filtering at 1600Hz
|
|
|
|
write_register(BMI270_REG_ACC_CONF, 1U<<7 | 0x2U<<4 | 0x0C);
|
|
|
|
// set acc to 16G full scale
|
|
|
|
write_register(BMI270_REG_ACC_RANGE, 0x03);
|
|
|
|
|
|
|
|
check_err_reg();
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::configure_gyro()
|
|
|
|
{
|
|
|
|
// set gyro in high performance filter mode, high performance noise mode. normal filtering at 1600Hz
|
|
|
|
write_register(BMI270_REG_GYRO_CONF, 1U<<7 | 1<<6 | 2<<4 | 0x0C);
|
|
|
|
// set gyro to 2000dps full scale
|
|
|
|
// for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well
|
|
|
|
// or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!)
|
|
|
|
write_register(BMI270_REG_GYRO_RANGE, 0x08);
|
|
|
|
|
|
|
|
check_err_reg();
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::configure_fifo()
|
|
|
|
{
|
|
|
|
// don't stop when full, disable sensortime frame
|
|
|
|
write_register(BMI270_REG_FIFO_CONFIG_0, 0x00);
|
|
|
|
// accel + gyro data in FIFO together with headers
|
|
|
|
write_register(BMI270_REG_FIFO_CONFIG_1, 1U<<7 | 1U<<6 | 1U<<4);
|
|
|
|
// unfiltered with gyro downsampled by 2^2 to give 1600Hz
|
|
|
|
write_register(BMI270_REG_FIFO_DOWNS, 0x02);
|
|
|
|
// disable advanced power save, enable FIFO self-wake
|
|
|
|
write_register(BMI270_REG_PWR_CONF, 0x02);
|
|
|
|
// Enable the gyro, accelerometer and temperature sensor - disable aux interface
|
|
|
|
write_register(BMI270_REG_PWR_CTRL, 0x0E);
|
|
|
|
// flush FIFO
|
|
|
|
write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH);
|
|
|
|
|
|
|
|
check_err_reg();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
read fifo
|
|
|
|
*/
|
|
|
|
void AP_InertialSensor_BMI270::read_fifo(void)
|
|
|
|
{
|
|
|
|
uint8_t len[2];
|
|
|
|
if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) {
|
|
|
|
_inc_accel_error_count(_accel_instance);
|
|
|
|
_inc_gyro_error_count(_gyro_instance);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
uint16_t fifo_length = len[0] + (len[1]<<8);
|
|
|
|
if (fifo_length & 0x8000) {
|
|
|
|
// empty
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// don't read more than 8 frames at a time
|
|
|
|
if (fifo_length > BMI270_MAX_FIFO_SAMPLES*13) {
|
|
|
|
fifo_length = BMI270_MAX_FIFO_SAMPLES*13;
|
|
|
|
}
|
|
|
|
if (fifo_length == 0) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint8_t data[fifo_length];
|
|
|
|
if (!read_registers(BMI270_REG_FIFO_DATA, data, fifo_length)) {
|
|
|
|
_inc_accel_error_count(_accel_instance);
|
|
|
|
_inc_gyro_error_count(_gyro_instance);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
const uint8_t *p = &data[0];
|
|
|
|
while (fifo_length >= 12) {
|
|
|
|
/*
|
|
|
|
the fifo frames are variable length, with the frame type in the first byte
|
|
|
|
*/
|
|
|
|
uint8_t frame_len = 2;
|
|
|
|
switch (p[0] & 0xFC) {
|
|
|
|
case 0x84: // accel
|
|
|
|
frame_len = 7;
|
|
|
|
parse_accel_frame(p+1);
|
|
|
|
break;
|
|
|
|
case 0x88: // gyro
|
|
|
|
frame_len = 7;
|
|
|
|
parse_gyro_frame(p+1);
|
|
|
|
break;
|
|
|
|
case 0x8C: // accel + gyro
|
|
|
|
frame_len = 13;
|
|
|
|
parse_gyro_frame(p+1);
|
|
|
|
parse_accel_frame(p+7);
|
|
|
|
break;
|
|
|
|
case 0x40:
|
|
|
|
// skip frame
|
|
|
|
frame_len = 2;
|
|
|
|
break;
|
|
|
|
case 0x44:
|
|
|
|
// sensortime frame
|
|
|
|
frame_len = 4;
|
|
|
|
break;
|
|
|
|
case 0x48:
|
|
|
|
// fifo config frame
|
|
|
|
frame_len = 2;
|
|
|
|
break;
|
|
|
|
case 0x50:
|
|
|
|
// sample drop frame
|
|
|
|
frame_len = 2;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
p += frame_len;
|
|
|
|
fifo_length -= frame_len;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (temperature_counter++ == 100) {
|
|
|
|
temperature_counter = 0;
|
|
|
|
uint8_t tbuf[2];
|
|
|
|
if (!read_registers(BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) {
|
|
|
|
_inc_accel_error_count(_accel_instance);
|
|
|
|
_inc_gyro_error_count(_gyro_instance);
|
|
|
|
} else {
|
|
|
|
uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5);
|
|
|
|
int16_t temp_int11 = temp_uint11>1023?temp_uint11-2048:temp_uint11;
|
|
|
|
float temp_degc = temp_int11 * 0.125f + 23;
|
|
|
|
_publish_temperature(_accel_instance, temp_degc);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::parse_accel_frame(const uint8_t* d)
|
|
|
|
{
|
|
|
|
// assume configured for 16g range
|
|
|
|
const float scale = (1.0/32768.0) * GRAVITY_MSS * 16.0;
|
|
|
|
int16_t xyz[3] {
|
|
|
|
int16_t(uint16_t(d[0] | (d[1]<<8))),
|
|
|
|
int16_t(uint16_t(d[2] | (d[3]<<8))),
|
|
|
|
int16_t(uint16_t(d[4] | (d[5]<<8)))};
|
|
|
|
Vector3f accel(xyz[0], xyz[1], xyz[2]);
|
|
|
|
|
|
|
|
accel *= scale;
|
|
|
|
|
|
|
|
_rotate_and_correct_accel(_accel_instance, accel);
|
|
|
|
_notify_new_accel_raw_sample(_accel_instance, accel);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
|
|
|
|
{
|
|
|
|
// data is 16 bits with 2000dps range
|
|
|
|
const float scale = radians(2000.0f) / 32767.0f;
|
|
|
|
int16_t xyz[3] {
|
|
|
|
int16_t(uint16_t(d[0] | d[1]<<8)),
|
|
|
|
int16_t(uint16_t(d[2] | d[3]<<8)),
|
|
|
|
int16_t(uint16_t(d[4] | d[5]<<8)) };
|
|
|
|
Vector3f gyro(xyz[0], xyz[1], xyz[2]);
|
|
|
|
gyro *= scale;
|
|
|
|
|
|
|
|
_rotate_and_correct_gyro(_gyro_instance, gyro);
|
|
|
|
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_InertialSensor_BMI270::poll_data()
|
|
|
|
{
|
|
|
|
read_fifo();
|
|
|
|
}
|
|
|
|
|
|
|
|
bool AP_InertialSensor_BMI270::hardware_init()
|
|
|
|
{
|
2022-06-04 08:56:10 -03:00
|
|
|
bool init = false;
|
|
|
|
bool read_chip_id = false;
|
2021-12-28 13:43:48 -04:00
|
|
|
|
|
|
|
hal.scheduler->delay(BMI270_POWERUP_DELAY_MSEC);
|
|
|
|
|
2022-06-04 08:56:10 -03:00
|
|
|
WITH_SEMAPHORE(_dev->get_semaphore());
|
2021-12-28 13:43:48 -04:00
|
|
|
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
|
|
|
|
|
|
|
for (unsigned i = 0; i < BMI270_HARDWARE_INIT_MAX_TRIES; i++) {
|
|
|
|
|
|
|
|
uint8_t chip_id = 0;
|
|
|
|
/* If CSB sees a rising edge after power-up, the device interface switches to SPI
|
|
|
|
* after 200 μs until a reset or the next power-up occurs therefore it is recommended
|
|
|
|
* to perform a SPI single read of register CHIP_ID (the obtained value will be invalid)
|
|
|
|
* before the actual communication start, in order to use the SPI interface */
|
|
|
|
read_registers(BMI270_REG_CHIP_ID, &chip_id, 1);
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
|
|
|
write_register(BMI270_REG_CMD, BMI270_CMD_SOFTRESET);
|
|
|
|
hal.scheduler->delay(BMI270_POWERUP_DELAY_MSEC); // power on and soft reset time is 2ms
|
|
|
|
|
|
|
|
// switch to SPI mode again
|
|
|
|
read_registers(BMI270_REG_CHIP_ID, &chip_id, 1);
|
|
|
|
hal.scheduler->delay(1);
|
|
|
|
|
|
|
|
read_registers(BMI270_REG_CHIP_ID, &chip_id, 1);
|
|
|
|
if (chip_id != BMI270_CHIP_ID) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2022-06-04 08:56:10 -03:00
|
|
|
// successfully identified the chip, proceed with initialisation
|
|
|
|
read_chip_id = true;
|
|
|
|
|
2021-12-28 13:43:48 -04:00
|
|
|
// disable power save
|
|
|
|
write_register(BMI270_REG_PWR_CONF, 0x00);
|
|
|
|
hal.scheduler->delay(1); // needs to be at least 450us
|
|
|
|
|
|
|
|
// upload config
|
|
|
|
write_register(BMI270_REG_INIT_CTRL, 0x00);
|
|
|
|
|
|
|
|
// Transfer the config file, data packet includes INIT_DATA
|
|
|
|
_dev->transfer(maximum_fifo_config_file, sizeof(maximum_fifo_config_file), nullptr, 0);
|
|
|
|
|
|
|
|
// config is done
|
|
|
|
write_register(BMI270_REG_INIT_CTRL, 1);
|
|
|
|
|
|
|
|
// check the results
|
|
|
|
hal.scheduler->delay(20);
|
|
|
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
read_registers(BMI270_REG_INTERNAL_STATUS, &status, 1);
|
|
|
|
|
|
|
|
if ((status & 1) == 1) {
|
2022-06-04 08:56:10 -03:00
|
|
|
init = true;
|
2022-03-21 06:25:18 -03:00
|
|
|
DEV_PRINTF("BMI270 initialized after %d retries\n", i+1);
|
2021-12-28 13:43:48 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
|
|
|
|
2022-06-04 08:56:10 -03:00
|
|
|
if (read_chip_id && !init) {
|
|
|
|
DEV_PRINTF("BMI270: failed to init\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
return init;
|
2021-12-28 13:43:48 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
bool AP_InertialSensor_BMI270::init()
|
|
|
|
{
|
|
|
|
_dev->set_read_flag(0x80);
|
|
|
|
|
2022-06-04 08:56:10 -03:00
|
|
|
return hardware_init();
|
2021-12-28 13:43:48 -04:00
|
|
|
}
|