ArduPilot开源代码之AP_RangeFinder

原创 已于 2023-10-04 07:50:05 修改 · 743 阅读 · 3 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#Ardupilot #开源

于 2023-10-03 13:22:46 首次发布
本文结合代码研读Ardupilot中AP_RangeFinder的应用，其主要用于测量对地距离。介绍了框架设计，包括启动代码对传感器检测初始化、任务代码循环遍历更新数据；还提及重要例程，如状态更新和传感设备检测，最后指出传感器因总线和协议有差异，在驱动代码实现。

ArduPilot开源代码之AP_RangeFinder

1. 源由

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

主要的应用有：

其他应用有：

本次结合代码进行研读，其中也请注意两个应用类型的传感器：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
 ├──> <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 传感设备检测

除了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
 ├──> <!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 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设计

ardupilot开发 --- 测试 篇
记录老板的每次pua，分享领导每天的毒鸡汤...
02-28 1686
ardupilot开发 --- 测试
无人机飞控 ardupilot Copter-4.0.7 库简介
嵌入式Stark
12-31 2459
ardupilot libraries 简介 官方文档-库说明 核心库： 库名称 作用 AP_AHRS 使用 DCM 或 EKF 进行姿态估计 AP_Common 所有草图和库所需的核心内容 AP_Math 各种数学函数，特别适用于矢量操作 AC_PID PID（比例积分-微分）控制器库 AP_InertialNav 惯性导航库，用于将加速度计输入与GPS和气压数据混合 AC_AttitudeControl ArduCopter的控制库包括各种功能的姿态，基于PID控制
Ardupilot — AP_RangeFinder代码梳理
热门推荐
无人机开发（开源Ardupilot）
09-08 1万+
本文主要梳理，Ardupilot中测距仪的代码运行流程，以及如何选择相应的测距仪类型。
PX4——Range Finder 篇
lib0000的博客
04-17 950
Range Finder 此处选用的是 Benewake 下的 Lidar 参数设置 General Configuration 除了官方的参数设置外，我在 EKF2 中还找到了 Benewake
Ardupilot 4.0（ChibiOS）学习笔记
weixin_47710106的博客
05-20 2200
其中，ap_common_libiraries在Tools/ardupilotwaf/ardupilotwaf.py中定义，为每个载具类型都需要包含的库（如驱动程序、EKF滤波、PID等）用于定义同一份代码在不同硬件上的抽象映射，获得硬件兼容性，在4.0的ChibiOs版本中，硬件定义在libraries/AP_HAL_ChibiOS/hwdef中，找到自己的硬件板子。//-----------------后续再根据需要记录如何使用Tools内的工具。5）Tools工具/外部支持代码。
Benewake(北醒) TFmini-i-CAN 雷达在Ardupilot (APM)上基于PixHawk的运用
zoran_wu的博客
03-23 3806
TFmini-i CAN PixHawk1 CAN 端口或任何已刷新 Ardupilot 固件并具有 CAN 接口的飞控连接。从 Copter 4.1.4 开始，Ardupilot 固件添加了对 CAN 协议的支持，用于避障和高度保持。
class RangeFinder { public: friend class AP_RangeFinder_Backend; RangeFinder(void); // RangeFinder driver types enum RangeFinder_Type { RangeFinder_TYPE_NONE = 0, RangeFinder_TYPE_ANALOG = 1, RangeFinder_TYPE_MBI2C = 2, RangeFinder_TYPE_PLI2C = 3, RangeFinder_TYPE_PX4 = 4, RangeFinder_TYPE_PX4_PWM= 5, RangeFinder_TYPE_BBB_PRU= 6 }; enum RangeFinder_Function { FUNCTION_LINEAR = 0, FUNCTION_INVERTED = 1, FUNCTION_HYPERBOLA = 2 }; enum RangeFinder_Status { RangeFinder_NotConnected = 0, RangeFinder_NoData, RangeFinder_OutOfRangeLow, RangeFinder_OutOfRangeHigh, RangeFinder_Good }; // The RangeFinder_State structure is filled in by the backend driver struct RangeFinder_State { uint8_t instance; // the instance number of this RangeFinder uint16_t distance_cm; // distance: in cm 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) bool pre_arm_check; // true if sensor has passed pre-arm checks uint16_t pre_arm_distance_min; // min distance captured during pre-arm checks uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks }; // parameters for each instance AP_Int8 _type[RANGEFINDER_MAX_INSTANCES]; AP_Int8 _pin[RANGEFINDER_MAX_INSTANCES]; AP_Int8 _ratiometric[RANGEFINDER_MAX_INSTANCES]; AP_Int8 _stop_pin[RANGEFINDER_MAX_INSTANCES]; AP_Int16 _settle_time_ms[RANGEFINDER_MAX_INSTANCES]; AP_Float _scaling[RANGEFINDER_MAX_INSTANCES]; AP_Float _offset[RANGEFINDER_MAX_INSTANCES]; AP_Int8 _function[RANGEFINDER_MAX_INSTANCES]; AP_Int16 _min_distance_cm[RANGEFINDER_MAX_INSTANCES]; AP_Int16 _max_distance_cm[RANGEFINDER_MAX_INSTANCES]; AP_Int8 _ground_clearance_cm[RANGEFINDER_MAX_INSTANCES]; AP_Int16 _powersave_range; static const struct AP_Param::GroupInfo var_info[]; // Return the number of range finder instances uint8_t num_sensors(void) const { return num_instances; } // detect and initialise any available rangefinders void init(void); // update state of all rangefinders. Should be called at around // 10Hz from main loop void update(void); #define _RangeFinder_STATE(instance) state[instance] uint16_t distance_cm(uint8_t instance) const { return _RangeFinder_STATE(instance).distance_cm; } uint16_t distance_cm() const { return distance_cm(primary_instance); } uint16_t voltage_mv(uint8_t instance) const { return _RangeFinder_STATE(instance).voltage_mv; } uint16_t voltage_mv() const { return voltage_mv(primary_instance); } int16_t max_distance_cm(uint8_t instance) const { return _max_distance_cm[instance]; } int16_t max_distance_cm() const { return max_distance_cm(primary_instance); } int16_t min_distance_cm(uint8_t instance) const { return _min_distance_cm[instance]; } int16_t min_distance_cm() const { return min_distance_cm(primary_instance); } int16_t ground_clearance_cm(uint8_t instance) const { return _ground_clearance_cm[instance]; } int16_t ground_clearance_cm() const { return _ground_clearance_cm[primary_instance]; } // query status RangeFinder_Status status(uint8_t instance) const; RangeFinder_Status status(void) const { return status(primary_instance); } // true if sensor is returning data bool has_data(uint8_t instance) const; bool has_data() const { return has_data(primary_instance); } // returns count of consecutive good readings uint8_t range_valid_count() const { return range_valid_count(primary_instance); } uint8_t range_valid_count(uint8_t instance) const { return _RangeFinder_STATE(instance).range_valid_count; } /* set an externally estimated terrain height. Used to enable power saving (where available) at high altitudes. */ void set_estimated_terrain_height(float height) { estimated_terrain_height = height; } /* returns true if pre-arm checks have passed for all range finders these checks involve the user lifting or rotating the vehicle so that sensor readings between the min and 2m can be captured */ bool pre_arm_check() const; private: RangeFinder_State state[RANGEFINDER_MAX_INSTANCES]; AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES]; uint8_t primary_instance:2; uint8_t num_instances:2; float estimated_terrain_height; void detect_instance(uint8_t instance); void update_instance(uint8_t instance); void update_pre_arm_check(uint8_t instance); }; #endif // __RANGEFINDER_H__ 这个class的定义说了什么？从代码结构角度进行分析
07-17
> **注意**：实际实现可能包含更复杂的健康监测、自动校准等功能，参考ArduPilot的`AP_RangeFinder`模块[^3]。 §§相关问题§§ 1. 在嵌入式系统中如何优化传感器数据读取的实时性？ 2. 多传感器融合时如何解决...
ardupilot/arduplane/altitude.cpp 高度保持程序/地形跟踪控制解析
qq_17031687的博客
11-16 1009
ardupilot/arduplane/altitude.cpp 高度保持程序/地形跟踪控制解析
ardupilot/Ardupilot V4.3.4 源码解析：Ardupilot.cpp
qq_17031687的博客
08-02 976
if (AP_HAL::millis() - plane.launch_detector_pin_last_inactive_time_ms < ((uint32_t)plane.g2.launch_detector_delay_ms)) { // 虽然已经触发，但是没有达到延时时间。if (hal.gpio->read(plane.g2.launch_detector_pin) == plane.g2.launch_detector_polarity) { // 处于已触发状态。
const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = { // @Param: _TYPE // @DisplayName: Rangefinder type // @Description: What type of rangefinder device that is connected // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0), // @Param: _PIN // @DisplayName: Rangefinder pin // @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. // @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port AP_GROUPINFO("_PIN", 1, RangeFinder, _pin[0], -1), // @Param: _SCALING // @DisplayName: Rangefinder scaling // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. // @Units: meters/Volt // @Increment: 0.001 AP_GROUPINFO("_SCALING", 2, RangeFinder, _scaling[0], 3.0f), // @Param: _OFFSET // @DisplayName: rangefinder offset // @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM and I2C Lidars // @Units: Volts // @Increment: 0.001 AP_GROUPINFO("_OFFSET", 3, RangeFinder, _offset[0], 0.0f), // @Param: _FUNCTION // @DisplayName: Rangefinder function // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. // @Values: 0:Linear,1:Inverted,2:Hyperbolic AP_GROUPINFO("_FUNCTION", 4, RangeFinder, _function[0], 0), // @Param: _MIN_CM // @DisplayName: Rangefinder minimum distance // @Description: Minimum distance in centimeters that rangefinder can reliably read // @Units: centimeters // @Increment: 1 AP_GROUPINFO("_MIN_CM", 5, RangeFinder, _min_distance_cm[0], 20), // @Param: _MAX_CM // @DisplayName: Rangefinder maximum distance // @Description: Maximum distance in centimeters that rangefinder can reliably read // @Units: centimeters // @Increment: 1 AP_GROUPINFO("_MAX_CM", 6, RangeFinder, _max_distance_cm[0], 700), // @Param: _STOP_PIN // @DisplayName: Rangefinder stop pin // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 AP_GROUPINFO("_STOP_PIN", 7, RangeFinder, _stop_pin[0], -1), // @Param: _SETTLE // @DisplayName: Rangefinder settle time // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. // @Units: milliseconds // @Increment: 1 AP_GROUPINFO("_SETTLE", 8, RangeFinder, _settle_time_ms[0], 0), // @Param: _RMETRIC // @DisplayName: Ratiometric // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. // @Values: 0:No,1:Yes AP_GROUPINFO("_RMETRIC", 9, RangeFinder, _ratiometric[0], 1), // @Param: RNGFND_PWRRNG // @DisplayName: Powersave range // @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled // @Units: meters // @Range: 0 32767 AP_GROUPINFO("_PWRRNG", 10, RangeFinder, _powersave_range, 0), // @Param: _GNDCLEAR // @DisplayName: Distance (in cm) from the range finder to the ground // @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground. // @Units: centimeters // @Range: 0 127 // @Increment: 1 // @User: Advanced AP_GROUPINFO("_GNDCLEAR", 11, RangeFinder, _ground_clearance_cm[0], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), // 10..12 left for future expansion #if RANGEFINDER_MAX_INSTANCES > 1 // @Param: 2_TYPE // @DisplayName: Second Rangefinder type // @Description: What type of rangefinder device that is connected // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0), // @Param: 2_PIN // @DisplayName: Rangefinder pin // @Description: Analog pin that rangefinder is connected to. Set this to 0..9 for the APM2 analog pins. Set to 64 on an APM1 for the dedicated 'airspeed' port on the end of the board. Set to 11 on PX4 for the analog 'airspeed' port. Set to 15 on the Pixhawk for the analog 'airspeed' port. // @Values: -1:Not Used, 0:APM2-A0, 1:APM2-A1, 2:APM2-A2, 3:APM2-A3, 4:APM2-A4, 5:APM2-A5, 6:APM2-A6, 7:APM2-A7, 8:APM2-A8, 9:APM2-A9, 11:PX4-airspeed port, 15:Pixhawk-airspeed port, 64:APM1-airspeed port AP_GROUPINFO("2_PIN", 13, RangeFinder, _pin[1], -1), // @Param: 2_SCALING // @DisplayName: Rangefinder scaling // @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. // @Units: meters/Volt // @Increment: 0.001 AP_GROUPINFO("2_SCALING", 14, RangeFinder, _scaling[1], 3.0f), // @Param: 2_OFFSET // @DisplayName: rangefinder offset // @Description: Offset in volts for zero distance // @Units: Volts // @Increment: 0.001 AP_GROUPINFO("2_OFFSET", 15, RangeFinder, _offset[1], 0.0f), // @Param: 2_FUNCTION // @DisplayName: Rangefinder function // @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters. // @Values: 0:Linear,1:Inverted,2:Hyperbolic AP_GROUPINFO("2_FUNCTION", 16, RangeFinder, _function[1], 0), // @Param: 2_MIN_CM // @DisplayName: Rangefinder minimum distance // @Description: Minimum distance in centimeters that rangefinder can reliably read // @Units: centimeters // @Increment: 1 AP_GROUPINFO("2_MIN_CM", 17, RangeFinder, _min_distance_cm[1], 20), // @Param: 2_MAX_CM // @DisplayName: Rangefinder maximum distance // @Description: Maximum distance in centimeters that rangefinder can reliably read // @Units: centimeters // @Increment: 1 AP_GROUPINFO("2_MAX_CM", 18, RangeFinder, _max_distance_cm[1], 700), // @Param: 2_STOP_PIN // @DisplayName: Rangefinder stop pin // @Description: Digital pin that enables/disables rangefinder measurement for an analog rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This can be used to ensure that multiple sonar rangefinders don't interfere with each other. // @Values: -1:Not Used,50:Pixhawk AUXOUT1,51:Pixhawk AUXOUT2,52:Pixhawk AUXOUT3,53:Pixhawk AUXOUT4,54:Pixhawk AUXOUT5,55:Pixhawk AUXOUT6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2 AP_GROUPINFO("2_STOP_PIN", 19, RangeFinder, _stop_pin[1], -1), // @Param: 2_SETTLE // @DisplayName: Sonar settle time // @Description: The time in milliseconds that the rangefinder reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the rangefinder to give a reading after we set the STOP_PIN high. For a sonar rangefinder with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again. // @Units: milliseconds // @Increment: 1 AP_GROUPINFO("2_SETTLE", 20, RangeFinder, _settle_time_ms[1], 0), // @Param: 2_RMETRIC // @DisplayName: Ratiometric // @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric. // @Values: 0:No,1:Yes AP_GROUPINFO("2_RMETRIC", 21, RangeFinder, _ratiometric[1], 1), // @Param: 2_GNDCLEAR // @DisplayName: Distance (in cm) from the second range finder to the ground // @Description: This parameter sets the expected range measurement(in cm) that the second range finder should return when the vehicle is on the ground. // @Units: centimeters // @Range: 0 127 // @Increment: 1 // @User: Advanced AP_GROUPINFO("2_GNDCLEAR", 22, RangeFinder, _ground_clearance_cm[1], RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), #endif AP_GROUPEND }; 这些代码说了什么
最新发布
07-17
由于用户提供的代码片段中没有`var_info`的定义，我们可以根据常见的ArduPilot代码结构来推测`RangeFinder`类中可能包含的`var_info`参数。 通常，在传感器驱动（如测距仪）中，`var_info`会定义如下参数： - 使...
Benewake(北醒) TFmini-S\TFmini Plus\TF-Luna\TF02-Pro 串口版本雷达在Arduino Nano上的例程
zoran_wu的博客
11-19 2773
TFminiArduino5V(红)5VGND(黑)GNDTX(绿)D0(RX)使用硬件串口、采用轮询方式注：使用硬件串口时，烧录程序过程中雷达绿色线不要插入Nano板D0TFminiArduino5V(红)5VGND(黑)GNDTX(绿)D0(RX)使用硬件串口、采用中断方式注：使用硬件串口时，烧录程序过程中雷达绿色线不要插入Nano板D0TFminiArduino5V(红)5VGND(黑)GNDTX(绿)D2(RX)使用软件件串口、采用轮询方式TFmini。
Benewake(北醒) TF-LC02 (TTL) 雷达使用TTL转USB转接板在Arduino Uno上的运用
zoran_wu的博客
12-27 5865
本例程利用Arduino Uno板和TTL转USB转接板，引入软串口用来打印收到雷达并处理后的数据
TFmini传感器使用
早起的鸟儿有虫吃
05-05 994
使用激光传感器 打开地面站，Mavlink控制台输入tfmini start --> tfmini status -->显示 current_distance:数字 即可得到距离； 将timini插到口UART&I2C B上面
DE-LiDAR TFmini 激光雷达测距
acchkr的博客
08-22 4605
转自：http://blog.sina.com.cn/s/blog_6266a8840102xcex.html 1.低成本激光雷达，测量范围为30cm到12m，这点很有优势。 2.修改ardupilot驱动，协议为： TFmini 激光雷达测距" title="DE-LiDAR TFmini 激光雷达测距" style="margin:0px;
TFmini Plus 在开源飞控Ardupilot上用作定高模块，并通过串口请求解析其数据
WZZ的博客
02-20 912
TFmini Plus 在开源飞控pixhawk上的应用，定高模块 并通过Mavlink消息请求激光数据
Ardupilot飞控接入多个GY-US42型号超声波测距传感器
weixin_43180028的博客
12-03 2003
引言 GY-US42超声波测距传感器成本低廉，测距范围20-720cm，拥有多种通信协议支持iic、uart、pwm三种模式，其iic协议和Maxsonar协议兼容，因此可以直接接入ardupilot。但是ardupilot默认Maxsonar测距代码并不兼容多个不同地址的设备同时运行，需要对其源码进行一定的修改。（下述源码修改基于4.0.0master版本） 改变 全局查找函数并定位 void ...
【ardupilot增加自定义测距传感器-NRA15】
qq_39905727的博客
06-02 728
ardupilot，Missionplanner。
ArduPilot开源代码之AP_OpticalFlow_CXOF
lida2003的专栏
07-10 675
CXOF是一种光流计协议，但是协议使用的方式来看，效率偏低，并不推荐。
ArduPilot开源代码之AP_RCProtocol_CRSF
lida2003的专栏
07-02 1468
目前使用的是ELRS接收机，本章将就`AP_RCProtocol_CRSF`进行一些研读，针对这一独特接收机的处理方法。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值