mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
SITL: add LightWare GRF I2C simulator
This commit is contained in:
rishabsingh3003
committed by
Andrew Tridgell
Andrew Tridgell
parent
c174f81311
commit
36d02097cf
@@ -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()
|
||||
|
||||
@@ -0,0 +1,140 @@
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_RF_LIGHTWARE_GRF_I2C_ENABLED
|
||||
|
||||
#include "SIM_RF_LightWare_GRF_I2C.h"
|
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <string.h>
|
||||
|
||||
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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user