Esempio n. 1
0
float
DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
{
	DataValidator *next = _first;

	float vibe = 0.0f;

	/* find the best RMS value of a non-timed out sensor */
	while (next != nullptr) {

		if (next->confidence(timestamp) > 0.5f) {
			float *rms = next->rms();

			for (unsigned j = 0; j < 3; j++) {
				if (rms[j] > vibe) {
					vibe = rms[j];
				}
			}
		}

		next = next->sibling();
	}

	return vibe;
}
Esempio n. 2
0
void
Sensors::diff_pres_poll(struct sensor_combined_s &raw)
{
	bool updated;
	orb_check(_diff_pres_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);

		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);

		_airspeed.timestamp = _diff_pres.timestamp;

		/* push data into validator */
		_airspeed_validator.put(_airspeed.timestamp, _diff_pres.differential_pressure_raw_pa, _diff_pres.error_count, 100);

#ifdef __PX4_POSIX
		_airspeed.confidence = 1.0f;
#else
		_airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
#endif

		/* don't risk to feed negative airspeed into the system */
		_airspeed.indicated_airspeed_m_s = math::max(0.0f,
						   calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));

		_airspeed.true_airspeed_m_s = math::max(0.0f,
							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _voted_sensors_update.baro_pressure() * 1e2f,
									_voted_sensors_update.baro_pressure() * 1e2f, air_temperature_celsius));
		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _voted_sensors_update.baro_pressure() * 1e2f,
						   _voted_sensors_update.baro_pressure() * 1e2f, air_temperature_celsius));

		_airspeed.air_temperature_celsius = air_temperature_celsius;
		_airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;

		int instance;
		orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &_airspeed, &instance, ORB_PRIO_DEFAULT);
	}
}
Esempio n. 3
0
float
DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
{
	DataValidator *next = _first;

	float vibe = -1.0f;

	/* find the best vibration value of a non-timed out sensor */
	while (next != nullptr) {

		if (next->confidence(timestamp) > 0.5f) {
			float *vibration_offset = next->vibration_offset();

			if (vibe < 0.0f || vibration_offset[axis] < vibe) {
				vibe = vibration_offset[axis];
			}
		}

		next = next->sibling();
	}

	return vibe;
}
Esempio n. 4
0
float *
DataValidatorGroup::get_best(uint64_t timestamp, int *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;

	unsigned i = 0;

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

		if (static_cast<int>(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++;
	}

	/* the current best sensor is not matching the previous best sensor,
	 * or the only sensor went bad */
	if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) {

		bool true_failsafe = true;

		/* check whether the switch was a failsafe or preferring a higher priority sensor */
		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
		    fabsf(pre_check_confidence - max_confidence) < 0.1f) {
			/* this is not a failover */
			true_failsafe = false;
			/* reset error flags, this is likely a hotplug sensor coming online late */
			best->reset_state();
		}

		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
		if (_curr_best < 0) {
			_prev_best = max_index;

		} else {
			/* we were initialized before, this is a real failsafe */
			_prev_best = pre_check_best;

			if (true_failsafe) {
				_toggle_count++;

				/* if this is the first time, log when we failed */
				if (_first_failover_time == 0) {
					_first_failover_time = timestamp;
				}
			}
		}

		/* for all cases we want to keep a record of the best index */
		_curr_best = max_index;
	}

	*index = max_index;
	return (best) ? best->value() : nullptr;
}