Line data    Source code

       1             : #include "AP_Baro_SITL.h"
       2             : 
       3             : #if AP_SIM_BARO_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       7             : 
       8             : extern const AP_HAL::HAL& hal;
       9             : 
      10             : /*
      11             :   constructor - registers instance at top Baro driver
      12             :  */
      13        4174 : AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) :
      14             :     AP_Baro_Backend(baro),
      15        8348 :     _sitl(AP::sitl()),
      16        4174 :     _has_sample(false)
      17             : {
      18        4174 :     if (_sitl != nullptr) {
      19        4174 :         _instance = _frontend.register_sensor();
      20             : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
      21         164 :         _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
      22             : #endif
      23        4174 :         set_bus_id(_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, _instance, DEVTYPE_BARO_SITL));
      24        4174 :         hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Baro_SITL::_timer, void));
      25             :     }
      26        4174 : }
      27             : 
      28             : // adjust for board temperature warmup on start-up
      29    30008817 : void AP_Baro_SITL::temperature_adjustment(float &p, float &T)
      30             : {
      31    30008817 :     const float tsec = AP_HAL::millis() * 0.001f;
      32    30008817 :     const float T_sensor = T + AP::sitl()->temp_board_offset;
      33    30008817 :     const float tconst = AP::sitl()->temp_tconst;
      34    30008817 :     if (tsec < 23 * tconst) { // time which past the equation below equals T_sensor within approx. 1E-9
      35    26802881 :         const float T0 = AP::sitl()->temp_start;
      36    26802881 :         T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst);
      37             :     }
      38             :     else {
      39     3205936 :         T = T_sensor;
      40             :     }
      41             : 
      42    30008817 :     const float baro_factor = AP::sitl()->temp_baro_factor;
      43    30008817 :     const float Tzero = 30.0f;  // start baro adjustment at 30C
      44    30008817 :     if (is_positive(baro_factor)) {
      45             :         // this produces a pressure change with temperature that
      46             :         // closely matches what has been observed with a ICM-20789
      47             :         // barometer. A typical factor is 1.2.
      48           0 :         p -= powf(MAX(T - Tzero, 0), baro_factor);
      49             :     }
      50    30008817 : }
      51             : 
      52   373826998 : void AP_Baro_SITL::_timer()
      53             : {
      54             : 
      55             :     // 100Hz
      56   373826998 :     const uint32_t now = AP_HAL::millis();
      57   373826998 :     if ((now - _last_sample_time) < 10) {
      58   342702363 :         return;
      59             :     }
      60    31143176 :     _last_sample_time = now;
      61             : 
      62    31143176 :     float sim_alt = _sitl->state.altitude;
      63             : 
      64    31143176 :     if (_sitl->baro[_instance].disable) {
      65             :         // barometer is disabled
      66       18541 :         return;
      67             :     }
      68             : 
      69             :     // ground effect: downwash raises static pressure so the baro under-reads altitude,
      70             :     // decaying to zero at 2m AGL; only applied while the motors are running
      71    31124635 :     const float geff = _sitl->baro[_instance].ground_effect_alt_err;
      72    31124635 :     if (is_positive(geff) && is_positive(_sitl->throttle)) {
      73       10402 :         const float h_agl_m = MAX(_sitl->state.height_agl, 0.0f);
      74       10402 :         const float decay_scale_m = 2.0f;
      75       10402 :         const float scale = MAX(0.0f, 1.0f - h_agl_m / decay_scale_m);
      76       10402 :         sim_alt -= geff * scale;
      77             :     }
      78             : 
      79    31124635 :     const auto drift_delta_t_ms = now - last_drift_delta_t_ms;
      80    31124635 :     last_drift_delta_t_ms = now;
      81    31124635 :     total_alt_drift += _sitl->baro[_instance].drift * drift_delta_t_ms * 0.001f;
      82             : 
      83    31124635 :     sim_alt += total_alt_drift;
      84    31124635 :     sim_alt += _sitl->baro[_instance].noise * rand_float();
      85             : 
      86             :     // add baro glitch
      87    31124635 :     sim_alt += _sitl->baro[_instance].glitch;
      88             : 
      89             :     // add delay
      90    31124635 :     uint32_t best_time_delta = 200;  // initialise large time representing buffer entry closest to current time - delay.
      91    31124635 :     uint8_t best_index = 0;  // initialise number representing the index of the entry in buffer closest to delay.
      92             : 
      93             :     // storing data from sensor to buffer
      94    31124635 :     if (now - _last_store_time >= 10) {  // store data every 10 ms.
      95    31124635 :         _last_store_time = now;
      96    31124635 :         if (_store_index > _buffer_length - 1) {  // reset buffer index if index greater than size of buffer
      97      621728 :             _store_index = 0;
      98             :         }
      99             : 
     100             :         // if freezed barometer, report altitude to last recorded altitude
     101    31124635 :         if (_sitl->baro[_instance].freeze == 1) {
     102         270 :             sim_alt = _last_altitude;
     103             :         } else {
     104    31124365 :             _last_altitude = sim_alt;
     105             :         }
     106             : 
     107    31124635 :         _buffer[_store_index].data = sim_alt;  // add data to current index
     108    31124635 :         _buffer[_store_index].time = _last_store_time;  // add time_stamp to current index
     109    31124635 :         _store_index = _store_index + 1;  // increment index
     110             :     }
     111             : 
     112             :     // return delayed measurement
     113    31124635 :     const uint32_t delayed_time = now - _sitl->baro[_instance].delay;  // get time corresponding to delay
     114             : 
     115             :     // find data corresponding to delayed time in buffer
     116  1587356385 :     for (uint8_t i = 0; i <= _buffer_length - 1; i++) {
     117             :         // find difference between delayed time and time stamp in buffer
     118  1556231750 :         uint32_t time_delta = abs(
     119  1556231750 :                 (int32_t)(delayed_time - _buffer[i].time));
     120             :         // if this difference is smaller than last delta, store this time
     121  1556231750 :         if (time_delta < best_time_delta) {
     122   503962915 :             best_index = i;
     123   503962915 :             best_time_delta = time_delta;
     124             :         }
     125             :     }
     126    31124635 :     if (best_time_delta < 200) {  // only output stored state if < 200 msec retrieval error
     127    31124635 :         sim_alt = _buffer[best_index].data;
     128             :     }
     129             : 
     130             : #if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
     131             :     float p, T_K;
     132    30008817 :     AP_Baro::get_pressure_temperature_for_alt_amsl(sim_alt, p, T_K);
     133    30008817 :     float T = KELVIN_TO_C(T_K);
     134    30008817 :     temperature_adjustment(p, T);
     135             : #else
     136             :     float rho, delta, theta;
     137     1115818 :     AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta);
     138     1115818 :     float p = SSL_AIR_PRESSURE * delta;
     139     1115818 :     float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
     140             : #endif
     141             : 
     142             :     // add in correction for wind effects
     143    31124635 :     p += wind_pressure_correction(_instance);
     144             : 
     145    31124635 :     _recent_press = p;
     146    31124635 :     _recent_temp = T;
     147    31124635 :     _has_sample = true;
     148             : }
     149             : 
     150             : // unhealthy if baro is turned off or beyond supported instances
     151     3071068 : bool AP_Baro_SITL::healthy(uint8_t instance) 
     152             : {
     153     3071068 :     return _last_sample_time != 0 && !_sitl->baro[instance].disable;
     154             : }
     155             : 
     156             : // Read the sensor
     157     3071068 : void AP_Baro_SITL::update(void)
     158             : {
     159     3071068 :     if (!_has_sample) {
     160        1683 :         return;
     161             :     }
     162             : 
     163     3069385 :     WITH_SEMAPHORE(_sem);
     164     3069385 :     _copy_to_frontend(_instance, _recent_press, _recent_temp);
     165     3069385 :     _has_sample = false;
     166     3069385 : }
     167             : 
     168             : /*
     169             :   return pressure correction for wind based on SIM_BARO_WCF parameters
     170             :  */
     171    31124635 : float AP_Baro_SITL::wind_pressure_correction(uint8_t instance)
     172             : {
     173    31124635 :     const auto &bp = AP::sitl()->baro[instance];
     174             : 
     175             :     // correct for static pressure position errors
     176    31124635 :     const Vector3f &airspeed_vec_bf = AP::sitl()->state.velocity_air_bf;
     177             : 
     178    31124635 :     float error = 0.0;
     179    31124635 :     const float sqx = sq(airspeed_vec_bf.x);
     180    31124635 :     const float sqy = sq(airspeed_vec_bf.y);
     181    31124635 :     const float sqz = sq(airspeed_vec_bf.z);
     182             : 
     183    31124635 :     if (is_positive(airspeed_vec_bf.x)) {
     184    13385137 :         error += bp.wcof_xp * sqx;
     185             :     } else {
     186    17739498 :         error += bp.wcof_xn * sqx;
     187             :     }
     188    31124635 :     if (is_positive(airspeed_vec_bf.y)) {
     189     9478466 :         error += bp.wcof_yp * sqy;
     190             :     } else {
     191    21646169 :         error += bp.wcof_yn * sqy;
     192             :     }
     193    31124635 :     if (is_positive(airspeed_vec_bf.z)) {
     194     6338998 :         error += bp.wcof_zp * sqz;
     195             :     } else {
     196    24785637 :         error += bp.wcof_zn * sqz;
     197             :     }
     198             : 
     199             :     // use the air density at the simulated vehicle's true altitude;
     200             :     // the vehicle's own estimates must play no part in simulating
     201             :     // the physical pressure error:
     202    31124635 :     return error * 0.5 * AP_Baro::get_air_density_for_alt_amsl(AP::sitl()->state.altitude);
     203             : }
     204             : 
     205             : #endif  // AP_SIM_BARO_ENABLED