PX4多传感器优先级判断

这篇博客讨论了PX4在很多传感器下如何自动选择最优传感器。
版本：v1.9.0
源码位置：

1. ~src/lib/ecl/sensors/sensors.cpp
2. ~src/lib/ecl/sensors/voted_sensors_update.cpp

1. 简介

在pixhawk系统里面，传感器一般有IMU（加速度计，陀螺仪，磁力计）和空速计，在冗余设计的前提下，这些传感器一般都会有很多个，如果有很多个加速度计（因为有好几个IMU），那么到底选哪一个加速度计的数据作为滤波的输入值就变得十分重要。
PX4里面关于传感器有一个很重要的投票机制，很好地解决了多传感器数据选取问题，这里我们以加速度计为例。

2. 代码分析

2.1 主体思路

PX4中将传感器分组，所有加速度计为一组，并且会有编号，在传感器运行的时候就会检测这些传感器是否有用或者获得的数据是否有效（initialize_sensors();），如果有比系统预设的传感器还多的话，会自动增加。
然后对每一组传感器分析，获得最优传感器编号，在最后数据发布的时候就只发布这个传感器编号的数据。

2.2 代码探究

我们找到sensors.cpp的主函数依次往下找
sensors::run()->_voted_sensors_update.sensors_poll(raw, airdata, magnetometer);->accel_poll(raw);->_accel.voter.get_best(hrt_absolute_time(), &best_index);
在这个函数里面就能看到如何判断的。
下面我们先列出函数的主体

DataValidator *next = _first;

	// XXX This should eventually also include voting
	int pre_check_best = _curr_best;
	float pre_check_confidence = 1.0f;
	int pre_check_prio = -1;
	float max_confidence = -1.0f;
	int max_priority = -1000;
	int max_index = -1;
	DataValidator *best = nullptr;

	int i = 0;

	while (next != nullptr) {
		float confidence = next->confidence(timestamp);

		if (i == pre_check_best) {
			pre_check_prio = next->priority();
			pre_check_confidence = confidence;
		}

		/*
		 * Switch if:
		 * 1) the confidence is higher and priority is equal or higher
		 * 2) the confidence is no less than 1% different and the priority is higher
		 */
		if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
		     (confidence > max_confidence && (next->priority() >= max_priority)) ||
		     (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))
		    ) && (confidence > 0.0f)) {

			max_index = i;
			max_confidence = confidence;
			max_priority = next->priority();
			best = next;
		}

		next = next->sibling();
		i++;
	}

PX4将多有传感器组成一个链表，在这个函数里，从链表开始的地方（_first）开始检索，对每个数据进行判断，如果发现满足下面两个条件：

1. 可信度更高并且优先级相同或者更高
2. 可信度差异不到百分之1并且优先级更高

然后就把这个传感器对应的编号传递出去。

下一步需要检查这个confidence和priority在哪里得到的。

这一部分我简单做了一个思维导图有利于对整体流程的判断。