dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged voltage compensation

This commit is contained in:
Andrew Tridgell 2014-02-12 13:48:37 +11:00 committed by Lorenz Meier
parent 00c82f0836
commit 9719af623d
1 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -126,7 +126,7 @@ protected:
/** /**
* Correct for 5V rail voltage variations * 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; int _t_system_power;
struct system_power_s system_power; struct system_power_s system_power;
@ -204,7 +204,7 @@ MEASAirspeed::collect()
dp_raw = 0x3FFF & dp_raw; dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3]; dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5; 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 /* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value * and can go positive or negative, enforce absolute value
@ -218,7 +218,7 @@ MEASAirspeed::collect()
} }
// correct for 5V rail voltage if possible // correct for 5V rail voltage if possible
voltage_correction(diff_press_pa); voltage_correction(diff_press_pa, temperature);
struct differential_pressure_s report; struct differential_pressure_s report;
@ -308,7 +308,7 @@ MEASAirspeed::cycle()
offset versus voltage for 3 sensors offset versus voltage for 3 sensors
*/ */
void void
MEASAirspeed::voltage_correction(float &diff_press_pa) MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{ {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) { if (_t_system_power == -1) {
@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa)
voltage_diff = -0.5f; voltage_diff = -0.5f;
} }
diff_press_pa -= voltage_diff * slope; 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 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
} }