Line data    Source code

       1             : #include "SIM_TFS20L.h"
       2             : #include "SITL/SIM_Aircraft.h"
       3             : #include <AP_HAL/AP_HAL.h>
       4             : #include <AP_Math/AP_Math.h>
       5             : #include <cmath>
       6             : 
       7             : #if AP_SIM_TFS20L_ENABLED
       8             : 
       9             : using namespace SITL;
      10             : 
      11             : // Register addresses matching real sensor
      12             : static const uint8_t REG_DIST_LOW = 0x00;
      13             : static const uint8_t REG_DIST_HIGH = 0x01;
      14             : static const uint8_t REG_AMP_LOW = 0x02;
      15             : static const uint8_t REG_AMP_HIGH = 0x03;
      16             : static const uint8_t REG_VERSION_MAJOR = 0x0C;
      17             : 
      18             : // Limits matching real sensor
      19             : static const uint16_t MAX_DIST_CM = 2000;  // 20m
      20             : static const uint16_t MIN_DIST_CM = 1;     // 1cm
      21             : static const uint16_t UPDATE_RATE_HZ = 20; // Sensor update rate
      22             : 
      23   178759942 : void TFS20L::update(const class Aircraft &aircraft)  
      24             : {
      25             :     // Rate limit updates to match real sensor (20Hz)
      26   178759942 :     const uint32_t now_ms = AP_HAL::millis();
      27   178759942 :     if (now_ms - last_update_ms < (1000U / UPDATE_RATE_HZ)) {
      28   175783114 :         return;
      29             :     }
      30     2976828 :     last_update_ms = now_ms;
      31             : 
      32             :     // Get range and convert to cm for checking limits
      33     2976828 :     rangefinder_range = aircraft.rangefinder_range();
      34             :     
      35             :     // Check for invalid readings first (infinite or negative)
      36             :     // Also check before casting to uint16_t to avoid overflow (>655m would wrap)
      37     2976828 :     if (rangefinder_range <= 0 || rangefinder_range > (MAX_DIST_CM * 0.01f)) {
      38     1433666 :         rangefinder_range = 0;  // Will trigger out of range
      39     1433666 :         strength = 0;          // Low strength for invalid reading
      40     1433666 :         return;
      41             :     }
      42             :     
      43     1543162 :     const uint16_t range_cm = rangefinder_range * 100.0f;
      44             :     
      45             :     // Apply real sensor limits (should always be in range due to check above)
      46     1543162 :     if (range_cm < MIN_DIST_CM) {
      47       72992 :         rangefinder_range = 0;  // Will trigger out of range
      48       72992 :         strength = 0;          // Low strength for invalid reading
      49             :     } else {
      50             :         // Simulate decreasing signal strength with distance
      51     1470170 :         const uint32_t str = 65535 - (range_cm * 20);
      52     1470170 :         strength = str < MIN_STRENGTH ? MIN_STRENGTH : (str > 65535 ? 65535 : str);
      53             :     }
      54             : }
      55             : 
      56        1399 : int TFS20L::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      57             : {
      58        1399 :     if (data->nmsgs == 1) {
      59             :         // Register selection
      60           0 :         reg = data->msgs[0].buf[0];
      61           0 :         return 0;
      62             :     }
      63             : 
      64        1399 :     if ((data->nmsgs != 2) || !(data->msgs[1].flags & I2C_M_RD)) {
      65           0 :         return -1;
      66             :     }
      67             : 
      68        1399 :     const uint16_t dist_cm = rangefinder_range * 100.0f;
      69             : 
      70             :     // Handle register reads
      71        1399 :     switch (reg) {
      72        1399 :         case REG_DIST_LOW:
      73             :         case REG_DIST_HIGH:
      74             :         case REG_AMP_LOW:
      75             :         case REG_AMP_HIGH: {
      76        1399 :             const uint8_t count = data->msgs[1].len;
      77        1399 :             if (reg + count > REG_AMP_HIGH + 1) {
      78           0 :                 return -1;
      79             :             }
      80             :             // Fill in sequential registers starting at reg
      81        6994 :             for (uint8_t i = 0; i < count; i++) {
      82        5595 :                 const uint8_t curr_reg = reg + i;
      83        5595 :                 if (curr_reg == REG_DIST_LOW) {
      84        1399 :                     data->msgs[1].buf[i] = dist_cm & 0xFF;
      85        4196 :                 } else if (curr_reg == REG_DIST_HIGH) {
      86        1399 :                     data->msgs[1].buf[i] = (dist_cm >> 8) & 0xFF;
      87        2797 :                 } else if (curr_reg == REG_AMP_LOW) {
      88        1399 :                     data->msgs[1].buf[i] = strength & 0xFF;
      89        1398 :                 } else if (curr_reg == REG_AMP_HIGH) {
      90        1398 :                     data->msgs[1].buf[i] = (strength >> 8) & 0xFF;
      91             :                 }
      92             :             }
      93        1399 :             return 0;
      94             :         }
      95           0 :         case REG_VERSION_MAJOR:
      96             :             // Return version 1.0.0
      97           0 :             data->msgs[1].buf[0] = 1;
      98           0 :             return 0;
      99             :     }
     100           0 :     return -1;
     101             : }
     102             : 
     103             : #endif // AP_SIM_TFS20L_ENABLED