private void Config_LogKalmanDataReceived(object sender, LogDataReceivedEventArgs e)
{
_firstValueTracked = true;
_historyVarianceX.Add((float)e.GetVariable("kalman.varPX"));
_historyVarianceY.Add((float)e.GetVariable("kalman.varPY"));
_historyVarianceZ.Add((float)e.GetVariable("kalman.varPZ"));
_historyVarianceX.RemoveAt(0);
_historyVarianceY.RemoveAt(0);
_historyVarianceZ.RemoveAt(0);
VarianceX = CalculatueMax(_historyVarianceX) - CalculatueMin(_historyVarianceX);
VarianceY = CalculatueMax(_historyVarianceY) - CalculatueMin(_historyVarianceY);
VarianceZ = CalculatueMax(_historyVarianceZ) - CalculatueMin(_historyVarianceZ);
CurrentPosition = new Position((float)e.GetVariable("kalman.stateX"), (float)e.GetVariable("kalman.stateY"), (float)e.GetVariable("kalman.stateZ"));
PositionUpdate?.Invoke(this, new PositionUpdateEventArgs(CurrentPosition));
_waitForVarianceUpdate.Set();
}
private static void Config_LogDataReceived(object sender, LogDataReceivedEventArgs e)
{
Log.Info($"log received: {e.TimeStamp} | " +
$"roll: {e.GetVariable("stabilizer.roll") } ,pitch: { e.GetVariable("stabilizer.pitch") }, yaw: {e.GetVariable("stabilizer.yaw")}");
}
