Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_MS5611_ENABLED
       4             : 
       5             : #include "SIM_MS5611.h"
       6             : 
       7             : #include <SITL/SITL.h>
       8             : 
       9             : #include <stdio.h>
      10             : 
      11             : using namespace SITL;
      12             : 
      13             : // forward conversion, copied from driver:
      14       19328 : void MS5611::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
      15             : {
      16             :     // _D1 and _D2 are stored as floats in driver
      17       19328 :     const float _D1 = D1;
      18       19328 :     const float _D2 = D2;
      19             : 
      20             :     float dT;
      21             :     float TEMP;
      22             :     float OFF;
      23             :     float SENS;
      24             : 
      25       19328 :     dT = _D2-(((uint32_t)prom[5])<<8);
      26       19328 :     TEMP = (dT * prom[6])/8388608;
      27       19328 :     OFF = prom[2] * 65536.0f + (prom[4] * dT) / 128;
      28       19328 :     SENS = prom[1] * 32768.0f + (prom[3] * dT) / 256;
      29             : 
      30       19328 :     TEMP += 2000;
      31             : 
      32       19328 :     if (TEMP < 2000) {
      33             :         // second order temperature compensation when under 20 degrees C
      34           4 :         float T2 = (dT*dT) / 0x80000000;
      35           4 :         float Aux = sq(TEMP-2000.0);
      36           4 :         float OFF2 = 2.5f*Aux;
      37           4 :         float SENS2 = 1.25f*Aux;
      38           4 :         if (TEMP < -1500) {
      39             :             // extra compensation for temperatures below -15C
      40           2 :             OFF2 += 7 * sq(TEMP+1500);
      41           2 :             SENS2 += sq(TEMP+1500) * 11.0*0.5;
      42             :         }
      43           4 :         TEMP = TEMP - T2;
      44           4 :         OFF = OFF - OFF2;
      45           4 :         SENS = SENS - SENS2;
      46             :     }
      47             : 
      48       19328 :     P_Pa = (_D1*SENS/2097152 - OFF)/32768;
      49       19328 :     Temp_C = TEMP * 0.01f;
      50       19328 : }
      51             : 
      52       24159 : void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
      53             : {
      54       24159 :     const uint8_t Q1 = Qx_coeff[0];
      55       24159 :     const uint8_t Q2 = Qx_coeff[1];
      56       24159 :     const uint8_t Q3 = Qx_coeff[2];
      57       24159 :     const uint8_t Q4 = Qx_coeff[3];
      58       24159 :     const uint8_t Q5 = Qx_coeff[4];
      59       24159 :     const uint8_t Q6 = Qx_coeff[5];
      60             : 
      61       24159 :     const float TEMP = Temp_C * 100;
      62             : 
      63             :     // second order temperature compensation when under 20 degrees C
      64       24159 :     if (TEMP < 2000) {
      65             :         // Solve the quadratic equation for D2 when TEMP < 2000
      66           4 :         D2 = 128 * (2 * int64_t(prom[5]) - sqrt(sq(int64_t(prom[6])) - 131072 * (TEMP - 2000)) + int64_t(prom[6]));
      67             : 
      68             :         // Must compute the pressure compensation values using D2
      69           4 :         const float dT = float(D2) - (int64_t(prom[5]) << Q5);
      70           4 :         float TEMP_forward = 2000 + (dT * int64_t(prom[6])) / (1L << Q6);
      71           4 :         float OFF  = int64_t(prom[2]) * (1L << Q2) + (int64_t(prom[4]) * dT) / (1L << Q4);
      72           4 :         float SENS = int64_t(prom[1]) * (1L << Q1) + (int64_t(prom[3]) * dT) / (1L << Q3);
      73             : 
      74           4 :         const float Aux = sq(TEMP_forward - 2000);
      75           4 :         float OFF2  = 2.5 * Aux;
      76           4 :         float SENS2 = 1.25 * Aux;
      77           4 :         if (TEMP < -1500) {
      78             :             // extra compensation for temperatures below -15C
      79           2 :             OFF2  += 7 * sq(TEMP_forward + 1500);
      80           2 :             SENS2 += sq(TEMP_forward + 1500) * 11.0 * 0.5;
      81             :         }
      82             : 
      83           4 :         OFF = OFF - OFF2;
      84           4 :         SENS = SENS - SENS2;
      85             : 
      86           4 :         D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
      87             :     } else {
      88       24155 :         const float dT = (TEMP - 2000) * (1L << Q6) / int64_t(prom[6]);
      89       24155 :         const float OFF  = int64_t(prom[2]) * (1L << Q2) + (int64_t(prom[4]) * dT) / (1L << Q4);
      90       24155 :         const float SENS = int64_t(prom[1]) * (1L << Q1) + (int64_t(prom[3]) * dT) / (1L << Q3);
      91             : 
      92       24155 :         D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
      93       24155 :         D2 = dT + (int64_t(prom[5]) << Q5);
      94             :     }
      95       24159 : }
      96             : 
      97       19322 : void MS5611::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
      98             : {
      99             :     float f_P_Pa;
     100             :     float f_Temp_C;
     101       19322 :     convert_forward(D1, D2, f_P_Pa, f_Temp_C);
     102             : 
     103       19322 :     if (fabs(f_P_Pa - P_Pa) > 0.2) {
     104           0 :         AP_HAL::panic("Invalid pressure conversion");
     105             :     }
     106       19322 :     if (fabs(f_Temp_C - Temp_C) > 0.02) {
     107           0 :         AP_HAL::panic("Invalid temperature conversion");
     108             :     }
     109       19322 : }
     110             : 
     111       24153 : void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
     112             : {
     113       24153 :     float sim_alt = AP::sitl()->state.altitude;
     114       24153 :     sim_alt += 2 * rand_float();
     115             : 
     116             :     float p, T_K;
     117       24153 :     AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, T_K);
     118             : 
     119       24153 :     P_Pa = p;
     120       24153 :     Temp_C = KELVIN_TO_C(T_K) + AP::sitl()->temp_board_offset;
     121             : 
     122             :     // TO DO add in temperature adjustment by inheritting from AP_Baro_SITL_Generic?
     123             :     // AP_Baro_SITL::temperature_adjustment(P_Pa, Temp_C);
     124             : 
     125             :     // TO DO add in wind correction by inheritting from AP_Baro_SITL_Generic?
     126             :     // P_Pa += AP_Baro_SITL::wind_pressure_correction(instance);
     127       24153 : }
     128             : 
     129             : #endif  // AP_SIM_MS5611_ENABLED