Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ajustes lever arm gnss

This commit is contained in:
Diego Freitas 2026-04-08 07:44:15 -03:00
parent 93c9aa248e
commit 521e0fc50d
1 changed files with 42 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
AgroBase/AgroBase/Services/GPSService.cs
View File

@ -336,6 +336,8 @@ namespace AgroBase.Services
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone(); PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
PenultimaLeitura.Latitude = UltimaLeitura.Latitude; PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude; PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Altitude = UltimaLeitura.Altitude; PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal; PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
PenultimaLeitura.DataHora = UltimaLeitura.DataHora; PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
@ -343,13 +345,22 @@ namespace AgroBase.Services
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now; UltimaLeitura.Momento = DateTime.Now;
double? lat = null;
double? lon = null;
if (parts[2] != "" && parts[3] != "") if (parts[2] != "" && parts[3] != "")
{ {
UltimaLeitura.Latitude = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2); lat = GPSUtils.ConvertToDecimalDegrees(parts[2], parts[3], 2);
} }
if (parts[4] != "" && parts[5] != "") if (parts[4] != "" && parts[5] != "")
{ {
UltimaLeitura.Longitude = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3); lon = GPSUtils.ConvertToDecimalDegrees(parts[4], parts[5], 3);
}
if (lat != null && lon != null)
{
UltimaLeitura.LatitudeAnt = lat.Value;
UltimaLeitura.LongitudeAnt = lon.Value;
AplicarPosicaoCorrigida();
} }
UltimaLeitura.NumeroSatelites = int.TryParse(parts[7], out int numSat) ? numSat : 0; UltimaLeitura.NumeroSatelites = int.TryParse(parts[7], out int numSat) ? numSat : 0;
UltimaLeitura.PrecisaoHorizontal = double.TryParse(parts[8], NumberStyles.Float, CultureInfo.InvariantCulture, out double hdop) ? hdop : 0; UltimaLeitura.PrecisaoHorizontal = double.TryParse(parts[8], NumberStyles.Float, CultureInfo.InvariantCulture, out double hdop) ? hdop : 0;
@ -422,10 +433,10 @@ namespace AgroBase.Services
PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone(); PenultimaLeitura.TimestampPos = UltimaLeitura.TimestampPos.Clone();
PenultimaLeitura.Momento = UltimaLeitura.Momento; PenultimaLeitura.Momento = UltimaLeitura.Momento;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.Latitude = UltimaLeitura.Latitude; PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude; PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Altitude = UltimaLeitura.Altitude; PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal; PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal;
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal; PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
@ -439,9 +450,7 @@ namespace AgroBase.Services
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now; UltimaLeitura.Momento = DateTime.Now;
UltimaLeitura.LatitudeAnt = latitude; UltimaLeitura.LatitudeAnt = latitude;
UltimaLeitura.Latitude = latitude;
UltimaLeitura.LongitudeAnt = longitude; UltimaLeitura.LongitudeAnt = longitude;
UltimaLeitura.Longitude = longitude;
UltimaLeitura.Altitude = altMSL; UltimaLeitura.Altitude = altMSL;
UltimaLeitura.AltitudeElipsoidal = altElipsoidal; UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
UltimaLeitura.PrecisaoHorizontal = hdopVal; UltimaLeitura.PrecisaoHorizontal = hdopVal;
@ -450,6 +459,8 @@ namespace AgroBase.Services
UltimaLeitura.IdadeCorrecao = idadeCorrecao; UltimaLeitura.IdadeCorrecao = idadeCorrecao;
UltimaLeitura.BaseID = base_id; UltimaLeitura.BaseID = base_id;
AplicarPosicaoCorrigida();
// Hora UTC no formato HHmmss.ss (fração opcional) // Hora UTC no formato HHmmss.ss (fração opcional)
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6) if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
{ {
@ -471,14 +482,6 @@ namespace AgroBase.Services
UltimaLeitura.EnuOriginSet = true; UltimaLeitura.EnuOriginSet = true;
} }
// 2) se já temos origem e um heading válido, aplica lever arm
if (UltimaLeitura.EnuOriginSet)
{
(double latCor, double lonCorr) = LeverArm.FixLeverArmLatLon_Fast(latitude, longitude, UltimaLeitura.OrientacaoReal);
UltimaLeitura.Latitude = latCor;
UltimaLeitura.Longitude = lonCorr;
}
} }
private static void ProcessarGNRMC(string sentenca) private static void ProcessarGNRMC(string sentenca)
@ -638,6 +641,8 @@ namespace AgroBase.Services
UltimaLeitura.OrientacaoReal = InverterHeading ? GPSUtils.NormalizarAngulo(headingTrue - 180.0) : headingTrue; UltimaLeitura.OrientacaoReal = InverterHeading ? GPSUtils.NormalizarAngulo(headingTrue - 180.0) : headingTrue;
UltimaLeitura.TipoOrientacao = status; UltimaLeitura.TipoOrientacao = status;
AplicarPosicaoCorrigida();
// ✅ Atualiza o ângulo unificado no GPSService // ✅ Atualiza o ângulo unificado no GPSService
DefinirAnguloCarro(); DefinirAnguloCarro();
} }
@ -676,9 +681,13 @@ namespace AgroBase.Services
// mantém histórico // mantém histórico
PenultimaLeitura.Latitude = UltimaLeitura.Latitude; PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude; PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
UltimaLeitura.Latitude = lat; UltimaLeitura.LatitudeAnt = lat;
UltimaLeitura.Longitude = lon; UltimaLeitura.LongitudeAnt = lon;
AplicarPosicaoCorrigida();
} }
} }
@ -796,8 +805,13 @@ namespace AgroBase.Services
PenultimaLeitura.Latitude = UltimaLeitura.Latitude; PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude; PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
UltimaLeitura.Latitude = lat; PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
UltimaLeitura.Longitude = lon; PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
UltimaLeitura.LatitudeAnt = lat;
UltimaLeitura.LongitudeAnt = lon;
AplicarPosicaoCorrigida();
} }
} }
} }
@ -960,6 +974,13 @@ namespace AgroBase.Services
} }
private static void AplicarPosicaoCorrigida()
{
(double latCor, double lonCorr) = LeverArm.FixLeverArmLatLon_Fast(UltimaLeitura.LatitudeAnt, UltimaLeitura.LongitudeAnt, UltimaLeitura.OrientacaoReal, UltimaLeitura.TimestampOri.frequencia);
UltimaLeitura.Latitude = latCor;
UltimaLeitura.Longitude = lonCorr;
}
public static void AtualizarCoordenadasGPS() public static void AtualizarCoordenadasGPS()
{ {
PenultimaLeitura.Ntrip_ativado = UltimaLeitura.Ntrip_ativado; PenultimaLeitura.Ntrip_ativado = UltimaLeitura.Ntrip_ativado;
@ -1407,8 +1428,10 @@ namespace AgroBase.Services
/// - frontal positivo desloca a posição corrigida para trás do robô /// - frontal positivo desloca a posição corrigida para trás do robô
/// - lateral positivo desloca a posição corrigida para a esquerda do robô /// - lateral positivo desloca a posição corrigida para a esquerda do robô
/// </summary> /// </summary>
public (double lat, double lon) FixLeverArmLatLon_Fast(double latAnt_deg, double lonAnt_deg, double headingDeg) public (double lat, double lon) FixLeverArmLatLon_Fast(double latAnt_deg, double lonAnt_deg, double headingDeg, double headingFreq)
{ {
if (headingFreq < 1) return (latAnt_deg, lonAnt_deg);
double xTotal = xFisico + xCampo; double xTotal = xFisico + xCampo;
double yTotal = yFisico + yCampo; double yTotal = yFisico + yCampo;