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             : /*
      17             :  *   AP_RangeFinder_analog.cpp - rangefinder for analog source
      18             :  *
      19             :  */
      20             : 
      21             : #include "AP_RangeFinder_analog.h"
      22             : 
      23             : #if AP_RANGEFINDER_ANALOG_ENABLED
      24             : 
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_Common/AP_Common.h>
      27             : #include <AP_Math/AP_Math.h>
      28             : #include "AP_RangeFinder.h"
      29             : #include "AP_RangeFinder_Params.h"
      30             : 
      31             : extern const AP_HAL::HAL& hal;
      32             : 
      33             : /* 
      34             :    The constructor also initialises the rangefinder. Note that this
      35             :    constructor is not called until detect() returns true, so we
      36             :    already know that we should setup the rangefinder
      37             : */
      38         130 : AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
      39         130 :     AP_RangeFinder_Backend(_state, _params)
      40             : {
      41         130 :     source = hal.analogin->channel(_params.pin);
      42         130 :     if (source == nullptr) {
      43             :         // failed to allocate a ADC channel? This shouldn't happen
      44           0 :         set_status(RangeFinder::Status::NotConnected);
      45           0 :         return;
      46             :     }
      47         130 :     set_status(RangeFinder::Status::NoData);
      48             : }
      49             : 
      50             : /* 
      51             :    detect if an analog rangefinder is connected. The only thing we
      52             :    can do is check if the pin number is valid. If it is, then assume
      53             :    that the device is connected
      54             : */
      55         130 : bool AP_RangeFinder_analog::detect(AP_RangeFinder_Params &_params)
      56             : {
      57         130 :     if (_params.pin != -1) {
      58         130 :         return true;
      59             :     }
      60           0 :     return false;
      61             : }
      62             : 
      63             : 
      64             : /*
      65             :   update raw voltage state
      66             :  */
      67      162019 : void AP_RangeFinder_analog::update_voltage(void)
      68             : {
      69      162019 :    if (source == nullptr || !source->set_pin(params.pin)) {
      70           0 :        state.voltage_mv = 0;
      71           0 :        set_status(RangeFinder::Status::NotConnected);
      72           0 :        return;
      73             :    }
      74      162019 :    if (params.ratiometric) {
      75      162019 :        state.voltage_mv = source->voltage_average_ratiometric() * 1000U;
      76             :    } else {
      77           0 :        state.voltage_mv = source->voltage_average() * 1000U;
      78             :    }
      79             : }
      80             : 
      81             : /*
      82             :   update distance_cm 
      83             :  */
      84      162019 : void AP_RangeFinder_analog::update(void)
      85             : {
      86      162019 :     update_voltage();
      87      162019 :     float v = state.voltage_mv * 0.001f;
      88      162019 :     float dist_m = 0;
      89      162019 :     float scaling = params.scaling;
      90      162019 :     float offset  = params.offset;
      91      162019 :     RangeFinder::Function function = (RangeFinder::Function)params.function.get();
      92      162019 :     const float _max_distance = max_distance();
      93             : 
      94      162019 :     switch (function) {
      95      162019 :     case RangeFinder::Function::LINEAR:
      96      162019 :         dist_m = (v - offset) * scaling;
      97      162019 :         break;
      98             :           
      99           0 :     case RangeFinder::Function::INVERTED:
     100           0 :         dist_m = (offset - v) * scaling;
     101           0 :         break;
     102             : 
     103           0 :     case RangeFinder::Function::HYPERBOLA:
     104           0 :         if (v <= offset) {
     105           0 :             dist_m = 0;
     106             :         } else {
     107           0 :             dist_m = scaling / (v - offset);
     108             :         }
     109           0 :         if (dist_m > _max_distance) {
     110           0 :             dist_m = _max_distance;
     111             :         }
     112           0 :         break;
     113             :     }
     114      162019 :     if (dist_m < 0) {
     115           0 :         dist_m = 0;
     116             :     }
     117      162019 :     state.distance_m = dist_m;
     118      162019 :     state.last_reading_ms = AP_HAL::millis();
     119             : 
     120             :     // update range_valid state based on distance measured
     121      162019 :     update_status();
     122      162019 : }
     123             : 
     124             : #endif  // AP_RANGEFINDER_ANALOG_ENABLED