diff options
author | Andrew Tridgell <tridge@samba.org> | 2014-02-12 13:48:37 +1100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-05 13:14:43 +0200 |
commit | 9719af623d92ceab0c5eebe21906b4d5cf515682 (patch) | |
tree | 53c286b644032adcd0f99a54a2d329f8b31bac83 /src/drivers/meas_airspeed/meas_airspeed.cpp | |
parent | 00c82f0836388ffa1e1c4a5827eefe28c0521df8 (diff) | |
download | px4-firmware-9719af623d92ceab0c5eebe21906b4d5cf515682.tar.gz px4-firmware-9719af623d92ceab0c5eebe21906b4d5cf515682.tar.bz2 px4-firmware-9719af623d92ceab0c5eebe21906b4d5cf515682.zip |
Merged voltage compensation
Diffstat (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp')
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 21 |
1 files changed, 17 insertions, 4 deletions
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ce..0b7986383 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() 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; + 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 @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } |