Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_MS5525_ENABLED
       4             : 
       5             : #include "SIM_MS5525.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : 
       9             : using namespace SITL;
      10             : 
      11       45427 : void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
      12             : {
      13       45427 :     const uint8_t Q1 = Qx_coeff[0];
      14       45427 :     const uint8_t Q2 = Qx_coeff[1];
      15       45427 :     const uint8_t Q3 = Qx_coeff[2];
      16       45427 :     const uint8_t Q4 = Qx_coeff[3];
      17       45427 :     const uint8_t Q5 = Qx_coeff[4];
      18       45427 :     const uint8_t Q6 = Qx_coeff[5];
      19             : 
      20             :     // this is the forward conversion copied from the driver:
      21       45427 :     float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
      22       45427 :     float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
      23       45427 :     float OFF  = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
      24       45427 :     float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
      25       45427 :     float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
      26       45427 :     const float PSI_to_Pa = 6894.757f;
      27       45427 :     P_Pa = PSI_to_Pa * 1.0e-4 * P;
      28       45427 :     Temp_C = TEMP * 0.01;
      29       45427 : }
      30             : 
      31       56787 : void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
      32             : {
      33       56787 :     const uint8_t Q1 = Qx_coeff[0];
      34       56787 :     const uint8_t Q2 = Qx_coeff[1];
      35       56787 :     const uint8_t Q3 = Qx_coeff[2];
      36       56787 :     const uint8_t Q4 = Qx_coeff[3];
      37       56787 :     const uint8_t Q5 = Qx_coeff[4];
      38       56787 :     const uint8_t Q6 = Qx_coeff[5];
      39             : 
      40       56787 :     const float TEMP = Temp_C * 100.0f;
      41       56787 :     const float dT = ((TEMP - 2000.0) * (1L<<Q6)) / int64_t(prom[6]);
      42       56787 :     const float PSI_to_Pa = 6894.757f;
      43       56787 :     const float P = P_Pa / (PSI_to_Pa * 1.0e-4);
      44       56787 :     const float OFF  = int64_t(prom[2]) * (1L<<Q2) + (int64_t(prom[4]) * dT) / (1L<<Q4);
      45       56787 :     const float SENS = int64_t(prom[1]) * (1L<<Q1) + (int64_t(prom[3]) * dT) / (1L<<Q3);
      46             : 
      47       56787 :     D1 = ((P * (1L<<15) + OFF) * (1L<<21)) / SENS;
      48       56787 :     D2 = dT + int64_t(prom[5]) * (1L<<Q5);
      49       56787 : }
      50             : 
      51       45419 : void MS5525::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
      52             : {
      53             :     float f_P_Pa;
      54             :     float f_Temp_C;
      55       45419 :     convert_forward(D1, D2, f_P_Pa, f_Temp_C);
      56             : 
      57       45419 :     const float p_error = fabs(f_P_Pa - P_Pa);
      58       45419 :     const float p_error_max = 0.1;
      59       45419 :     if (p_error > p_error_max) {
      60           0 :         AP_HAL::panic("Invalid pressure conversion: error %f Pa > %f Pa error max", p_error, p_error_max);
      61             :     }
      62             : 
      63       45419 :     const float t_error = fabs(f_Temp_C - Temp_C);
      64       45419 :     const float t_error_max = 0.01;
      65       45419 :     if (t_error > t_error_max) {
      66           0 :         AP_HAL::panic("Invalid temperature conversion: error %f degC > %f degC error max", t_error, t_error_max);
      67             :     }
      68       45419 : }
      69             : 
      70       56779 : void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
      71             : {
      72       56779 :     float sim_alt = AP::sitl()->state.altitude;
      73       56779 :     sim_alt += 2 * rand_float();
      74             : 
      75       56779 :     Temp_C = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
      76       56779 :     const uint8_t instance = 0;  // TODO: work out which sensor this is
      77       56779 :     P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset;
      78       56779 : }
      79             : 
      80             : #endif  // AP_SIM_MS5525_ENABLED