Line data    Source code

       1             : /*
       2             :   SITL handling
       3             : 
       4             :   This simulates a rangefinder
       5             : 
       6             :  */
       7             : 
       8             : #include <AP_HAL/AP_HAL.h>
       9             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
      10             : 
      11             : #include "AP_HAL_SITL.h"
      12             : #include "AP_HAL_SITL_Namespace.h"
      13             : #include "HAL_SITL_Class.h"
      14             : #include "SITL_State.h"
      15             : #include <SITL/SITL.h>
      16             : #include <AP_Math/AP_Math.h>
      17             : 
      18             : extern const AP_HAL::HAL& hal;
      19             : 
      20             : using namespace HALSITL;
      21             : 
      22             : // returns a voltage between 0V to 5V which should appear as the
      23             : // voltage from the sensor
      24   187268325 : float SITL_State::_sonar_pin_voltage() const
      25             : {
      26             :     // Use glitch defines as the probablility between 0-1 that any
      27             :     // given sonar sample will read as max distance
      28   187268325 :     if (!is_zero(_sitl->sonar_glitch) &&
      29           0 :         _sitl->sonar_glitch >= (rand_float() + 1.0f) / 2.0f) {
      30             :         // glitched
      31           0 :         return 5.0f;
      32             :     }
      33             : 
      34   187268325 :     const float altitude = sitl_model->rangefinder_range();
      35             : #pragma GCC diagnostic push
      36             : #pragma GCC diagnostic ignored "-Wfloat-equal"
      37   187268325 :     if (altitude == INFINITY) {
      38     2707600 :         return 5.0f;
      39             :     }
      40             : #pragma GCC diagnostic pop
      41             : 
      42             :     // Altitude in in m, scaler in meters/volt
      43   184560725 :     const float voltage = altitude / _sitl->sonar_scale;
      44             : 
      45             :     // constrain to 0-5V
      46   184560725 :     return constrain_float(voltage, 0.0f, 5.0f);
      47             : }
      48             : 
      49             : /*
      50             :   setup the rangefinder with new input
      51             :  */
      52   187268325 : void SITL_State::_update_rangefinder()
      53             : {
      54   187268325 :     sonar_pin_voltage = _sonar_pin_voltage();
      55   187268325 : }
      56             : 
      57             : #endif