-
ardupilot开发 ---传感器驱动，外设驱动篇
ardupilot支持不同厂商的传感器，如雷达，声呐，激光，相机等；
支持的通信协议
I2C,
SPI,
UART (aka Serial)
CANBUS驱动程序的前后台分离
ardupilot中传感器驱动的重要结构是前后分离；
Library库调用前端程序；
前端程序调用后端程序；
后端程序一般是个for循环？？
以距离传感器为例，ardupilot是如何调用传感器的串口驱动的？
- 任务列表中注册了一个线程：SCHED_TASK(read_rangefinder, 20, 100, 33)
- 回调函数read_rangefinder中：前台实例，调用前台类的update：
RangeFinder rangefinder;
rangefinder.update(); - 前台实例的update函数中：后台实例调用了后台类的update：
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
drivers[i]->update(); - 自行以F12查看调用逻辑，如果找不到对应的update函数，先找到实例的定义，再进入对应的类中查找；
前后台各自的作用？前后台实例的调用地点？前后台实现代码的位置？
- 前台为上层应用提供访问驱动数据的接口（函数）；
后台负责跟硬件打交道，需要实时从硬件读取到对应的buffer中，因此需要为驱动后台开辟对应后端线程，串口驱动除外（因为串口有自己的缓存，通过DMA实现数据读写不需要特意开辟一个后端线程）；
前台调用了后台，而不是让上层应用调用后台； - 前台实例的update在任务线程中被调用：
SCHED_TASK(read_rangefinder, 20, 100, 33)；
update实现代码在：
libraries\AP_RangeFinder\AP_RangeFinder.cpp - 后台实例的update在前台实例update中被调用；
update实现代码在：
libraries\AP_RangeFinder\AP_RangeFinder_Backend_Serial.cpp - 从上可知，前后台代码实现都在libraries\AP_RangeFinder目录下；
- 因为串口协议有自己的buffer，所以对于rangefinder所有的数据处理（读写）都可以在驱动主线程read_rangefinder中实现；
- 用户可设置的参数始终保存在前端；
- spi、I^2C、can 驱动的 back-ends 以后台线程的形式运行，uart的 back-ends 不需要以后台线程的形式运行，因为串口的硬件读写通过DMA技术进行，不需要back-ends进行实时处理；
串口协议与驱动程序
通过串口协议实现通讯，即实现数据位的收发；
驱动程序的作用：- 将接收的数据位打包成传感器数据，供上层调用；
- 将发送数据解包成位数据，用串口协议发送；
接近传感器 Proximity sensor 解析
- 串口协议程序
- 这个参数HAL_PROXIMITY_ENABLED决定了开不开启传感器驱动线程：
#if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36), #endif- 1
- 2
- 3
- AP_Proximity中的update函数中，num_instances是几？drivers是啥？是怎么被赋值的？在哪里被定义的？
for (uint8_t i=0; i<num_instances; i++) { if (!valid_instance(i)) { continue; } drivers[i]->update(); }- 1
- 2
- 3
- 4
- 5
- 6
利用understand软件来帮助查看！！
选中num_instances，右键 vie imformastion，左下角选择data flow in即可查看 num_instances 的赋值，可知其在 init 中被赋值，在类AP_Proximity中被定义：
将右上角的variant切换成cluster模式看着更清晰：
解读AP_Proximity类的构造函数
- 构造函数在实例定义时被调用；
- 用户在地面站设置的参数会在构造函数中传递给实例，不信往下看；
代码解析
- AP_Proximity类的构造函数比初始化AP_Proximity::init()先执行；
- 初始化函数中：
for (uint8_t instance=0; instance<PROXIMITY_MAX_INSTANCES; instance++) { switch (get_type(instance)) { case Type::None: break; case Type::RPLidarA2: if (AP_Proximity_RPLidarA2::detect(serial_instance)) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance], serial_instance); serial_instance++; } break; ...... }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
PROXIMITY_MAX_INSTANCES为3，在定义时被赋值，即最多可接3个接近传感器；
问题1：get_type(instance)返回什么？AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const { if (instance < PROXIMITY_MAX_INSTANCES) { return (Type)((uint8_t)params[instance].type); } return Type::None; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
返回值与数组params有关，进类中查看并寻找它的赋值语句，如果找不到初始化则应该在构造函数中查找！
// parameters for backends AP_Proximity_Params params[PROXIMITY_MAX_INSTANCES];- 1
- 2
可知params类型是AP_Proximity_Params ：
class AP_Proximity_Params { public: static const struct AP_Param::GroupInfo var_info[]; AP_Proximity_Params(void); /* Do not allow copies */ CLASS_NO_COPY(AP_Proximity_Params); AP_Int8 type; // type of sensor AP_Int8 orientation; // orientation (e.g. right-side-up or upside-down) AP_Int16 yaw_correction; // yaw correction in degrees AP_Int16 ignore_angle_deg[PROXIMITY_MAX_IGNORE]; // angle (in degrees) of area that should be ignored by sensor (i.e. leg shows up) AP_Int8 ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored AP_Float max_m; // maximum range in meters AP_Float min_m; // minimum range in meters AP_Int8 address; // proximity address (for AP_Periph CAN) };- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
在AP_Proximity中查看params是如何被赋值的，构造函数：
AP_Proximity::AP_Proximity() { AP_Param::setup_object_defaults(this, var_info); }- 1
- 2
- 3
- 4
其中this是类当前的实例，var_info包含了？？？信息？
var_info的类型为AP_Param::GroupInfo：struct GroupInfo { const char *name; ptrdiff_t offset; // offset within the object union { const struct GroupInfo *group_info; const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags const float def_value; ptrdiff_t def_value_offset; // Default value offset from param object, when AP_PARAM_FLAG_DEFAULT_POINTER is set in flags }; uint16_t flags; uint8_t idx; // identifier within the group uint8_t type; // AP_PARAM_* };- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
数组 var_info[ ] 赋值语句：
// table of user settable parameters const AP_Param::GroupInfo AP_Proximity::var_info[] = { AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0), AP_GROUPINFO("_FILT", 18, AP_Proximity, _filt_freq, 0.25f), AP_GROUPINFO_FRAME("_ALT_MIN", 25, AP_Proximity, _alt_min, 1.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params), AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params), AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params), AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params), AP_GROUPEND };- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
var_info[] 数组的每个元素（每一行宏定义）代表每个独立参数的信息合集合；可知一共有三种不同的宏定义可以使用，下面分别解析；
AP_GROUPINFO_FRAME("_IGN_GND", 16, AP_Proximity, _ign_gnd_enable, 0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER)- 1
AP_GROUPINFO("_LOG_RAW", 17, AP_Proximity, _raw_log_enable, 0),- 1
AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params),- 1
题外话：ptrdiff_t是C/C++标准库中定义的一个与机器相关的数据类型。ptrdiff_t类型变量通常用来保存两个指针减法操作的结果。
小结：
var_info[] 数组每个元素分别保存了类AP_Proximity中的params、_alt_min、_filt_freq、_ign_gnd_enable等这几个成员的相关信息（包括变量名称，默认值，地址偏移量，数据类型等信息），以便传递给构造函数根据地面站的用户设置进行赋值；
比如，用户在地面站上设定参数AP_Proximity_IGN_GND为0，则代码将会把PRX_IGN_GND的值通过名称匹配地址匹配的方式赋值给当前AP_Proximity实例中的成员_ign_gnd_enable！！
那么问题来了，这个赋值过程是怎么样的呢？？
这就要看看类AP_Proximity构造函数中的 AP_Param::setup_object_defaults(this, var_info) 了！
this：当前实例对象的指针有了；
var_info：要给哪些参数赋值也懂了；
就看底层怎么操作了：// load default values for scalars in a group. This does not recurse // into other objects. This is a static function that should be called // in the objects constructor void AP_Param::setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info) { ptrdiff_t base = (ptrdiff_t)object_pointer; uint8_t type; for (uint8_t i=0; (type=group_info[i].type) != AP_PARAM_NONE; i++) { if (type <= AP_PARAM_FLOAT) { void *ptr = (void *)(base + group_info[i].offset); set_value((enum ap_var_type)type, ptr, get_default_value((const AP_Param *)ptr, group_info[i])); } else if (type == AP_PARAM_VECTOR3F) { // Single default for all components void *ptr = (void *)(base + group_info[i].offset); const float default_val = get_default_value((const AP_Param *)ptr, group_info[i]); ((AP_Vector3f *)ptr)->set(Vector3f{default_val, default_val, default_val}); } } }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
很羞耻地说，上面这19行c++代码我拿出了28年的所有真才实学仍旧没看明白！
看了上面的if else if 后，貌似要拉闸？AP_Param的构造函数中并没有对AP_Proximity::params进行赋值？？！params的类型并不满足if也不满足else if啊！那params是如何被初始化的啊？答案只有一个，params的类型是类AP_Proximity_Params，那去这个类的构造函数看看！
果不其然啊！！类AP_Proximity_Params有自己的构造函数，并且有自己的AP_Proximity_Params::var_info，params在构造函数中初始化：// table of user settable parameters const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO("_ORIENT", 2, AP_Proximity_Params, orientation, 0), ...... } AP_Proximity_Params::AP_Proximity_Params(void) { AP_Param::setup_object_defaults(this, var_info); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
从这个var_info[]可知，_TYPE被初始化成了0，也就是sensor type为none！这也回答了一开的问题1：get_type(instance)返回什么的问题！
小结：- 每个用户参数的初始化信息都在对应的类的var_info[]中，然后类、子类调用自己的构造函数，构造函数再调用AP_Param::setup_object_defaults(this, var_info);来对参数进行初始化！
- 地面站上的用户参数一般都可以找到与之相对应的类参数，一般都被定义在对应名称的类中，例如地面站参数名为PRX_IGN_GND，与它对应的类参数在类AP_Proximity中名为_ign_gnd_enable，PRX_IGN_GND的值会被传递给_ign_gnd_enable！
- 关于var_info[]的一点补充，var_info[]的每个元素即某个参数的初始化信息，如果参数是普通类型则默认值是union中的def_value，如果类型是类则默认值是union中的group_info，从上面的几个宏定义的示意图中不难看出。
问题2：除了初始化外，这些参数如何被mavlink消息覆盖的呢？
用户通过地面站对参数进行设置，去线程SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102)中一探究竟！！
再往下阅读之前建议读者还可以浅浅参考本人之前写过的一篇博文：ardupilot参数的从mavlink消息到底层参数赋值
这里重新梳理一下这个赋值过程和逻辑，毕竟不同时期有不同的理解：
从前期工作“ardupilot参数的从mavlink消息到底层参数赋值”中可知，某些参数的赋值在函数handle_param_set中进行，上下游的调用流程为：
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102)~>
update_receive ~> packetReceived ~> packetReceived ~> handleMessage ~> handle_common_message ~> handle_common_param_message ~> handle_param_set()
那么现在直接去handle_param_set()中查看：void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) { mavlink_param_set_t packet; mavlink_msg_param_set_decode(&msg, &packet); enum ap_var_type var_type; // set parameter AP_Param *vp; char key[AP_MAX_NAME_SIZE+1]; strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE); key[AP_MAX_NAME_SIZE] = 0; // find existing param so we can get the old value uint16_t parameter_flags = 0; vp = AP_Param::find(key, &var_type, ¶meter_flags); if (vp == nullptr || isnan(packet.param_value) || isinf(packet.param_value)) { return; } float old_value = vp->cast_to_float(var_type); if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { // the user can set BRD_OPTIONS to enable set of internal // parameters, for developer testing or unusual use cases if (AP_BoardConfig::allow_set_internal_parameters()) { parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY; } } if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key); // send the readonly value send_parameter_value(key, var_type, old_value); return; } // set the value vp->set_float(packet.param_value, var_type); /* we force the save if the value is not equal to the old value. This copes with the use of override values in constructors, such as PID elements. Otherwise a set to the default value which differs from the constructor value doesn't save the change */ bool force_save = !is_equal(packet.param_value, old_value); // save the change vp->save(force_save); if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) { AP_Param::invalidate_count(); } AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Parameter(key, vp->cast_to_float(var_type)); } }- 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
最主要的就是下面这两个函数：
- 通过find函数根据参数名找到指向该参数的指针：vp = AP_Param::find(key, &var_type, ¶meter_flags);
- 通过set函数赋值：vp->set_float(packet.param_value, var_type);
- 但是我没完全看懂！！待续…
言归正传，我们还是继续看驱动后台如何被调用的吧！！
继续嗑void AP_Proximity::init()函数：
阅读到这已经懂得switch (get_type(instance))的get_type(instance)是什么，那么就可以知道要执行哪个case了；
以case Type::RPLidarA2为例，接下来要弄懂的是：
AP_Proximity_RPLidarA2::detect(serial_instance)是如何检测雷达是否正常接入的？
很坑！类AP_Proximity_RPLidarA2中找死都找不到detect函数！
原来是在AP_Proximity_RPLidarA2的父类AP_Proximity_Backend_Serial中：class AP_Proximity_Backend_Serial : public AP_Proximity_Backend { public: AP_Proximity_Backend_Serial(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params, uint8_t serial_instance); // static detection function // detect if a proximity sensor is connected by looking for a configured serial port // serial_instance affects which serial port is used. Should be 0 or 1 depending on whether this is the 1st or 2nd proximity sensor with a serial interface static bool detect(uint8_t serial_instance); protected: virtual uint16_t rxspace() const { return 0; }; AP_HAL::UARTDriver *_uart; // uart for communicating with sensor };- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
我哭死！终于找到了从获取雷达数据到上传到用于避障的数据库的调用结构：
知道了可行性后，下面开始尝试将一个新的驱动代码集成到ardupilot中！！
集成新的驱动代码到ardupilot（以RPLidarS1为例）：运行update之前肯定init函数先被调用，init只在reboot时调用？所以在地面站更改串口协议参数时需要reboot？？
假如要在init中的Switch中增加一个case，以增加RPLidarS1雷达的驱动为例：- 要增加一个case，switch (get_type(instance)) 中的get_type(instance)的返回值类型必须先增加一个枚举；
也就是在AP_Proximity.h中，将类AP_Proximity的type枚举增加一种新类型：
// Proximity driver types enum class Type { None = 0, // 1 was SF40C_v09 #if AP_PROXIMITY_MAV_ENABLED MAV = 2, #endif #if AP_PROXIMITY_TERARANGERTOWER_ENABLED TRTOWER = 3, #endif #if AP_PROXIMITY_RANGEFINDER_ENABLED RangeFinder = 4, #endif #if AP_PROXIMITY_RPLIDARA2_ENABLED RPLidarA2 = 5, #endif #if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED TRTOWEREVO = 6, #endif #if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED SF40C = 7, #endif #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED SF45B = 8, #endif #if AP_PROXIMITY_SITL_ENABLED SITL = 10, #endif #if AP_PROXIMITY_AIRSIMSITL_ENABLED AirSimSITL = 12, #endif #if AP_PROXIMITY_CYGBOT_ENABLED CYGBOT_D1 = 13, #endif #if AP_PROXIMITY_DRONECAN_ENABLED DroneCAN = 14, #endif };- 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
这个枚举与地面站参数PRX#_TYPE相关！！
注意：get_type(instance)只会检测用户是否配置了参数PRX#_TYPE ，与传感器是否连接飞控连接后正不正常都无关！！
- 增加case代码
要与以下代码类似，把detect函数和AP_Proximity_RPLidarA2函数换成自己传感器的类型：
#if AP_PROXIMITY_RPLIDARA2_ENABLED case Type::RPLidarA2: if (AP_Proximity_RPLidarA2::detect(serial_instance)) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_RPLidarA2(*this, state[instance], params[instance], serial_instance); serial_instance++; } break; #endif- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
-
- 为新传感器的驱动定义一个类叫AP_Proximity_RPLidarS1；
-
- 关于detect：
让类AP_Proximity_RPLidarS1继承AP_Proximity_Backend_Serial，因为detect定义在AP_Proximity_Backend_Serial中；detect不用做改动因为detect使用了枚举enum SerialProtocol，而枚举enum SerialProtocol不需要改动因为新加的驱动类型在原有枚举中已经存在，若不存在则需要添加新的枚举类型！
**注意：**detect只会检测用户是否配置了参数SERIAL#_PROTOCOL ，与传感器是否连接飞控连接后正不正常都无关！！
- 关于detect：
-
- 关于驱动后端实例构造函数new AP_Proximity_RPLidarS1 ：
AP_Proximity_RPLidarA2类中没有对应参数列表的构造函数？则根据参数列表来调用父类的对应构造函数？实锤了！父类的构造函数如下，参数列表刚好符合：
- 关于驱动后端实例构造函数new AP_Proximity_RPLidarS1 ：
AP_Proximity_Backend_Serial(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params, uint8_t serial_instance);- 1
- 2
- 3
- 4
所以并不需要添加一个AP_Proximity_RPLidarS1 构造函数，只需要类AP_Proximity_RPLidarS1 继承类AP_Proximity_Backend_Serial即可使用！
- 针对本次例子init中只需要修改以上内容，若要新加其他的传感器驱动则使用与此类似的分析方法。下面分析后驱动后台的update函数如何修改，也就是drivers[i]->update(); 中的update函数如何修改。
- drivers[i]->update()的update函数定义在不同的传感器类中，以现有的类AP_Proximity_RPLidarA2为例来学习和修改：
调用逻辑：
0）_state被初始化为State::RESET
1）请求设备信息
send_request_for_device_info(); _state = State::AWAITING_RESPONSE;- 1
- 2
2）执行case State::AWAITING_RESPONSE:
_state = State::AWAITING_DEVICE_INFO;- 1
3）执行case State::AWAITING_DEVICE_INFO:
parse_response_device_info(); consume_bytes(sizeof(_payload.device_info));- 1
- 2
其中 parse_response_device_info()要执行以下语句：
send_scan_mode_request(); _state = State::AWAITING_RESPONSE;- 1
- 2
4）执行执行case State::AWAITING_RESPONSE:
_state = State::AWAITING_SCAN_DATA;- 1
5）执行case State::AWAITING_SCAN_DATA:
parse_response_data();//解码扫描数据 consume_bytes(sizeof(_payload.sensor_scan));- 1
- 2
其中parse_response_data()要执行以下语句：
const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;//安装方向 const float angle_deg = wrap_360(_payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction);//安装角度 const float distance_m = (_payload.sensor_scan.distance_q2/4000.0f);//计算扫描点我雷达坐标系的距离 // update OA database database_push(_last_angle_deg, _last_distance_m);- 1
- 2
- 3
- 4
- 5
写了一些注释的完整代码请参考附录。
下面代码没明白：S1 9? A1A2A3 63？？？// request device info 3sec after reset // required for S1 support that sends only 9 bytes after a reset (A1,A2 send 63) uint32_t now_ms = AP_HAL::millis(); if ((_state == State::RESET) && (now_ms - _last_reset_ms > 3000)) { send_request_for_device_info(); _state = State::AWAITING_RESPONSE; _byte_count = 0;//reset后清除已读取到_payload的数据 }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
小结
- 总共可以支持3个接近传感器（只允许运行3个实例），分别称作传感器0，1，2；相关变量PROXIMITY_MAX_INSTANCES被默认定义为3。
- 逐个判断接近传感器0，1，2的传感器类型，雷达？声呐？其他？还是None？默认是None。
instance表示012；相关函数：get_type(instance)，判断PRX1_TYPE，PRX2_TYPE，PRX3_TYPE的用户配置类型，如5表示RPLidarA2； - 若配置了传感器类型如RPLidarA2则判断使用哪个串口作为飞控与传感器的物理通讯，相关函数：detect(serial_instance)，判断SERIAL1_PROTOCOL~SERIAL8_PROTOCOL是否有配置成Lidar360协议的。若有，则通过serial_instance变量来告诉驱动后台实例构造函数要去串口读取雷达数据：
实现代码：
- 几个重要的变量
state[instance]：==3
drivers[instance] ：驱动后台实例，==3，有的可能为None因为不一定3个都被配置；
serial_instance：配置了传感器协议的实例数量，<=3 ；
A2传感器一些基本命令对应的串口字节
A2和S1的命令字节格式是否一样？不一样是否需要改动？改动哪里？
- 重置雷达：
{ RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET }
{ RPLIDAR_PREAMBLE, 0x40 } - 请求设备信息：
{ RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_INFO }
{ RPLIDAR_PREAMBLE, 0x50 } - 设定扫描模式：
{ RPLIDAR_PREAMBLE, RPLIDAR_CMD_SCAN }
{ RPLIDAR_PREAMBLE, 0x20 }
#define RPLIDAR_PREAMBLE 0xA5 #define RPLIDAR_CMD_STOP 0x25 #define RPLIDAR_CMD_SCAN 0x20 #define RPLIDAR_CMD_FORCE_SCAN 0x21 #define RPLIDAR_CMD_RESET 0x40 // Commands without payload but have response #define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 #define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 // Commands with payload and have response #define RPLIDAR_CMD_EXPRESS_SCAN 0x82- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
附录
void AP_Proximity_RPLidarA2::update(void) { //要区别两种reset: (1) _state = State::RESET (2) reset_rplidar(); // 第一种reset是清除已读取到_payload的数据，令_byte_count = 0就可以； //第二种，要调用reset_rplidar()，不仅要_state = State::RESET;_byte_count = 0;还要_uart->write(tx_buffer, 2); if (_uart == nullptr) { return; } // request device info 3sec after reset // required for S1 support that sends only 9 bytes after a reset (A1,A2 send 63) uint32_t now_ms = AP_HAL::millis(); if ((_state == State::RESET) && (now_ms - _last_reset_ms > 3000)) { send_request_for_device_info(); _state = State::AWAITING_RESPONSE; _byte_count = 0;//reset后清除已读取到_payload的数据 } get_readings(); // check for timeout and set health status if (AP_HAL::millis() - _last_distance_received_ms > COMM_ACTIVITY_TIMEOUT_MS) { set_status(AP_Proximity::Status::NoData); Debug(1, "LIDAR NO DATA"); if (AP_HAL::millis() - _last_reset_ms > 10000) { reset_rplidar(); } } else { set_status(AP_Proximity::Status::Good); } }- 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
void AP_Proximity_RPLidarA2::get_readings() { Debug(2, " CURRENT STATE: %u ", (unsigned)_state); const uint32_t nbytes = _uart->available();//返回可从队列前面读取的对象数 if (nbytes == 0) { return; } //_payload：接收串口数据的buffer，是个数组，类型是个union，根据不同响应接收到的字节数是不一样的！ //sizeof(_payload)：_payload能容纳的最大字节数 //_byte_count：已经读取到_payload中的字节数 //sizeof(_payload)-_byte_count：剩余容纳的字节数 //nbytes：串口buffer可用的字节数 //bytes_to_read：本次要读取到_payload的字节数 //bytes_read：本次成功读取到_payload的字节数，不出意外的话bytes_read和bytes_to_read应该相等 //注意：buffer和_payload的区别，buffer是串口的实时缓存，_payload是存放从buffer中读取到数据的变量 const uint32_t bytes_to_read = MIN(nbytes, sizeof(_payload)-_byte_count); if (bytes_to_read == 0) { INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); reset(); return; } const uint32_t bytes_read = _uart->read(&_payload[_byte_count], bytes_to_read); if (bytes_read == 0) { // this is bad; we were told there were bytes available INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); reset(); return; } _byte_count += bytes_read;//成功将bytes_read个串口字节读取到 _payload，则更改_byte_count uint32_t previous_loop_byte_count = UINT32_MAX;//previous_loop_byte_count：上一次循环_payload中的字节数，为了防止无限循环而引入的变量，具体使用往下看 while (_byte_count) { if (_byte_count >= previous_loop_byte_count) { // this is a serious error, we should always consume some // bytes. Avoid looping forever. INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); _uart = nullptr; return; } previous_loop_byte_count = _byte_count; switch(_state){ case State::RESET: { // looking for 0x52 at start of buffer; the 62 following // bytes are "information" if (!make_first_byte_in_payload('R')) { // that's 'R' as in RPiLidar return; } if (_byte_count < 63) { return; } #if RP_DEBUG_LEVEL // optionally spit out via mavlink the 63-bytes of cruft // that is spat out on device reset Debug(1, "Got RPLidar Information"); char xbuffer[64]{}; memcpy((void*)xbuffer, (void*)&_payload.information, 63); gcs().send_text(MAV_SEVERITY_INFO, "RPLidar: (%s)", xbuffer); #endif // 63 is the magic number of bytes in the spewed-out // reset data ... so now we'll just drop that stuff on // the floor. consume_bytes(63); send_request_for_device_info(); _state = State::AWAITING_RESPONSE; continue; } case State::AWAITING_RESPONSE: //reset()后（ send_request_for_device_info()后 ），串口接收到的第一个字节应该是RPLIDAR_PREAMBLE，即 0xA5 //如果不是则判定为协议错误！ if (_payload[0] != RPLIDAR_PREAMBLE) { // this is a protocol error. Reset. reset(); return; } // reset()后（ send_request_for_device_info()后 ），串口buffer接收到的第一个packet应该是_descriptor，并且_descriptor应该是7个字节 // descriptor packet has 7 byte in total if (_byte_count < sizeof(_descriptor)) { return; } // identify the payload data after the descriptor static const _descriptor SCAN_DATA_DESCRIPTOR[] { { RPLIDAR_PREAMBLE, 0x5A, 0x05, 0x00, 0x00, 0x40, 0x81 } }; static const _descriptor HEALTH_DESCRIPTOR[] { { RPLIDAR_PREAMBLE, 0x5A, 0x03, 0x00, 0x00, 0x00, 0x06 } }; static const _descriptor DEVICE_INFO_DESCRIPTOR[] { { RPLIDAR_PREAMBLE, 0x5A, 0x14, 0x00, 0x00, 0x00, 0x04 } }; Debug(2,"LIDAR descriptor found"); //根据第一个字节（）判断设备状态，以判断便下一步应该做什么动作！！ //memcmp：memcmp(const void *str1, const void *str2, size_t n))其功能是把存储区 str1 和存储区 str2 的前 n 个字节进行比较；如果返回值 = 0，则表示 str1 等于 str2 if (memcmp((void*)&_payload[0], SCAN_DATA_DESCRIPTOR, sizeof(_descriptor)) == 0) { //if成立则表明_payload[0]==SCAN_DATA_DESCRIPTOR，下面的else if 则同理 _state = State::AWAITING_SCAN_DATA; } else if (memcmp((void*)&_payload[0], DEVICE_INFO_DESCRIPTOR, sizeof(_descriptor)) == 0) { _state = State::AWAITING_DEVICE_INFO; } else if (memcmp((void*)&_payload[0], HEALTH_DESCRIPTOR, sizeof(_descriptor)) == 0) { _state = State::AWAITING_HEALTH; } else { // unknown descriptor. Ignore it. } consume_bytes(sizeof(_descriptor)); //将读取到的数据从_payload中清除，以便后续的buffer数据读取进来 break; case State::AWAITING_DEVICE_INFO: if (_byte_count < sizeof(_payload.device_info)) { return; } parse_response_device_info(); consume_bytes(sizeof(_payload.device_info)); break; case State::AWAITING_SCAN_DATA://重要的case分支，等待扫描数据的读取！！ if (_byte_count < sizeof(_payload.sensor_scan)) { return; } parse_response_data();//解码扫描数据 consume_bytes(sizeof(_payload.sensor_scan)); break; case State::AWAITING_HEALTH: if (_byte_count < sizeof(_payload.sensor_health)) { return; } parse_response_health(); consume_bytes(sizeof(_payload.sensor_health)); break; } } }- 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
void AP_Proximity_RPLidarA2::parse_response_data() { if (_sync_error) { // out of 5-byte sync mask -> catch new revolution Debug(1, " OUT OF SYNC"); // on first revolution bit 1 = 1, bit 2 = 0 of the first byte if ((_payload[0] & 0x03) == 0x01) { _sync_error = 0; Debug(1, " RESYNC"); } else { return; } } Debug(2, "UART %02x %02x%02x %02x%02x", _payload[0], _payload[2], _payload[1], _payload[4], _payload[3]); //show HEX values // check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1 if (!((_payload.sensor_scan.startbit == !_payload.sensor_scan.not_startbit) && _payload.sensor_scan.checkbit)) {//判断雷达扫描数据是否正常 Debug(1, "Invalid Payload"); _sync_error++; return; } const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f;//安装方向 const float angle_deg = wrap_360(_payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction);//安装角度 const float distance_m = (_payload.sensor_scan.distance_q2/4000.0f);//计算扫描点我雷达坐标系的距离 #if RP_DEBUG_LEVEL >= 2 const float quality = _payload.sensor_scan.quality; Debug(2, " D%02.2f A%03.1f Q%0.2f", distance_m, angle_deg, quality); #endif _last_distance_received_ms = AP_HAL::millis(); if (!ignore_reading(angle_deg, distance_m)) {//判断扫描点是否是忽略点 const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg); //Face是什么？临时保存的3D边界信息，包括layer和sector // uint8_t layer; // vertical "steps" on the 3D Boundary. 0th layer is the bottom most layer, 1st layer is 30 degrees above (in body frame) and so on // uint8_t sector; // horizontal "steps" on the 3D Boundary. 0th sector is directly in front of the vehicle. Each sector is 45 degrees wide. if (face != _last_face) { // distance is for a new face, the previous one can be updated now if (_last_distance_valid) { frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance); } else { // reset distance from last face frontend.boundary.reset_face(face, state.instance); } // initialize the new face _last_face = face; _last_distance_valid = false; } if (distance_m > distance_min()) { // update shortest distance if (!_last_distance_valid || (distance_m < _last_distance_m)) { _last_distance_m = distance_m; _last_distance_valid = true; _last_angle_deg = angle_deg; } // update OA database database_push(_last_angle_deg, _last_distance_m); } } }- 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
-
相关阅读:
MyBatis 的在使用上的注意事项及其辨析
代码随想录算法训练营第23期day15| 104.二叉树的最大深度、111.二叉树的最小深度、222.完全二叉树的节点个数
C#判断语句
25.10 MySQL 约束
机器学习笔记之贝叶斯线性回归(一)线性回归背景介绍
day4：Node.js 核心库
GO语言框架中如何快速集成日志模块
不care工具，在大数据平台中Hive能自动处理SQL
[ansible]playbook结合项目解释执行步骤
Sentinel-限流降级
- 原文地址：https://blog.csdn.net/weixin_43321489/article/details/133312281
