yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

ms5525 use int64 for calculation

Browse Source
This commit is contained in:
Daniel Agar
2017-06-17 16:30:35 -04:00
parent b66b734a74
commit c45d369004
3 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/drivers/ms5525_airspeed/MS5525.cpp
View File

@@ -117,7 +117,7 @@ MS5525::init_ms5525()
C5 = prom[5];
C6 = prom[6];
Tref = C5 * (1UL << Q5);
Tref = int64_t(C5) * (1UL << Q5);
return true;
@@ -223,31 +223,31 @@ MS5525::collect()
// Difference between actual and reference temperature
// dT = D2 - Tref
const int32_t dT = D2 - Tref;
const int64_t dT = D2 - Tref;
// Measured temperature
// TEMP = 20°C + dT * TEMPSENS
const int32_t TEMP = 2000 + (dT * C6) / (1L << Q6);
const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6);
// Offset at actual temperature
// OFF = OFF_T1 + TCO * dT
const int64_t OFF = C2 * (1L << Q2) + (C4 * dT) / (1L << Q4);
const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4);
// Sensitivity at actual temperature
// SENS = SENS_T1 + TCS * dT
const int64_t SENS = C1 * (1L << Q1) + (C3 * dT) / (1L << Q3);
const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3);
// Temperature Compensated Pressure (example 24996 = 2.4996 psi)
// P = D1 * SENS - OFF
const int64_t P = (((int64_t)D1 * SENS) / (1L << 21) - OFF) / (1L << 15);
const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15);
const float diff_press_PSI = (float)P * 0.0001f;
const float diff_press_PSI = P * 0.0001f;
// 1 PSI = 6894.76 Pascals
const float PSI_to_Pa = 6894.757f;
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
const float temperature_c = (float)TEMP * 0.01f;
const float temperature_c = TEMP * 0.01f;
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;

5
src/drivers/ms5525_airspeed/MS5525.hpp
View File

@@ -78,7 +78,8 @@ private:
int measure() override;
int collect() override;
math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
// temperature is read once every 10 cycles
math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ};
static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
@@ -122,7 +123,7 @@ private:
uint16_t C5{0};
uint16_t C6{0};
uint32_t Tref{0};
int64_t Tref{0};
// last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature)
uint32_t D1{0};

2
src/modules/commander/airspeed_calibration.cpp
View File

@@ -216,8 +216,10 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_filtered_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true);
}
} else if (diff_pres.differential_pressure_filtered_pa < 0.0f) {
/* do not allow negative values */