Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : #include "AP_RangeFinder_SITL.h"
      16             : 
      17             : #if AP_RANGEFINDER_SIM_ENABLED
      18             : 
      19             : #include <AP_HAL/AP_HAL.h>
      20             : #include <SITL/SITL.h>
      21             : 
      22             : /*
      23             :   constructor - registers instance at top RangeFinder driver
      24             :  */
      25          23 : AP_RangeFinder_SITL::AP_RangeFinder_SITL(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params, uint8_t instance) :
      26             :     AP_RangeFinder_Backend(_state, _params),
      27          23 :     _instance(instance)
      28          23 : {}
      29             : 
      30             : /*
      31             :   update distance_cm
      32             :  */
      33       98183 : void AP_RangeFinder_SITL::update(void)
      34             : {
      35             :     // Support the power down state
      36       98183 :     if (should_power_down()) {
      37         146 :         set_status(RangeFinder::Status::PoweredDown);
      38        6060 :         return;
      39             :     }
      40             : 
      41       98037 :     const float dist = AP::sitl()->get_rangefinder(_instance);
      42             : 
      43             :     // nan distance means nothing is connected
      44       98037 :     if (isnan(dist) || isinf(dist)) {
      45        5914 :         set_status(RangeFinder::Status::NoData);
      46        5914 :         return;
      47             :     }
      48             : 
      49       92123 :     state.distance_m = MAX(0, dist);
      50       92123 :     state.last_reading_ms = AP_HAL::millis();
      51             : 
      52             :     // update range_valid state based on distance measured
      53       92123 :     update_status();
      54             : }
      55             : 
      56             : #endif  // AP_RANGEFINDER_SIM_ENABLED