aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Airspeed: average two readings for airspeed

Browse Source 
given we're using it to check for errors, we may as well reduce analog
noise too
This commit is contained in:
Andrew Tridgell 2017-04-02 10:53:45 +10:00
parent 48ae2f2de0
commit 6ec32472c5
2 changed files with 39 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
View File

@ -98,6 +98,34 @@ void AP_Airspeed_MS4525::_measure()
}
}
/*
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 AP_Airspeed_MS4525::_get_pressure(int16_t dp_raw) const
{
const float P_max = get_psi_range();
const float P_min = - P_max;
const float PSI_to_Pa = 6894.757f;
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float press = diff_press_PSI * PSI_to_Pa;
return press;
}
/*
convert raw temperature to temperature in degrees C
*/
float AP_Airspeed_MS4525::_get_temperature(int16_t dT_raw) const
{
float temp = ((200.0f * dT_raw) / 2047) - 50;
return temp;
}
// read the values from the sensor
void AP_Airspeed_MS4525::_collect()
{
@ -145,29 +173,19 @@ void AP_Airspeed_MS4525::_collect()
return;
}
const float P_max = get_psi_range();
const float P_min = - P_max;
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 press = diff_press_PSI * PSI_to_Pa;
float temp = ((200.0f * dT_raw) / 2047) - 50;
float press = _get_pressure(dp_raw);
float press2 = _get_pressure(dp_raw2);
float temp = _get_temperature(dT_raw);
float temp2 = _get_temperature(dT_raw2);
_voltage_correction(press, temp);
_voltage_correction(press2, temp2);
if (sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_press_sum += press;
_temp_sum += temp;
_press_count++;
_temp_count++;
_press_sum += press + press2;
_temp_sum += temp + temp2;
_press_count += 2;
_temp_count += 2;
sem->give();
}

2
libraries/AP_Airspeed/AP_Airspeed_MS4525.h
View File

@ -47,6 +47,8 @@ private:
void _collect();
void _timer();
void _voltage_correction(float &diff_press_pa, float &temperature);
float _get_pressure(int16_t dp_raw) const;
float _get_temperature(int16_t dT_raw) const;
float _temp_sum;
float _press_sum;