unit u_calc;

// Purpose: Calculations on Hall Data

{$mode objfpc}{$H+}

interface

uses Classes, SysUtils;

var
  EllipseShortAxis_mm,
  PrecessionAngle_Degrees: real;

// For extra calculations on the ellipse:
const
  MaxPositionCounts = 41293;
var
  Points: integer;
  LastPositionCounter: word;
  PrevHalfSwing: boolean;
  SumCosX, SumSinX, SumCosY, SumSinY: real;
  AmplitudeX, AmplitudeY, AngleX, AngleY: real;
procedure AddHallData (PosX, PosY: real; PositionCounter: integer; HalfSwing: boolean);

// forwards
procedure ProcessHallData;

implementation

uses u_main, u_messages, u_logging, u_compass, u_numstrings, math, graphics;

var
  PosNS, PosEW, PrevPosNS, PrevPosEW,
  SumPosNS, SumPosEW,
  avZeroCorrectionNS, avZeroCorrectionEW,
  HallZeroAverageFactor,
  DistanceSquared, PrevDistanceSquared,
  avFarestPosNS, avFarestPosEW,
  avMinorAxisNS, avMinorAxisEW,
  EllipseMinorAxis,
  EllipseRatio_Percent,
  EllipseCalibrationFactor, EllipseMajorAxis,
  PrecessionAngle_Radians: real;
  HaveFirst, FoundFarest: boolean;
  CompassScale, CalibrationScale: integer;

procedure ProcessHallData; // executed at each message arrival
const
  avFactor= 0.1;
var
  RotX, RotY: real;
  HallOffset: byte;
