diff --git a/src/drivers/ms5525_airspeed/MS5525.cpp b/src/drivers/ms5525_airspeed/MS5525.cpp index 6666c75510..e5d518f4c0 100644 --- a/src/drivers/ms5525_airspeed/MS5525.cpp +++ b/src/drivers/ms5525_airspeed/MS5525.cpp @@ -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; diff --git a/src/drivers/ms5525_airspeed/MS5525.hpp b/src/drivers/ms5525_airspeed/MS5525.hpp index 7518bf32b6..ecaba5fa15 100644 --- a/src/drivers/ms5525_airspeed/MS5525.hpp +++ b/src/drivers/ms5525_airspeed/MS5525.hpp @@ -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}; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 4af0772b18..4f17e95bed 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -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 */