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_HAL/AP_HAL.h>
      21             : #include "AP_RangeFinder_Backend_Serial.h"
      22             : #include <AP_SerialManager/AP_SerialManager.h>
      23             : 
      24             : #include <ctype.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : /*
      29             :    The constructor also initialises the rangefinder. Note that this
      30             :    constructor is not called until detect() returns true, so we
      31             :    already know that we should setup the rangefinder
      32             : */
      33          26 : AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial(
      34             :     RangeFinder::RangeFinder_State &_state,
      35          26 :     AP_RangeFinder_Params &_params) :
      36          26 :     AP_RangeFinder_Backend(_state, _params)
      37             : {
      38             : 
      39          26 : }
      40             : 
      41          26 : void AP_RangeFinder_Backend_Serial::init_serial(uint8_t serial_instance)
      42             : {
      43          26 :     uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
      44          26 :     if (uart != nullptr) {
      45          26 :         uart->begin(initial_baudrate(serial_instance), rx_bufsize(), tx_bufsize());
      46             :     }
      47          26 : }
      48             : 
      49          17 : uint32_t AP_RangeFinder_Backend_Serial::initial_baudrate(const uint8_t serial_instance) const
      50             : {
      51          17 :     return AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
      52             : }
      53             : 
      54             : /*
      55             :    update the state of the sensor
      56             : */
      57       46166 : void AP_RangeFinder_Backend_Serial::update(void)
      58             : {
      59       46166 :     if (get_reading(state.distance_m)) {
      60       13715 :         state.signal_quality_pct = get_signal_quality_pct();
      61             :         // update range_valid state based on distance measured
      62       13715 :         state.last_reading_ms = AP_HAL::millis();
      63       13715 :         update_status();
      64       32451 :     } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
      65          33 :         set_status(RangeFinder::Status::NoData);
      66             :     }
      67       46166 : }
      68             : 
      69             : #endif  // AP_RANGEFINDER_ENABLED