|
在第二篇文章中讲过 传感器驱动程序的前后端设计方法,这一篇文章我们详细分析一下它是如何实现的。这一系列文章都以ArduSub和Pixhawk1(狗骨头)进行说明,如果读者查看自己工程的源码,请先编译,否则有一些宏定义会找不到。
以惯性导航器件为例,首先完成设备的初始化,然后再定时更新设备数据。由于总线结构问题,可能多个设备共用一条总线,需要给总线通信单独开辟一个OS线程,如spi bus总线,然后每个设备在这个线程中注册回调函数,以完成自身的定时刷新。
飞控的应用层程序从上述传感器中取数据,为了使应用层不需要关心设备驱动层,Ardupilot设计了传感器驱动前后端程序结构,后端在上述线程中定时读取传感器原始数据并处理,应用程序从前端更新数据,前后端对象分别持有对方的指针,以便互通数据,为了兼容每款飞控板使用的不同型号的传感器,又分别根据具体型号对后端程序进行继承。
INS前端Class INS后端Class(基类) INS具体型号Class
AP_InertialSensor_BMI055
AP_InertialSensor AP_InertialSensor_Backend AP_InertialSensor_Invensense
AP_InertialSensor_LSM9DS0 这里我们仍以惯性器件来说明,在Ardupilot工程中就是ins对象,它被定义在Sub.h中。
AP_Baro barometer;
Compass compass;
AP_InertialSensor ins;
我们先来看一下AP_InertialSensor类的定义,去除类内定义的一些setXX getXX函数,剩余一些初始化、采样刷新、后端进程相关的精简程序如下。
//libraries/AP_InertialSensor/AP_InertialSensor.h
class AP_InertialSensor_Backend;
class AuxiliaryBus;
class AP_AHRS;
class AP_InertialSensor : AP_AccelCal_Client
{
friend class AP_InertialSensor_Backend;
public:
void init(uint16_t sample_rate_hz);
// update gyro and accel values from accumulated samples
void update(void);
// wait for a sample to be available
void wait_for_sample(void);
void detect_backends(void);
private:
// load backend drivers
bool _add_backend(AP_InertialSensor_Backend *backend);
void _start_backends();
AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
// backend objects
AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES];//前后端数据交换数组
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];//前后端数据交换数组
};
namespace AP {
AP_InertialSensor &ins();
};
ins的初始化
setup()
init_ardupilot()
startup_INS_ground()
startup_INS_ground()
ins.init(scheduler.get_loop_rate_hz());
其中scheduler.get_loop_ratehz()是指main_loop函数的执行频率,ArduCopter和ArduSub中默认为400HZ,其他类型车辆默认50HZ。init函数原型如下。
//libraries/AP_InertialSensor/AP_InertialSensor.cpp
//精简代码
void AP_InertialSensor::init(uint16_t sample_rate)
{
//启动后端
if (_gyro_count == 0 && _accel_count == 0) {
_start_backends();
}
}
ins init初始化之前ACCEL和GYRO的数量都为0,所以先启动后端,在启动后端时会先检测有哪些后端,即根据板型确定板载的传感器有哪些,传感器的数量,进一步确定开启对应的后端。
启动后端
//精简代码
void AP_InertialSensor::_start_backends()
{
detect_backends();
for (uint8_t i = 0; i < _backend_count; i++) {
_backends->start();
}
}
精简版的函数定义如下。
/*detect available backends for this board*/
void AP_InertialSensor::detect_backends(void)
{
uint8_t probe_count = 0;
uint8_t enable_mask = 0x7F;
uint8_t found_mask = 0;
/*use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends */
#define ADD_BACKEND(x) do { \
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
found_mask |= (1U<<probe_count); \
} \
probe_count++; \
} while (0)
#if defined(HAL_INS_PROBE_LIST)
// IMUs defined by IMU lines in hwdef.dat
HAL_INS_PROBE_LIST;
#endif
}
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
{
if (!backend) {
return false;
}
if (_backend_count == INS_MAX_BACKENDS) {
AP_HAL::panic(&#34;Too many INS backends&#34;);
}
_backends[_backend_count++] = backend;
return true;
}
HAL_INS_PROBE_LIST宏定义在中,如下所示。
//build/Pixhawk1/hwdef.h
//不编译程序,没有这个文件
#define HAL_INS_PROBE1 ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this,hal.spi->get_device(&#34;mpu6000&#34;),ROTATION_ROLL_180))
#define HAL_INS_PROBE2 ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,hal.spi->get_device(&#34;lsm9ds0_g&#34;),hal.spi->get_device(&#34;lsm9ds0_am&#34;),ROTATION_ROLL_180,ROTATION_ROLL_180_YAW_270,ROTATION_PITCH_180))
#define HAL_INS_PROBE_LIST HAL_INS_PROBE1;HAL_INS_PROBE2
ADD_BACKEND()宏应传入一个AP_InertialSensor_Backend *类型的指针,也就是我们要添加的具体型号的INS设备对象的指针,probe函数用于new一个这样的对象,并返回指针。通过detect_backends函数我们便将每个INS设备作为一个具体的后端实现加入到_backends数组中,以便下一步依次启动。
for (uint8_t i = 0; i < _backend_count; i++)
{
_backends->start();
}
start函数内容较复杂,多是根据具体传感器型号进行初始化的内容,如设置量称,采样频率,低通滤波频率以及传感器的方向等这里不再叙述,其精简内容如下。
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
void AP_InertialSensor_Invensense::start()
{
//向前端注册
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
... ... ... ..
// allocate fifo buffer
//数据缓冲区
_fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
// start the timer process to read samples
//在SPI驱动线程里注册一个回调函数用来刷新传感器数据
_dev->register_periodic_callback(1000000UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
}
这里主要分析一下register_periodic_callback函数,这个函数属于&#34;_dev&#34;,&#34;_dev&#34;即上述的&#34;hal.spi->get_device(&#34;mpu6000&#34;)&#34;或&#34;hal.spi->get_device(&#34;lsm9ds0_g&#34;)&#34;所返回的驱动实例。
//libraries/AP_HAL_ChibiOS/SPIDevice.cpp
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
return bus.register_periodic_callback(period_usec, cb, this);
}
//libraries/AP_HAL_ChibiOS/Device.cpp
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
{
//如果这个驱动的线程还没有被创建,创建
if (!thread_started)
{
thread_started = true;
hal_device = _hal_device;
// setup a name for the thread
const uint8_t name_len = 7;
char *name = (char *)malloc(name_len);
switch (hal_device->bus_type()) {
case AP_HAL::Device::BUS_TYPE_I2C:
snprintf(name, name_len, &#34;I2C:%u&#34;,
hal_device->bus_num());
break;
case AP_HAL::Device::BUS_TYPE_SPI:
snprintf(name, name_len, &#34;SPI:%u&#34;,
hal_device->bus_num());
break;
default:
break;
}
//开辟线程
thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(HAL_DEVICE_THREAD_STACK),
name,
thread_priority, /* Initial priority. */
DeviceBus::bus_thread, /* Thread function. */
this); /* Thread parameter. */
if (thread_ctx == nullptr) {
AP_HAL::panic(&#34;Failed to create bus thread %s&#34;, name);
}
}
//在线程内注册回调函数
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
if (callback == nullptr) {
return nullptr;
}
callback->cb = cb;
callback->period_usec = period_usec;
callback->next_usec = AP_HAL::micros64() + period_usec;
// add to linked list of callbacks on thread
callback->next = callbacks;
callbacks = callback;
return callback;
}
通过创建通信驱动模块(SPI或IIC)的线程后,再将传感器的数据刷新函数(AP_InertialSensor_Invensense::_poll_data)注册到这个线程内,这样便实现了传感器的初始化和后端的持续运行。
到这里原始的传感器数据就源源不断的被读出来了,我们来看一下_poll_data函数是如何处理的。
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
/*Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held*/
void AP_InertialSensor_Invensense::_poll_data()
{
_read_fifo();
}
_read_fifo()
//精简代码
void AP_InertialSensor_Invensense::_read_fifo()
{
uint8_t *rx = _fifo_buffer;
//检测MPU内数据包数量
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
goto check_registers;
}
bytes_read = uint16_val(rx, 0);
n_samples = bytes_read / MPU_SAMPLE_SIZE;
//将n包数据循环读取到数组内
while (n_samples > 0)
{
uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
i_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE);
_accumulate(rx, n);
n_samples -= n;
}
}
_accumulate()
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
{
//将n包数据循环处理
for (uint8_t i = 0; i < n_samples; i++)
{
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
Vector3f accel, gyro;
bool fsync_set = false;
//加速度
accel = Vector3f(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
accel *= _accel_scale;
//温度
int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) {
debug(&#34;temp reset IMU[%u] %d %d&#34;, _accel_instance, _raw_temp, t2);
_fifo_reset();
return false;
}
float temp = t2 * temp_sensitivity + temp_zero;
//角速度
gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
gyro *= _gyro_scale;
//加速度、角速度信息坐标系变换、数据处理
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
//通知新的加速度、角速度数据刷新。
//对原始采样数据低通滤波。根据加速度计算速度增量。
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
}
return true;
}
至此,传感器的后端回调函数便完成了数据的读取,误差处理,坐标系变换,以及低通滤波和速度增量计算。
但是,后端采集到的数据如何被前端使用呢,也就是说前后端是如何通信的呢,前后端同时访问这些数据的话,如何保证正确访问?
前后端数据交换是通过几个变量完成的,后端将&#34;_notify_new_xxx_raw_sample&#34;函数处理好的数据放到前端的数组内,主线程中前端ins按一定频率update访问这个数组,这样便将数据传递了。
下边对&#34;_notify_new_xxx_raw_sample&#34;函数和update函数分别分析说明。
//libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
//精简代码
void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
const Vector3f &accel,
uint64_t sample_us,
bool fsync_set)
{
float dt;
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
{
WITH_SEMAPHORE(_sem);
// delta velocity
_imu._delta_velocity_acc[instance] += accel * dt;
_imu._delta_velocity_acc_dt[instance] += dt;
_imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
_imu._new_accel_data[instance] = true;
}
}
这段代码是对加速度数据的处理,根据原始的加速度信息计算速度增量,并给加速度低通滤波,最终通过标志位设为true通知前端数据刷新了。
//libraries/AP_InertialSensor/AP_InertialSensor.cpp
//前端update精简代码
/*update gyro and accel values from backends*/
void AP_InertialSensor::update(void)
{
wait_for_sample();//等待一定的时间间隔
for (uint8_t i=0; i<_backend_count; i++) {
_backends->update();//调用后端刷新
}
}
后端刷新代码如下
//libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
/*publish any pending data*/
bool AP_InertialSensor_Invensense::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
return true;
}
以update_accel为例说明
//libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
/* common accel update function for all backends*/
void AP_InertialSensor_Backend::update_accel(uint8_t instance)
{
WITH_SEMAPHORE(_sem);
if (_imu._new_accel_data[instance]) {
_publish_accel(instance, _imu._accel_filtered[instance]);
_imu._new_accel_data[instance] = false;
}
}
通过上述数据刷新标志位判断是否发布加速度信息。
//精简代码
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
{
_imu._accel[instance] = accel;
_imu._accel_healthy[instance] = true;
// publish delta velocity
_imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
_imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
_imu._delta_velocity_valid[instance] = true;
}
这样便将传感器的数据存储到了_imu._accel和_imu._delta_velocity数组中了,完成了数据的传递,完成了前后端的通信。其他数据,如陀螺仪等都是相同的方式。
数据访问锁
以上程序中定义有一句宏定义:
WITH_SEMAPHORE(_sem); 这个宏是定义了一个局部Semaphore,在其定义时开始锁定线程,跳出其作用域后自动解锁。
// semaphore for access to shared frontend data
HAL_Semaphore_Recursive _sem; |
|