2.2[Lib]ardupilot设备驱动实现方式
来源:互联网 发布:淘宝主图背景 编辑:程序博客网 时间:2024/05/29 02:25
前言
之前看了APM的驱动(mpu6000、sr-04等)的库(srv/driver)基本上是各种class,并且也尝试按照这种格式编写了Xsens的驱动,这次需要编写应用层的代码,就顺藤摸瓜的从上面梳理,结果从ins.update()的入口进去后发现居然有变化,而且从nsh的终端上/dev下也没有看到相关的sensor,这不由得让我疑虑之前的思考方式;
顶层入口
在APM的代码里面,主要的就是::scheduler_tasks[]来实现整个功能的,不管是GPS、还是各种算法都是从这里入口进去,所以先来分析一个和我之前写的驱动能衔接上的;
GPS
首先直接进入由调度器调度GPS的入口:
void Rover::update_GPS_50Hz(void){ static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; gps.update();//这里进行了GPS数据更新 for (uint8_t i=0; i < gps.num_sensors(); i++) { if (gps.last_message_time_ms(i) != last_gps_reading[i]) { last_gps_reading[i] = gps.last_message_time_ms(i); if (should_log(MASK_LOG_GPS)) { DataFlash.Log_Write_GPS(gps, i); } } }}
这段程序主要做的是跟新GPS信息、log GPS信息、对获取的GPS信息判断并处理(只是为了有逻辑的运行,并没必要详细阅读)。重点是gps.update();这里可以看见gps.update();是更新的入口,进一步往下:
/* update all GPS instances */voidAP_GPS::update(void){ for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { update_instance(i);//更新每一个GPS实例,有几个更新几个 } // work out which GPS is the primary, and how many sensors we have for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { if (state[i].status != NO_GPS) { num_instances = i+1; } if (_auto_switch) { if (i == primary_instance) { continue; } if (state[i].status > state[primary_instance].status) { // we have a higher status lock, change GPS primary_instance = i; continue; } bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1); if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) { uint32_t now = AP_HAL::millis(); bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2); if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) || (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) { // this GPS has more satellites than the // current primary, switch primary. Once we switch we will // then tend to stick to the new GPS as primary. We don't // want to switch too often as it will look like a // position shift to the controllers. primary_instance = i; _last_instance_swap_ms = now; } } } else { primary_instance = 0; } } // update notify with gps status. We always base this on the primary_instance AP_Notify::flags.gps_status = state[primary_instance].status; AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;}
接下来的是一个选取哪个GPS作为起作用的GPS的算法。update_instance(i);继续往下:
/* update one GPS instance. This should be called at 10Hz or greater */voidAP_GPS::update_instance(uint8_t instance){ if (_type[instance] == GPS_TYPE_HIL) { // in HIL, leave info alone return; } if (_type[instance] == GPS_TYPE_NONE) { // not enabled state[instance].status = NO_GPS; state[instance].hdop = 9999; return; } if (locked_ports & (1U<<instance)) { // the port is locked by another driver return; } if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { // we don't yet know the GPS type of this one, or it has timed // out and needs to be re-initialised detect_instance(instance); return; } if (_auto_config == 1) { send_blob_update(instance); } // we have an active driver for this instance bool result = drivers[instance]->read();//这里读取GPS的入口,这里特别需要注意 uint32_t tnow = AP_HAL::millis(); // if we did not get a message, and the idle timer of 2 seconds // has expired, re-initialise the GPS. This will cause GPS // detection to run again if (!result) { if (tnow - timing[instance].last_message_time_ms > 4000) { // free the driver before we run the next detection, so we // don't end up with two allocated at any time delete drivers[instance]; drivers[instance] = nullptr; memset(&state[instance], 0, sizeof(state[instance])); state[instance].instance = instance; state[instance].status = NO_GPS; state[instance].hdop = 9999; timing[instance].last_message_time_ms = tnow; } } else { timing[instance].last_message_time_ms = tnow; if (state[instance].status >= GPS_OK_FIX_2D) { timing[instance].last_fix_time_ms = tnow; } }}
这段代码bool result =drivers[instance]->read();是实现数据跟新的函数,之后有一个计时,如果2s没收到数据则重新初始化GPS。这里read是class AP_GPS_Backend的虚函数,这里需要划个重点,就是apm在实现传感器设备的驱动时候提出了一个前后端的概念:
啥意思呢,就是说APM会为这个传感器设备定义一个前段class用来提供数据给上层应用数据;同时也会存在一个后端class用来实现读取传感器的值;
需要特别注意的是APM自带的传感器驱动都会存在libraries/AP_xxx下,通过AP_xxx_PX4.cpp来实现对PX4驱动的复用;
问题就来了,APM在实现这个后端class的时候,读取传感器的方式有两种,下面挨个介绍:
GPS这种是第一种,也就是我之前写驱动理解的那种;继续。。。
public AP_GPS_Backend这个后端的class会被AP_GPS_PX4公共继承,所以里面也就会有read方法的实现:
// update internal state if new GPS information is availableboolAP_GPS_PX4::read(void){ bool updated = false; orb_check(_gps_sub, &updated); if (updated) { if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) {//这里清晰可见采用了orb的方式从发布者那里获取到了数据 state.last_gps_time_ms = AP_HAL::millis(); state.status = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX); state.num_sats = _gps_pos.satellites_used; state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f); if (_gps_pos.fix_type >= 2) { state.location.lat = _gps_pos.lat; state.location.lng = _gps_pos.lon; state.location.alt = _gps_pos.alt/10; state.ground_speed = _gps_pos.vel_m_s; state.ground_course = wrap_360(degrees(_gps_pos.cog_rad)); state.hdop = _gps_pos.eph*100; // convert epoch timestamp back to gps epoch - evil hack until we get the genuine // raw week information (or APM switches to Posix epoch ;-) ) const uint64_t posix_offset = 3657ULL * 24 * 3600 * 1000 - GPS_LEAPSECONDS_MILLIS; const uint64_t ms_per_week = 7ULL * 24 * 3600 * 1000; uint64_t epoch_ms = _gps_pos.time_utc_usec/1000; uint64_t gps_ms = epoch_ms - posix_offset; state.time_week = (uint16_t)(gps_ms / ms_per_week); state.time_week_ms = (uint32_t)(gps_ms - (state.time_week) * ms_per_week); if (_gps_pos.time_utc_usec == 0) { // This is a work-around for https://github.com/PX4/Firmware/issues/1474 // reject position reports with invalid time, as APM adjusts it's clock after the first lock has been acquired state.status = AP_GPS::NO_FIX; } } if (_gps_pos.fix_type >= 3) { state.have_vertical_velocity = _gps_pos.vel_ned_valid; state.velocity.x = _gps_pos.vel_n_m_s; state.velocity.y = _gps_pos.vel_e_m_s; state.velocity.z = _gps_pos.vel_d_m_s; state.speed_accuracy = _gps_pos.s_variance_m_s; state.have_speed_accuracy = true; } else { state.have_vertical_velocity = false; } } } return updated;}
采用了orb的方式来订阅了消息,下半部分也就是src/driver下面就针对性的写了GPS的对应驱动,这和我之前写xsens的方法是一样的;
那么问题来了,这第二中是怎样的呢?这个就要从ins的更新说起;
AHRS
还是从顶层代码入手SCHED_TASK(ahrs_update, 50, 6400),这里是对ahrs的更新,同样我们继续往下:
// update AHRS systemvoid Rover::ahrs_update(){ update_soft_armed();#if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update gcs_update();#endif // when in reverse we need to tell AHRS not to assume we are a // 'fly forward' vehicle, otherwise it will see a large // discrepancy between the mag and the GPS heading and will try to // correct for it, leading to a large yaw error ahrs.set_fly_forward(!in_reverse); ahrs.update();//这里实现AHRS的更新 // if using the EKF get a speed update now (from accelerometers) Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { ground_speed = norm(velocity.x, velocity.y); } if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(ins); }}因为AP_AHRS_DCM是公共继承AHRS的,所以继续AP_AHRS_DCM.update();往下:// run a full DCM update roundvoidAP_AHRS_DCM::update(void){ float delta_t; if (_last_startup_ms == 0) { _last_startup_ms = AP_HAL::millis(); } // tell the IMU to grab some data _ins.update();//这里进行了ins更新 // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { memset(&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; return; } // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // update trig values including _cos_roll, cos_pitch update_trig();}/* update gyro and accel values from backends */void AP_InertialSensor::update(void){ // during initialisation update() may be called without // wait_for_sample(), and a wait is implied wait_for_sample(); if (!_hil_mode) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { // mark sensors unhealthy and let update() in each backend // mark them healthy via _publish_gyro() and // _publish_accel() _gyro_healthy[i] = false; _accel_healthy[i] = false; _delta_velocity_valid[i] = false; _delta_angle_valid[i] = false; } for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update();//这里就是重点了 } // clear accumulators for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { _delta_velocity_acc[i].zero(); _delta_velocity_acc_dt[i] = 0; _delta_angle_acc[i].zero(); _delta_angle_acc_dt[i] = 0; } if (!_startup_error_counts_set) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { _accel_startup_error_count[i] = _accel_error_count[i]; _gyro_startup_error_count[i] = _gyro_error_count[i]; } if (_startup_ms == 0) { _startup_ms = AP_HAL::millis(); } else if (AP_HAL::millis()-_startup_ms > 2000) { _startup_error_counts_set = true; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_error_count[i] < _accel_startup_error_count[i]) { _accel_startup_error_count[i] = _accel_error_count[i]; } if (_gyro_error_count[i] < _gyro_startup_error_count[i]) { _gyro_startup_error_count[i] = _gyro_error_count[i]; } } // adjust health status if a sensor has a non-zero error count // but another sensor doesn't. bool have_zero_accel_error_count = false; bool have_zero_gyro_error_count = false; for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) { have_zero_accel_error_count = true; } if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) { have_zero_gyro_error_count = true; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) { // we prefer not to use a gyro that has had errors _gyro_healthy[i] = false; } if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) { // we prefer not to use a accel that has had errors _accel_healthy[i] = false; } } // set primary to first healthy accel and gyro for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i] && _use[i]) { _primary_gyro = i; break; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i] && _use[i]) { _primary_accel = i; break; } } } _have_sample = false;}
_backends就是之前提到的前后端的后端class,但是这个后端class却不是按照之前的驱动实现方式来获取传感器的值的,同样我们找到了继承后端的类的实现class AP_InertialSensor_Invensense : public
AP_InertialSensor_Backend
在AP_InertialSensor_Invensense也就是mpu6000中,驱动的实现方式是这样的:
void AP_InertialSensor_Invensense::_read_fifo(){ uint8_t n_samples; uint16_t bytes_read; uint8_t *rx = _fifo_buffer; bool need_reset = false; if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) { goto check_registers; } bytes_read = uint16_val(rx, 0); n_samples = bytes_read / MPU_SAMPLE_SIZE; if (n_samples == 0) { /* Not enough data in FIFO */ goto check_registers; } /* 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 */ if (n_samples > 32) { need_reset = true; n_samples = 24; } while (n_samples > 0) { uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN); 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)) { hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE); _dev->set_chip_select(false); goto check_registers; } _dev->set_chip_select(false); } if (_fast_sampling) { if (!_accumulate_fast_sampling(rx, n)) { debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE); break; } } else { if (!_accumulate(rx, n)) { break; } } n_samples -= n; } if (need_reset) { //debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE); _fifo_reset(); }check_registers: // check next register value for correctness _dev->set_speed(AP_HAL::Device::SPEED_LOW); if (!_dev->check_next_register()) { _inc_gyro_error_count(_gyro_instance); _inc_accel_error_count(_accel_instance); } _dev->set_speed(AP_HAL::Device::SPEED_HIGH);}bool AP_InertialSensor_Invensense::_block_read(uint8_t reg, uint8_t *buf, uint32_t size){ return _dev->read_registers(reg, buf, size);}
AP_HAL::OwnPtr _dev;这个_dev是之前我们梳理过的APM的抽象框架,这个设备就可以绕过之前我们编写xsens驱动的方式来直接读取传感器的值了,下面是这个dev的定义;
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr dev,
enum Rotation rotation = ROTATION_NONE);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr dev,
enum Rotation rotation = ROTATION_NONE);
总结一下
HAL和单独的驱动区别有那些,共同的基础又由哪些:
1.不管是HAL还是单独存在src/drv下的驱动,他们都是基于nuttx这个RTOS的,所以这是它们唯一接触到硬件的接口;
2.即使是独立驱动,它也是通过继承AP_HAL::I2C或者AP_HAL::SPI等等来调用nuttx的驱动接口来获取传感器的数据的;
3.区别就在于两点:其一就是,单独驱动传输数据是需要通过orb来实现的,直接通过上面这种则更直接;其二就是,libraries下的AP_库是包含了数据的处理算法的,所以说,数据和驱动几乎就融合在了一起,包括对传感器的设置等等操作都包含在这里面;
说说优劣
显然采用直接的方式,可以去掉orb环节,保证数据的及时性;但是在代码的模块化程度上相对于采用orb的独立驱动就低了很多;
- 2.2[Lib]ardupilot设备驱动实现方式
- linux设备驱动加载方式
- platform设备驱动组织方式
- 用拖拉实现设备驱动配置(EsayHMI最新驱动配置方式)
- 用拖拉实现设备驱动配置(EsayHMI最新驱动配置方式)
- 用拖拉实现设备驱动配置(EsayHMI最新驱动配置方式)
- ARDupilot
- Ardupilot下移植CAN总线驱动
- 谈论attribute驱动实现方式(及device_create与设备节点的关系)
- linux 驱动-----字符设备globalmem驱动实现
- newstyle方式的i2c设备驱动
- PCI驱动访问设备内存方式
- 驱动,设备和总线分块编程方式
- 字符设备驱动-中断方式操控按键
- 字符设备驱动应用---LED设备驱动实现
- Linux设备驱动开发-linux驱动中的阻塞访问方式
- Linux设备驱动开发-linux驱动中的阻塞访问方式
- 字符设备驱动--查询方式的按键驱动
- javascript基础
- 真正的REST ful 架构
- 二叉树的镜像、对称的二叉树
- log4g配置(一)
- 初探Java装饰者模式和继承模式
- 2.2[Lib]ardupilot设备驱动实现方式
- centos 关闭防火墙以及关闭selinux方法
- strrchr
- HTML5 -- canvas实现简易画板
- 从0开始搭建阿里云(腾讯云)ubuntu16.04 nodejs + nginx + mysql + pm2 服务(二、搭建nginx环境)
- Blobs' Exhibition opentrains
- 找出两个不相交连续子数组的最大和
- 《Python全栈开发》学习过程笔记【3】
- input requried 提示文字修改