ArduPilot开源代码之AP

ArduPilot开源代码之AP_RangeFinder

1. 源由
2. 框架设计
- 2.1 启动代码
- 2.2 任务代码
3. 重要例程
- 3.1 状态更新
- 3.2 传感设备检测
4. 总结
5. 参考资料

1. 源由

AP_RangeFinder的应用的主要用途是用于测量对地距离的，这个与大家通常理解的障碍物避障还是有比较大的差异的。

本次结合代码进行研读。

2. 框架设计

启动代码：

Copter::init_ardupilot└──> Copter::init_rangefinder└──> rangefinder.init

任务代码：

SCHED_TASK(read_rangefinder,      20,    100,  33),└──> Copter::read_rangefinder└──> rangefinder.update

2.1 启动代码

主要作用是对传感器设备进行检测和初始化。

RangeFinder::init├──> <num_instances != 0> // don't re-init if we've found some sensors already│   └──> return├──> <uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++> // set orientation defaults│   └──> params[i].orientation.set_default(orientation_default)├──> <uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++>│   │  // serial_instance will be increased inside detect_instance│   │  // if a serial driver is loaded for this instance│   ├──> WITH_SEMAPHORE(detect_sem);│   ├──> detect_instance(i, serial_instance);│   └──> <drivers[i] != nullptr>│       │  // we loaded a driver for this instance, so it must be│       │  // present (although it may not be healthy). We use MAX()│       │  // here as a UAVCAN rangefinder may already have been│       │  // found│       └──> num_instances = MAX(num_instances, i+1);└──> [initialise status]├──> state[i].status = Status::NotConnected;└──> state[i].range_valid_count = 0;

2.2 任务代码

以固定的频率进行循环遍历，得到相应的数据更新。

// return rangefinder altitude in centimeters
Copter::read_rangefinder├──> <RANGEFINDER_ENABLED != ENABLED>│   ├──> [downward facing rangefinder]│   │   ├──> rangefinder_state.enabled = false;│   │   ├──> rangefinder_state.alt_healthy = false;│   │   └──> rangefinder_state.alt_cm = 0;│   ├──> [upward facing rangefinder]│   │   ├──> rangefinder_up_state.enabled = false;│   │   ├──> rangefinder_up_state.alt_healthy = false;│   │   └──> rangefinder_up_state.alt_cm = 0;│   └──> return├──> rangefinder.update();├──> <RANGEFINDER_TILT_CORRECTION == ENABLED>│   └──> const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);├──> <RANGEFINDER_TILT_CORRECTION != ENABLED>│   └──> const float tilt_correction = 1.0f;│││  // iterate through downward and upward facing lidar│  struct {│      RangeFinderState &state;│      enum Rotation orientation;│  } rngfnd[2] = {{rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};│└──> <for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++)>├──> [local variables to make accessing simpler]│   ├──> RangeFinderState &rf_state = rngfnd[i].state;│   └──> enum Rotation rf_orient = rngfnd[i].orientation;├──> [update health]│   └──> rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::Status::Good) &&│                          (rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));├──> [tilt corrected but unfiltered, not glitch protected alt]│   └──> rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);├──> [remember inertial alt to allow us to interpolate rangefinder]│   └──> rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();├──> [glitch handling] │   //rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading│   // are considered a glitch and glitch_count becomes non-zero│   // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.│   // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset│   ├──> const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected;│   ├──> bool reset_terrain_offset = false;│   ├──> <if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM)>│   │   └──> rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);│   ├──> <else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) >│   │   └──> rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);│   ├──> <else>│   │   ├──> rf_state.glitch_count = 0;│   │   └──> rf_state.alt_cm_glitch_protected = rf_state.alt_cm;│   └──> <abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES> // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes│       ├──> rf_state.glitch_count = 0;│       ├──> rf_state.alt_cm_glitch_protected = rf_state.alt_cm;│       ├──> rf_state.glitch_cleared_ms = AP_HAL::millis();│       └──> reset_terrain_offset = true;├──> [filter rangefinder altitude]│   ├──> uint32_t now = AP_HAL::millis();│   ├──> const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;│   ├──> <rf_state.alt_healthy>│   │   ├──> <if (timed_out)>│   │   │     // reset filter if we haven't used it within the last second│   │   │   ├──> rf_state.alt_cm_filt.reset(rf_state.alt_cm);│   │   │   └──> reset_terrain_offset = true;│   │   └──> <else >│   │       └──> rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);│   └──> rf_state.last_healthy_ms = now;├──> <reset_terrain_offset> // handle reset of terrain offset│   ├──> <if (rf_orient == ROTATION_PITCH_90)> // upward facing│   │   └──> rf_state.terrain_offset_cm = rf_state.inertial_alt_cm + rf_state.alt_cm;│   └──> < else > // assume downward facing│       └──> rf_state.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm; └──> <HAL_PROXIMITY_ENABLED> <rf_orient == ROTATION_PITCH_270> <rangefinder_state.alt_healthy || timed_out>│  // send downward facing lidar altitude and health to the libraries that require it└──> g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());

