Line data    Source code

       1             : #include "AP_Baro_Backend.h"
       2             : 
       3             : #include <stdio.h>
       4             : #include <AP_Math/AP_Math.h>
       5             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9             : #ifndef AP_BARO_DATA_CHANGE_TIMEOUT_MS
      10             : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
      11             : #define AP_BARO_DATA_CHANGE_TIMEOUT_MS     20000    // timeout in ms since last successful read that involved temperature of pressure changing
      12             : #else
      13             : #define AP_BARO_DATA_CHANGE_TIMEOUT_MS     2000    // timeout in ms since last successful read that involved temperature of pressure changing
      14             : #endif  // APM_BUILD_ArduSub
      15             : #endif  // ifndef(AP_BARO_DATA_CHANGE_TIMEOUT_MS)
      16             : 
      17             : // constructor
      18        4319 : AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : 
      19        4319 :     _frontend(baro) 
      20             : {
      21        4319 : }
      22             : 
      23        4237 : void AP_Baro_Backend::set_bus_id(uint8_t instance, uint32_t id)
      24             : {
      25        4237 :     _frontend.sensors[instance].bus_id.set(int32_t(id));
      26        4237 : }
      27             : 
      28       15425 : void AP_Baro_Backend::update_healthy_flag(uint8_t instance)
      29             : {
      30       15425 :     if (instance >= _frontend._num_sensors) {
      31           0 :         return;
      32             :     }
      33       15425 :     WITH_SEMAPHORE(_sem);
      34             : 
      35             :     // consider a sensor as healthy if it has had an update in the
      36             :     // last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
      37       15425 :     const uint32_t now = AP_HAL::millis();
      38       15425 :     _frontend.sensors[instance].healthy =
      39       30837 :         (now - _frontend.sensors[instance].last_update_ms < BARO_TIMEOUT_MS) &&
      40       30837 :         (now - _frontend.sensors[instance].last_change_ms < AP_BARO_DATA_CHANGE_TIMEOUT_MS) &&
      41       15412 :         !is_zero(_frontend.sensors[instance].pressure);
      42             : 
      43       15425 :     if (_frontend.sensors[instance].temperature < -200 ||
      44       15425 :         _frontend.sensors[instance].temperature > 200) {
      45             :         // if temperature is way out of range then we likely have bad
      46             :         // data from the sensor, treat is as unhealthy. This is done
      47             :         // so SPI sensors which have no data validity checking can
      48             :         // mark a sensor unhealthy
      49           0 :         _frontend.sensors[instance].healthy = false;
      50             :     }
      51       15425 : }
      52             : 
      53     3086493 : void AP_Baro_Backend::backend_update(uint8_t instance)
      54             : {
      55     3086493 :     update();
      56     3086493 :     update_healthy_flag(instance);
      57     3086493 : }
      58             : 
      59             : 
      60             : /*
      61             :   copy latest data to the frontend from a backend
      62             :  */
      63     3084795 : void AP_Baro_Backend::_copy_to_frontend(uint8_t instance, float pressure, float temperature)
      64             : {
      65     3084795 :     if (instance >= _frontend._num_sensors) {
      66           0 :         return;
      67             :     }
      68     3084795 :     uint32_t now = AP_HAL::millis();
      69             : 
      70             :     // check for changes in data values
      71     3084795 :     if (!is_equal(_frontend.sensors[instance].pressure, pressure) || !is_equal(_frontend.sensors[instance].temperature, temperature)) {
      72     3000701 :         _frontend.sensors[instance].last_change_ms = now;
      73             :     }
      74             : 
      75             :     // update readings
      76     3084795 :     _frontend.sensors[instance].pressure = pressure;
      77     3084795 :     _frontend.sensors[instance].temperature = temperature;
      78     3084795 :     _frontend.sensors[instance].last_update_ms = now;
      79             : }
      80             : 
      81             : static constexpr float FILTER_KOEF = 0.1f;
      82             : 
      83             : /* Check that the baro value is valid by using a mean filter. If the
      84             :  * value is further than filter_range from mean value, it is
      85             :  * rejected. 
      86             : */
      87       19321 : bool AP_Baro_Backend::pressure_ok(float press)
      88             : {
      89             :     
      90       19321 :     if (isinf(press) || isnan(press)) {
      91           0 :         return false;
      92             :     }
      93             : 
      94       19321 :     const float range = (float)_frontend.get_filter_range();
      95       19321 :     if (range <= 0) {
      96       19321 :         return true;
      97             :     }
      98             : 
      99           0 :     bool ret = true;
     100           0 :     if (is_zero(_mean_pressure)) {
     101           0 :         _mean_pressure = press;
     102             :     } else {
     103           0 :         const float d = fabsf(_mean_pressure - press) / (_mean_pressure + press);  // diff divide by mean value in percent ( with the * 200.0f on later line)
     104           0 :         float koeff = FILTER_KOEF;
     105             : 
     106           0 :         if (d * 200.0f > range) {  // check the difference from mean value outside allowed range
     107             :             // printf("\nBaro pressure error: mean %f got %f\n", (double)_mean_pressure, (double)press );
     108           0 :             ret = false;
     109           0 :             koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
     110           0 :             _error_count++;
     111             :         }
     112           0 :         _mean_pressure = _mean_pressure * (1 - koeff) + press * koeff; // complimentary filter 1/k
     113             :     }
     114           0 :     return ret;
     115             : }