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.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
PenultimaLeitura.DataHora = UltimaLeitura.DataHora;
@ -343,13 +345,22 @@ namespace AgroBase.Services
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now;
double? lat = null;
double? lon = null;
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] != "")
{
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.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.Momento = UltimaLeitura.Momento;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
PenultimaLeitura.Altitude = UltimaLeitura.Altitude;
PenultimaLeitura.AltitudeElipsoidal = UltimaLeitura.AltitudeElipsoidal;
PenultimaLeitura.PrecisaoHorizontal = UltimaLeitura.PrecisaoHorizontal;
@ -439,9 +450,7 @@ namespace AgroBase.Services
UltimaLeitura.TimestampPos.valor = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
UltimaLeitura.Momento = DateTime.Now;
UltimaLeitura.LatitudeAnt = latitude;
UltimaLeitura.Latitude = latitude;
UltimaLeitura.LongitudeAnt = longitude;
UltimaLeitura.Longitude = longitude;
UltimaLeitura.Altitude = altMSL;
UltimaLeitura.AltitudeElipsoidal = altElipsoidal;
UltimaLeitura.PrecisaoHorizontal = hdopVal;
@ -450,6 +459,8 @@ namespace AgroBase.Services
UltimaLeitura.IdadeCorrecao = idadeCorrecao;
UltimaLeitura.BaseID = base_id;
AplicarPosicaoCorrigida();
// Hora UTC no formato HHmmss.ss (fração opcional)
if (!string.IsNullOrEmpty(horaUTC) && horaUTC.Length >= 6)
{
@ -471,14 +482,6 @@ namespace AgroBase.Services
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)
@ -638,6 +641,8 @@ namespace AgroBase.Services
UltimaLeitura.OrientacaoReal = InverterHeading ? GPSUtils.NormalizarAngulo(headingTrue - 180.0) : headingTrue;
UltimaLeitura.TipoOrientacao = status;
AplicarPosicaoCorrigida();
// ✅ Atualiza o ângulo unificado no GPSService
DefinirAnguloCarro();
}
@ -676,9 +681,13 @@ namespace AgroBase.Services
// mantém histórico
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
PenultimaLeitura.LongitudeAnt = UltimaLeitura.LongitudeAnt;
UltimaLeitura.Latitude = lat;
UltimaLeitura.Longitude = lon;
UltimaLeitura.LatitudeAnt = lat;
UltimaLeitura.LongitudeAnt = lon;
AplicarPosicaoCorrigida();
}
}
@ -796,8 +805,13 @@ namespace AgroBase.Services
PenultimaLeitura.Latitude = UltimaLeitura.Latitude;
PenultimaLeitura.Longitude = UltimaLeitura.Longitude;
UltimaLeitura.Latitude = lat;
UltimaLeitura.Longitude = lon;
PenultimaLeitura.LatitudeAnt = UltimaLeitura.LatitudeAnt;
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()
{
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ô
/// - lateral positivo desloca a posição corrigida para a esquerda do robô
/// </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 yTotal = yFisico + yCampo;