ArduPilot开源代码之AP_InertialSensor_Backend
1. 源由
惯性传感器是飞控关于姿态最重要的一个传感器。从复杂度角度看,除了数据融合外,算是逻辑上最为复杂的。
本章我们将重点了解下ArduPilot是如何来获取该传感器的数据,并如何处理的。
2. 设计
惯性传感器遵循Sensor Drivers的front-end/back-end分层设计。
鉴于传感器硬件种类很多,因此在该现实情况下,惯性传感器对back-end进一步抽象了AP_InertialSensor_Backend
类。
从该AP_InertialSensor_Backend类模版上,可以主要关注以下几个方法:
- virtual void start() { }
- virtual bool update() = 0; /* front end */
- device driver IDs
class AP_InertialSensor_Backend
{
public:
AP_InertialSensor_Backend(AP_InertialSensor &imu);
AP_InertialSensor_Backend(const AP_InertialSensor_Backend &that) = delete;
// we declare a virtual destructor so that drivers can
// override with a custom destructor if need be.
virtual ~AP_InertialSensor_Backend(void) {}
/*
* Update the sensor data. Called by the frontend to transfer
* accumulated sensor readings to the frontend structure via the
* _publish_gyro() and _publish_accel() functions
*/
virtual bool update() = 0; /* front end */
/*
* optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
*/
virtual void accumulate() {}
/*
* Configure and start all sensors. The empty implementation allows
* subclasses to already start the sensors when it's detected
*/
virtual void start() { }
/*
* Return an AuxiliaryBus if backend has another bus it is able to export
*/
virtual AuxiliaryBus *get_auxiliary_bus() { return nullptr; }
/*
* Return the unique identifier for this backend: it's the same for
* several sensors if the backend registers more gyros/accels
*/
int16_t get_id() const { return _id; }
//Returns the Clip Limit
float get_clip_limit() const { return _clip_limit; }
// get a startup banner to output to the GCS
virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
#if HAL_EXTERNAL_AHRS_ENABLED
virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
#endif
/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as INS*ID* parameters to
users. The values are chosen for compatibility with existing PX4
drivers.
If a change is made to a driver that would make existing
calibration values invalid then this number must be changed.
*/
enum DevTypes {
DEVTYPE_BMI160 = 0x09,
DEVTYPE_L3G4200D = 0x10,
DEVTYPE_ACC_LSM303D = 0x11,
DEVTYPE_ACC_BMA180 = 0x12,
DEVTYPE_ACC_MPU6000 = 0x13,
DEVTYPE_ACC_MPU9250 = 0x16,
DEVTYPE_ACC_IIS328DQ = 0x17,
DEVTYPE_ACC_LSM9DS1 = 0x18,
DEVTYPE_GYR_MPU6000 = 0x21,
DEVTYPE_GYR_L3GD20 = 0x22,
DEVTYPE_GYR_MPU9250 = 0x24,
DEVTYPE_GYR_I3G4250D = 0x25,
DEVTYPE_GYR_LSM9DS1 = 0x26,
DEVTYPE_INS_ICM20789 = 0x27,
DEVTYPE_INS_ICM20689 = 0x28,
DEVTYPE_INS_BMI055 = 0x29,
DEVTYPE_SITL = 0x2A,
DEVTYPE_INS_BMI088 = 0x2B,
DEVTYPE_INS_ICM20948 = 0x2C,
DEVTYPE_INS_ICM20648 = 0x2D,
DEVTYPE_INS_ICM20649 = 0x2E,
DEVTYPE_INS_ICM20602 = 0x2F,
DEVTYPE_INS_ICM20601 = 0x30,
DEVTYPE_INS_ADIS1647X = 0x31,
DEVTYPE_SERIAL = 0x32,
DEVTYPE_INS_ICM40609 = 0x33,
DEVTYPE_INS_ICM42688 = 0x34,
DEVTYPE_INS_ICM42605 = 0x35,
DEVTYPE_INS_ICM40605 = 0x36,
DEVTYPE_INS_IIM42652 = 0x37,
DEVTYPE_BMI270 = 0x38,
DEVTYPE_INS_BMI085 = 0x39,
DEVTYPE_INS_ICM42670 = 0x3A,
DEVTYPE_INS_ICM45686 = 0x3B,
DEVTYPE_INS_SCHA63T = 0x3C,
};
protected:
// access to frontend
AP_InertialSensor &_imu;
// semaphore for access to shared frontend data
HAL_Semaphore _sem;
//Default Clip Limit
float _clip_limit = 15.5f * GRAVITY_MSS;
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
// rotate gyro vector, offset and publish
void _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */
// apply notch and lowpass gyro filters and sample for FFT
void apply_gyro_filters(const uint8_t instance, const Vector3f &gyro);
void save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase);
// this should be called every time a new gyro raw sample is
// available - be it published or not the sample is raw in the
// sense that it's not filtered yet, but it must be rotated and
// corrected (_rotate_and_correct_gyro)
// The sample_us value must be provided for non-FIFO based
// sensors, and should be set to zero for FIFO based sensors
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0) __RAMFUNC__;
// alternative interface using delta-angles. Rotation and correction is handled inside this function
void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);
// rotate accel vector, scale, offset and publish
void _publish_accel(uint8_t instance, const Vector3f &accel) __RAMFUNC__; /* front end */
// this should be called every time a new accel raw sample is available -
// be it published or not
// the sample is raw in the sense that it's not filtered yet, but it must
// be rotated and corrected (_rotate_and_correct_accel)
// The sample_us value must be provided for non-FIFO based
// sensors, and should be set to zero for FIFO based sensors
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false) __RAMFUNC__;
// alternative interface using delta-velocities. Rotation and correction is handled inside this function
void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
// set the amount of oversamping a accel is doing
void _set_accel_oversampling(uint8_t instance, uint8_t n);
// set the amount of oversamping a gyro is doing
void _set_gyro_oversampling(uint8_t instance, uint8_t n);
// indicate the backend is doing sensor-rate sampling for this accel
void _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
const uint8_t bit = (1<<instance);
if (value) {
_imu._accel_sensor_rate_sampling_enabled |= bit;
} else {
_imu._accel_sensor_rate_sampling_enabled &= ~bit;
}
}
void _set_gyro_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
const uint8_t bit = (1<<instance);
if (value) {
_imu._gyro_sensor_rate_sampling_enabled |= bit;
} else {
_imu._gyro_sensor_rate_sampling_enabled &= ~bit;
}
}
void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) {
_imu._accel_raw_sampling_multiplier[instance] = mul;
}
void _set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul) {
_imu._gyro_raw_sampling_multiplier[instance] = mul;
}
// update the sensor rate for FIFO sensors
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;
// return true if the sensors are still converging and sampling rates could change significantly
bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; }
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);
// get accelerometer raw sample rate.
float _accel_raw_sample_rate(uint8_t instance) const {
return _imu._accel_raw_sample_rates[instance];
}
// set accelerometer raw sample rate; note that the storage type
// is actually float!
void _set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {
_imu._accel_raw_sample_rates[instance] = rate_hz;
}
// get gyroscope raw sample rate
float _gyro_raw_sample_rate(uint8_t instance) const {
return _imu._gyro_raw_sample_rates[instance];
}
// set gyro raw sample rate; note that the storage type is
// actually float!
void _set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {
_imu._gyro_raw_sample_rates[instance] = rate_hz;
}
// publish a temperature value
void _publish_temperature(uint8_t instance, float temperature); /* front end */
// increment accelerometer error_count
void _inc_accel_error_count(uint8_t instance) __RAMFUNC__;
// increment gyro error_count
void _inc_gyro_error_count(uint8_t instance) __RAMFUNC__;
// backend unique identifier or -1 if backend doesn't identify itself
int16_t _id = -1;
// return the default filter frequency in Hz for the sample rate
uint16_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; }
// return the default filter frequency in Hz for the sample rate
uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
// return the requested loop rate at which samples will be made available in Hz
uint16_t get_loop_rate_hz(void) const {
// enum can be directly cast to Hz
return (uint16_t)_imu._loop_rate;
}
// common gyro update function for all backends
void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
// common accel update function for all backends
void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
// support for updating filter at runtime
uint16_t _last_accel_filter_hz;
uint16_t _last_gyro_filter_hz;
void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
_imu._gyro_orientation[instance] = rotation;
}
void set_accel_orientation(uint8_t instance, enum Rotation rotation) {
_imu._accel_orientation[instance] = rotation;
}
// increment clipping counted. Used by drivers that do decimation before supplying
// samples to the frontend
void increment_clip_count(uint8_t instance) {
_imu._accel_clip_count[instance]++;
}
// should fast sampling be enabled on this IMU?
bool enable_fast_sampling(uint8_t instance) {
return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
}
// if fast sampling is enabled, the rate to use in kHz
uint8_t get_fast_sampling_rate() const {
return (1 << uint8_t(_imu._fast_sampling_rate));
}
// called by subclass when data is received from the sensor, thus
// at the 'sensor rate'
void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) __RAMFUNC__;
void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) __RAMFUNC__;
/*
notify of a FIFO reset so we don't use bad data to update observed sensor rate
*/
void notify_accel_fifo_reset(uint8_t instance) __RAMFUNC__;
void notify_gyro_fifo_reset(uint8_t instance) __RAMFUNC__;
// log an unexpected change in a register for an IMU
void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®) __RAMFUNC__;
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available
private:
bool should_log_imu_raw() const ;
void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) __RAMFUNC__;
void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo) __RAMFUNC__;
// logging
void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel data
void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const __RAMFUNC__; // Write GYR data packet: raw gyro data
};
注:back-end实际上可以看做是驱动类,针对不同硬件其名称略有差异。这里以手头BMI270硬件为参考。
3. 实例BMI270
从类抽象的角度,上面给出了概括性的back-end抽象类,以及对front-end的接口函数;但是不知道为什么该抽象类并未从框架代码角度进行抽象。
这里所说的框架代码的意思更多侧重代码流程上的API接口。
通常我们会将框架和业务通过一些标准的API接口来进行定义(入参,出参),对具体器件尤其硬件寄存器等操作进行抽象,具体放在后面继承类进行进一步的实现。
可能是历史或者其他的原因,从上述AP_InertialSensor_Backend
抽象看,并未做到上述抽象,不过front-end/back-end之间的区分是有明确给出的,同时针对具有共性的API进行了实现。
鉴于上述原因,后面将根据BMI270具体实现进行讨论。
3.1 AP_InertialSensor_BMI270
延续前面我们关注的实现方法,将对以下方法进行展开:
- init //硬件初始化
- start //硬件启动
- read_fifo //获取原始数据
- update //front-end数据更新API
class AP_InertialSensor_BMI270 : public AP_InertialSensor_Backend {
public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation=BMI270_DEFAULT_ROTATION);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation=BMI270_DEFAULT_ROTATION);
/**
* Configure the sensors and start reading routine.
*/
void start() override;
bool update() override;
private:
AP_InertialSensor_BMI270(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
bool read_registers(uint8_t reg, uint8_t *data, uint8_t len);
bool write_register(uint8_t reg, uint8_t v);
/**
* If the macro BMI270_DEBUG is defined, check if there are errors reported
* on the device's error register and panic if so. The implementation is
* empty if the BMI270_DEBUG isn't defined.
*/
void check_err_reg();
/**
* Try to perform initialization of the BMI270 device.
*
* The device semaphore must be taken and released by the caller.
*
* @return true on success, false otherwise.
*/
bool hardware_init();
/**
* Try to initialize this driver.
*
* Do sensor and other required initializations.
*
* @return true on success, false otherwise.
*/
bool init();
/**
* Configure accelerometer sensor. The device semaphore must already be
* taken before calling this function.
*
* @return true on success, false otherwise.
*/
void configure_accel();
/**
* Configure gyroscope sensor. The device semaphore must already be
* taken before calling this function.
*
* @return true on success, false otherwise.
*/
void configure_gyro();
/**
* Reset FIFO.
*/
void fifo_reset();
/**
* Configure FIFO.
*/
void configure_fifo();
/**
* Read samples from fifo.
*/
void read_fifo();
void parse_accel_frame(const uint8_t* d);
void parse_gyro_frame(const uint8_t* d);
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_HAL::Device::PeriodicHandle periodic_handle;
enum Rotation _rotation;
uint8_t _accel_instance;
uint8_t _gyro_instance;
uint8_t temperature_counter;
static const uint8_t maximum_fifo_config_file[];
};
3.2 probe
IMU硬件是如何上电后自动检测到的。
注:AP_Vehicle::setup
如何被调用,详见ArduPilot飞控启动&运行过程简介。
AP_Vehicle::setup
└──> init_ardupilot
└──> startup_INS_ground
└──> ins.init(scheduler.get_loop_rate_hz());
└──> _start_backends
└──> detect_backends
└──> HAL_INS_PROBE_LIST --> HAL_INS_PROBE1;HAL_INS_PROBE2
├──> ADD_BACKEND(AP_InertialSensor_BMI270::probe(*this,hal.spi->get_device("bmi270_1"),ROTATION_ROLL_180_YAW_90))
└──> ADD_BACKEND(AP_InertialSensor_BMI270::probe(*this,hal.spi->get_device("bmi270_2"),ROTATION_PITCH_180))
IMU有两种硬件总线可供使用:I2C和SPI,现在慢慢倾向于使用SPI总线,尤其是小四轴航模(Betaflight/iNav等)。
当probe
失败,可能存在以下几种情况:
- 非法入参,dev空指针;
- 内存空间不够,无法new对象;
- probe发现设设备失败(总线异常/上电异常/芯片不存在);
注:上述情况需要逐一排查,以便找到rootcasue。
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;
}
3.3 init
init方法主要是对硬件进行初始化配置,详见hardware_init
。
bool AP_InertialSensor_BMI270::init()
{
_dev->set_read_flag(0x80);
return hardware_init();
}
硬件由于上电以及内部芯片初始化,可能存在时序上的一些同步问题,因此这里做了BMI270_HARDWARE_INIT_MAX_TRIES
动作。
bool AP_InertialSensor_BMI270::hardware_init()
{
bool init = false;
bool read_chip_id = false;
hal.scheduler->delay(BMI270_POWERUP_DELAY_MSEC);
WITH_SEMAPHORE(_dev->get_semaphore());
_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;
}
// successfully identified the chip, proceed with initialisation
read_chip_id = true;
// 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) {
init = true;
DEV_PRINTF("BMI270 initialized after %d retries\n", i+1);
break;
}
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
if (read_chip_id && !init) {
DEV_PRINTF("BMI270: failed to init\n");
}
return init;
}
3.4 start
ACC和GYRO可以配置不同的采样频率,但是要注意的是如果采样频率不同,对应的read_fifo
也要调整,确保数据的有效性以及减少无意义的pulling检查。
注:Interrupt的方式要比pulling的方法节省资源。
AP_Vehicle::setup
└──> init_ardupilot
└──> startup_INS_ground
└──> ins.init(scheduler.get_loop_rate_hz());
└──> _start_backends
├──> detect_backends
└──> _backends[i]->start()
当硬件芯片检测到之后,就开始启动并配置芯片功能。
void AP_InertialSensor_BMI270::start()
{
_dev->get_semaphore()->take_blocking();
configure_accel();
configure_gyro();
configure_fifo();
_dev->get_semaphore()->give();
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 read_fifo() at 1600Hz */
periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
}
3.5 read_fifo
通过fifo数据获取传感数据
- 检查fifo状态
- 检查fifo内容长度
- 检查fifo空字节
- 获取一次传感数据集
- 获取传感数据时间同步
- 获取传感数据
- 获取IMU器件温度
注:在start
中注册了定时回调的钩子函数&AP_InertialSensor_BMI270::read_fifo
,因此会被定时同步IMU数据。
void AP_InertialSensor_BMI270::read_fifo(void)
{
// check for FIFO errors/overflow //fifo异常检查
uint8_t err = 0;
read_registers(BMI270_REG_ERR_REG, &err, 1);
if ((err>>6 & 1) == 1) {
fifo_reset();
return;
}
uint8_t len[2];
if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) { //获取fifo长度
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(_gyro_instance);
return;
}
uint16_t fifo_length = len[0] + (len[1]<<8); //检查fifo空字节
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存在超过一次的传感数据集
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)) { //获取fifo传感数据
_inc_accel_error_count(_accel_instance);
_inc_gyro_error_count(_gyro_instance);
return;
}
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo() without updating the sensor data
_dev->adjust_periodic_callback(periodic_handle, BACKEND_PERIOD_US); //数据更新时间同步
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 = 5;
break;
case 0x50:
// sample drop frame
frame_len = 2;
break;
case 0x80:
// invalid frame
fifo_reset();
return;
}
p += frame_len;
fifo_length -= frame_len;
}
// temperature sensor updated every 10ms
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 tval = tbuf[0] | (tbuf[1] << 8);
if (tval != 0x8000) { // 0x8000 is invalid
int16_t klsb = static_cast<int16_t>(tval);
float temp_degc = klsb * 0.002f + 23.0f;
_publish_temperature(_accel_instance, temp_degc);
}
}
}
}
3.6 update
front-end更新ACC和GYRO传感数据,是back-end和front-end之间的接口。
bool AP_InertialSensor_BMI270::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
return true;
}
ACC数据更新
void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);
if ((1U<<instance) & _imu.imu_kill_mask) { //IMU硬件和实例不一致,直接返回
return;
}
if (_imu._new_accel_data[instance]) { //有ACC新数据
_publish_accel(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = false;
}
// possibly update filter frequency
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
}
GYRO数据更新
void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
{
WITH_SEMAPHORE(_sem);
if ((1U<<instance) & _imu.imu_kill_mask) { //IMU硬件和实例不一致,直接返回
return;
}
if (_imu._new_gyro_data[instance]) { //有GYRO新数据
_publish_gyro(instance, _imu._gyro_filtered[instance]);
#if HAL_GYROFFT_ENABLED
// copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
_imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
#endif
_imu._new_gyro_data[instance] = false;
}
// possibly update filter frequency
const float gyro_rate = _gyro_raw_sample_rate(instance);
if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {
_imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#if HAL_GYROFFT_ENABLED
_imu._post_filter_gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#endif
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
for (auto ¬ch : _imu.harmonic_notches) {
if (notch.params.enabled()) {
notch.update_params(instance, sensors_converging(), gyro_rate);
}
}
}
4. 总结
整个AP_InertialSensor_XXX驱动,可以理解为以下三部分:
- 初始化:probe/init/start (被AP_Vehicle::setup调用)
- 数据更新:read_fifo (被start中定时函数调用)
- API接口:update (被AP_InertialSensor::update调用)
注:关于SPI的DMA优化在ChibiOS操作系统中进行BSP的支持。换句话说AP中更多的是High Level Design。
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param
【13】ArduPilot开源代码之AP_Scheduler
【14】ArduPilot开源代码之AP_VideoTX