3. 重要例程

3.1 状态更新

针对当前传感器数据有效性进行状态更新。

NotConnected
NoData
OutOfRangeLow
OutOfRangeHigh
Good

RangeFinder::update├──> <uint8_t i=0; i<num_instances; i++>│   └──> <drivers[i] != nullptr>│       ├──> <(Type)params[i].type.get() == Type::NONE> // allow user to disable a rangefinder at runtime│       │   ├──> state[i].status = Status::NotConnected│       │   ├──> state[i].range_valid_count = 0│       │   └──> continue│       └──> drivers[i]->update()└──> <HAL_LOGGING_ENABLED>└──> Log_RFND()

3.2 传感设备检测

ANALOG
MBI2C
PLI2C
PX4_PWM
BBB_PRU
LWI2C
LWSER
BEBOP
MAVLink
USD1_Serial
LEDDARONE
MBSER
TRI2C
PLI2CV3
VL53L0X
NMEA
WASP
BenewakeTF02
BenewakeTFmini
PLI2CV3HP
PWM
BLPing
UAVCAN
BenewakeTFminiPlus
Lanbao
BenewakeTF03
VL53L1X_Short
LeddarVu8_Serial
HC_SR04
GYUS42v2
MSP
USD1_CAN
Benewake_CAN
TeraRanger_Serial
Lua_Scripting
NoopLoop_P
TOFSenseP_CAN
NRA24_CAN
SIM

