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             : 
      16             : #include "AP_RangeFinder_config.h"
      17             : 
      18             : #if AP_RANGEFINDER_ENABLED
      19             : 
      20             : #include <AP_Common/AP_Common.h>
      21             : #include <AP_HAL/AP_HAL.h>
      22             : #include "AP_RangeFinder.h"
      23             : #include "AP_RangeFinder_Backend.h"
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : /*
      28             :   base class constructor. 
      29             :   This incorporates initialisation as well.
      30             : */
      31         209 : AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
      32         209 :         state(_state),
      33         209 :                 params(_params)
      34             : {
      35         209 :     _backend_type = type();
      36         209 : }
      37             : 
      38      105945 : MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const {
      39      105945 :     if (type() == RangeFinder::Type::NONE) {
      40           0 :         return MAV_DISTANCE_SENSOR_UNKNOWN;
      41             :     }
      42      105945 :     return _get_mav_distance_sensor_type();
      43             : }
      44             : 
      45    16906571 : RangeFinder::Status AP_RangeFinder_Backend::status() const {
      46    16906571 :     if (type() == RangeFinder::Type::NONE) {
      47             :         // turned off at runtime?
      48           0 :         return RangeFinder::Status::NotConnected;
      49             :     }
      50    16906570 :     return state.status;
      51             : }
      52             : 
      53             : // true if sensor is returning data
      54      230930 : bool AP_RangeFinder_Backend::has_data() const
      55             : {
      56      230930 :     switch (state.status) {
      57        3427 :         case RangeFinder::Status::NotConnected:
      58             :         case RangeFinder::Status::NoData:
      59             :         case RangeFinder::Status::PoweredDown:
      60        3427 :             break;
      61             : 
      62      227503 :         case RangeFinder::Status::OutOfRangeLow:
      63             :         case RangeFinder::Status::OutOfRangeHigh:
      64             :         case RangeFinder::Status::Good:
      65      227503 :             return true;
      66             :     }
      67             : 
      68        3427 :     return false;
      69             : }
      70             : 
      71             : // update status based on distance measurement
      72      282084 : void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const
      73             : {
      74             :     // check distance
      75      282084 :     if (state_arg.distance_m > max_distance()) {
      76      103523 :         set_status(state_arg, RangeFinder::Status::OutOfRangeHigh);
      77      178561 :     } else if (state_arg.distance_m < min_distance()) {
      78       18970 :         set_status(state_arg, RangeFinder::Status::OutOfRangeLow);
      79             :     } else {
      80      159591 :         set_status(state_arg, RangeFinder::Status::Good);
      81             :     }
      82      282084 : }
      83             : 
      84             : // set status and update valid count
      85      291695 : void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status _status)
      86             : {
      87      291695 :     state_arg.status = _status;
      88             : 
      89             :     // update valid count
      90      291695 :     if (_status == RangeFinder::Status::Good) {
      91      160944 :         if (state_arg.range_valid_count < 10) {
      92        1589 :             state_arg.range_valid_count++;
      93             :         }
      94             :     } else {
      95      130751 :         state_arg.range_valid_count = 0;
      96             :     }
      97      291695 : }
      98             : 
      99             : // get temperature reading in C.  returns true on success and populates temp argument
     100             : // checks external temperature source first, then falls back to backend-specific reading
     101      347993 : bool AP_RangeFinder_Backend::get_temp(float &temp) const
     102             : {
     103             : #if AP_TEMPERATURE_SENSOR_ENABLED
     104      347993 :     if (state.temperature_valid && (AP_HAL::millis() - state.temperature_update_ms < 5000U)) {
     105        1020 :         temp = state.temperature_C;
     106        1020 :         return true;
     107             :     }
     108             : #endif
     109      346973 :     return _get_temp(temp);
     110             : }
     111             : 
     112             : // return true if rangefinder should be powered down
     113      101355 : bool AP_RangeFinder_Backend::should_power_down() const
     114             : {
     115             :     // Power saving range must be configured
     116      101355 :     if (params.powersave_range <= 0) {
     117       97695 :         return false;
     118             :     }
     119             : 
     120             :     // Must be above configured height
     121        3660 :     RangeFinder &frontend = *AP::rangefinder();
     122        3660 :     return frontend.estimated_terrain_height > params.powersave_range;
     123             : }
     124             : 
     125             : #if AP_SCRIPTING_ENABLED
     126             : // get a copy of state structure
     127          11 : void AP_RangeFinder_Backend::get_state(RangeFinder::RangeFinder_State &state_arg)
     128             : {
     129          11 :     WITH_SEMAPHORE(_sem);
     130          11 :     state_arg = state;
     131          11 : }
     132             : #endif
     133             : 
     134             : #endif  // AP_RANGEFINDER_ENABLED