Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
       4             : 
       5             : #include "SIM_RF_LightWare_GRF_I2C.h"
       6             : 
       7             : #include <AP_HAL/utility/sparse-endian.h>
       8             : #include <string.h>
       9             : 
      10             : using namespace SITL;
      11             : 
      12             : #define GRF_SIM_REG_PRODUCT_NAME     0
      13             : #define GRF_SIM_REG_DISTANCE_OUTPUT  27
      14             : #define GRF_SIM_REG_DISTANCE_DATA_CM 44
      15             : #define GRF_SIM_REG_UPDATE_RATE      74
      16             : #define GRF_SIM_PRODUCT_NAME_LEN 16
      17             : #define GRF_SIM_U32_LEN          4
      18             : #define GRF_SIM_DISTANCE_LEN     8
      19             : #define GRF_SIM_MIN_RANGE_M   0.2f
      20             : #define GRF_SIM_MAX_RANGE_M   500.0f
      21             : #define GRF_SIM_STRENGTH_DB   100
      22             : 
      23             : // Copy src into dst (capped at dst_max) and zero-fill any leftover bytes.
      24             : // Returns the number of bytes copied from src.
      25        3449 : static uint16_t emit_padded(uint8_t *dst, uint16_t dst_max,
      26             :                             const uint8_t *src, uint16_t src_len)
      27             : {
      28        3449 :     const uint16_t copy = MIN(src_len, dst_max);
      29        3449 :     memset(dst, 0, dst_max);
      30        3449 :     memcpy(dst, src, copy);
      31        3449 :     return copy;
      32             : }
      33             : 
      34   178759942 : void LightWareGRF_I2C::update(const class Aircraft &aircraft)
      35             : {
      36   178759942 :     range_m = aircraft.rangefinder_range();
      37   178759942 : }
      38             : 
      39        3451 : uint8_t LightWareGRF_I2C::apply_write(const uint8_t *buf, uint16_t len)
      40             : {
      41        3451 :     if (len == 0) {
      42           0 :         return last_register;
      43             :     }
      44        3451 :     const uint8_t reg = buf[0];
      45        3451 :     last_register = reg;
      46             : 
      47             :     // Just a register select, nothing to apply.
      48        3451 :     if (len == 1) {
      49        3449 :         return reg;
      50             :     }
      51             : 
      52             :     // Remaining bytes are the data for that register.
      53           2 :     const uint8_t *data = &buf[1];
      54           2 :     const uint16_t data_len = len - 1U;
      55           2 :     switch (reg) {
      56           1 :     case GRF_SIM_REG_UPDATE_RATE:
      57           1 :         if (data_len >= GRF_SIM_U32_LEN) {
      58           1 :             const uint32_t rate = le32toh_ptr(data);
      59           1 :             update_rate_hz = rate > 0 ? (uint8_t)rate : 1;
      60             :         }
      61           1 :         break;
      62           1 :     case GRF_SIM_REG_DISTANCE_OUTPUT:
      63           1 :         if (data_len >= GRF_SIM_U32_LEN) {
      64           1 :             distance_output_mask = le32toh_ptr(data);
      65             :         }
      66           1 :         break;
      67           0 :     default:
      68           0 :         break;
      69             :     }
      70           2 :     return reg;
      71             : }
      72             : 
      73        3449 : uint16_t LightWareGRF_I2C::read_register(uint8_t reg, uint8_t *buf, uint16_t buf_max)
      74             : {
      75        3449 :     switch (reg) {
      76           1 :     case GRF_SIM_REG_PRODUCT_NAME: {
      77           1 :         const uint8_t name[GRF_SIM_PRODUCT_NAME_LEN] = "GRF250";
      78           1 :         return emit_padded(buf, buf_max, name, sizeof(name));
      79             :     }
      80           1 :     case GRF_SIM_REG_UPDATE_RATE: {
      81             :         uint8_t p[GRF_SIM_U32_LEN];
      82           1 :         put_le32_ptr(p, update_rate_hz);
      83           1 :         return emit_padded(buf, buf_max, p, sizeof(p));
      84             :     }
      85           1 :     case GRF_SIM_REG_DISTANCE_OUTPUT: {
      86             :         uint8_t p[GRF_SIM_U32_LEN];
      87           1 :         put_le32_ptr(p, distance_output_mask);
      88           1 :         return emit_padded(buf, buf_max, p, sizeof(p));
      89             :     }
      90        3446 :     case GRF_SIM_REG_DISTANCE_DATA_CM: {
      91             :         // Sensor reports distance in 0.1 m steps; driver scales back up to cm.
      92        3446 :         float dist_m = range_m;
      93        3446 :         uint32_t strength = GRF_SIM_STRENGTH_DB;
      94        3446 :         if (dist_m < GRF_SIM_MIN_RANGE_M) {
      95        2455 :             dist_m = GRF_SIM_MIN_RANGE_M;
      96        2455 :             strength = 0;
      97             :         }
      98        3446 :         if (dist_m > GRF_SIM_MAX_RANGE_M) {
      99           0 :             dist_m = GRF_SIM_MAX_RANGE_M;
     100           0 :             strength = 0;
     101             :         }
     102             : 
     103        3446 :         const uint32_t raw = (uint32_t)(dist_m * 100.0f) / 10U;
     104             :         uint8_t p[GRF_SIM_DISTANCE_LEN];
     105        3446 :         put_le32_ptr(&p[0], raw);
     106        3446 :         put_le32_ptr(&p[4], strength);
     107        3446 :         return emit_padded(buf, buf_max, p, sizeof(p));
     108             :     }
     109           0 :     default:
     110           0 :         memset(buf, 0, buf_max);
     111           0 :         return buf_max;
     112             :     }
     113             : }
     114             : 
     115        3451 : int LightWareGRF_I2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
     116             : {
     117        3451 :     if (data->nmsgs == 2 &&
     118        3449 :         data->msgs[0].flags == I2C_RDWR &&
     119        3449 :         data->msgs[1].flags == I2C_M_RD) {
     120        3449 :         const uint8_t reg = apply_write(data->msgs[0].buf, data->msgs[0].len);
     121        3449 :         read_register(reg, data->msgs[1].buf, data->msgs[1].len);
     122        3449 :         return 0;
     123             :     }
     124             : 
     125             :     // Write-only: register select or a config write.
     126           2 :     if (data->nmsgs == 1 && data->msgs[0].flags == I2C_RDWR) {
     127           2 :         apply_write(data->msgs[0].buf, data->msgs[0].len);
     128           2 :         return 0;
     129             :     }
     130             : 
     131             :     // Read-only: respond based on the last register we were pointed at.
     132           0 :     if (data->nmsgs == 1 && data->msgs[0].flags == I2C_M_RD) {
     133           0 :         read_register(last_register, data->msgs[0].buf, data->msgs[0].len);
     134           0 :         return 0;
     135             :     }
     136             : 
     137           0 :     return -1;
     138             : }
     139             : 
     140             : #endif  // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED