ajustado correção de lever arm e desloamento no mapa
This commit is contained in:
parent
295057ca67
commit
4badba3a05
|
|
@ -1,4 +1,4 @@
|
|||
using AgroBase.Models.Operacoes;
|
||||
using AgroBase.Models.Operacoes;
|
||||
using AgroBase.Services;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
|
@ -71,6 +71,11 @@ namespace AgroBase.Models
|
|||
public double LeverArmFrontal { get; set; } = 0;
|
||||
public double LeverArmFrontalOp { get; set; } = 0;
|
||||
|
||||
// Correção operacional mapa <-> RTK já convertida para o frame global.
|
||||
// Estes campos NÃO giram com o heading do rover.
|
||||
public double CorrecaoMapaNorteCm { get; set; } = 0;
|
||||
public double CorrecaoMapaLesteCm { get; set; } = 0;
|
||||
|
||||
public GPSModel Clone()
|
||||
{
|
||||
return new GPSModel()
|
||||
|
|
@ -113,6 +118,8 @@ namespace AgroBase.Models
|
|||
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||
LeverArmLateral = LeverArmLateral,
|
||||
LeverArmLateralOp = LeverArmLateralOp,
|
||||
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||
X = X,
|
||||
Y = Y
|
||||
};
|
||||
|
|
@ -140,7 +147,11 @@ namespace AgroBase.Models
|
|||
PrecisaoHorizontal = PrecisaoHorizontal,
|
||||
QualidadeFix = QualidadeFix,
|
||||
UltimoComandoRespondido = UltimoComandoRespondido,
|
||||
Velocidade = Velocidade
|
||||
Velocidade = Velocidade,
|
||||
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||
LeverArmLateralOp = LeverArmLateralOp,
|
||||
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -588,4 +599,4 @@ namespace AgroBase.Models
|
|||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -2589,41 +2589,48 @@ namespace AgroBase.Models
|
|||
|
||||
private void ExecutarComandoGps(OperacaoComandoBaseModel controleBase)
|
||||
{
|
||||
if (!TryDeserializeValue(controleBase._comp_value, out ValueTuple<double?, double?> lever))
|
||||
if (!TryDeserializeValue(
|
||||
controleBase._comp_value,
|
||||
out ValueTuple<double?, double?> lever))
|
||||
{
|
||||
Variaveis.MostrarLog("[ComandoBase] Payload inválido para lever arm.");
|
||||
Variaveis.MostrarLog(
|
||||
"[ComandoBase] Payload inválido para correção operacional do GNSS."
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
double? campoLateralCm = lever.Item1;
|
||||
double? campoFrontalCm = lever.Item2;
|
||||
|
||||
if (campoLateralCm == null && campoFrontalCm == null)
|
||||
if (!campoLateralCm.HasValue && !campoFrontalCm.HasValue)
|
||||
return;
|
||||
|
||||
double novoLateralCm = campoLateralCm.HasValue ? campoLateralCm.Value : GPSService.LeverArm.LateralCampoCm;
|
||||
double novoFrontalCm = campoFrontalCm.HasValue ? campoFrontalCm.Value : GPSService.LeverArm.FrontalCampoCm;
|
||||
bool aplicado = GPSService.AtualizarCorrecaoOperacional(
|
||||
campoLateralCm: campoLateralCm,
|
||||
campoFrontalCm: campoFrontalCm
|
||||
);
|
||||
|
||||
double novoTotalLateralCm = VariaveisEquipamento.LeverArmLateralCm + novoLateralCm;
|
||||
double novoTotalFrontalCm = VariaveisEquipamento.LeverArmFrontalCm + novoFrontalCm;
|
||||
if (!aplicado)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
"[ComandoBase][CorrecaoMapa] Comando não aplicado. " +
|
||||
"Verifique se o heading GNSS está válido."
|
||||
);
|
||||
return;
|
||||
}
|
||||
|
||||
GeoLeverArm ajuste = GPSService.LeverArm;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[ComandoBase][LeverArm] Total RX: frontal={novoTotalFrontalCm:0.##}cm, lateral={novoTotalLateralCm:0.##}cm | " +
|
||||
$"Fisico: frontal={VariaveisEquipamento.LeverArmFrontalCm:0.##}cm, lateral={VariaveisEquipamento.LeverArmLateralCm:0.##}cm | " +
|
||||
$"Campo: frontal={campoFrontalCm:0.##}cm, lateral={campoLateralCm:0.##}cm"
|
||||
);
|
||||
|
||||
GPSService.LeverArm = new GeoLeverArm(
|
||||
offsetFisicoFrontalCm: VariaveisEquipamento.LeverArmFrontalCm,
|
||||
offsetFisicoLateralCm: VariaveisEquipamento.LeverArmLateralCm,
|
||||
offsetCampoFrontalCm: novoFrontalCm,
|
||||
offsetCampoLateralCm: novoLateralCm,
|
||||
mirrL: true,
|
||||
invF: true,
|
||||
invH: false
|
||||
"[ComandoBase][CorrecaoMapa] Aplicada | " +
|
||||
$"op_frontal={ajuste.FrontalCampoCm:0.##}cm | " +
|
||||
$"op_lateral={ajuste.LateralCampoCm:0.##}cm | " +
|
||||
$"N={ajuste.CorrecaoMapaNorteCm:+0.##;-0.##;0}cm | " +
|
||||
$"E={ajuste.CorrecaoMapaLesteCm:+0.##;-0.##;0}cm"
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
private void ExecutarComandoCameraCaminho(OperacaoComandoBaseModel controleBase)
|
||||
{
|
||||
string cameraId = VariaveisEquipamento.Parametros.camera_caminhho_id;
|
||||
|
|
|
|||
|
|
@ -1782,6 +1782,10 @@ namespace AgroBase.Models.Operacoes
|
|||
public double IdadeCorrecao { get; set; }
|
||||
public double LeverArmLateral { get; set; }
|
||||
public double LeverArmFrontal { get; set; }
|
||||
public double LeverArmLateralOp { get; set; }
|
||||
public double LeverArmFrontalOp { get; set; }
|
||||
public double CorrecaoMapaNorteCm { get; set; }
|
||||
public double CorrecaoMapaLesteCm { get; set; }
|
||||
public DateTime UltimoComandoRespondido { get; set; }
|
||||
|
||||
public GPSModel ToGpsModel()
|
||||
|
|
@ -1805,6 +1809,10 @@ namespace AgroBase.Models.Operacoes
|
|||
IdadeCorrecao = IdadeCorrecao,
|
||||
LeverArmFrontal = LeverArmFrontal,
|
||||
LeverArmLateral = LeverArmLateral,
|
||||
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||
LeverArmLateralOp = LeverArmLateralOp,
|
||||
UltimoComandoRespondido = UltimoComandoRespondido
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -188,6 +188,107 @@ namespace AgroBase.Services
|
|||
invH: false
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// Atualiza a correção operacional solicitada pelo operador.
|
||||
///
|
||||
/// IMPORTANTE:
|
||||
/// - campoFrontalCm/campoLateralCm continuam sendo contadores ABSOLUTOS
|
||||
/// no referencial do rover, para preservar o contrato da base;
|
||||
/// - somente o DELTA desde o comando anterior é convertido, UMA VEZ,
|
||||
/// para Norte/Leste usando o heading atual;
|
||||
/// - a correção Norte/Leste acumulada fica presa ao mundo e não gira
|
||||
/// novamente quando o rover muda de heading.
|
||||
/// </summary>
|
||||
public static bool AtualizarCorrecaoOperacional(
|
||||
double? campoLateralCm = null,
|
||||
double? campoFrontalCm = null)
|
||||
{
|
||||
if (!campoLateralCm.HasValue && !campoFrontalCm.HasValue)
|
||||
return true;
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (!PossuiHeadingValido(UltimaLeitura))
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService][CorrecaoMapa] Comando ignorado: heading GNSS inválido."
|
||||
);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (LeverArm == null)
|
||||
{
|
||||
LeverArm = new GeoLeverArm(
|
||||
offsetFisicoFrontalCm: VariaveisEquipamento.LeverArmFrontalCm,
|
||||
offsetFisicoLateralCm: VariaveisEquipamento.LeverArmLateralCm,
|
||||
mirrL: true,
|
||||
invF: true,
|
||||
invH: false
|
||||
);
|
||||
}
|
||||
|
||||
double novoLateralCm = campoLateralCm.HasValue
|
||||
? campoLateralCm.Value
|
||||
: LeverArm.LateralCampoCm;
|
||||
|
||||
double novoFrontalCm = campoFrontalCm.HasValue
|
||||
? campoFrontalCm.Value
|
||||
: LeverArm.FrontalCampoCm;
|
||||
|
||||
double deltaLateralCm =
|
||||
novoLateralCm - LeverArm.LateralCampoCm;
|
||||
|
||||
double deltaFrontalCm =
|
||||
novoFrontalCm - LeverArm.FrontalCampoCm;
|
||||
|
||||
LeverArm = LeverArm.ComAtualizacaoOperacional(
|
||||
novoFrontalCampoCm: novoFrontalCm,
|
||||
novoLateralCampoCm: novoLateralCm,
|
||||
headingDeg: UltimaLeitura.OrientacaoReal
|
||||
);
|
||||
|
||||
// Faz o efeito aparecer imediatamente no snapshot atual, sem
|
||||
// esperar o próximo GGA/THS. A próxima leitura repetirá a mesma
|
||||
// transformação sobre a posição bruta mais recente.
|
||||
AplicarPosicaoCorrigida();
|
||||
AtualizarMetadadosCorrecaoNoModelo();
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService][CorrecaoMapa] Atualizada | " +
|
||||
$"heading={UltimaLeitura.OrientacaoReal:0.##}° | " +
|
||||
$"delta_frontal={deltaFrontalCm:+0.##;-0.##;0}cm | " +
|
||||
$"delta_lateral={deltaLateralCm:+0.##;-0.##;0}cm | " +
|
||||
$"op_frontal={LeverArm.FrontalCampoCm:0.##}cm | " +
|
||||
$"op_lateral={LeverArm.LateralCampoCm:0.##}cm | " +
|
||||
$"mundo_N={LeverArm.CorrecaoMapaNorteCm:+0.##;-0.##;0}cm | " +
|
||||
$"mundo_E={LeverArm.CorrecaoMapaLesteCm:+0.##;-0.##;0}cm"
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Zera a calibração operacional mundo/mapa e os contadores usados
|
||||
/// pela interface. Não altera o lever arm físico do equipamento.
|
||||
/// </summary>
|
||||
public static void ResetarCorrecaoOperacional()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (LeverArm == null)
|
||||
return;
|
||||
|
||||
LeverArm = LeverArm.SemCorrecaoOperacional();
|
||||
AplicarPosicaoCorrigida();
|
||||
AtualizarMetadadosCorrecaoNoModelo();
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
"[GPSService][CorrecaoMapa] Correção operacional zerada."
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
public static Queue<GPSModel> historicoPosicao = new Queue<GPSModel>();
|
||||
|
||||
// ============================================================
|
||||
|
|
@ -4411,29 +4512,53 @@ namespace AgroBase.Services
|
|||
|
||||
private static void AplicarPosicaoCorrigida()
|
||||
{
|
||||
if (!PossuiHeadingValido(UltimaLeitura))
|
||||
if (LeverArm == null)
|
||||
{
|
||||
UltimaLeitura.Latitude =
|
||||
UltimaLeitura.LatitudeAnt;
|
||||
|
||||
UltimaLeitura.Longitude =
|
||||
UltimaLeitura.LongitudeAnt;
|
||||
|
||||
UltimaLeitura.Latitude = UltimaLeitura.LatitudeAnt;
|
||||
UltimaLeitura.Longitude = UltimaLeitura.LongitudeAnt;
|
||||
return;
|
||||
}
|
||||
|
||||
bool headingValido = PossuiHeadingValido(UltimaLeitura);
|
||||
|
||||
// Se o heading estiver inválido, o lever arm FÍSICO não pode ser
|
||||
// rotacionado com segurança. A correção global Norte/Leste, porém,
|
||||
// continua válida porque pertence ao mundo e independe do heading.
|
||||
var corrigida =
|
||||
LeverArm.FixLeverArmLatLon_Fast(
|
||||
UltimaLeitura.LatitudeAnt,
|
||||
UltimaLeitura.LongitudeAnt,
|
||||
UltimaLeitura.OrientacaoReal,
|
||||
UltimaLeitura.TimestampOri.frequencia
|
||||
headingValido ? UltimaLeitura.OrientacaoReal : 0.0,
|
||||
headingValido
|
||||
? Math.Max(1.0, UltimaLeitura.TimestampOri.frequencia)
|
||||
: 0.0
|
||||
);
|
||||
|
||||
UltimaLeitura.Latitude = corrigida.lat;
|
||||
UltimaLeitura.Longitude = corrigida.lon;
|
||||
}
|
||||
|
||||
private static void AtualizarMetadadosCorrecaoNoModelo()
|
||||
{
|
||||
UltimaLeitura.LeverArmFrontal =
|
||||
LeverArm != null ? LeverArm.FrontalTotalCm : 0;
|
||||
|
||||
UltimaLeitura.LeverArmLateral =
|
||||
LeverArm != null ? LeverArm.LateralTotalCm : 0;
|
||||
|
||||
UltimaLeitura.LeverArmFrontalOp =
|
||||
LeverArm != null ? LeverArm.FrontalCampoCm : 0;
|
||||
|
||||
UltimaLeitura.LeverArmLateralOp =
|
||||
LeverArm != null ? LeverArm.LateralCampoCm : 0;
|
||||
|
||||
UltimaLeitura.CorrecaoMapaNorteCm =
|
||||
LeverArm != null ? LeverArm.CorrecaoMapaNorteCm : 0;
|
||||
|
||||
UltimaLeitura.CorrecaoMapaLesteCm =
|
||||
LeverArm != null ? LeverArm.CorrecaoMapaLesteCm : 0;
|
||||
}
|
||||
|
||||
public static void AtualizarCoordenadasGPS()
|
||||
{
|
||||
if (!Variaveis.IsAgroBase)
|
||||
|
|
@ -4447,13 +4572,12 @@ namespace AgroBase.Services
|
|||
PenultimaLeitura.LeverArmLateral = UltimaLeitura.LeverArmLateral;
|
||||
PenultimaLeitura.LeverArmFrontalOp = UltimaLeitura.LeverArmFrontalOp;
|
||||
PenultimaLeitura.LeverArmLateralOp = UltimaLeitura.LeverArmLateralOp;
|
||||
PenultimaLeitura.CorrecaoMapaNorteCm = UltimaLeitura.CorrecaoMapaNorteCm;
|
||||
PenultimaLeitura.CorrecaoMapaLesteCm = UltimaLeitura.CorrecaoMapaLesteCm;
|
||||
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
||||
|
||||
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
||||
UltimaLeitura.LeverArmFrontal = LeverArm != null ? LeverArm.FrontalTotalCm : 0;
|
||||
UltimaLeitura.LeverArmLateral = LeverArm != null ? LeverArm.LateralTotalCm : 0;
|
||||
UltimaLeitura.LeverArmFrontalOp = LeverArm != null ? LeverArm.FrontalCampoCm : 0;
|
||||
UltimaLeitura.LeverArmLateralOp = LeverArm != null ? LeverArm.LateralCampoCm : 0;
|
||||
AtualizarMetadadosCorrecaoNoModelo();
|
||||
|
||||
UltimasLeituras.Add(UltimaLeitura.Clone());
|
||||
|
||||
|
|
@ -5904,11 +6028,21 @@ namespace AgroBase.Services
|
|||
|
||||
public class GeoLeverArm
|
||||
{
|
||||
// Geometria física do equipamento, presa ao corpo do rover.
|
||||
private readonly double xFisico;
|
||||
private readonly double yFisico;
|
||||
|
||||
// Contadores operacionais recebidos da base, também no referencial do
|
||||
// rover. Eles existem para preservar o contrato/UI (ex.: -5 cm, -10 cm),
|
||||
// mas NÃO entram diretamente na correção geodésica a cada leitura.
|
||||
private readonly double xCampo;
|
||||
private readonly double yCampo;
|
||||
|
||||
// Correção autoritativa mapa <-> RTK no referencial do mundo.
|
||||
// Norte positivo e Leste positivo, em metros.
|
||||
private readonly double correcaoMapaNorteM;
|
||||
private readonly double correcaoMapaLesteM;
|
||||
|
||||
private readonly bool invertHeading;
|
||||
private readonly bool invertFrontal;
|
||||
private readonly bool mirrorLateral;
|
||||
|
|
@ -5918,6 +6052,8 @@ namespace AgroBase.Services
|
|||
double offsetFisicoLateralCm = 0,
|
||||
double offsetCampoFrontalCm = 0,
|
||||
double offsetCampoLateralCm = 0,
|
||||
double correcaoMapaNorteCm = 0,
|
||||
double correcaoMapaLesteCm = 0,
|
||||
bool invH = false,
|
||||
bool invF = false,
|
||||
bool mirrL = false
|
||||
|
|
@ -5928,6 +6064,9 @@ namespace AgroBase.Services
|
|||
xCampo = offsetCampoFrontalCm / 100.0;
|
||||
yCampo = offsetCampoLateralCm / 100.0;
|
||||
|
||||
correcaoMapaNorteM = correcaoMapaNorteCm / 100.0;
|
||||
correcaoMapaLesteM = correcaoMapaLesteCm / 100.0;
|
||||
|
||||
invertHeading = invH;
|
||||
invertFrontal = invF;
|
||||
mirrorLateral = mirrL;
|
||||
|
|
@ -5938,11 +6077,24 @@ namespace AgroBase.Services
|
|||
return deg * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
private static bool Finito(double valor)
|
||||
{
|
||||
return !double.IsNaN(valor) && !double.IsInfinity(valor);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Compatibilidade de telemetria/UI legada. Representa físico + contador
|
||||
/// operacional, mas NÃO significa que ambos sejam somados no mesmo frame
|
||||
/// durante o cálculo da posição.
|
||||
/// </summary>
|
||||
public double FrontalTotalCm
|
||||
{
|
||||
get { return (xFisico + xCampo) * 100.0; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Compatibilidade de telemetria/UI legada. Veja FrontalTotalCm.
|
||||
/// </summary>
|
||||
public double LateralTotalCm
|
||||
{
|
||||
get { return (yFisico + yCampo) * 100.0; }
|
||||
|
|
@ -5950,27 +6102,53 @@ namespace AgroBase.Services
|
|||
|
||||
public double FrontalCampoCm
|
||||
{
|
||||
get { return (xCampo) * 100.0; }
|
||||
get { return xCampo * 100.0; }
|
||||
}
|
||||
|
||||
public double LateralCampoCm
|
||||
{
|
||||
get { return (yCampo) * 100.0; }
|
||||
get { return yCampo * 100.0; }
|
||||
}
|
||||
|
||||
public (double lat, double lon) FixLeverArmLatLon_Fast(double latAnt_deg, double lonAnt_deg, double headingDeg, double headingFreq)
|
||||
public double CorrecaoMapaNorteCm
|
||||
{
|
||||
if (headingFreq < 1)
|
||||
return (latAnt_deg, lonAnt_deg);
|
||||
get { return correcaoMapaNorteM * 100.0; }
|
||||
}
|
||||
|
||||
double xTotal = xFisico + xCampo;
|
||||
double yTotal = yFisico + yCampo;
|
||||
public double CorrecaoMapaLesteCm
|
||||
{
|
||||
get { return correcaoMapaLesteM * 100.0; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Recebe os novos valores ABSOLUTOS dos contadores operacionais e
|
||||
/// converte somente o DELTA desde o estado atual para o frame global.
|
||||
///
|
||||
/// Isso é essencial: recalcular todo xCampo/yCampo com um heading novo
|
||||
/// faria a calibração já acumulada girar junto com o rover novamente.
|
||||
/// </summary>
|
||||
public GeoLeverArm ComAtualizacaoOperacional(
|
||||
double novoFrontalCampoCm,
|
||||
double novoLateralCampoCm,
|
||||
double headingDeg)
|
||||
{
|
||||
if (!Finito(headingDeg))
|
||||
throw new ArgumentException("Heading inválido.", nameof(headingDeg));
|
||||
|
||||
double novoXCampo = novoFrontalCampoCm / 100.0;
|
||||
double novoYCampo = novoLateralCampoCm / 100.0;
|
||||
|
||||
double deltaX = novoXCampo - xCampo;
|
||||
double deltaY = novoYCampo - yCampo;
|
||||
|
||||
// Mantém exatamente a convenção de sinais que a versão anterior
|
||||
// aplicava aos offsets de campo. A diferença é que agora convertemos
|
||||
// o incremento uma única vez e o congelamos no frame Norte/Leste.
|
||||
if (invertFrontal)
|
||||
xTotal = -xTotal;
|
||||
deltaX = -deltaX;
|
||||
|
||||
if (mirrorLateral)
|
||||
yTotal = -yTotal;
|
||||
deltaY = -deltaY;
|
||||
|
||||
double th = ToRad(headingDeg);
|
||||
|
||||
|
|
@ -5983,37 +6161,132 @@ namespace AgroBase.Services
|
|||
double rE = fN;
|
||||
double rN = -fE;
|
||||
|
||||
double dE =
|
||||
xTotal * fE +
|
||||
yTotal * rE;
|
||||
double dE = deltaX * fE + deltaY * rE;
|
||||
double dN = deltaX * fN + deltaY * rN;
|
||||
|
||||
double dN =
|
||||
xTotal * fN +
|
||||
yTotal * rN;
|
||||
// Na implementação legada, a posição final era lat/lon - dN/dE.
|
||||
// Por isso o incremento equivalente no mundo é -dN / -dE.
|
||||
double novoNorteM = correcaoMapaNorteM - dN;
|
||||
double novoLesteM = correcaoMapaLesteM - dE;
|
||||
|
||||
double latRad = ToRad(latAnt_deg);
|
||||
|
||||
double dLatDeg =
|
||||
dN / GPSUtils.RaioDaTerra *
|
||||
180.0 / Math.PI;
|
||||
|
||||
double cosLat = Math.Cos(latRad);
|
||||
|
||||
if (Math.Abs(cosLat) < 1e-12)
|
||||
return (latAnt_deg, lonAnt_deg);
|
||||
|
||||
double dLonDeg =
|
||||
dE /
|
||||
(GPSUtils.RaioDaTerra * cosLat) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
return (
|
||||
latAnt_deg - dLatDeg,
|
||||
lonAnt_deg - dLonDeg
|
||||
return new GeoLeverArm(
|
||||
offsetFisicoFrontalCm: xFisico * 100.0,
|
||||
offsetFisicoLateralCm: yFisico * 100.0,
|
||||
offsetCampoFrontalCm: novoFrontalCampoCm,
|
||||
offsetCampoLateralCm: novoLateralCampoCm,
|
||||
correcaoMapaNorteCm: novoNorteM * 100.0,
|
||||
correcaoMapaLesteCm: novoLesteM * 100.0,
|
||||
invH: invertHeading,
|
||||
invF: invertFrontal,
|
||||
mirrL: mirrorLateral
|
||||
);
|
||||
}
|
||||
|
||||
public (double x, double y) GeodeticToENU(double lat, double lon, double lat0, double lon0)
|
||||
public GeoLeverArm SemCorrecaoOperacional()
|
||||
{
|
||||
return new GeoLeverArm(
|
||||
offsetFisicoFrontalCm: xFisico * 100.0,
|
||||
offsetFisicoLateralCm: yFisico * 100.0,
|
||||
offsetCampoFrontalCm: 0.0,
|
||||
offsetCampoLateralCm: 0.0,
|
||||
correcaoMapaNorteCm: 0.0,
|
||||
correcaoMapaLesteCm: 0.0,
|
||||
invH: invertHeading,
|
||||
invF: invertFrontal,
|
||||
mirrL: mirrorLateral
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 1) Corrige a posição da antena para o centro físico usando apenas o
|
||||
/// lever arm físico, que gira com o heading.
|
||||
/// 2) Aplica a correção operacional Norte/Leste, fixa no mundo.
|
||||
/// </summary>
|
||||
public (double lat, double lon) FixLeverArmLatLon_Fast(
|
||||
double latAnt_deg,
|
||||
double lonAnt_deg,
|
||||
double headingDeg,
|
||||
double headingFreq)
|
||||
{
|
||||
double latCentro = latAnt_deg;
|
||||
double lonCentro = lonAnt_deg;
|
||||
|
||||
bool headingValido =
|
||||
headingFreq >= 1.0 &&
|
||||
Finito(headingDeg);
|
||||
|
||||
if (headingValido)
|
||||
{
|
||||
double x = xFisico;
|
||||
double y = yFisico;
|
||||
|
||||
if (invertFrontal)
|
||||
x = -x;
|
||||
|
||||
if (mirrorLateral)
|
||||
y = -y;
|
||||
|
||||
double th = ToRad(headingDeg);
|
||||
|
||||
if (invertHeading)
|
||||
th = -th;
|
||||
|
||||
double fE = Math.Sin(th);
|
||||
double fN = Math.Cos(th);
|
||||
|
||||
double rE = fN;
|
||||
double rN = -fE;
|
||||
|
||||
double dE = x * fE + y * rE;
|
||||
double dN = x * fN + y * rN;
|
||||
|
||||
double latRadAnt = ToRad(latAnt_deg);
|
||||
double cosLatAnt = Math.Cos(latRadAnt);
|
||||
|
||||
if (Math.Abs(cosLatAnt) >= 1e-12)
|
||||
{
|
||||
double dLatDeg =
|
||||
dN / GPSUtils.RaioDaTerra *
|
||||
180.0 / Math.PI;
|
||||
|
||||
double dLonDeg =
|
||||
dE /
|
||||
(GPSUtils.RaioDaTerra * cosLatAnt) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
latCentro = latAnt_deg - dLatDeg;
|
||||
lonCentro = lonAnt_deg - dLonDeg;
|
||||
}
|
||||
}
|
||||
|
||||
// A correção de mundo continua válida mesmo se o heading estiver
|
||||
// temporariamente inválido, porque não depende da orientação do rover.
|
||||
double latRadCentro = ToRad(latCentro);
|
||||
double cosLatCentro = Math.Cos(latRadCentro);
|
||||
|
||||
if (Math.Abs(cosLatCentro) < 1e-12)
|
||||
return (latCentro, lonCentro);
|
||||
|
||||
double correcaoLatDeg =
|
||||
correcaoMapaNorteM / GPSUtils.RaioDaTerra *
|
||||
180.0 / Math.PI;
|
||||
|
||||
double correcaoLonDeg =
|
||||
correcaoMapaLesteM /
|
||||
(GPSUtils.RaioDaTerra * cosLatCentro) *
|
||||
180.0 / Math.PI;
|
||||
|
||||
return (
|
||||
latCentro + correcaoLatDeg,
|
||||
lonCentro + correcaoLonDeg
|
||||
);
|
||||
}
|
||||
|
||||
public (double x, double y) GeodeticToENU(
|
||||
double lat,
|
||||
double lon,
|
||||
double lat0,
|
||||
double lon0)
|
||||
{
|
||||
double latR = ToRad(lat);
|
||||
double lonR = ToRad(lon);
|
||||
|
|
@ -6034,4 +6307,5 @@ namespace AgroBase.Services
|
|||
return (xEast, yNorth);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -56,14 +56,30 @@ namespace OperationControl.Views.Operacao.Monitoramento
|
|||
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Snr, false, tipo);
|
||||
}
|
||||
|
||||
private double? _leverArmLateralSolicitadoCm = null;
|
||||
private void AtualizarLeverArm(double valorCm)
|
||||
{
|
||||
double la_frontal = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.LeverArmFrontal ?? 0;
|
||||
double la_lateral = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.LeverArmLateral ?? 0;
|
||||
double novo = la_lateral + valorCm;
|
||||
VariaveisControleOperacao.EnviarComandoLeverArm(lateral: novo, frontal: la_frontal);
|
||||
if (!_leverArmLateralSolicitadoCm.HasValue)
|
||||
{
|
||||
_leverArmLateralSolicitadoCm =
|
||||
VariaveisControleOperacao.RoverEmFoco?
|
||||
.DadosLeitura?
|
||||
.Gnss?
|
||||
.LeverArmLateralOp ?? 0.0;
|
||||
}
|
||||
|
||||
lblLeverArmLateral.Text = $"Lever Arm Lateral: {novo} cm";
|
||||
_leverArmLateralSolicitadoCm += valorCm;
|
||||
|
||||
double novoLateralOp =
|
||||
_leverArmLateralSolicitadoCm.Value;
|
||||
|
||||
VariaveisControleOperacao.EnviarComandoLeverArm(
|
||||
lateral: novoLateralOp,
|
||||
frontal: null
|
||||
);
|
||||
|
||||
lblLeverArmLateral.Text =
|
||||
$"Correção Lateral: {novoLateralOp:+0;-0;0} cm";
|
||||
}
|
||||
|
||||
private void btnLeverArmEsquerda_Click(object sender, System.Windows.RoutedEventArgs e)
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ Estrutura esperada:
|
|||
labels/
|
||||
naonavegavel/
|
||||
...
|
||||
naonavegave_navegavel/
|
||||
naonavegavel_navegavel/
|
||||
...
|
||||
|
||||
Também aceita split/train ou split/val.
|
||||
|
|
@ -955,7 +955,7 @@ def derive_composition_group(
|
|||
group = "navegavel"
|
||||
|
||||
else:
|
||||
group = "naonavegave_navegavel"
|
||||
group = "naonavegavel_navegavel"
|
||||
|
||||
return (
|
||||
group,
|
||||
|
|
|
|||
Loading…
Reference in New Issue