begin
  // put raw Hall data into the textboxes
  with Frmmain do
  begin
    TbxNorth.text:= format ('%3d', [AdcHallNorth]);
    TbxSouth.text:= format ('%3d', [AdcHallSouth]);
    TbxEast.text:=  format ('%3d', [AdcHallEast]);
    TbxWest.text:=  format ('%3d', [AdcHallWest]);
  end;

  if CalibrateHallSensors then
  begin
    // For adjusting the offsets, while Bob is still, we use the
    // enlargement scale for fine adjustment.
    HallOffset:= 85; // The value for Bob in center.
    if Enlarge then CalibrationScale:= 10 else CalibrationScale:= 2;
    // We will adjust the amplitude of the individual Hall Channels to the
    // same value when the bob is at the same, known distance from the center
    if not Persistence then Compass.Init (FrmMain, CompassLeft, CompassTop, CompassSize);
    Compass.PlotFilledBox ((AdcHallEast - HallOffset) *       CalibrationScale, 0, 3, clYellow);  // color sunrise
    Compass.PlotFilledBox ((AdcHallWest - HallOffset) *      -CalibrationScale, 0, 3, clRed);     // color sunset
    Compass.PlotFilledBox ( 0, (AdcHallNorth - HallOffset) *  CalibrationScale,    3, clFuchsia); // color northern light
    Compass.PlotFilledBox ( 0, (AdcHallSouth - HallOffset) * -CalibrationScale,    3, clAqua);    // color ice
  end
  else
  begin
    // Calculate normalized positions from Hall sensors
    // Normalized means they are between -1 and +1
    // The > 0 test is to prevent a possible div by zero crash if data fails
    if (AdcHallNorth + AdcHallSouth) > 0 then
      PosNS:= (real (AdcHallNorth - AdcHallSouth) / real (AdcHallNorth + AdcHallSouth));
    if (AdcHallEast + AdcHallWest) > 0 then
      PosEW:= (real (AdcHallEast - AdcHallWest) / real (AdcHallEast + AdcHallWest));

    // Sum for calculation of autozero cerrection
    // Calculation is done after we have a full swing
    SumPosNS += PosNS;
    SumPosEW += PosEW;

    if AutoZeroHallData then
    begin
      PosNS -= avZeroCorrectionNS;
      PosEW -= avZeroCorrectionEW;
    end;

    // For extra calculations on the ellipse:
    AddHallData (PosEW, PosNS, PositionCounter, HalfSwing);

    // Show normalized positions
    FrmMain.TbxPosNS.text:= format ('%5.3f', [PosNS]);
    Frmmain.TbxPosEW.text:= format ('%5.3f', [PosEW]);
    // Plot on Compass display
    CompassScale:= fIval (FrmMain.TbxCompassScale.text);
    if Enlarge then CompassScale *= 4;
    if not Persistence then
    begin // remove previous blip
      Compass.PlotPoint (round (PrevPosEW * CompassScale), round (PrevPosNS * CompassScale), 1, clBlack);
      Compass.ReDraw; // redraw grid
    end;
    // save previous positions for removal in plot and for precession angle calculation.
    PrevPosNS:= PosNS;
    PrevPosEW:= PosEW;

    if HallOnly then // for calibrating the Hall sensor system.
    // we show only Halldata in the compass and textboxes
    // and do no other calculations or reports
    begin
      Compass.PlotPoint (round (PosEW * CompassScale), round (PosNS * CompassScale), 1, clWhite) // new blip
    end
    else
    begin
      if HalfSwing then // show active halfswing in aqua color
        Compass.PlotPoint (round (PosEW * CompassScale), round (PosNS * CompassScale), 1, clAqua) // new blip
      else
        Compass.PlotPoint (round (PosEW * CompassScale), round (PosNS * CompassScale), 1, clWhite);

      // Plot Rimpasses for diagnosis
      // Note: Rimpasses are plotted to late, 'cause the message in which the pass is reported
      // contains position data from the time of the message, not the time of the rimpass.
      if PlotRim_1 then if SeenRim_1 then
        Compass.DrawCircle (round (PosEW * CompassScale), round (PosNS * CompassScale), 2, clRed);
      if PlotRim_2 then if SeenRim_2 then
        Compass.DrawCircle (round (PosEW * CompassScale), round (PosNS * CompassScale), 2, clRed);
      SeenRim_1:= false;
      SeenRim_2:= false;

      if HalfSwing then // We do further processing only for one HalfSwing
      begin
        if SeenCenter then // prepare for searches
        begin
          //FrmMain.memo.lines.add ('SeenCenter');
          PrevDistanceSquared:= 0; // to start a new search
          FoundFarest:= false;     // for finding the precession angle
          HaveFirst:= false;       // for finding the short axis of the ellipse
        end;

        // To find the precession angle we search for the farest distance from the center
        if not FoundFarest then
        begin
          DistanceSquared:= sqr (PosEW) + sqr (PosNS); // we dont need the squareroot
          if DistanceSquared >= PrevDistanceSquared then // track increasing distance
            PrevDistanceSquared:= DistanceSquared
          else // we are on the way back, so the previous position was the farest away
          begin
            FoundFarest:= true;
            // Enter the farest coordinates into the leaking bucket averagers
            avFarestPosNS:= avFarestPosNS * (1-avFactor) + PrevPosNS * avFactor;
            avFarestPosEW:= avFarestPosEW * (1-avFactor) + PrevPosEW * avFactor;
            // Plot the farest position averaged
            Compass.PlotFilledBox (round (avFarestPosEW * CompassScale),
                                   round (avFarestPosNS * CompassScale), 2, clAqua); // new blip
            FrmMain.TbxFarestNS.text:= format ('%5.3f', [avFarestPosNS]);
            FrmMain.TbxFarestEW.text:= format ('%5.3f', [avFarestPosEW]);
            // calculate the angle
            PrecessionAngle_Radians:= arctan2 (avFarestPosNS, avFarestPosEW);
            PrecessionAngle_Degrees:= PrecessionAngle_Radians * 180 / pi();
            FrmMain.TbxPrecessionAngle.text:= format ('%5.1f', [PrecessionAngle_Degrees]);
            Compass.PlotAngleMarker (PrecessionAngle_Degrees, clRed);
            // To have calibrated values for the Ellipse dimensions we take the longest axis in Hall-units:
            EllipseMajorAxis:= sqrt (PrevDistanceSquared);
            FrmMain.TbxEllipseLongAxis.text:= format ('%5.3f', [EllipseMajorAxis]);
            // But we know that this is equal to the Setpoint, if Amplitude Control is working properly
            if EllipseMajorAxis > 0.00000001 then // prevent div by nearly zero
              EllipseCalibrationFactor:= SetPoint_Amplitude_mm / EllipseMajorAxis;
          end;
        end;

        // We calculate the Ellipse Short Axis from the first sample after a Center Pass
        if not HaveFirst then
        begin
          HaveFirst:= true;
          // and put it into the leaking bucket averagers.
          avMinorAxisNS:= avMinorAxisNS * (1-avFactor) + PosNS * avFactor;
          avMinorAxisEW:= avMinorAxisEW * (1-avFactor) + PosEW * avFactor;
          // The short axis is the distance between the center and this sample
          EllipseMinorAxis:= sqrt (sqr (avMinorAxisNS) + sqr (avMinorAxisEW)); // Hall units
          Compass.PlotFilledBox (round (PrevPosEW * CompassScale), round (PrevPosNS * CompassScale), 2, clLime); // new blip
          // To find the polarity of the short axis, we rotate the sample over -PrecessionAngle.
          // Otherwise said: the ellipse is now laying horizontally (if all points were rotated).
          Rotate (avMinorAxisEW, avMinorAxisNS, -PrecessionAngle_Radians, RotX, RotY);
          Compass.PlotPoint (round (RotX * CompassScale), round (RotY * CompassScale), 1, clRed);
          if RotY > 0 then EllipseMinorAxis *= -1; // Positive rotation is CW
          EllipseRatio_Percent:= 100.0 * EllipseMinorAxis / EllipseMajorAxis;
          FrmMain.TbxEllipseRatio_Percent.text:= format ('%5.2f', [EllipseRatio_Percent]);
          // Calibrate the short axis:
          EllipseShortAxis_mm:= EllipseMinorAxis * EllipseCalibrationFactor;
          FrmMain.TbxEllipseShortAxis.text:= format ('%6.2f', [EllipseShortAxis_mm]);
        end;
        if SeenCenter then begin LogData; end; // also extra calculations
      end; // if HalfSwing
    end;
  end;
end;

// We do an extra calculation of the ellipse parameters, based on the dataset of
// one complete swing.
// This is the so called "single frequency Fourier" method or "Quadrature" method.

procedure AddHallData (PosX, PosY: real; PositionCounter: integer; HalfSwing: boolean);
var
  Angle, SinAngle, CosAngle: real;
  PositionCounts: word;
begin
  // because PositionCounter is nulled at each CenterPass we
  // add its last value to the other half
  if HalfSwing then
  begin
    PositionCounts:= PositionCounter + LastPositionCounter;
  end
  else
  begin
    PositionCounts:= PositionCounter;
    LastPositionCounter:= PositionCounter; // track it
  end;

  //MemoAdd (format ('Add %6d', [PositionCounts]));
  Angle:= PositionCounts / MaxPositionCounts * 2 * pi;
  Points += 1;
  SinAngle:= sin (Angle);
  CosAngle:= cos (Angle);
  SumCosX += PosX * CosAngle;
  SumSinX += PosX * SinAngle;
  SumCosY += PosY * CosAngle;
  SumSinY += PosY * SinAngle;

  if HalfSwing <> PrevHalfSwing then  // detect transitions
  begin
    PrevHalfSwing:= HalfSwing;
    if not HalfSwing then // we have a complete swing
    begin
      //FrmMain.Memo.append ('Finish and Start Over');
      PrevHalfSwing:= false;
      AmplitudeX:= sqrt (sqr (SumCosX) + sqr (SumSinX)) / Points;
      AmplitudeY:= sqrt (sqr (SumCosY) + sqr (SumSinY)) / Points;
      AngleX:= arctan2 (SumSinX, SumCosX);   // arctan works with Y,X
      AngleY:= arctan2 (SumSinY, SumCosY);

      SumCosX:= 0;
      SumSinX:= 0;
      SumCosY:= 0;
      SumSinY:= 0;

      // Calculate Auto zero for Hall data
      // with leaking bucket averaging;
      HallZeroAverageFactor:= fRval (FrmMain.TbxHallZeroAverageFactor.text);
      avZeroCorrectionNS *= (1.0 - HallZeroAverageFactor);
      avZeroCorrectionNS += SumPosNS / Points * HallZeroAverageFactor;
      avZeroCorrectionEW *= (1.0 - HallZeroAverageFactor);
      avZeroCorrectionEW += SumPosEW / Points * HallZeroAverageFactor;
      FrmMain.TbxAutoZeroNS.text:= format ('%8.5f', [avZeroCorrectionNS]);
      FrmMain.TbxAutoZeroEW.text:= format ('%8.5f', [avZeroCorrectionEW]);
      SumPosNS:= 0;
      SumPosEW:= 0;
      Points:= 0;
    end;
  end;
end;

// &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
begin
// no initialisation code
end.

