598 lines
28 KiB
C#
598 lines
28 KiB
C#
using AgroBase.Models;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO.Ports;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class WT901CService
|
|
{
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
return DadosLeitura.Momento.AddMilliseconds(ModbusService.TempoLimiteConexao) > DateTime.Now;
|
|
}
|
|
}
|
|
|
|
public static byte slaveAddress = 0x50;
|
|
private static Dictionary<string, object> DadosSensor { get; } = new Dictionary<string, object>();
|
|
public static WT901CModel DadosLeitura { get; set; } = new WT901CModel();
|
|
public static byte[] ComandoAquisicao = new byte[8] { slaveAddress, 0x03, 0x00, 0x30, 0x00, 0x30, 0x48, 0x50 };
|
|
private static byte[] RegistrosIniciais = new byte[]
|
|
{
|
|
0x01,
|
|
0x02,
|
|
0x03,
|
|
0x04,
|
|
0x0E,
|
|
0x0F,
|
|
0x10,
|
|
0x11,
|
|
0x12,
|
|
0x16,
|
|
0x1A,
|
|
0x1F,
|
|
0x21,
|
|
0x23,
|
|
0x24,
|
|
0x2D,
|
|
0x61,
|
|
0x63,
|
|
0x63,
|
|
0x69,
|
|
0x6B,
|
|
0x6E,
|
|
0x6E,
|
|
0x6F,
|
|
0x6F,
|
|
0x75,
|
|
0x7F,
|
|
0x83,
|
|
0x84,
|
|
0x8F,
|
|
0x90,
|
|
0x91,
|
|
};
|
|
|
|
|
|
public static async Task<bool> VerificaPortaWT901C(SerialPort porta)
|
|
{
|
|
ModbusService.DefinirPortaCOM(porta);
|
|
|
|
var Comando = new ModbusComandoModel()
|
|
{
|
|
Dispositivo = T_Code.Wit,
|
|
Endereco = slaveAddress,
|
|
Comando = ComandoAquisicaoDados(),
|
|
Verificacao = true,
|
|
TamanhoEsperado = 101,
|
|
TimeoutResposta = 1000
|
|
};
|
|
|
|
ModbusService.AdicionarComandoNaFila(Comando);
|
|
|
|
while (Comando.RespondidoEm == DateTime.MinValue)
|
|
{
|
|
await Task.Delay(200);
|
|
}
|
|
|
|
DecifrarRespostaAquisicao(Comando.Resposta, ComandoAquisicaoDados());
|
|
|
|
if (Iniciado)
|
|
{
|
|
DefinirDispositivo();
|
|
ConfiguraModulo();
|
|
return Iniciado;
|
|
}
|
|
|
|
if (!ModbusService.Iniciado)
|
|
{
|
|
ModbusService.LimparDadosPorta();
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
private static void DefinirDispositivo()
|
|
{
|
|
if (Iniciado)
|
|
{
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Endereco = ModbusService._PortaModbus.PortName,
|
|
Dispositivo = T_Code.Wit,
|
|
Erro = !Iniciado,
|
|
Versao = DadosLeitura.Versao,
|
|
});
|
|
}
|
|
}
|
|
|
|
public static bool ProcessarResposta(byte[] Resposta, byte[] Comando)
|
|
{
|
|
bool Sucesso = DecifrarRespostaAquisicao(Resposta, Comando);
|
|
|
|
if (Iniciado)
|
|
{
|
|
PreencherDadosLeitura();
|
|
|
|
// ✅ Atualiza o ângulo unificado no GPSService
|
|
GPSService.DefinirAnguloCarro();
|
|
}
|
|
|
|
return Sucesso;
|
|
}
|
|
|
|
|
|
private static void ConfiguraModulo()
|
|
{
|
|
foreach (byte Registro in RegistrosIniciais)
|
|
{
|
|
var Comando = new ModbusComandoModel()
|
|
{
|
|
Dispositivo = T_Code.Wit,
|
|
Endereco = slaveAddress,
|
|
Comando = ModbusService.ComandoGetParametro(slaveAddress, new ModbusParametrosModel() { Registro = Registro, N_Registros = 0x04 }),
|
|
Verificacao = true,
|
|
TamanhoEsperado = 13,
|
|
TimeoutResposta = 500
|
|
};
|
|
|
|
ModbusService.AdicionarComandoNaFila(Comando);
|
|
}
|
|
}
|
|
|
|
private static async Task CalibrarAceleracao(bool Notificacao)
|
|
{
|
|
var ComandoLiberacaoParametro = new byte[] { slaveAddress, 0x06, 0x00, 0x69, 0xB5, 0x88, 0x22, 0xA1 };
|
|
var ComandoModoCalibragem = new byte[] { slaveAddress, 0x06, 0x00, 0x01, 0x00, 0x01, 0x14, 0x4B };
|
|
var ComandoSalvaCalibragem = new byte[] { slaveAddress, 0x06, 0x00, 0x00, 0x00, 0x00, 0x84, 0x4B };
|
|
|
|
var Comando1 = new ModbusComandoModel() { Comando = ComandoLiberacaoParametro, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando1);
|
|
var Comando2 = new ModbusComandoModel() { Comando = ComandoModoCalibragem, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando2);
|
|
await Task.Delay(5000);
|
|
var Comando3 = new ModbusComandoModel() { Comando = ComandoLiberacaoParametro, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando3);
|
|
var Comando4 = new ModbusComandoModel() { Comando = ComandoSalvaCalibragem, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando4);
|
|
|
|
if (Notificacao)
|
|
{
|
|
MessageBox.Show("Aceleração calibrado com sucesso!");
|
|
}
|
|
}
|
|
|
|
private static async Task CalibrarMagnetometro(bool Notificacao)
|
|
{
|
|
var ComandoLiberacaoParametro = new byte[] { slaveAddress, 0x06, 0x00, 0x69, 0xB5, 0x88, 0x22, 0xA1 };
|
|
var Comando_1 = new byte[] { slaveAddress, 0x06, 0x00, 0x0B, 0xFC, 0x65, 0x74, 0xA2 };
|
|
var Comando_2 = new byte[] { slaveAddress, 0x06, 0x00, 0x0C, 0x0F, 0x54, 0x40, 0x47 };
|
|
var Comando_3 = new byte[] { slaveAddress, 0x06, 0x00, 0x0D, 0x10, 0x07, 0x59, 0x8A };
|
|
var Comando_4 = new byte[] { slaveAddress, 0x06, 0x00, 0x1C, 0x27, 0x10, 0x5F, 0xB1 };
|
|
var Comando_5 = new byte[] { slaveAddress, 0x06, 0x00, 0x1D, 0x26, 0x67, 0x4F, 0xC7 };
|
|
var Comando_6 = new byte[] { slaveAddress, 0x06, 0x00, 0x1E, 0x27, 0xC3, 0xBF, 0xEC };
|
|
var ComandoSalvaCalibragem = new byte[] { slaveAddress, 0x06, 0x00, 0x00, 0x00, 0x00, 0x84, 0x4B };
|
|
|
|
var Comando0 = new ModbusComandoModel() { Comando = ComandoLiberacaoParametro, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando0);
|
|
var Comando1 = new ModbusComandoModel() { Comando = Comando_1, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando1);
|
|
var Comando2 = new ModbusComandoModel() { Comando = Comando_2, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando2);
|
|
var Comando3 = new ModbusComandoModel() { Comando = Comando_3, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando3);
|
|
var Comando4 = new ModbusComandoModel() { Comando = Comando_4, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando4);
|
|
var Comando5 = new ModbusComandoModel() { Comando = Comando_5, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando5);
|
|
var Comando6 = new ModbusComandoModel() { Comando = Comando_6, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando6);
|
|
var Comando7 = new ModbusComandoModel() { Comando = ComandoSalvaCalibragem, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando7);
|
|
|
|
if (Notificacao)
|
|
{
|
|
MessageBox.Show("Magnetometro calibrado com sucesso!");
|
|
}
|
|
}
|
|
|
|
public static async Task AlterarAlgoritmo(bool nineAxis)
|
|
{
|
|
var ComandoLiberacaoParametro = new byte[] { slaveAddress, 0x06, 0x00, 0x69, 0xB5, 0x88, 0x22, 0xA1 };
|
|
var Comando9Eixos = new byte[] { slaveAddress, 0x06, 0x00, 0x24, 0x00, 0x00, 0xC4, 0x40 };
|
|
var Comando6Eixos = new byte[] { slaveAddress, 0x06, 0x00, 0x24, 0x00, 0x01, 0x05, 0x80 };
|
|
var ComandoSalvaCalibragem = new byte[] { slaveAddress, 0x06, 0x00, 0x00, 0x00, 0x00, 0x84, 0x4B };
|
|
|
|
var Comando1 = new ModbusComandoModel() { Comando = ComandoLiberacaoParametro, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando1);
|
|
var Comando2 = new ModbusComandoModel() { Comando = nineAxis ? Comando9Eixos : Comando6Eixos, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando2);
|
|
await Task.Delay(5000);
|
|
var Comando3 = new ModbusComandoModel() { Comando = ComandoLiberacaoParametro, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando3);
|
|
var Comando4 = new ModbusComandoModel() { Comando = ComandoSalvaCalibragem, Dispositivo = T_Code.Wit, Endereco = slaveAddress, Verificacao = false, TamanhoEsperado = 8, TimeoutResposta = 500 };
|
|
ModbusService.AdicionarComandoNaFila(Comando4);
|
|
}
|
|
|
|
public static byte[] ComandoAquisicaoDados()
|
|
{
|
|
byte[] Comando = ModbusService.ComandoGetParametro(slaveAddress, new ModbusParametrosModel() { N_Registros = 0x30, Registro = 0x30 });
|
|
return Comando;
|
|
}
|
|
|
|
private static bool DecifrarRespostaAquisicao(byte[] resposta, byte[] comando)
|
|
{
|
|
byte[] modbusBytes;
|
|
if (comando != null && resposta != null && resposta.Length >= 7 && ModbusService.FindModbus(comando, resposta, out modbusBytes))
|
|
{
|
|
int readReg = comando[2] << 8 | comando[3];
|
|
byte[] regData = modbusBytes.Skip(3).Take(modbusBytes.Length - 5).ToArray();
|
|
|
|
for (int j = 0; regData != null && j < regData.Length - 1; j += 2)
|
|
{
|
|
string key = string.Format("{0:X2}", readReg++);
|
|
short value = (short)(regData[j] << 8 | regData[j + 1]);
|
|
|
|
DadosSensor[key] = value;
|
|
}
|
|
|
|
DadosLeitura.Momento = DateTime.Now;
|
|
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("Erro ao ler dados do WT901C");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
private static dynamic GetDeviceData(string key)
|
|
{
|
|
if (DadosSensor.ContainsKey(key))
|
|
{
|
|
return DadosSensor[key].ToString();
|
|
}
|
|
else
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
private static void PreencherDadosLeitura()
|
|
{
|
|
var wt901CModel = DadosLeitura;
|
|
|
|
// Versão
|
|
var reg2e = GetDeviceData("2E"); // Versão
|
|
var reg2f = GetDeviceData("2F"); // Versão
|
|
|
|
// Se houver versão
|
|
if (!string.IsNullOrEmpty(reg2e) && !string.IsNullOrEmpty(reg2f))
|
|
{
|
|
var reg2eValue = (ushort)short.Parse(reg2e);
|
|
var vbytes = BitConverter.GetBytes((ushort)short.Parse(reg2e)).Concat(BitConverter.GetBytes((ushort)short.Parse(reg2f))).ToArray();
|
|
UInt32 tempVerSion = BitConverter.ToUInt32(vbytes, 0);
|
|
string sbinary = Convert.ToString(tempVerSion, 2);
|
|
sbinary = ("").PadLeft((32 - sbinary.Length), '0') + sbinary;
|
|
if (sbinary.StartsWith("1")) // Nova versão
|
|
{
|
|
string tempNewVS = Convert.ToUInt32(sbinary.Substring(4 - 3, 14 + 3), 2).ToString();
|
|
tempNewVS += "." + Convert.ToUInt32(sbinary.Substring(18, 6), 2);
|
|
tempNewVS += "." + Convert.ToUInt32(sbinary.Substring(24), 2);
|
|
wt901CModel.Versao = tempNewVS;
|
|
}
|
|
else
|
|
{
|
|
wt901CModel.Versao = reg2eValue.ToString();
|
|
}
|
|
}
|
|
|
|
// Número de série
|
|
var reg7f = GetDeviceData("7F"); // Número de série
|
|
var reg80 = GetDeviceData("80"); // Número de série
|
|
var reg81 = GetDeviceData("81"); // Número de série
|
|
var reg82 = GetDeviceData("82"); // Número de série
|
|
var reg83 = GetDeviceData("83"); // Número de série
|
|
var reg84 = GetDeviceData("84"); // Número de série
|
|
if (!string.IsNullOrEmpty(reg7f) && !string.IsNullOrEmpty(reg80) && !string.IsNullOrEmpty(reg81) &&
|
|
!string.IsNullOrEmpty(reg82) && !string.IsNullOrEmpty(reg83) && !string.IsNullOrEmpty(reg84))
|
|
{
|
|
// Converter cada valor de registro em bytes
|
|
var bytes7f = BitConverter.GetBytes(short.Parse(reg7f));
|
|
var bytes80 = BitConverter.GetBytes(short.Parse(reg80));
|
|
var bytes81 = BitConverter.GetBytes(short.Parse(reg81));
|
|
var bytes82 = BitConverter.GetBytes(short.Parse(reg82));
|
|
var bytes83 = BitConverter.GetBytes(short.Parse(reg83));
|
|
var bytes84 = BitConverter.GetBytes(short.Parse(reg84));
|
|
|
|
// Criar um array para armazenar todos os bytes concatenados
|
|
var allBytes = new byte[bytes7f.Length + bytes80.Length + bytes81.Length +
|
|
bytes82.Length + bytes83.Length + bytes84.Length];
|
|
|
|
// Copiar os arrays de bytes individuais para o array final
|
|
int offset = 0;
|
|
Array.Copy(bytes7f, 0, allBytes, offset, bytes7f.Length);
|
|
offset += bytes7f.Length;
|
|
Array.Copy(bytes80, 0, allBytes, offset, bytes80.Length);
|
|
offset += bytes80.Length;
|
|
Array.Copy(bytes81, 0, allBytes, offset, bytes81.Length);
|
|
offset += bytes81.Length;
|
|
Array.Copy(bytes82, 0, allBytes, offset, bytes82.Length);
|
|
offset += bytes82.Length;
|
|
Array.Copy(bytes83, 0, allBytes, offset, bytes83.Length);
|
|
offset += bytes83.Length;
|
|
Array.Copy(bytes84, 0, allBytes, offset, bytes84.Length);
|
|
|
|
// Converter o array de bytes concatenados em uma string
|
|
string sn = Encoding.Default.GetString(allBytes);
|
|
wt901CModel.Modelo = sn;
|
|
// Aqui você pode adicionar o número de série ao modelo, se necessário
|
|
}
|
|
|
|
// Dados de tempo
|
|
var reg30 = GetDeviceData("30"); // Ano e mês
|
|
var reg31 = GetDeviceData("31"); // Dia e hora
|
|
var reg32 = GetDeviceData("32"); // Minuto e segundo
|
|
var reg33 = GetDeviceData("33"); // Milissegundo
|
|
// Se os dados de tempo foram retornados
|
|
if (!string.IsNullOrEmpty(reg30) && !string.IsNullOrEmpty(reg31) && !string.IsNullOrEmpty(reg32) && !string.IsNullOrEmpty(reg33))
|
|
{
|
|
// Decodificar os dados e salvar no modelo
|
|
var yy = 2000 + (byte)int.Parse(reg30);
|
|
var MM = 1 + (byte)(int.Parse(reg30) >> 8);
|
|
var dd = 1 + (byte)int.Parse(reg31);
|
|
var hh = (byte)(int.Parse(reg31) >> 8);
|
|
var mm = (byte)int.Parse(reg32);
|
|
var ss = (byte)(int.Parse(reg32) >> 8);
|
|
var ms = int.Parse(reg33).ToString("000");
|
|
|
|
string dateTimeStr = $"{yy}-{MM}-{dd} {hh}:{mm}:{ss}.{ms}";
|
|
DateTime.TryParse(dateTimeStr, out DateTime dateTime);
|
|
wt901CModel.DataHora = dateTime;
|
|
}
|
|
|
|
// Dados de aceleração
|
|
var reg34 = GetDeviceData("34"); // Aceleração X
|
|
var reg35 = GetDeviceData("35"); // Aceleração Y
|
|
var reg36 = GetDeviceData("36"); // Aceleração Z
|
|
// Se os dados de aceleração foram retornados
|
|
if (!string.IsNullOrEmpty(reg34) && !string.IsNullOrEmpty(reg35) && !string.IsNullOrEmpty(reg36))
|
|
{
|
|
// Decodificar os dados e salvar no modelo
|
|
wt901CModel.AccX = Math.Round(double.Parse(reg34) / 32768.0 * 16, 3);
|
|
wt901CModel.AccY = Math.Round(double.Parse(reg35) / 32768.0 * 16, 3);
|
|
wt901CModel.AccZ = Math.Round(double.Parse(reg36) / 32768.0 * 16, 3);
|
|
wt901CModel.AccM = Math.Round(Math.Sqrt(Math.Pow(wt901CModel.AccX, 2) + Math.Pow(wt901CModel.AccY, 2) + Math.Pow(wt901CModel.AccZ, 2)), 3);
|
|
}
|
|
|
|
// Dados de velocidade angular
|
|
var reg37 = GetDeviceData("37"); // Velocidade angular X
|
|
var reg38 = GetDeviceData("38"); // Velocidade angular Y
|
|
var reg39 = GetDeviceData("39"); // Velocidade angular Z
|
|
// Se os dados de velocidade angular foram retornados
|
|
if (!string.IsNullOrEmpty(reg37) && !string.IsNullOrEmpty(reg38) && !string.IsNullOrEmpty(reg39))
|
|
{
|
|
// Decodificar os dados e salvar no modelo
|
|
wt901CModel.AsX = Math.Round(double.Parse(reg37) / 32768.0 * 2000, 3);
|
|
wt901CModel.AsY = Math.Round(double.Parse(reg38) / 32768.0 * 2000, 3);
|
|
wt901CModel.AsZ = Math.Round(double.Parse(reg39) / 32768.0 * 2000, 3);
|
|
wt901CModel.AsM = Math.Round(Math.Sqrt(Math.Pow(wt901CModel.AsX, 2) + Math.Pow(wt901CModel.AsY, 2) + Math.Pow(wt901CModel.AsZ, 2)), 3);
|
|
}
|
|
|
|
// Dados de ângulo
|
|
var reg3D = GetDeviceData("3D"); // Ângulo X
|
|
var reg3E = GetDeviceData("3E"); // Ângulo Y
|
|
var reg3F = GetDeviceData("3F"); // Ângulo Z
|
|
// Se os dados de ângulo foram retornados
|
|
if (!string.IsNullOrEmpty(reg3D) && !string.IsNullOrEmpty(reg3E) && !string.IsNullOrEmpty(reg3F))
|
|
{
|
|
// Decodificar os dados e salvar no modelo
|
|
wt901CModel.AngleX = Math.Round(double.Parse(reg3D) / 32768.0 * 180, 3);
|
|
wt901CModel.AngleY = Math.Round(double.Parse(reg3E) / 32768.0 * 180, 3);
|
|
wt901CModel.AngleZ = Math.Round(double.Parse(reg3F) / 32768.0 * 180, 3);
|
|
}
|
|
|
|
// Dados do campo magnético
|
|
var reg3A = GetDeviceData("3A"); // Campo magnético X
|
|
var reg3B = GetDeviceData("3B"); // Campo magnético Y
|
|
var reg3C = GetDeviceData("3C"); // Campo magnético Z
|
|
var magType = GetDeviceData("72") ?? "6"; // Tipo de campo magnético
|
|
|
|
// Se os dados do campo magnético foram retornados
|
|
if (!string.IsNullOrEmpty(reg3A) && !string.IsNullOrEmpty(reg3B) && !string.IsNullOrEmpty(reg3C) && !string.IsNullOrEmpty(magType))
|
|
{
|
|
short type = short.Parse(magType);
|
|
// Decodificar os dados e salvar no modelo
|
|
wt901CModel.HX = GetMagToUt(type, double.Parse(reg3A));
|
|
wt901CModel.HY = GetMagToUt(type, double.Parse(reg3B));
|
|
wt901CModel.HZ = GetMagToUt(type, double.Parse(reg3C));
|
|
wt901CModel.HM = Math.Round(Math.Sqrt(Math.Pow(wt901CModel.HX, 2) + Math.Pow(wt901CModel.HY, 2) + Math.Pow(wt901CModel.HZ, 2)), 2);
|
|
}
|
|
|
|
// Dados de temperatura
|
|
var reg40 = GetDeviceData("40"); // Temperatura
|
|
// Se os dados de temperatura foram retornados
|
|
if (!string.IsNullOrEmpty(reg40))
|
|
{
|
|
wt901CModel.Temperatura = Math.Round(double.Parse(reg40) / 100.0, 2);
|
|
}
|
|
|
|
// Dados dos portos
|
|
var reg41 = GetDeviceData("41"); // Porto 0
|
|
var reg42 = GetDeviceData("42"); // Porto 1
|
|
var reg43 = GetDeviceData("43"); // Porto 2
|
|
var reg44 = GetDeviceData("44"); // Porto 3
|
|
// Se os dados dos portos foram retornados
|
|
if (reg41 != null && reg42 != null && reg43 != null && reg44 != null)
|
|
{
|
|
wt901CModel.D0 = short.Parse(reg41);
|
|
wt901CModel.D1 = short.Parse(reg42);
|
|
wt901CModel.D2 = short.Parse(reg43);
|
|
wt901CModel.D3 = short.Parse(reg44);
|
|
}
|
|
|
|
// Dados de pressão e altitude
|
|
var reg45 = GetDeviceData("45"); // Pressão baixa
|
|
var reg46 = GetDeviceData("46"); // Pressão alta
|
|
var reg47 = GetDeviceData("47"); // Altitude baixa
|
|
var reg48 = GetDeviceData("48"); // Altitude alta
|
|
// Se os dados de pressão e altitude foram retornados
|
|
if (!string.IsNullOrEmpty(reg45) && !string.IsNullOrEmpty(reg46) && !string.IsNullOrEmpty(reg47) && !string.IsNullOrEmpty(reg48))
|
|
{
|
|
var pbytes = BitConverter.GetBytes((short)short.Parse(reg45)).Concat(BitConverter.GetBytes((short)short.Parse(reg46))).ToArray();
|
|
var hbytes = BitConverter.GetBytes((short)short.Parse(reg47)).Concat(BitConverter.GetBytes((short)short.Parse(reg48))).ToArray();
|
|
wt901CModel.Pressao = BitConverter.ToInt32(pbytes, 0);
|
|
wt901CModel.Altitude = BitConverter.ToInt32(hbytes, 0) / 100.0;
|
|
}
|
|
|
|
// Dados de latitude e longitude
|
|
var reg49 = GetDeviceData("49"); // Longitude baixa
|
|
var reg4A = GetDeviceData("4A"); // Longitude alta
|
|
var reg4B = GetDeviceData("4B"); // Latitude baixa
|
|
var reg4C = GetDeviceData("4C"); // Latitude alta
|
|
// Se os dados de latitude e longitude foram retornados
|
|
if (!string.IsNullOrEmpty(reg49) && !string.IsNullOrEmpty(reg4A) && !string.IsNullOrEmpty(reg4B) && !string.IsNullOrEmpty(reg4C))
|
|
{
|
|
var lonbytes = BitConverter.GetBytes((short)short.Parse(reg49)).Concat(BitConverter.GetBytes((short)short.Parse(reg4A))).ToArray();
|
|
var lon = BitConverter.ToInt32(lonbytes, 0);
|
|
var latbytes = BitConverter.GetBytes((short)short.Parse(reg4B)).Concat(BitConverter.GetBytes((short)short.Parse(reg4C))).ToArray();
|
|
var lat = BitConverter.ToInt32(latbytes, 0);
|
|
|
|
double lon_D = DmsToD((double)lon / 100000.0);
|
|
wt901CModel.Longitude = Math.Round(lon_D, 6);
|
|
double lat_D = DmsToD((double)lat / 100000.0);
|
|
wt901CModel.Latitude = Math.Round(lat_D, 6);
|
|
}
|
|
|
|
// Dados de altura, orientação e velocidade do GPS
|
|
var reg4D = GetDeviceData("4D"); // Altura do GPS
|
|
var reg4E = GetDeviceData("4E"); // Orientação do GPS
|
|
var reg4F = GetDeviceData("4F"); // Velocidade baixa
|
|
var reg50 = GetDeviceData("50"); // Velocidade alta
|
|
// Se os dados de altura, orientação e velocidade do GPS foram retornados
|
|
if (!string.IsNullOrEmpty(reg4D) && !string.IsNullOrEmpty(reg4E) && !string.IsNullOrEmpty(reg4F) && !string.IsNullOrEmpty(reg50))
|
|
{
|
|
var svbytes = BitConverter.GetBytes((short)short.Parse(reg4F)).Concat(BitConverter.GetBytes((short)short.Parse(reg50))).ToArray();
|
|
|
|
wt901CModel.GPS_Altitude = Math.Round(short.Parse(reg4D) / 10.0, 3);
|
|
byte[] temp = BitConverter.GetBytes((short)short.Parse(reg4E)); // Decodificar orientação sem valores negativos
|
|
uint reg4EValue = BitConverter.ToUInt16(temp, 0);
|
|
wt901CModel.GPS_Orientacao = Math.Round(reg4EValue / 100.0, 3);
|
|
wt901CModel.GPS_Velcodiade = Math.Round(BitConverter.ToInt32(svbytes, 0) / 1e3, 3);
|
|
}
|
|
|
|
// Dados dos quaternions
|
|
var reg51 = GetDeviceData("51"); // Quaternion 0
|
|
var reg52 = GetDeviceData("52"); // Quaternion 1
|
|
var reg53 = GetDeviceData("53"); // Quaternion 2
|
|
var reg54 = GetDeviceData("54"); // Quaternion 3
|
|
// Se os dados dos quaternions foram retornados
|
|
if (!string.IsNullOrEmpty(reg51) && !string.IsNullOrEmpty(reg52) && !string.IsNullOrEmpty(reg53) && !string.IsNullOrEmpty(reg54))
|
|
{
|
|
wt901CModel.Q0 = Math.Round(short.Parse(reg51) / 32768.0, 3);
|
|
wt901CModel.Q1 = Math.Round(short.Parse(reg52) / 32768.0, 3);
|
|
wt901CModel.Q2 = Math.Round(short.Parse(reg53) / 32768.0, 3);
|
|
wt901CModel.Q3 = Math.Round(short.Parse(reg54) / 32768.0, 3);
|
|
}
|
|
|
|
// Dados de precisão de posição
|
|
var reg55 = GetDeviceData("55"); // Número de satélites
|
|
var reg56 = GetDeviceData("56"); // PDOP
|
|
var reg57 = GetDeviceData("57"); // HDOP
|
|
var reg58 = GetDeviceData("58"); // VDOP
|
|
// Se os dados de precisão de posição foram retornados
|
|
if (!string.IsNullOrEmpty(reg55) && !string.IsNullOrEmpty(reg56) && !string.IsNullOrEmpty(reg57) && !string.IsNullOrEmpty(reg58))
|
|
{
|
|
wt901CModel.GPS_NumeroSatelites = short.Parse(reg55);
|
|
wt901CModel.PDOP = Math.Round(short.Parse(reg56) / 100.0, 3);
|
|
wt901CModel.HDOP = Math.Round(short.Parse(reg57) / 100.0, 3);
|
|
wt901CModel.VDOP = Math.Round(short.Parse(reg58) / 100.0, 3);
|
|
}
|
|
}
|
|
|
|
private static double GetMagToUt(short reg72, double regMag)
|
|
{
|
|
double dRet = regMag;
|
|
switch (reg72)
|
|
{
|
|
case 2:
|
|
dRet = dRet * 0.15;
|
|
break;
|
|
case 3:
|
|
dRet = dRet * 13 / 1000.0;
|
|
break;
|
|
case 4:
|
|
dRet = dRet * 0.058;
|
|
break;
|
|
case 5:
|
|
dRet = dRet * 0.098;
|
|
break;
|
|
case 6:
|
|
dRet = dRet / 120;
|
|
break;
|
|
case 7:
|
|
dRet = dRet * 20 / 1000.0;
|
|
break;
|
|
}
|
|
return Math.Round(dRet, 3);
|
|
}
|
|
|
|
private static double GetUtToMag(short reg72, double reguTMag)
|
|
{
|
|
double dRet = reguTMag;
|
|
switch (reg72)
|
|
{
|
|
case 2:
|
|
dRet = dRet / 0.15;
|
|
break;
|
|
case 3:
|
|
dRet = dRet * 1000.0 / 13.0;
|
|
break;
|
|
case 4:
|
|
dRet = dRet / 0.058;
|
|
break;
|
|
case 5:
|
|
dRet = dRet / 0.098;
|
|
break;
|
|
case 6:
|
|
dRet = dRet * 120;
|
|
break;
|
|
case 7:
|
|
dRet = dRet * 1000.0 / 20.0;
|
|
break;
|
|
}
|
|
return dRet;
|
|
}
|
|
|
|
private static double DmsToD(double value, int digits)
|
|
{
|
|
double Degree = Math.Floor(value / 100);
|
|
|
|
double points = (value / 100 - Degree) * 100;
|
|
Degree = Degree + points / 60;
|
|
|
|
return Math.Round(Degree, digits);
|
|
}
|
|
|
|
private static double DmsToD(double value)
|
|
{
|
|
return DmsToD(value, 7);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
}
|