Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2025 Shubham Jolapara <[email protected]>
       3             :  *
       4             :  * This file is free software: you can redistribute it and/or modify it
       5             :  * under the terms of the GNU General Public License as published by the
       6             :  * Free Software Foundation, either version 3 of the License, or
       7             :  * (at your option) any later version.
       8             :  *
       9             :  * This file is distributed in the hope that it will be useful, but
      10             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      12             :  * See the GNU General Public License for more details.
      13             :  *
      14             :  * You should have received a copy of the GNU General Public License along
      15             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : #include "AP_RangeFinder_Benewake_TFS20L.h"
      18             : 
      19             : #if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
      20             : 
      21             : #include <GCS_MAVLink/GCS.h>
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_HAL/I2CDevice.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #define DRIVER "TFS20L"
      28             : #define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
      29             : 
      30             : 
      31             : // TFS20L register addresses
      32             : static constexpr uint8_t TFS20L_DIST_LOW = 0x00;         // Distance low byte
      33             : // static constexpr uint8_t TFS20L_DIST_HIGH = 0x01;        // Distance high byte
      34             : // static constexpr uint8_t TFS20L_AMP_LOW = 0x02;          // Signal strength/amplitude low byte
      35             : // static constexpr uint8_t TFS20L_AMP_HIGH = 0x03;         // Signal strength/amplitude high byte
      36             : // static constexpr uint8_t TFS20L_TEMP_LOW = 0x04;         // Temperature low byte
      37             : // static constexpr uint8_t TFS20L_TEMP_HIGH = 0x05;        // Temperature high byte
      38             : // static constexpr uint8_t TFS20L_VERSION_REVISION = 0x0A; // Version revision byte
      39             : // static constexpr uint8_t TFS20L_VERSION_MINOR = 0x0B;    // Version minor byte
      40             : static constexpr uint8_t TFS20L_VERSION_MAJOR = 0x0C;    // Version major byte
      41             : // static constexpr uint8_t TFS20L_ENABLE = 0x25;           // Enable register
      42             : 
      43             : // Distance and strength limits
      44             : static constexpr uint16_t MAX_DIST_CM = 2000;
      45             : static constexpr uint16_t MIN_DIST_CM = 1;
      46             : static constexpr uint16_t MIN_STRENGTH = 100;
      47             : 
      48           1 : bool AP_RangeFinder_Benewake_TFS20L::init()
      49             : {
      50             :     {
      51           1 :         WITH_SEMAPHORE(dev.get_semaphore());
      52             :         uint8_t version_data[3];
      53           1 :         dev.set_retries(3);
      54             : 
      55             :         // Read firmware version to detect if sensor is present
      56           1 :         if (!dev.read_registers(TFS20L_VERSION_MAJOR, version_data, 3)) {
      57           0 :             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TFS20L: No response from sensor");
      58           0 :             return false;
      59             :         }
      60             : 
      61           1 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TFS20L: found fw version %u.%u.%u",
      62             :                       version_data[0], version_data[1], version_data[2]);
      63           1 :     }
      64             : 
      65             :     // Register periodic callback to read sensor data
      66           1 :     dev.register_periodic_callback(50000,  // 20Hz sampling rate
      67             :                                      FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Benewake_TFS20L::timer, void));
      68             : 
      69           1 :     return true;
      70             : }
      71             : 
      72        1344 : void AP_RangeFinder_Benewake_TFS20L::update()
      73             : {
      74        1344 :     WITH_SEMAPHORE(_sem);
      75             : 
      76        1344 :     const uint32_t total_count = accum.count + accum.out_of_range_count;
      77        1344 :     if (total_count > 0) {
      78        1344 :         if (accum.out_of_range_count == total_count) {
      79             :             // All readings were out of range
      80         953 :             state.distance_m = max_distance() + (BENEWAKE_OUT_OF_RANGE_ADD_CM * 0.01f);
      81         953 :             set_status(RangeFinder::Status::OutOfRangeHigh);
      82         391 :         } else if (accum.count > 0) {
      83             :             // Some readings were valid
      84         391 :             state.distance_m = (accum.sum * 0.01f) / accum.count;
      85         391 :             update_status();
      86             :         }
      87        1344 :         state.last_reading_ms = AP_HAL::millis();
      88        1344 :         accum.sum = 0;
      89        1344 :         accum.count = 0;
      90        1344 :         accum.out_of_range_count = 0;
      91           0 :     } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
      92           0 :         set_status(RangeFinder::Status::NoData);
      93             :     }
      94        1344 : }
      95             : 
      96        1398 : void AP_RangeFinder_Benewake_TFS20L::timer()
      97             : {
      98             :     /*
      99             :      * Read the first 4 registers to get distance and strength data
     100             :      * TFS20L_DIST_LOW (0x00) through TFS20L_AMP_HIGH (0x03)
     101             :      */
     102             :     uint8_t raw_data[4]; // Buffer for distance + strength data only
     103        1398 :     if (!dev.read_registers(TFS20L_DIST_LOW, raw_data, sizeof(raw_data))) {
     104         983 :         return;
     105             :     }
     106             : 
     107             :     // Combine bytes to form distance and strength (little-endian)
     108        1398 :     uint16_t distance_cm = (uint16_t(raw_data[1]) << 8) | raw_data[0];
     109        1398 :     uint16_t strength = (uint16_t(raw_data[3]) << 8) | raw_data[2];
     110             : 
     111             :     // Validate reading and handle invalid cases
     112        1398 :     if (strength < MIN_STRENGTH || strength == 0xFFFF || 
     113         415 :         distance_cm > MAX_DIST_CM || distance_cm < MIN_DIST_CM) {
     114             :         /*
     115             :          * When signal strength is too low or invalid, or distance is outside
     116             :          * sensor's specified range, mark as out of range
     117             :          */
     118         983 :         WITH_SEMAPHORE(_sem);
     119         983 :         accum.out_of_range_count++;
     120         983 :         return;
     121         983 :     }
     122             : 
     123             :     {
     124         415 :         WITH_SEMAPHORE(_sem);
     125         415 :         accum.sum += distance_cm;
     126         415 :         accum.count++;
     127         415 :     }
     128             : }
     129             : 
     130             : #endif  // AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED