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 AgroBase.Services;
|
||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
|
@ -71,6 +71,11 @@ namespace AgroBase.Models
|
||||||
public double LeverArmFrontal { get; set; } = 0;
|
public double LeverArmFrontal { get; set; } = 0;
|
||||||
public double LeverArmFrontalOp { 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()
|
public GPSModel Clone()
|
||||||
{
|
{
|
||||||
return new GPSModel()
|
return new GPSModel()
|
||||||
|
|
@ -113,6 +118,8 @@ namespace AgroBase.Models
|
||||||
LeverArmFrontalOp = LeverArmFrontalOp,
|
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||||
LeverArmLateral = LeverArmLateral,
|
LeverArmLateral = LeverArmLateral,
|
||||||
LeverArmLateralOp = LeverArmLateralOp,
|
LeverArmLateralOp = LeverArmLateralOp,
|
||||||
|
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||||
|
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||||
X = X,
|
X = X,
|
||||||
Y = Y
|
Y = Y
|
||||||
};
|
};
|
||||||
|
|
@ -140,7 +147,11 @@ namespace AgroBase.Models
|
||||||
PrecisaoHorizontal = PrecisaoHorizontal,
|
PrecisaoHorizontal = PrecisaoHorizontal,
|
||||||
QualidadeFix = QualidadeFix,
|
QualidadeFix = QualidadeFix,
|
||||||
UltimoComandoRespondido = UltimoComandoRespondido,
|
UltimoComandoRespondido = UltimoComandoRespondido,
|
||||||
Velocidade = Velocidade
|
Velocidade = Velocidade,
|
||||||
|
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||||
|
LeverArmLateralOp = LeverArmLateralOp,
|
||||||
|
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||||
|
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -2589,41 +2589,48 @@ namespace AgroBase.Models
|
||||||
|
|
||||||
private void ExecutarComandoGps(OperacaoComandoBaseModel controleBase)
|
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;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
double? campoLateralCm = lever.Item1;
|
double? campoLateralCm = lever.Item1;
|
||||||
double? campoFrontalCm = lever.Item2;
|
double? campoFrontalCm = lever.Item2;
|
||||||
|
|
||||||
if (campoLateralCm == null && campoFrontalCm == null)
|
if (!campoLateralCm.HasValue && !campoFrontalCm.HasValue)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
double novoLateralCm = campoLateralCm.HasValue ? campoLateralCm.Value : GPSService.LeverArm.LateralCampoCm;
|
bool aplicado = GPSService.AtualizarCorrecaoOperacional(
|
||||||
double novoFrontalCm = campoFrontalCm.HasValue ? campoFrontalCm.Value : GPSService.LeverArm.FrontalCampoCm;
|
campoLateralCm: campoLateralCm,
|
||||||
|
campoFrontalCm: campoFrontalCm
|
||||||
|
);
|
||||||
|
|
||||||
double novoTotalLateralCm = VariaveisEquipamento.LeverArmLateralCm + novoLateralCm;
|
if (!aplicado)
|
||||||
double novoTotalFrontalCm = VariaveisEquipamento.LeverArmFrontalCm + novoFrontalCm;
|
{
|
||||||
|
Variaveis.MostrarLog(
|
||||||
|
"[ComandoBase][CorrecaoMapa] Comando não aplicado. " +
|
||||||
|
"Verifique se o heading GNSS está válido."
|
||||||
|
);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
GeoLeverArm ajuste = GPSService.LeverArm;
|
||||||
|
|
||||||
Variaveis.MostrarLog(
|
Variaveis.MostrarLog(
|
||||||
$"[ComandoBase][LeverArm] Total RX: frontal={novoTotalFrontalCm:0.##}cm, lateral={novoTotalLateralCm:0.##}cm | " +
|
"[ComandoBase][CorrecaoMapa] Aplicada | " +
|
||||||
$"Fisico: frontal={VariaveisEquipamento.LeverArmFrontalCm:0.##}cm, lateral={VariaveisEquipamento.LeverArmLateralCm:0.##}cm | " +
|
$"op_frontal={ajuste.FrontalCampoCm:0.##}cm | " +
|
||||||
$"Campo: frontal={campoFrontalCm:0.##}cm, lateral={campoLateralCm:0.##}cm"
|
$"op_lateral={ajuste.LateralCampoCm:0.##}cm | " +
|
||||||
);
|
$"N={ajuste.CorrecaoMapaNorteCm:+0.##;-0.##;0}cm | " +
|
||||||
|
$"E={ajuste.CorrecaoMapaLesteCm:+0.##;-0.##;0}cm"
|
||||||
GPSService.LeverArm = new GeoLeverArm(
|
|
||||||
offsetFisicoFrontalCm: VariaveisEquipamento.LeverArmFrontalCm,
|
|
||||||
offsetFisicoLateralCm: VariaveisEquipamento.LeverArmLateralCm,
|
|
||||||
offsetCampoFrontalCm: novoFrontalCm,
|
|
||||||
offsetCampoLateralCm: novoLateralCm,
|
|
||||||
mirrL: true,
|
|
||||||
invF: true,
|
|
||||||
invH: false
|
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
private void ExecutarComandoCameraCaminho(OperacaoComandoBaseModel controleBase)
|
private void ExecutarComandoCameraCaminho(OperacaoComandoBaseModel controleBase)
|
||||||
{
|
{
|
||||||
string cameraId = VariaveisEquipamento.Parametros.camera_caminhho_id;
|
string cameraId = VariaveisEquipamento.Parametros.camera_caminhho_id;
|
||||||
|
|
|
||||||
|
|
@ -1782,6 +1782,10 @@ namespace AgroBase.Models.Operacoes
|
||||||
public double IdadeCorrecao { get; set; }
|
public double IdadeCorrecao { get; set; }
|
||||||
public double LeverArmLateral { get; set; }
|
public double LeverArmLateral { get; set; }
|
||||||
public double LeverArmFrontal { 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 DateTime UltimoComandoRespondido { get; set; }
|
||||||
|
|
||||||
public GPSModel ToGpsModel()
|
public GPSModel ToGpsModel()
|
||||||
|
|
@ -1805,6 +1809,10 @@ namespace AgroBase.Models.Operacoes
|
||||||
IdadeCorrecao = IdadeCorrecao,
|
IdadeCorrecao = IdadeCorrecao,
|
||||||
LeverArmFrontal = LeverArmFrontal,
|
LeverArmFrontal = LeverArmFrontal,
|
||||||
LeverArmLateral = LeverArmLateral,
|
LeverArmLateral = LeverArmLateral,
|
||||||
|
CorrecaoMapaLesteCm = CorrecaoMapaLesteCm,
|
||||||
|
CorrecaoMapaNorteCm = CorrecaoMapaNorteCm,
|
||||||
|
LeverArmFrontalOp = LeverArmFrontalOp,
|
||||||
|
LeverArmLateralOp = LeverArmLateralOp,
|
||||||
UltimoComandoRespondido = UltimoComandoRespondido
|
UltimoComandoRespondido = UltimoComandoRespondido
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -188,6 +188,107 @@ namespace AgroBase.Services
|
||||||
invH: false
|
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>();
|
public static Queue<GPSModel> historicoPosicao = new Queue<GPSModel>();
|
||||||
|
|
||||||
// ============================================================
|
// ============================================================
|
||||||
|
|
@ -4411,29 +4512,53 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
private static void AplicarPosicaoCorrigida()
|
private static void AplicarPosicaoCorrigida()
|
||||||
{
|
{
|
||||||
if (!PossuiHeadingValido(UltimaLeitura))
|
if (LeverArm == null)
|
||||||
{
|
{
|
||||||
UltimaLeitura.Latitude =
|
UltimaLeitura.Latitude = UltimaLeitura.LatitudeAnt;
|
||||||
UltimaLeitura.LatitudeAnt;
|
UltimaLeitura.Longitude = UltimaLeitura.LongitudeAnt;
|
||||||
|
|
||||||
UltimaLeitura.Longitude =
|
|
||||||
UltimaLeitura.LongitudeAnt;
|
|
||||||
|
|
||||||
return;
|
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 =
|
var corrigida =
|
||||||
LeverArm.FixLeverArmLatLon_Fast(
|
LeverArm.FixLeverArmLatLon_Fast(
|
||||||
UltimaLeitura.LatitudeAnt,
|
UltimaLeitura.LatitudeAnt,
|
||||||
UltimaLeitura.LongitudeAnt,
|
UltimaLeitura.LongitudeAnt,
|
||||||
UltimaLeitura.OrientacaoReal,
|
headingValido ? UltimaLeitura.OrientacaoReal : 0.0,
|
||||||
UltimaLeitura.TimestampOri.frequencia
|
headingValido
|
||||||
|
? Math.Max(1.0, UltimaLeitura.TimestampOri.frequencia)
|
||||||
|
: 0.0
|
||||||
);
|
);
|
||||||
|
|
||||||
UltimaLeitura.Latitude = corrigida.lat;
|
UltimaLeitura.Latitude = corrigida.lat;
|
||||||
UltimaLeitura.Longitude = corrigida.lon;
|
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()
|
public static void AtualizarCoordenadasGPS()
|
||||||
{
|
{
|
||||||
if (!Variaveis.IsAgroBase)
|
if (!Variaveis.IsAgroBase)
|
||||||
|
|
@ -4447,13 +4572,12 @@ namespace AgroBase.Services
|
||||||
PenultimaLeitura.LeverArmLateral = UltimaLeitura.LeverArmLateral;
|
PenultimaLeitura.LeverArmLateral = UltimaLeitura.LeverArmLateral;
|
||||||
PenultimaLeitura.LeverArmFrontalOp = UltimaLeitura.LeverArmFrontalOp;
|
PenultimaLeitura.LeverArmFrontalOp = UltimaLeitura.LeverArmFrontalOp;
|
||||||
PenultimaLeitura.LeverArmLateralOp = UltimaLeitura.LeverArmLateralOp;
|
PenultimaLeitura.LeverArmLateralOp = UltimaLeitura.LeverArmLateralOp;
|
||||||
|
PenultimaLeitura.CorrecaoMapaNorteCm = UltimaLeitura.CorrecaoMapaNorteCm;
|
||||||
|
PenultimaLeitura.CorrecaoMapaLesteCm = UltimaLeitura.CorrecaoMapaLesteCm;
|
||||||
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
UltimaLeitura.Ntrip_ativado = CorrecaoRTK_Ntrip;
|
||||||
|
|
||||||
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
UltimaLeitura.Heartbeat = (UltimaLeitura.Heartbeat + 1) % 10;
|
||||||
UltimaLeitura.LeverArmFrontal = LeverArm != null ? LeverArm.FrontalTotalCm : 0;
|
AtualizarMetadadosCorrecaoNoModelo();
|
||||||
UltimaLeitura.LeverArmLateral = LeverArm != null ? LeverArm.LateralTotalCm : 0;
|
|
||||||
UltimaLeitura.LeverArmFrontalOp = LeverArm != null ? LeverArm.FrontalCampoCm : 0;
|
|
||||||
UltimaLeitura.LeverArmLateralOp = LeverArm != null ? LeverArm.LateralCampoCm : 0;
|
|
||||||
|
|
||||||
UltimasLeituras.Add(UltimaLeitura.Clone());
|
UltimasLeituras.Add(UltimaLeitura.Clone());
|
||||||
|
|
||||||
|
|
@ -5904,11 +6028,21 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
public class GeoLeverArm
|
public class GeoLeverArm
|
||||||
{
|
{
|
||||||
|
// Geometria física do equipamento, presa ao corpo do rover.
|
||||||
private readonly double xFisico;
|
private readonly double xFisico;
|
||||||
private readonly double yFisico;
|
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 xCampo;
|
||||||
private readonly double yCampo;
|
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 invertHeading;
|
||||||
private readonly bool invertFrontal;
|
private readonly bool invertFrontal;
|
||||||
private readonly bool mirrorLateral;
|
private readonly bool mirrorLateral;
|
||||||
|
|
@ -5918,6 +6052,8 @@ namespace AgroBase.Services
|
||||||
double offsetFisicoLateralCm = 0,
|
double offsetFisicoLateralCm = 0,
|
||||||
double offsetCampoFrontalCm = 0,
|
double offsetCampoFrontalCm = 0,
|
||||||
double offsetCampoLateralCm = 0,
|
double offsetCampoLateralCm = 0,
|
||||||
|
double correcaoMapaNorteCm = 0,
|
||||||
|
double correcaoMapaLesteCm = 0,
|
||||||
bool invH = false,
|
bool invH = false,
|
||||||
bool invF = false,
|
bool invF = false,
|
||||||
bool mirrL = false
|
bool mirrL = false
|
||||||
|
|
@ -5928,6 +6064,9 @@ namespace AgroBase.Services
|
||||||
xCampo = offsetCampoFrontalCm / 100.0;
|
xCampo = offsetCampoFrontalCm / 100.0;
|
||||||
yCampo = offsetCampoLateralCm / 100.0;
|
yCampo = offsetCampoLateralCm / 100.0;
|
||||||
|
|
||||||
|
correcaoMapaNorteM = correcaoMapaNorteCm / 100.0;
|
||||||
|
correcaoMapaLesteM = correcaoMapaLesteCm / 100.0;
|
||||||
|
|
||||||
invertHeading = invH;
|
invertHeading = invH;
|
||||||
invertFrontal = invF;
|
invertFrontal = invF;
|
||||||
mirrorLateral = mirrL;
|
mirrorLateral = mirrL;
|
||||||
|
|
@ -5938,11 +6077,24 @@ namespace AgroBase.Services
|
||||||
return deg * Math.PI / 180.0;
|
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
|
public double FrontalTotalCm
|
||||||
{
|
{
|
||||||
get { return (xFisico + xCampo) * 100.0; }
|
get { return (xFisico + xCampo) * 100.0; }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Compatibilidade de telemetria/UI legada. Veja FrontalTotalCm.
|
||||||
|
/// </summary>
|
||||||
public double LateralTotalCm
|
public double LateralTotalCm
|
||||||
{
|
{
|
||||||
get { return (yFisico + yCampo) * 100.0; }
|
get { return (yFisico + yCampo) * 100.0; }
|
||||||
|
|
@ -5950,27 +6102,53 @@ namespace AgroBase.Services
|
||||||
|
|
||||||
public double FrontalCampoCm
|
public double FrontalCampoCm
|
||||||
{
|
{
|
||||||
get { return (xCampo) * 100.0; }
|
get { return xCampo * 100.0; }
|
||||||
}
|
}
|
||||||
|
|
||||||
public double LateralCampoCm
|
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)
|
get { return correcaoMapaNorteM * 100.0; }
|
||||||
return (latAnt_deg, lonAnt_deg);
|
}
|
||||||
|
|
||||||
double xTotal = xFisico + xCampo;
|
public double CorrecaoMapaLesteCm
|
||||||
double yTotal = yFisico + yCampo;
|
{
|
||||||
|
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)
|
if (invertFrontal)
|
||||||
xTotal = -xTotal;
|
deltaX = -deltaX;
|
||||||
|
|
||||||
if (mirrorLateral)
|
if (mirrorLateral)
|
||||||
yTotal = -yTotal;
|
deltaY = -deltaY;
|
||||||
|
|
||||||
double th = ToRad(headingDeg);
|
double th = ToRad(headingDeg);
|
||||||
|
|
||||||
|
|
@ -5983,37 +6161,132 @@ namespace AgroBase.Services
|
||||||
double rE = fN;
|
double rE = fN;
|
||||||
double rN = -fE;
|
double rN = -fE;
|
||||||
|
|
||||||
double dE =
|
double dE = deltaX * fE + deltaY * rE;
|
||||||
xTotal * fE +
|
double dN = deltaX * fN + deltaY * rN;
|
||||||
yTotal * rE;
|
|
||||||
|
|
||||||
double dN =
|
// Na implementação legada, a posição final era lat/lon - dN/dE.
|
||||||
xTotal * fN +
|
// Por isso o incremento equivalente no mundo é -dN / -dE.
|
||||||
yTotal * rN;
|
double novoNorteM = correcaoMapaNorteM - dN;
|
||||||
|
double novoLesteM = correcaoMapaLesteM - dE;
|
||||||
|
|
||||||
double latRad = ToRad(latAnt_deg);
|
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 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 =
|
double dLatDeg =
|
||||||
dN / GPSUtils.RaioDaTerra *
|
dN / GPSUtils.RaioDaTerra *
|
||||||
180.0 / Math.PI;
|
180.0 / Math.PI;
|
||||||
|
|
||||||
double cosLat = Math.Cos(latRad);
|
|
||||||
|
|
||||||
if (Math.Abs(cosLat) < 1e-12)
|
|
||||||
return (latAnt_deg, lonAnt_deg);
|
|
||||||
|
|
||||||
double dLonDeg =
|
double dLonDeg =
|
||||||
dE /
|
dE /
|
||||||
(GPSUtils.RaioDaTerra * cosLat) *
|
(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;
|
180.0 / Math.PI;
|
||||||
|
|
||||||
return (
|
return (
|
||||||
latAnt_deg - dLatDeg,
|
latCentro + correcaoLatDeg,
|
||||||
lonAnt_deg - dLonDeg
|
lonCentro + correcaoLonDeg
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
public (double x, double y) GeodeticToENU(double lat, double lon, double lat0, double lon0)
|
public (double x, double y) GeodeticToENU(
|
||||||
|
double lat,
|
||||||
|
double lon,
|
||||||
|
double lat0,
|
||||||
|
double lon0)
|
||||||
{
|
{
|
||||||
double latR = ToRad(lat);
|
double latR = ToRad(lat);
|
||||||
double lonR = ToRad(lon);
|
double lonR = ToRad(lon);
|
||||||
|
|
@ -6034,4 +6307,5 @@ namespace AgroBase.Services
|
||||||
return (xEast, yNorth);
|
return (xEast, yNorth);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -56,14 +56,30 @@ namespace OperationControl.Views.Operacao.Monitoramento
|
||||||
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Snr, false, tipo);
|
VariaveisControleOperacao.EnviarComandoTransmissaoVideo(AgroBase.Models.Enums.T_Code.Snr, false, tipo);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private double? _leverArmLateralSolicitadoCm = null;
|
||||||
private void AtualizarLeverArm(double valorCm)
|
private void AtualizarLeverArm(double valorCm)
|
||||||
{
|
{
|
||||||
double la_frontal = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.LeverArmFrontal ?? 0;
|
if (!_leverArmLateralSolicitadoCm.HasValue)
|
||||||
double la_lateral = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.LeverArmLateral ?? 0;
|
{
|
||||||
double novo = la_lateral + valorCm;
|
_leverArmLateralSolicitadoCm =
|
||||||
VariaveisControleOperacao.EnviarComandoLeverArm(lateral: novo, frontal: la_frontal);
|
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)
|
private void btnLeverArmEsquerda_Click(object sender, System.Windows.RoutedEventArgs e)
|
||||||
|
|
|
||||||
|
|
@ -73,7 +73,7 @@ Estrutura esperada:
|
||||||
labels/
|
labels/
|
||||||
naonavegavel/
|
naonavegavel/
|
||||||
...
|
...
|
||||||
naonavegave_navegavel/
|
naonavegavel_navegavel/
|
||||||
...
|
...
|
||||||
|
|
||||||
Também aceita split/train ou split/val.
|
Também aceita split/train ou split/val.
|
||||||
|
|
@ -955,7 +955,7 @@ def derive_composition_group(
|
||||||
group = "navegavel"
|
group = "navegavel"
|
||||||
|
|
||||||
else:
|
else:
|
||||||
group = "naonavegave_navegavel"
|
group = "naonavegavel_navegavel"
|
||||||
|
|
||||||
return (
|
return (
|
||||||
group,
|
group,
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue