int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
perf_count(_comms_errors);
return ret;
}
float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]);
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
if (diff_pres_pa_raw < FLT_EPSILON) {
// a zero value indicates no measurement
// since the noise floor has been arbitrarily killed
// it defeats our stuck sensor detection - the best we
// can do is to output some numerical noise to show
// that we are still correctly sampling.
diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.device_id = _device_id.devid;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
perf_count(_comms_errors);
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
perf_count(_comms_errors);
log("zero value from sensor");
return -1;
}
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
}
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
}
// XXX we may want to smooth out the readings to remove noise.
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = val[0] & 0xC0;
if (status == 2) {
log("err: stale data");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
} else if (status == 3) {
log("err: fault");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
/* mask the used bits */
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
/* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value
*/
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
if (diff_press_pa < 0.0f)
diff_press_pa = 0.0f;
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = (val[0] & 0xC0) >> 6;
switch (status) {
case 0:
break;
case 1:
/* fallthrough */
case 2:
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
}
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
/* mask the used bits */
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
We negate the result so that positive differential pressures
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
float diff_press_pa = fabsf(diff_press_pa_raw);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub > 0 && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
perf_count(_comms_errors);
return ret;
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
perf_count(_comms_errors);
DEVICE_LOG("zero value from sensor");
return -1;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = val[0] & 0xC0;
if (status == 2) {
log("err: stale data");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
} else if (status == 3) {
log("err: fault");
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
// XXX leaving this in until new calculation method has been cross-checked
//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
//diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
dp_raw = 0x3FFF & dp_raw; //mask the used bits
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
// XXX we may want to smooth out the readings to remove noise.
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset);
struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
MEASAirspeedSim::collect()
{
int ret = -EIO;
/* read from the sensor */
#pragma pack(push, 1)
struct {
float temperature;
float diff_pressure;
} airspeed_report;
#pragma pack(pop)
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint8_t status = 0;
switch (status) {
case 0:
break;
case 1:
/* fallthrough */
case 2:
/* fallthrough */
case 3:
perf_count(_comms_errors);
perf_end(_sample_perf);
return -EAGAIN;
}
float temperature = airspeed_report.temperature;
float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub != nullptr && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
