void
DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (i == index) {
next->put(timestamp, val, error_count, priority);
break;
}
next = next->sibling();
i++;
}
}
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);
}
}