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             :   driver for TeraRanger I2C rangefinders
      17             :  */
      18             : #include "AP_RangeFinder_TeraRangerI2C.h"
      19             : 
      20             : #if AP_RANGEFINDER_TRI2C_ENABLED
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_HAL/I2CDevice.h>
      24             : 
      25             : #include <AP_Math/crc.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : // registers
      30             : #define TR_MEASURE 0x00
      31             : #define TR_WHOAMI  0x01
      32             : #define TR_WHOAMI_VALUE 0xA1
      33             : 
      34             : #define TR_OUT_OF_RANGE_ADD_CM 100 //cm
      35             : 
      36             : /*
      37             :   initialise sensor
      38             :  */
      39           1 : bool AP_RangeFinder_TeraRangerI2C::init(void)
      40             : {
      41           1 :     WITH_SEMAPHORE(dev.get_semaphore());
      42             : 
      43           1 :     dev.set_retries(10);
      44             : 
      45             :     // check WHOAMI
      46             :     uint8_t whoami;
      47           2 :     if (!dev.read_registers(TR_WHOAMI, &whoami, 1) ||
      48           1 :         whoami != TR_WHOAMI_VALUE) {
      49           0 :         return false;
      50             :     }
      51             : 
      52           1 :     if (!measure()) {
      53           0 :         return false;
      54             :     }
      55             : 
      56             :     // give time for the sensor to process the request
      57           1 :     hal.scheduler->delay(70);
      58             : 
      59             :     uint16_t _distance_cm;
      60           1 :     if (!collect_raw(_distance_cm)) {
      61           0 :         return false;
      62             :     }
      63             : 
      64             :     // ask for a new reading for the timer to collect:
      65           1 :     measure();
      66             : 
      67           1 :     dev.set_retries(1);
      68             : 
      69           1 :     dev.register_periodic_callback(10000,
      70             :                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_TeraRangerI2C::timer, void));
      71             : 
      72           1 :     return true;
      73           1 : }
      74             : 
      75             : // measure() - ask sensor to make a range reading
      76        7147 : bool AP_RangeFinder_TeraRangerI2C::measure()
      77             : {
      78        7147 :     uint8_t cmd = TR_MEASURE;
      79        7147 :     return dev.transfer(&cmd, 1, nullptr, 0);
      80             : }
      81             : 
      82             : // collect_raw() - return last value measured by sensor
      83        7146 : bool AP_RangeFinder_TeraRangerI2C::collect_raw(uint16_t &raw_distance)
      84             : {
      85             :     uint8_t d[3];
      86             : 
      87             :     // Take range reading
      88        7146 :     if (!dev.transfer(nullptr, 0, d, sizeof(d))) {
      89           0 :         return false;
      90             :     }
      91             : 
      92             :     // Check for CRC
      93        7146 :     if (d[2] != crc_crc8(d, 2)) {
      94           0 :         return false;
      95             :     } else {
      96        7146 :         raw_distance = ((uint16_t(d[0]) << 8) | d[1]);
      97        7146 :         return true;
      98             :     }
      99             : }
     100             : 
     101             : // Checks for error code and if correct converts to cm
     102        7145 : bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, uint16_t &output_distance_cm)
     103             : {
     104             :     // Check for error codes
     105        7145 :     if (raw_distance == 0xFFFF) {
     106             :         // Too far away
     107           0 :         output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM;
     108        7145 :     } else if (raw_distance == 0x0000) {
     109             :         // Too close
     110        4979 :         output_distance_cm = 0;
     111        2166 :     } else if (raw_distance == 0x0001) {
     112             :         // Unable to measure
     113             :         // This can also include the sensor pointing to the horizon when used as a proximity sensor
     114           0 :         output_distance_cm = max_distance()*100 + TR_OUT_OF_RANGE_ADD_CM;
     115             :     } else {
     116        2166 :         output_distance_cm = raw_distance/10; // Conversion to centimeters
     117             :     }
     118        7145 :     return true;
     119             : }
     120             : 
     121             : /*
     122             :   timer called at 100Hz, EVO sensors max freq is 100..240Hz
     123             : */
     124        7145 : void AP_RangeFinder_TeraRangerI2C::timer(void)
     125             : {
     126             :     // Take a reading
     127        7145 :     uint16_t _raw_distance = 0;
     128        7145 :     uint16_t _distance_cm = 0;
     129             : 
     130        7145 :     if (collect_raw(_raw_distance)) {
     131        7145 :         WITH_SEMAPHORE(_sem);
     132             : 
     133        7145 :         if (process_raw_measure(_raw_distance, _distance_cm)){
     134        7145 :             accum.sum += _distance_cm;
     135        7145 :             accum.count++;
     136             :         }
     137        7145 :     }
     138             :     // and immediately ask for a new reading
     139        7145 :     measure();
     140        7145 : }
     141             : 
     142             : /*
     143             :    update the state of the sensor
     144             : */
     145        1344 : void AP_RangeFinder_TeraRangerI2C::update(void)
     146             : {
     147        1344 :     WITH_SEMAPHORE(_sem);
     148             : 
     149        1344 :     if (accum.count > 0) {
     150        1344 :         state.distance_m = (accum.sum * 0.01f) / accum.count;
     151        1344 :         state.last_reading_ms = AP_HAL::millis();
     152        1344 :         accum.sum = 0;
     153        1344 :         accum.count = 0;
     154        1344 :         update_status();        
     155           0 :     } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
     156           0 :         set_status(RangeFinder::Status::NoData);
     157             :     }
     158        1344 : }
     159             : 
     160             : #endif  // AP_RANGEFINDER_TRI2C_ENABLED