From 36d02097cf4e4ce2a635949271b9074eddc892db Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 20 Apr 2026 15:17:33 -0400 Subject: [PATCH] SITL: add LightWare GRF I2C simulator --- libraries/SITL/SIM_I2C.cpp | 8 ++ libraries/SITL/SIM_RF_LightWare_GRF_I2C.cpp | 140 ++++++++++++++++++++ libraries/SITL/SIM_RF_LightWare_GRF_I2C.h | 53 ++++++++ libraries/SITL/SIM_config.h | 4 + 4 files changed, 205 insertions(+) create mode 100644 libraries/SITL/SIM_RF_LightWare_GRF_I2C.cpp create mode 100644 libraries/SITL/SIM_RF_LightWare_GRF_I2C.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index ac7f9f836d8..235e4859e24 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -33,6 +33,7 @@ #include "SIM_IS31FL3195.h" #include "SIM_RF_LightWareI2C_Legacy16Bit.h" #include "SIM_RF_TOFSenseF_I2C.h" +#include "SIM_RF_LightWare_GRF_I2C.h" #include "SIM_LM2755.h" #include "SIM_LP5562.h" #include "SIM_MaxSonarI2CXL.h" @@ -140,6 +141,10 @@ static TFS20L tfs20l; // Benewake TFS20L rangefinder static TOFSenseF_I2C tofsensef; #endif // AP_SIM_RF_TOFSENSEF_I2C_ENABLED +#if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED +static LightWareGRF_I2C lightware_grf_i2c; +#endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED + struct i2c_device_at_address { uint8_t bus; uint8_t addr; @@ -221,6 +226,9 @@ struct i2c_device_at_address { #if AP_SIM_RF_TOFSENSEF_I2C_ENABLED { 0, 0x08, tofsensef }, // RNGFNDx_TYPE = 40, RNGFNDx_ADDR = 0x08 #endif // AP_SIM_RF_TOFSENSEF_I2C_ENABLED +#if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED + { 2, 0x66, lightware_grf_i2c }, // RNGFNDx_TYPE = 48, RNGFNDx_ADDR = 0x66 +#endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED }; void I2C::init() diff --git a/libraries/SITL/SIM_RF_LightWare_GRF_I2C.cpp b/libraries/SITL/SIM_RF_LightWare_GRF_I2C.cpp new file mode 100644 index 00000000000..cf34775923a --- /dev/null +++ b/libraries/SITL/SIM_RF_LightWare_GRF_I2C.cpp @@ -0,0 +1,140 @@ +#include "SIM_config.h" + +#if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED + +#include "SIM_RF_LightWare_GRF_I2C.h" + +#include +#include + +using namespace SITL; + +#define GRF_SIM_REG_PRODUCT_NAME 0 +#define GRF_SIM_REG_DISTANCE_OUTPUT 27 +#define GRF_SIM_REG_DISTANCE_DATA_CM 44 +#define GRF_SIM_REG_UPDATE_RATE 74 +#define GRF_SIM_PRODUCT_NAME_LEN 16 +#define GRF_SIM_U32_LEN 4 +#define GRF_SIM_DISTANCE_LEN 8 +#define GRF_SIM_MIN_RANGE_M 0.2f +#define GRF_SIM_MAX_RANGE_M 500.0f +#define GRF_SIM_STRENGTH_DB 100 + +// Copy src into dst (capped at dst_max) and zero-fill any leftover bytes. +// Returns the number of bytes copied from src. +static uint16_t emit_padded(uint8_t *dst, uint16_t dst_max, + const uint8_t *src, uint16_t src_len) +{ + const uint16_t copy = MIN(src_len, dst_max); + memset(dst, 0, dst_max); + memcpy(dst, src, copy); + return copy; +} + +void LightWareGRF_I2C::update(const class Aircraft &aircraft) +{ + range_m = aircraft.rangefinder_range(); +} + +uint8_t LightWareGRF_I2C::apply_write(const uint8_t *buf, uint16_t len) +{ + if (len == 0) { + return last_register; + } + const uint8_t reg = buf[0]; + last_register = reg; + + // Just a register select, nothing to apply. + if (len == 1) { + return reg; + } + + // Remaining bytes are the data for that register. + const uint8_t *data = &buf[1]; + const uint16_t data_len = len - 1U; + switch (reg) { + case GRF_SIM_REG_UPDATE_RATE: + if (data_len >= GRF_SIM_U32_LEN) { + const uint32_t rate = le32toh_ptr(data); + update_rate_hz = rate > 0 ? (uint8_t)rate : 1; + } + break; + case GRF_SIM_REG_DISTANCE_OUTPUT: + if (data_len >= GRF_SIM_U32_LEN) { + distance_output_mask = le32toh_ptr(data); + } + break; + default: + break; + } + return reg; +} + +uint16_t LightWareGRF_I2C::read_register(uint8_t reg, uint8_t *buf, uint16_t buf_max) +{ + switch (reg) { + case GRF_SIM_REG_PRODUCT_NAME: { + const uint8_t name[GRF_SIM_PRODUCT_NAME_LEN] = "GRF250"; + return emit_padded(buf, buf_max, name, sizeof(name)); + } + case GRF_SIM_REG_UPDATE_RATE: { + uint8_t p[GRF_SIM_U32_LEN]; + put_le32_ptr(p, update_rate_hz); + return emit_padded(buf, buf_max, p, sizeof(p)); + } + case GRF_SIM_REG_DISTANCE_OUTPUT: { + uint8_t p[GRF_SIM_U32_LEN]; + put_le32_ptr(p, distance_output_mask); + return emit_padded(buf, buf_max, p, sizeof(p)); + } + case GRF_SIM_REG_DISTANCE_DATA_CM: { + // Sensor reports distance in 0.1 m steps; driver scales back up to cm. + float dist_m = range_m; + uint32_t strength = GRF_SIM_STRENGTH_DB; + if (dist_m < GRF_SIM_MIN_RANGE_M) { + dist_m = GRF_SIM_MIN_RANGE_M; + strength = 0; + } + if (dist_m > GRF_SIM_MAX_RANGE_M) { + dist_m = GRF_SIM_MAX_RANGE_M; + strength = 0; + } + + const uint32_t raw = (uint32_t)(dist_m * 100.0f) / 10U; + uint8_t p[GRF_SIM_DISTANCE_LEN]; + put_le32_ptr(&p[0], raw); + put_le32_ptr(&p[4], strength); + return emit_padded(buf, buf_max, p, sizeof(p)); + } + default: + memset(buf, 0, buf_max); + return buf_max; + } +} + +int LightWareGRF_I2C::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2 && + data->msgs[0].flags == I2C_RDWR && + data->msgs[1].flags == I2C_M_RD) { + const uint8_t reg = apply_write(data->msgs[0].buf, data->msgs[0].len); + read_register(reg, data->msgs[1].buf, data->msgs[1].len); + return 0; + } + + // Write-only: register select or a config write. + if (data->nmsgs == 1 && data->msgs[0].flags == I2C_RDWR) { + apply_write(data->msgs[0].buf, data->msgs[0].len); + return 0; + } + + // Read-only: respond based on the last register we were pointed at. + if (data->nmsgs == 1 && data->msgs[0].flags == I2C_M_RD) { + read_register(last_register, data->msgs[0].buf, data->msgs[0].len); + return 0; + } + + return -1; +} + +#endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED diff --git a/libraries/SITL/SIM_RF_LightWare_GRF_I2C.h b/libraries/SITL/SIM_RF_LightWare_GRF_I2C.h new file mode 100644 index 00000000000..a4a8d6b2b76 --- /dev/null +++ b/libraries/SITL/SIM_RF_LightWare_GRF_I2C.h @@ -0,0 +1,53 @@ +#pragma once + +/* + Simulator for the LightWare GRF rangefinder on I2C. + + Usage: + ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter --speedup=1 + + param set RNGFND1_TYPE 48 # LightWare-GRF-I2C + param set RNGFND1_ADDR 102 # 0x66 + reboot +*/ + +#include "SIM_config.h" + +#if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED + +#include "SIM_I2CDevice.h" + +namespace SITL { + +class LightWareGRF_I2C : public I2CDevice +{ +public: + using I2CDevice::I2CDevice; + + void update(const class Aircraft &aircraft) override; + +private: + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + + // Handle an incoming write. The first byte is the register we're + // selecting; any remaining bytes are data for that register. Returns + // the register id so the caller can respond to a follow-up read. + uint8_t apply_write(const uint8_t *buf, uint16_t len); + + // Fill buf with the bytes this register would return on the real sensor. + uint16_t read_register(uint8_t reg, uint8_t *buf, uint16_t buf_max); + + // Sensor state mirrored from writes the autopilot sends us. + uint32_t distance_output_mask = (1U << 0) | (1U << 2); // first_raw + first_strength + uint8_t update_rate_hz = 5; + + // Latest simulated range from the aircraft model (metres). + float range_m = 0.0f; + + // Tracks the most-recent register select for split write+read flows. + uint8_t last_register = 0; +}; + +} // namespace SITL + +#endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index a6fa5609930..b1485405b31 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -278,6 +278,10 @@ #define AP_SIM_RF_TOFSENSEF_I2C_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // AP_SIM_RF_TOFSENSEF_I2C_ENABLED +#ifndef AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED +#define AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED + #ifndef AP_SIM_RAMTRON_ENABLED #define AP_SIM_RAMTRON_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // AP_SIM_RAMTRON_ENABLED