Line data    Source code

       1             : /*
       2             :   SITL handling
       3             : 
       4             :   This simulates an analog airspeed sensor
       5             : 
       6             :   Andrew Tridgell November 2011
       7             :  */
       8             : 
       9             : #include <AP_HAL/AP_HAL.h>
      10             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
      11             : 
      12             : #include "AP_HAL_SITL.h"
      13             : #include "AP_HAL_SITL_Namespace.h"
      14             : #include "HAL_SITL_Class.h"
      15             : #include "SITL_State.h"
      16             : #include <SITL/SITL.h>
      17             : #include <AP_Math/AP_Math.h>
      18             : 
      19             : extern const AP_HAL::HAL& hal;
      20             : 
      21             : using namespace HALSITL;
      22             : 
      23             : // scaling value taken from AP_Airspeed_analog.cpp
      24             : #define VOLTS_TO_PASCAL 819
      25             : #define PASCAL_TO_VOLTS(_p) (_p/VOLTS_TO_PASCAL)
      26             : 
      27             : /*
      28             :   convert equivalent airspeed (EAS) in m/s to an airspeed sensor value
      29             :  */
      30   187270419 : void SITL_State::_update_airspeed(float eas)
      31             : {
      32   561811257 :     for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
      33   374540838 :         const auto &arspd = _sitl->airspeed[i];
      34   374540838 :         float airspeed = eas;
      35   374540838 :         const float diff_pressure = sq(airspeed) / arspd.ratio;
      36             :         float airspeed_raw;
      37             :     
      38             :         // apply noise to the differential pressure. This emulates the way
      39             :         // airspeed noise reduces with speed
      40   374540838 :         airspeed = sqrtf(fabsf(arspd.ratio*(diff_pressure + arspd.noise * rand_float())));
      41             : 
      42             :         // check sensor failure
      43   374540838 :         if (is_positive(arspd.fail)) {
      44       27274 :             airspeed = arspd.fail;
      45             :         }
      46             : 
      47   374540838 :         if (!is_zero(arspd.fail_pressure)) {
      48             :             // compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
      49             :             // algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
      50           0 :             float tube_pressure = fabsf(arspd.fail_pressure - AP::baro().get_pressure() + arspd.fail_pitot_pressure);
      51           0 :             airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
      52             :         }
      53   374540838 :         float airspeed_pressure = (airspeed * airspeed) / arspd.ratio;
      54             : 
      55             :         // flip sign here for simulating reversed pitot/static connections
      56   374540838 :         if (arspd.signflip) {
      57           0 :             airspeed_pressure *= -1;
      58             :         }
      59             : 
      60             :         // apply airspeed sensor offset in m/s
      61   374540838 :         airspeed_raw = airspeed_pressure + arspd.offset;
      62             : 
      63   374540838 :         _sitl->state.airspeed_raw_pressure[i] = airspeed_pressure;
      64             :         
      65   374540838 :         airspeed_pin_voltage[i] = PASCAL_TO_VOLTS(airspeed_raw);
      66             :     }
      67   187270419 : }
      68             : 
      69             : #endif