随着GNSS接收机配置系统的逐渐完善,目前基本上所有功能基本齐备,包括前端静态解算、坐标转换、文件保存、屏幕、电台等常见功能。
有朋友提出能够加入村田SCL3300倾角传感器的读取,并设置阈值自动上传倾角数据,依旧采用T113硬件方案实现,倾角采用SPI协议。
着实费了一番功夫,实现代码如下,供大家参考
cs
public class InclinometerService
{
ILog log = LogManager.GetLogger("NtripShare", typeof(InclinometerService));
private const uint CMD_READ_ANG_X = 0x240000C7;
private const uint CMD_READ_ANG_Y = 0x280000CD;
private const uint CMD_READ_ANG_Z = 0x2C0000CB;
private const uint CMD_READ_TEMPERATURE = 0x140000EF;
private const uint CMD_SW_RESET = 0xB4002098;
private const uint CMD_CHANGE_TO_MODE4 = 0xB4000338; // 低噪声倾角模式
private const uint CMD_READ_STATUS = 0x180000E5;
private const uint CMD_ENABLE_ANGLE = 0xB0001F6F;
private const uint CMD_READ_ID = 0x40000091;
private SpiDevice _sensor;
public float T { get; set; } = 0;
public float X { get; set; } = 0;
public float Y { get; set; } = 0;
public float Z { get; set; } = 0;
private static InclinometerService Instance = null;
public static InclinometerService getInstance()
{
if (Instance == null)
{
Instance = new InclinometerService();
}
return Instance;
}
private InclinometerService()
{
}
/// <summary>
/// 启动服务
/// </summary>
public void StartService()
{
if (_sensor == null)
{
InclinometerConfig inclinometerConfig = SysConfig.getInstance().InclinometerConfig;
if (inclinometerConfig == null)
{
inclinometerConfig = new InclinometerConfig();
}
// 1. 初始化SPI连接
var settings = new SpiConnectionSettings(inclinometerConfig.BusId, 0) // 根据实际连接选择总线号和片选号
{
Mode = SpiMode.Mode0, // 关键:设置为模式0
ClockFrequency = 2_000_000, // 设置时钟频率为2MHz
ChipSelectLineActiveState = PinValue.Low,
DataBitLength = 8 // 每次传输32位
// MSB First是默认设置,SCL3300也是MSB First
};
_sensor = SpiDevice.Create(settings);
}
Task.Run(() =>
{
try
{
// 2. 传感器初始化序列 [1](@ref)
Thread.Sleep(10); // 上电后等待10ms
WriteRegister(CMD_SW_RESET); // 软件复位
Thread.Sleep(10); // 等待复位完成
WriteRegister(CMD_CHANGE_TO_MODE4); // 切换到模式4(低噪声倾角模式)
ReadResponse(); // 丢弃响应,遵循OFF-FRAME协议
// 清除状态寄存器
WriteRegister(CMD_READ_STATUS);
ReadResponse();
WriteRegister(CMD_READ_STATUS);
ReadResponse();
WriteRegister(CMD_ENABLE_ANGLE); // 使能角度输出
ReadResponse();
uint id = ReadSensorRegister(CMD_READ_ID);
while (true)
{
if (!SysConfig.getInstance().Inclinometer)
{
return;
}
InclinometerConfig inclinometerConfig = SysConfig.getInstance().InclinometerConfig;
if (!inclinometerConfig.Enable)
{
return;
}
X = ReadAngleX();
Y = ReadAngleY();
Z = ReadAngleZ();
T = ReadT(CMD_READ_TEMPERATURE);
if (X != 0 && Y != 0 && Z != 0)
{
if (inclinometerConfig.SendMode == 1)
{
if (SysConfig.getInstance().MqttConfig.MqttEnable)
{
MqttService.getInstance().SendData(inclinometerConfig.MqttTopic, Encoding.Default.GetBytes(JsonSerializer.Serialize(new { X = X, Y = Y, Z = Z, T = T })));
}
}
else if (inclinometerConfig.SendMode == 2)
{
if (Math.Abs(X - inclinometerConfig.InitX) > inclinometerConfig.Limit ||
Math.Abs(Y - inclinometerConfig.InitY) > inclinometerConfig.Limit ||
Math.Abs(Z - inclinometerConfig.InitZ) > inclinometerConfig.Limit)
{
if (SysConfig.getInstance().MqttConfig.MqttEnable)
{
MqttService.getInstance().SendData(inclinometerConfig.MqttTopic, Encoding.Default.GetBytes(JsonSerializer.Serialize(new { X = X, Y = Y, Z = Z, T = T })));
}
}
}
}
Thread.Sleep(inclinometerConfig.ReadTime * 1000);
}
}
catch (Exception e)
{
log.Error(e);
}
});
}
public float ReadAngleX()
{
return ReadAngle(CMD_READ_ANG_X);
}
public float ReadAngleY()
{
return ReadAngle(CMD_READ_ANG_Y);
}
public float ReadAngleZ()
{
return ReadAngle(CMD_READ_ANG_Z);
}
private float ReadAngle(uint command)
{
// 发送读取角度命令
uint response = ReadSensorRegister(command);
// 提取数据位(第2和第3字节)[1](@ref)
// 响应帧格式: [RS | Data High Byte | Data Low Byte | CRC]
ushort rawData = (ushort)((response >> 8) & 0xFFFF);
// 将原始数据转换为角度(度) [1,3](@ref)
// Angle[°] = (raw_data / 2^14) * 90
// 注意:rawData是16位有符号整数,但实际有效数据是14位
int signedData = (short)rawData; // 注意符号扩展,先转为有符号数
float angle = (signedData / 16384.0f) * 90.0f; // 2^14 = 16384
return angle;
}
private float ReadT(uint command)
{
// 发送读取角度命令
uint response = ReadSensorRegister(command);
// 提取数据位(第2和第3字节)[1](@ref)
// 响应帧格式: [RS | Data High Byte | Data Low Byte | CRC]
ushort rawData = (ushort)((response >> 8) & 0xFFFF);
int signedData = (short)rawData;
float t = (signedData / 18.9f) - 273;
return t;
}
private uint ReadSensorRegister(uint command)
{
WriteRegister(command);
// 根据OFF-FRAME协议,响应需要在下一次传输中读取
return ReadResponse();
}
private void WriteRegister(uint command)
{
// 将32位命令转换为字节数组
byte[] data = BitConverter.GetBytes(command);
// 确保大端序,或根据硬件调整
if (BitConverter.IsLittleEndian)
{
Array.Reverse(data);
}
_sensor.Write(data); // 实际发送可能需要一次发送完整的4个字节
// 注意:SpiDevice.WriteByte可能不是最佳方式,建议使用Write(byte[])一次写入整个帧。
// 此处为逻辑示例,具体发送方式请根据所选SPI库的API调整。
}
private uint ReadResponse()
{
byte[] receiveBuffer = new byte[4];
// 发送空数据(如0x00000000)以触发从设备返回响应
// 注意:SpiDevice.ReadByte()可能不是全双工操作。
// 更准确的做法是使用全双工传输(TransferFullDuplex)。
// 以下为概念性代码:
byte[] sendBuffer = new byte[4] { 0x00, 0x00, 0x00, 0x00 }; // 哑传输
_sensor.TransferFullDuplex(sendBuffer, receiveBuffer); // 同时发送和接收
// 将接收到的4字节转换为uint,并调整端序
uint response = BitConverter.ToUInt32(receiveBuffer, 0);
if (BitConverter.IsLittleEndian)
{
// 可能需要调整端序以匹配主机序
response = (uint)((response << 24) | ((response << 8) & 0x00FF0000) | ((response >> 8) & 0x0000FF00) | (response >> 24));
}
return response;
}
public void Dispose() => _sensor?.Dispose();
}
欢迎交流V:NtripShare