• ArduPilot开源代码之AP_RangeFinder

    ArduPilot开源代码之AP_RangeFinder

    1. 源由

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

    主要的应用有：

    其他应用有：

    本次结合代码进行研读，其中也请注意两个应用类型的传感器：upward facing rangefinder和downward facing rangefinder。

    2. 框架设计

    启动代码：

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

    • 1
    • 2
    • 3

    任务代码：

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

    • 1
    • 2
    • 3

    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;

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19

    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());

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74

    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()

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    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

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72
    • 73
    • 74
    • 75
    • 76
    • 77
    • 78
    • 79
    • 80
    • 81
    • 82
    • 83
    • 84
    • 85
    • 86
    • 87
    • 88
    • 89
    • 90
    • 91
    • 92
    • 93
    • 94
    • 95
    • 96
    • 97
    • 98
    • 99
    • 100
    • 101
    • 102
    • 103
    • 104
    • 105
    • 106
    • 107
    • 108
    • 109
    • 110
    • 111
    • 112
    • 113
    • 114
    • 115
    • 116
    • 117
    • 118
    • 119
    • 120
    • 121
    • 122
    • 123
    • 124
    • 125
    • 126
    • 127
    • 128
    • 129
    • 130
    • 131
    • 132
    • 133
    • 134
    • 135
    • 136
    • 137
    • 138
    • 139
    • 140
    • 141
    • 142
    • 143
    • 144
    • 145
    • 146
    • 147
    • 148
    • 149
    • 150

    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;
    };


    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11

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

    5. 参考资料

    【1】ArduPilot开源飞控系统之简单介绍
    【2】ArduPilot飞控启动&运行过程简介
    【3】ArduPilot之开源代码Library&Sketches设计

  • 相关阅读:
    C#的多线程UI窗体控件显示方案 - 开源研究系列文章
    事件对象学习
    Netfilter之连接跟踪（Connection Tracking）和反向 SNAT（Reverse SNAT）
    问题汇总20231110
    图片如何转换为文字？这些软件可以实现
    Django学习(三) 之 模板中标签的使用
    信奥中的数学：同余
    IMAGEBIND: One Embedding Space To Bind Them All论文笔记
    Spring MVC简介与概述
    Transformers库中的pipeline模块支持的NLP任务
  • 原文地址：https://blog.csdn.net/lida2003/article/details/133517047