ArduPilot开源代码之AP_RangeFinder
ArduPilot开源代码之AP_RangeFinder
- 1. 源由
- 2. 框架设计
-
- 2.1 启动代码
- 2.2 任务代码
- 3. 重要例程
-
- 3.1 状态更新
- 3.2 传感设备检测
- 4. 总结
- 5. 参考资料
1. 源由
AP_RangeFinder的应用的主要用途是用于测量对地距离的，这个与大家通常理解的障碍物避障还是有比较大的差异的。
主要的应用有：
- Terrain Following (in Auto, Guided, etc)
- Surface Tracking
其他应用有：
- Simple Object Avoidance
- Copter Object Avoidance
本次结合代码进行研读，其中也请注意两个应用类型的传感器：upward facing rangefinder和downward facing 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
├──> // don't re-init if we've found some sensors already
│ └──> return
├──> // set orientation defaults
│ └──> params[i].orientation.set_default(orientation_default)
├──>
│ │ // 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);
│ └──>
│ │ // 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
├──>
│ ├──> [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();
├──>
│ └──> const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
├──>
│ └──> 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}};
│
└──>
├──> [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;
│ ├──> = RANGEFINDER_GLITCH_ALT_CM)>
│ │ └──> rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);
│ ├──>
│ │ └──> rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);
│ ├──>
│ │ ├──> rf_state.glitch_count = 0;
│ │ └──> rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
│ └──> = 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;
│ ├──>
│ │ ├──>
│ │ │ // 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;
│ │ └──>
│ │ └──> rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
│ └──> rf_state.last_healthy_ms = now;
├──> // handle reset of terrain offset
│ ├──> // 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;
└──>
│ // 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
├──>
│ └──>
│ ├──> <(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()
└──>
└──> Log_RFND()
3.2 传感设备检测
除了AP_RangeFinder里面使用到的传感器外，衍生阅读可看下360度雷达以及视觉传感，详见Rangefinders (landing page)。
- 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
├──>
│ └──> return
├──> AP_RangeFinder_Backend_Serial *(*serial_create_fn)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&) = nullptr;
├──> const Type _type = (Type)params[instance].type.get();
├──>
│ ├──> _add_backend(AP_RangeFinder_PulsedLightLRF::detect
│ └──> break;
├──>
│ ├──> uint8_t addr = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR;
│ ├──>
│ │ └──> addr = params[instance].address;
│ ├──> _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect
│ └──> break;
├──>
│ ├──> // the LW20 needs a long time to boot up, so we delay 1.5s here
│ │ ├──>
│ │ │ └──> _add_backend(AP_RangeFinder_LightWareI2C::detect
│ │ └──>
│ │ └──> _add_backend(AP_RangeFinder_LightWareI2C::detect
│ └──> break;
├──>
│ ├──>
│ │ └──> __add_backend(AP_RangeFinder_TeraRangerI2C::detect
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(AP_RangeFinder_VL53L0X::detect
│ ├──>
│ │ └──> _add_backend(AP_RangeFinder_VL53L1X::detect
│ └──> break;
├──>
│ ├──> uint8_t addr = TFMINIPLUS_ADDR_DEFAULT;
│ ├──>
│ │ └──> addr = params[instance].address;
│ ├──> _add_backend(AP_RangeFinder_Benewake_TFMiniPlus::detect
│ └──> break;
├──>
│ │ // to ease moving from PX4 to ChibiOS we'll lie a little about
│ │ // the backend driver...
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_BBB_PRU(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_LightWareSerial::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_LeddarOne::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_USD1_Serial::create;
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_Bebop(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_MAVLink(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;
│ └──> break;
├──>
│ ├──> // 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;
├──>
│ ├──> // 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;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_NMEA::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_Wasp::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_Benewake_TF02::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_Benewake_TF03::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height), instance);
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_BLPing::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_Lanbao::create;
│ └──> break;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_LeddarVu8::create;
│ └──> break;
├──>
│ │ /*
│ │ 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;
├──>
│ ├──> serial_create_fn = AP_RangeFinder_GYUS42v2::create;
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_SITL(state[instance], params[instance], instance), instance);
│ └──> break;
├──>
│ ├──>
│ │ └──> _add_backend(new AP_RangeFinder_MSP(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance);
│ └──> break;
├──> ,AP_RANGEFINDER_NOOPLOOP_ENABLED>
│ ├──> serial_create_fn = AP_RangeFinder_NoopLoop::create;
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> _add_backend(new AP_RangeFinder_NRA24_CAN(state[instance], params[instance]), instance);
│ └──> break;
├──>
│ ├──> auto *b = serial_create_fn(state[instance], params[instance]);
│ └──>
│ └──> _add_backend(b, instance, serial_instance++);
└──> // 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 driver
struct RangeFinder_State {
float distance_m; // distance in meters
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
enum RangeFinder::Status status; // sensor status
uint8_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 sensor
const struct AP_Param::GroupInfo *var_info;
};
注：不同设备更新频率方面主要在驱动中进行数据采集更新，详见驱动代码的init方法中如何挂定时回调钩子函数。
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot飞控启动&运行过程简介
【3】ArduPilot之开源代码Library&Sketches设计