RangeFinder::detect_instance├──> <!AP_RANGEFINDER_ENABLED>│   └──> return├──> AP_RangeFinder_Backend_Serial *(*serial_create_fn)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&) = nullptr;├──> const Type _type = (Type)params[instance].type.get();├──> <case PLI2C/PLI2CV3/PLI2CV3HP> <AP_RANGEFINDER_PULSEDLIGHTLRF_ENABLED>│   ├──> _add_backend(AP_RangeFinder_PulsedLightLRF::detect│   └──> break;├──> <MBI2C> <AP_RANGEFINDER_MAXSONARI2CXL_ENABLED>│   ├──> uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR;│   ├──> <params[instance].address != 0>│   │   └──> addr = params[instance].address;│   ├──> _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect│   └──> break;├──> <LWI2C> <AP_RANGEFINDER_LWI2C_ENABLED>│   ├──> <params[instance].address> // the LW20 needs a long time to boot up, so we delay 1.5s here│   │   ├──> <HAL_RANGEFINDER_LIGHTWARE_I2C_BUS>│   │   │   └──> _add_backend(AP_RangeFinder_LightWareI2C::detect│   │   └──> <!HAL_RANGEFINDER_LIGHTWARE_I2C_BUS>│   │       └──> _add_backend(AP_RangeFinder_LightWareI2C::detect│   └──> break;├──> <TRI2C> <AP_RANGEFINDER_TRI2C_ENABLED>│   ├──> <params[instance].address>│   │   └──> __add_backend(AP_RangeFinder_TeraRangerI2C::detect│   └──> break;├──> <VL53L0X/VL53L1X_Short> │   ├──> <AP_RANGEFINDER_VL53L0X_ENABLED>│   │   └──> _add_backend(AP_RangeFinder_VL53L0X::detect│   ├──> <AP_RANGEFINDER_VL53L1X_ENABLED>│   │   └──> _add_backend(AP_RangeFinder_VL53L1X::detect│   └──> break;├──> <BenewakeTFminiPlus> <AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED>│   ├──> uint8_t addr = TFMINIPLUS_ADDR_DEFAULT;│   ├──> <params[instance].address != 0>│   │   └──> addr = params[instance].address;│   ├──> _add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect│   └──> break;├──> <PX4_PWM> <AP_RANGEFINDER_PWM_ENABLED>│   │  // to ease moving from PX4 to ChibiOS we'll lie a little about│   │  // the backend driver...│   ├──> <AP_RangeFinder_PWM::detect()>│   │   └──> _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);│   └──> break;├──> <BBB_PRU> <AP_RANGEFINDER_BBB_PRU_ENABLED>│   ├──> <AP_RangeFinder_BBB_PRU::detect()>│   │   └──> _add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance);│   └──> break;├──> <LWSER> <AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_LightWareSerial::create;│   └──> break;├──> <LEDDARONE> <AP_RANGEFINDER_LEDDARONE_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_LeddarOne::create;│   └──> break;├──> <USD1_Serial> <AP_RANGEFINDER_USD1_SERIAL_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_USD1_Serial::create;│   └──> break;├──> <BEBOP> <AP_RANGEFINDER_BEBOP_ENABLED>│   ├──> <AP_RangeFinder_Bebop::detect()>│   │   └──> _add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance);│   └──> break;├──> <MAVLink> <AP_RANGEFINDER_MAVLINK_ENABLED>│   ├──> <AP_RangeFinder_MAVLink::detect()>│   │   └──> _add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance);│   └──> break;├──> <MBSER> <AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;│   └──> break;├──> <ANALOG> <AP_RANGEFINDER_ANALOG_ENABLED>│   ├──> <AP_RangeFinder_analog::detect(params[instance])> // note that analog will always come back as present if the pin is valid│   │   └──> _add_backend(new AP_RangeFinder_analog(state[instance], params[instance]), instance);│   └──> break;├──> <HC_SR04> <AP_RANGEFINDER_HC_SR04_ENABLED>│   ├──> <AP_RangeFinder_HC_SR04::detect(params[instance])>  // note that this will always come back as present if the pin is valid│   │   └──> _add_backend(new AP_RangeFinder_HC_SR04(state[instance], params[instance]), instance);│   └──> break;├──> <NMEA> <AP_RANGEFINDER_NMEA_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_NMEA::create;│   └──> break;├──> <WASP> <AP_RANGEFINDER_WASP_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_Wasp::create;│   └──> break;├──> <BenewakeTF02> <AP_RANGEFINDER_BENEWAKE_TF02_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_Benewake_TF02::create;│   └──> break;├──> <BenewakeTFmini> <AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;│   └──> break;├──> <BenewakeTF03> <AP_RANGEFINDER_BENEWAKE_TF03_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_Benewake_TF03::create;│   └──> break;├──> <TeraRanger_Serial> <AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;│   └──> break;├──> <PWM> <AP_RANGEFINDER_PWM_ENABLED>│   ├──> <AP_RangeFinder_PWM::detect()>│   │   └──> _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);│   └──> break;├──> <BLPing> <AP_RANGEFINDER_BLPING_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_BLPing::create;│   └──> break;├──> <Lanbao> <AP_RANGEFINDER_LANBAO_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_Lanbao::create;│   └──> break;├──> <LeddarVu8_Serial> <AP_RANGEFINDER_LEDDARVU8_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_LeddarVu8::create;│   └──> break;├──> <UAVCAN> <AP_RANGEFINDER_DRONECAN_ENABLED>│   │  /*│   │    the UAVCAN driver gets created when we first receive a│   │    measurement. We take the instance slot now, even if we don't│   │    yet have the driver│   │   */│   ├──> num_instances = MAX(num_instances, instance+1);│   └──> break;├──> <GYUS42v2> <AP_RANGEFINDER_GYUS42V2_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_GYUS42v2::create;│   └──> break;├──> <SIM> <AP_RANGEFINDER_SIM_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);│   └──> break;├──> <MSP> <HAL_MSP_RANGEFINDER_ENABLED>│   ├──> <AP_RangeFinder_MSP::detect()>│   │   └──> _add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance);│   └──> break;├──> <USD1_CAN> <AP_RANGEFINDER_USD1_CAN_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);│   └──> break;├──> <Benewake_CAN> <AP_RANGEFINDER_BENEWAKE_CAN_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);│   └──> break;├──> <Lua_Scripting> <AP_SCRIPTING_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance);│   └──> break;├──> <NoopLoop_P> ,AP_RANGEFINDER_NOOPLOOP_ENABLED>│   ├──> serial_create_fn = AP_RangeFinder_NoopLoop::create;│   └──> break;├──> <TOFSenseP_CAN> <AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);│   └──> break;├──> <NRA24_CAN> <AP_RANGEFINDER_NRA24_CAN_ENABLED>│   ├──> _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);│   └──> break;├──> <serial_create_fn != nullptr> <AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)>│   ├──> auto *b = serial_create_fn(state[instance], params[instance]);│   └──> <b != nullptr>│       └──> _add_backend(b, instance, serial_instance++);└──> <drivers[instance] && state[instance].var_info>  // if the backend has some local parameters then make those available in the tree├──> backend_var_info[instance] = state[instance].var_info;├──> AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]);      └──> AP_Param::invalidate_count(); // param count could have changed

4. 总结

各传感器设备因设备总线以及协议的差异，存在一定的差异化，在相应的驱动代码中实现，可参考：ArduPilot之开源代码Library&Sketches设计。

作为RangeFinder传感器主要的目的是检测飞机与地面之间的距离

    // The RangeFinder_State structure is filled in by the backend driverstruct RangeFinder_State {float distance_m;               // distance in metersuint16_t voltage_mv;            // voltage in millivolts, if applicable, otherwise 0enum RangeFinder::Status status; // sensor statusuint8_t  range_valid_count;     // number of consecutive valid readings (maxes out at 10)uint32_t last_reading_ms;       // system time of last successful update from sensorconst struct AP_Param::GroupInfo *var_info;};

注：不同设备更新频率方面主要在驱动中进行数据采集更新，详见驱动代码的init方法中如何挂定时回调钩子函数。