forked from Archive/PX4-Autopilot
more fixes for the airspeed readout
This commit is contained in:
parent
2c2c65d446
commit
2707d2c1dd
|
@ -993,7 +993,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
/* look for battery channel */
|
||||
|
||||
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
|
||||
|
||||
|
||||
if (ret >= (int)sizeof(buf_adc[0])) {
|
||||
|
||||
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
|
@ -1025,8 +1025,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
|
||||
/* calculate airspeed, raw is the difference from */
|
||||
|
||||
float voltage = buf_adc[i].am_data / 4096.0f;
|
||||
float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
|
||||
|
||||
/**
|
||||
* The voltage divider pulls the signal down, only act on
|
||||
|
@ -1034,21 +1033,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
*/
|
||||
if (voltage > 0.4f) {
|
||||
|
||||
float pres_raw = fabsf(voltage - (3.3f / 2.0f));
|
||||
float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
|
||||
// float pres_raw = fabsf(voltage - (3.3f / 2.0f));
|
||||
// float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
|
||||
//XXX depends on sensor used..., where are the above numbers from?
|
||||
float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration
|
||||
|
||||
float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
|
||||
_barometer.pressure, _barometer.temperature - 5.0f);
|
||||
float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
|
||||
_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
|
||||
// XXX HACK - true temperature is much less than indicated temperature in baro,
|
||||
// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
|
||||
|
||||
float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure,
|
||||
_barometer.pressure, _barometer.temperature - 5.0f);
|
||||
// XXX HACK
|
||||
float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
|
||||
|
||||
//printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true);
|
||||
|
||||
_differential_pressure.timestamp = hrt_absolute_time();
|
||||
_differential_pressure.static_pressure_mbar = _barometer.pressure;
|
||||
_differential_pressure.differential_pressure_mbar = pres_mbar;
|
||||
_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
|
||||
_differential_pressure.temperature_celcius = _barometer.temperature;
|
||||
_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
|
||||
_differential_pressure.true_airspeed_m_s = airspeed_true;
|
||||
|
@ -1064,7 +1065,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
}
|
||||
|
||||
_last_adc = hrt_absolute_time();
|
||||
break;
|
||||
//break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -40,14 +40,25 @@
|
|||
*
|
||||
*/
|
||||
|
||||
#include "math.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "conversions.h"
|
||||
#include "airspeed.h"
|
||||
|
||||
|
||||
float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
|
||||
/**
|
||||
* Calculate indicated airspeed.
|
||||
*
|
||||
* Note that the indicated airspeed is not the true airspeed because it
|
||||
* lacks the air density compensation. Use the calc_true_airspeed functions to get
|
||||
* the true airspeed.
|
||||
*
|
||||
* @param differential_pressure total_ pressure - static pressure
|
||||
* @return indicated airspeed in m/s
|
||||
*/
|
||||
float calc_indicated_airspeed(float differential_pressure)
|
||||
{
|
||||
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -55,14 +66,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa
|
|||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param speed current indicated airspeed
|
||||
* @param speed_indicated current indicated airspeed
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature air temperature in degrees celcius
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
|
||||
float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
|
||||
{
|
||||
return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature));
|
||||
return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius));
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -70,12 +81,17 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo
|
|||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param pressure_front pressure inside the pitot/prandl tube
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature air temperature in degrees celcius
|
||||
* @param total_pressure pressure inside the pitot/prandl tube
|
||||
* @param static_pressure pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
|
||||
float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
|
||||
{
|
||||
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
|
||||
float density = get_air_density(static_pressure, temperature_celsius);
|
||||
if (density < 0.0001f || isnan(density)) {
|
||||
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
|
||||
printf("Invalid air density, using density at sea level\n");
|
||||
}
|
||||
return sqrtf((2.0f*(total_pressure - static_pressure)) / density);
|
||||
}
|
||||
|
|
|
@ -48,43 +48,42 @@
|
|||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* Calculate indicated airspeed.
|
||||
*
|
||||
* Note that the indicated airspeed is not the true airspeed because it
|
||||
* lacks the air density compensation. Use the calc_true_airspeed functions to get
|
||||
* the true airspeed.
|
||||
*
|
||||
* @param pressure_front pressure inside the pitot/prandl tube
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature air temperature in degrees celcius
|
||||
* @return indicated airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature);
|
||||
/**
|
||||
* Calculate indicated airspeed.
|
||||
*
|
||||
* Note that the indicated airspeed is not the true airspeed because it
|
||||
* lacks the air density compensation. Use the calc_true_airspeed functions to get
|
||||
* the true airspeed.
|
||||
*
|
||||
* @param total_pressure pressure inside the pitot/prandtl tube
|
||||
* @param static_pressure pressure at the side of the tube/airplane
|
||||
* @return indicated airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_indicated_airspeed(float differential_pressure);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from indicated airspeed.
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param speed current indicated airspeed
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature);
|
||||
/**
|
||||
* Calculate true airspeed from indicated airspeed.
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param speed_indicated current indicated airspeed
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius);
|
||||
|
||||
/**
|
||||
* Directly calculate true airspeed
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param pressure_front pressure inside the pitot/prandl tube
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
|
||||
/**
|
||||
* Directly calculate true airspeed
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param total_pressure pressure inside the pitot/prandl tube
|
||||
* @param static_pressure pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
|
|
@ -49,7 +49,7 @@
|
|||
*/
|
||||
|
||||
/**
|
||||
* Battery voltages and status
|
||||
* Differential pressure and airspeed
|
||||
*/
|
||||
struct differential_pressure_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
|
@ -67,4 +67,4 @@ struct differential_pressure_s {
|
|||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(differential_pressure);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue