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_TOFSenseF_I2C.h"
      17             : 
      18             : #if AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED
      19             : 
      20             : #define TOFSENSEP_I2C_COMMAND_TAKE_RANGE_READING 0x24
      21             : #define TOFSENSEP_I2C_COMMAND_SIGNAL_STATUS 0x28
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : #include <AP_HAL/utility/sparse-endian.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : // initialise sensor
      29           1 : bool AP_RangeFinder_TOFSenseF_I2C::init(void)
      30             : {
      31           1 :     WITH_SEMAPHORE(dev.get_semaphore());
      32             : 
      33           1 :     if (!start_reading()) {
      34           0 :         return false;
      35             :     }
      36             : 
      37             :     // give time for the sensor to process the request
      38           1 :     hal.scheduler->delay(100);
      39             : 
      40             :     uint32_t reading_mm;
      41             :     uint16_t status;
      42             :     uint16_t signal_strength;
      43             : 
      44           1 :     if (!get_reading(reading_mm, signal_strength, status)) {
      45           0 :         return false;
      46             :     }
      47             : 
      48           1 :     dev.register_periodic_callback(100000,
      49             :                                      FUNCTOR_BIND_MEMBER(&AP_RangeFinder_TOFSenseF_I2C::timer, void));
      50             : 
      51           1 :     return true;
      52           1 : }
      53             : 
      54             : // start_reading() - ask sensor to make a range reading
      55         694 : bool AP_RangeFinder_TOFSenseF_I2C::start_reading()
      56             : {
      57         694 :     const uint8_t cmd[] = {TOFSENSEP_I2C_COMMAND_TAKE_RANGE_READING, TOFSENSEP_I2C_COMMAND_SIGNAL_STATUS};
      58             : 
      59             :     // send command to take reading
      60         694 :     return dev.transfer(cmd, sizeof(cmd), nullptr, 0);
      61             : }
      62             : 
      63             : // read - return last value measured by sensor
      64         693 : bool AP_RangeFinder_TOFSenseF_I2C::get_reading(uint32_t &reading_mm, uint16_t &signal_strength, uint16_t &status)
      65             : {
      66             : 
      67             :     struct PACKED {
      68             :         uint32_t distance_mm;
      69             :         uint32_t signal_strength_and_status;
      70             :     } packet;
      71             : 
      72             :     // take range reading and read back results
      73         693 :     const bool ret = dev.transfer(nullptr, 0, (uint8_t *) &packet, sizeof(packet));
      74             : 
      75         693 :     if (ret) {
      76             :         // combine results into distance
      77         693 :         reading_mm = packet.distance_mm;
      78         693 :         signal_strength = (uint16_t)(packet.signal_strength_and_status >> 16);
      79         693 :         status = (uint16_t)(packet.signal_strength_and_status);
      80             :     }
      81             : 
      82             :     // trigger a new reading
      83         693 :     start_reading();
      84             : 
      85         693 :     return ret;
      86             : }
      87             : 
      88             : //  timer called at 10Hz
      89         692 : void AP_RangeFinder_TOFSenseF_I2C::timer(void)
      90             : {
      91             :     uint32_t dist_mm;
      92             :     uint16_t status;
      93             :     uint16_t signal_strength;
      94             : 
      95         692 :     if (get_reading(dist_mm, signal_strength, status)) {
      96         692 :         WITH_SEMAPHORE(_sem);
      97         692 :         if (status == 1) {
      98             :             // healthy data
      99         692 :             distance_mm = dist_mm;
     100         692 :             new_distance = true;
     101         692 :             state.last_reading_ms = AP_HAL::millis();
     102             :         }
     103         692 :     }
     104         692 : }
     105             : 
     106             : // update the state of the sensor
     107        1348 : void AP_RangeFinder_TOFSenseF_I2C::update(void)
     108             : {
     109        1348 :     WITH_SEMAPHORE(_sem);
     110        1348 :     if (new_distance) {
     111         674 :         state.distance_m = distance_mm * 0.001f;
     112         674 :         new_distance = false;
     113         674 :         update_status();
     114         674 :     } else if (AP_HAL::millis() - state.last_reading_ms > 300) {
     115             :         // if no updates for 0.3 seconds set no-data
     116           0 :         set_status(RangeFinder::Status::NoData);
     117             :     }
     118        1348 : }
     119             : 
     120             : #endif  // AP_RANGEFINDER_TOFSENSEF_I2C_ENABLED