forked from Archive/PX4-Autopilot
Included raw differential pressure field
This commit is contained in:
parent
9719af623d
commit
cdec5e1d05
|
@ -206,20 +206,46 @@ MEASAirspeed::collect()
|
|||
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, enforce absolute value
|
||||
*/
|
||||
// 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;
|
||||
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;
|
||||
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
|
||||
|
||||
if (diff_press_pa < 0.0f) {
|
||||
diff_press_pa = 0.0f;
|
||||
}
|
||||
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, temperature);
|
||||
voltage_correction(diff_press_pa_raw, temperature);
|
||||
|
||||
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
|
||||
|
||||
/*
|
||||
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). */
|
||||
|
@ -232,6 +258,7 @@ MEASAirspeed::collect()
|
|||
report.temperature = temperature;
|
||||
report.differential_pressure_pa = diff_press_pa;
|
||||
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
|
||||
report.differential_pressure_raw_pa = diff_press_pa_raw;
|
||||
report.voltage = 0;
|
||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||
|
||||
|
@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
|
|||
return;
|
||||
}
|
||||
|
||||
const float slope = 70.0f;
|
||||
const float slope = 65.0f;
|
||||
/*
|
||||
apply a piecewise linear correction, flattening at 0.5V from 5V
|
||||
*/
|
||||
|
@ -478,7 +505,7 @@ test()
|
|||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
|
@ -506,7 +533,7 @@ test()
|
|||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
|
||||
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
|
||||
}
|
||||
|
||||
|
|
|
@ -52,13 +52,14 @@
|
|||
* Differential pressure.
|
||||
*/
|
||||
struct differential_pressure_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
uint64_t error_count;
|
||||
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
|
||||
uint64_t error_count; /**< Number of errors detected by driver */
|
||||
float differential_pressure_pa; /**< Differential pressure reading */
|
||||
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
|
||||
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
|
||||
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
||||
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue