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_MaxsonarI2CXL.cpp - Arduino Library for MaxBotix I2C XL sonar
      18             :  *       Code by Randy Mackay. DIYDrones.com
      19             :  *
      20             :  *       datasheet: http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf
      21             :  *
      22             :  *       Sensor should be connected to the I2C port
      23             :  */
      24             : #include "AP_RangeFinder_MaxsonarI2CXL.h"
      25             : 
      26             : #if AP_RANGEFINDER_MAXSONARI2CXL_ENABLED
      27             : 
      28             : #include <AP_HAL/AP_HAL.h>
      29             : #include <AP_HAL/utility/sparse-endian.h>
      30             : 
      31             : extern const AP_HAL::HAL& hal;
      32             : 
      33             : /*
      34             :    detect if a Maxbotix rangefinder is connected. We'll detect by
      35             :    trying to take a reading on I2C. If we get a result the sensor is
      36             :    there.
      37             : */
      38           8 : bool AP_RangeFinder_MaxsonarI2CXL::init(void)
      39             : {
      40           8 :     WITH_SEMAPHORE(dev.get_semaphore());
      41             : 
      42           8 :     if (!start_reading()) {
      43           0 :         return false;
      44             :     }
      45             : 
      46             :     // give time for the sensor to process the request
      47           8 :     hal.scheduler->delay(100);
      48             : 
      49             :     uint16_t reading_cm;
      50           8 :     if (!get_reading(reading_cm)) {
      51           0 :         return false;
      52             :     }
      53             : 
      54           8 :     dev.register_periodic_callback(100000,
      55             :                                      FUNCTOR_BIND_MEMBER(&AP_RangeFinder_MaxsonarI2CXL::_timer, void));
      56             : 
      57           8 :     return true;
      58           8 : }
      59             : 
      60             : // start_reading() - ask sensor to make a range reading - must be
      61             : // called with semaphore held
      62        4510 : bool AP_RangeFinder_MaxsonarI2CXL::start_reading()
      63             : {
      64        4510 :     uint8_t cmd = AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING;
      65             : 
      66             :     // send command to take reading
      67        4510 :     return dev.transfer(&cmd, sizeof(cmd), nullptr, 0);
      68             : }
      69             : 
      70             : // read - return last value measured by sensor
      71        4502 : bool AP_RangeFinder_MaxsonarI2CXL::get_reading(uint16_t &reading_cm)
      72             : {
      73             :     // take range reading and read back results
      74             :     be16_t val;
      75        4502 :     bool ret = dev.transfer(nullptr, 0, (uint8_t *) &val, sizeof(val));
      76             : 
      77        4502 :     if (ret) {
      78             :         // combine results into distance
      79        4502 :         reading_cm = be16toh(val);
      80             :     }
      81             : 
      82             :     // trigger a new reading
      83        4502 :     start_reading();
      84             : 
      85        4502 :     return ret;
      86             : }
      87             : 
      88             : /*
      89             :   timer called at 10Hz
      90             : */
      91        4494 : void AP_RangeFinder_MaxsonarI2CXL::_timer(void)
      92             : {
      93             :     uint16_t d;
      94        4494 :     if (get_reading(d)) {
      95        4494 :         WITH_SEMAPHORE(_sem);
      96        4494 :         distance = d;
      97        4494 :         new_distance = true;
      98        4494 :         state.last_reading_ms = AP_HAL::millis();
      99        4494 :     }
     100        4494 : }
     101             : 
     102             : /*
     103             :    update the state of the sensor
     104             : */
     105       19263 : void AP_RangeFinder_MaxsonarI2CXL::update(void)
     106             : {
     107       19263 :     WITH_SEMAPHORE(_sem);
     108       19263 :     if (new_distance) {
     109        4335 :         state.distance_m = distance * 0.01f;
     110        4335 :         new_distance = false;
     111        4335 :         update_status();
     112       14928 :     } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
     113             :         // if no updates for 0.3 seconds set no-data
     114           0 :         set_status(RangeFinder::Status::NoData);
     115             :     }
     116       19263 : }
     117             : 
     118             : #endif  // AP_RANGEFINDER_MAXSONARI2CXL_ENABLED