Add angle sensor integration for hydraulic excavator arm/stick compen…

SourceCode/GPS/Classes/CAngleSensor.cs

@@ -0,0 +1,189 @@
+//Please, if you use this, share the improvements
+
+using System;
+using System.Globalization;
+
+namespace OpenGrade
+{
+    /// <summary>
+    /// Handles angle sensor input and filtering for excavator arm and stick
+    /// Compensates for movement of arm and stick affecting GPS antenna reading
+    /// </summary>
+    public class CAngleSensor
+    {
+        // Angle sensor data
+        public double armAngle; // Angle of main boom arm (degrees)
+        public double stickAngle; // Angle of bucket stick (degrees)
+
+        // Filtered angle values using moving average
+        private double[] armAngleBuffer;
+        private double[] stickAngleBuffer;
+        private int bufferIndex = 0;
+        private const int FILTER_BUFFER_SIZE = 10;
+
+        // Raw sensor values
+        public double armAngleRaw;
+        public double stickAngleRaw;
+
+        // Calibration offsets
+        public double armAngleOffset = 0.0;
+        public double stickAngleOffset = 0.0;
+
+        // Sensor connection status
+        public bool isConnected = false;
+        public string lastError = "";
+
+        // Update timestamp
+        public DateTime lastUpdate;
+
+        public CAngleSensor()
+        {
+            armAngleBuffer = new double[FILTER_BUFFER_SIZE];
+            stickAngleBuffer = new double[FILTER_BUFFER_SIZE];
+
+            // Initialize buffers
+            for (int i = 0; i < FILTER_BUFFER_SIZE; i++)
+            {
+                armAngleBuffer[i] = 0.0;
+                stickAngleBuffer[i] = 0.0;
+            }
+
+            lastUpdate = DateTime.Now;
+        }
+
+        /// <summary>
+        /// Parse angle sensor data from serial input
+        /// Expected format: $ANGLE,armAngle,stickAngle*checksum
+        /// Example: $ANGLE,45.2,32.5*2F
+        /// </summary>
+        public bool ParseAngleData(string sentence)
+        {
+            try
+            {
+                if (string.IsNullOrEmpty(sentence) || !sentence.StartsWith("$ANGLE"))
+                    return false;
+
+                // Remove checksum
+                int checksumIndex = sentence.IndexOf('*');
+                if (checksumIndex > 0)
+                    sentence = sentence.Substring(0, checksumIndex);
+
+                // Split by comma
+                string[] parts = sentence.Split(',');
+                if (parts.Length < 3)
+                {
+                    lastError = "Invalid angle sensor format";
+                    return false;
+                }
+
+                // Parse arm angle
+                if (!double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out armAngleRaw))
+                {
+                    lastError = "Failed to parse arm angle";
+                    return false;
+                }
+
+                // Parse stick angle
+                if (!double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out stickAngleRaw))
+                {
+                    lastError = "Failed to parse stick angle";
+                    return false;
+                }
+
+                // Apply calibration offsets
+                armAngle = armAngleRaw - armAngleOffset;
+                stickAngle = stickAngleRaw - stickAngleOffset;
+
+                // Apply filtering
+                UpdateFilteredValues();
+
+                lastUpdate = DateTime.Now;
+                isConnected = true;
+                lastError = "";
+
+                return true;
+            }
+            catch (Exception e)
+            {
+                lastError = "Parse error: " + e.Message;
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Update filtered angle values using moving average
+        /// </summary>
+        private void UpdateFilteredValues()
+        {
+            armAngleBuffer[bufferIndex] = armAngle;
+            stickAngleBuffer[bufferIndex] = stickAngle;
+
+            bufferIndex++;
+            if (bufferIndex >= FILTER_BUFFER_SIZE)
+                bufferIndex = 0;
+
+            // Calculate moving averages
+            double armSum = 0;
+            double stickSum = 0;
+
+            for (int i = 0; i < FILTER_BUFFER_SIZE; i++)
+            {
+                armSum += armAngleBuffer[i];
+                stickSum += stickAngleBuffer[i];
+            }
+
+            armAngle = armSum / FILTER_BUFFER_SIZE;
+            stickAngle = stickSum / FILTER_BUFFER_SIZE;
+        }
+
+        /// <summary>
+        /// Calibrate angle sensors (call when arm/stick in known position)
+        /// </summary>
+        public void CalibrateAngles(double armKnownAngle, double stickKnownAngle)
+        {
+            armAngleOffset = armAngleRaw - armKnownAngle;
+            stickAngleOffset = stickAngleRaw - stickKnownAngle;
+        }
+
+        /// <summary>
+        /// Normalize angles to 0-360 range
+        /// </summary>
+        public double NormalizeAngle(double angle)
+        {
+            angle = angle % 360.0;
+            if (angle < 0)
+                angle += 360.0;
+            return angle;
+        }
+
+        /// <summary>
+        /// Convert degrees to radians
+        /// </summary>
+        public double DegreesToRadians(double degrees)
+        {
+            return degrees * Math.PI / 180.0;
+        }
+
+        /// <summary>
+        /// Convert radians to degrees
+        /// </summary>
+        public double RadiansToDegrees(double radians)
+        {
+            return radians * 180.0 / Math.PI;
+        }
+
+        /// <summary>
+        /// Check connection timeout (30 seconds)
+        /// </summary>
+        public bool IsConnectionAlive()
+        {
+            TimeSpan elapsed = DateTime.Now - lastUpdate;
+            if (elapsed.TotalSeconds > 30)
+            {
+                isConnected = false;
+                return false;
+            }
+            return true;
+        }
+    }
+}

SourceCode/GPS/Classes/CArmKinematics.cs

@@ -0,0 +1,179 @@
+//Please, if you use this, share the improvements
+
+using System;
+
+namespace OpenGrade
+{
+    /// <summary>
+    /// Calculates excavator arm kinematics to determine antenna position
+    /// relative to the excavator base, accounting for arm and stick angles
+    /// </summary>
+    public class CArmKinematics
+    {
+        // Excavator arm dimensions (in meters) - adjust for your specific machine
+        public double armLength = 5.0;        // Main boom length
+        public double stickLength = 3.0;      // Bucket stick length
+
+        // Antenna offset from stick endpoint (in meters)
+        public double antennaOffsetX = 0.2;   // Forward offset
+        public double antennaOffsetY = 0.1;   // Vertical offset
+        public double antennaOffsetZ = 0.0;   // Side offset
+
+        // Base reference point (where arm connects to excavator body)
+        public double baseHeight = 2.0;       // Height of arm pivot from ground
+        public double baseOffsetX = 0.5;      // Forward offset from center
+        public double baseOffsetY = 0.0;      // Side offset from center
+
+        // Calculated antenna position relative to base
+        public vec3 antennaPosition;
+
+        // Previous position for velocity calculation
+        private vec3 previousPosition;
+        private DateTime lastCalculation;
+
+        public CArmKinematics()
+        {
+            antennaPosition = new vec3(0, 0, 0);
+            previousPosition = new vec3(0, 0, 0);
+            lastCalculation = DateTime.Now;
+        }
+
+        /// <summary>
+        /// Calculate antenna position based on arm and stick angles
+        /// angles in degrees
+        /// returns vec3 with easting (X), northing (Y), altitude (Z) offsets from base
+        /// </summary>
+        public vec3 CalculateAntennaPosition(double armAngleDeg, double stickAngleDeg)
+        {
+            // Convert to radians
+            double armAngleRad = armAngleDeg * Math.PI / 180.0;
+            double stickAngleRad = stickAngleDeg * Math.PI / 180.0;
+
+            // Calculate arm endpoint position (2D)
+            // Assuming 0 degrees = horizontal to the right, positive = upward
+            double armEndX = armLength * Math.Cos(armAngleRad);
+            double armEndY = armLength * Math.Sin(armAngleRad);
+
+            // Calculate stick endpoint relative to arm endpoint
+            // Stick angle is relative to ground (not relative to arm)
+            double stickEndX = armEndX + stickLength * Math.Cos(stickAngleRad);
+            double stickEndY = armEndY + stickLength * Math.Sin(stickAngleRad);
+
+            // Add antenna offset from stick endpoint
+            double antennaX = stickEndX + antennaOffsetX;
+            double antennaY = stickEndY + antennaOffsetY;
+
+            // Add base offsets
+            double totalX = baseOffsetX + antennaX;
+            double totalY = baseHeight + antennaY;
+            double totalZ = baseOffsetY + antennaOffsetZ;
+
+            // Store previous position
+            previousPosition = antennaPosition;
+
+            // Update current position
+            antennaPosition = new vec3(totalX, totalY, totalZ);
+            lastCalculation = DateTime.Now;
+
+            return antennaPosition;
+        }
+
+        /// <summary>
+        /// Calculate antenna position with stick angle relative to arm
+        /// (alternative calculation method)
+        /// </summary>
+        public vec3 CalculateAntennaPositionRelative(double armAngleDeg, double stickAngleRelativeDeg)
+        {
+            double armAngleRad = armAngleDeg * Math.PI / 180.0;
+            double stickRelativeRad = stickAngleRelativeDeg * Math.PI / 180.0;
+
+            // Arm endpoint
+            double armEndX = armLength * Math.Cos(armAngleRad);
+            double armEndY = armLength * Math.Sin(armAngleRad);
+
+            // Stick angle relative to arm
+            double stickAbsoluteAngle = armAngleDeg + stickAngleRelativeDeg;
+            double stickAbsoluteRad = stickAbsoluteAngle * Math.PI / 180.0;
+
+            // Stick endpoint
+            double stickEndX = armEndX + stickLength * Math.Cos(stickAbsoluteRad);
+            double stickEndY = armEndY + stickLength * Math.Sin(stickAbsoluteRad);
+
+            // Add antenna offset
+            double antennaX = stickEndX + antennaOffsetX;
+            double antennaY = stickEndY + antennaOffsetY;
+
+            previousPosition = antennaPosition;
+            antennaPosition = new vec3(baseOffsetX + antennaX, baseHeight + antennaY, baseOffsetY + antennaOffsetZ);
+            lastCalculation = DateTime.Now;
+
+            return antennaPosition;
+        }
+
+        /// <summary>
+        /// Get antenna velocity (change in position per second)
+        /// </summary>
+        public vec3 GetAntennaVelocity()
+        {
+            TimeSpan timeDelta = DateTime.Now - lastCalculation;
+            if (timeDelta.TotalSeconds < 0.001) // Avoid division by zero
+                return new vec3(0, 0, 0);
+
+            double timeFactor = timeDelta.TotalSeconds;
+            double velX = (antennaPosition.easting - previousPosition.easting) / timeFactor;
+            double velY = (antennaPosition.northing - previousPosition.northing) / timeFactor;
+            double velZ = (antennaPosition.heading - previousPosition.heading) / timeFactor;
+
+            return new vec3(velX, velY, velZ);
+        }
+
+        /// <summary>
+        /// Get antenna displacement from previous calculation
+        /// </summary>
+        public double GetAntennaDisplacement()
+        {
+            double dx = antennaPosition.easting - previousPosition.easting;
+            double dy = antennaPosition.northing - previousPosition.northing;
+            double dz = antennaPosition.heading - previousPosition.heading;
+
+            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
+        }
+
+        /// <summary>
+        /// Set arm dimensions for specific excavator model
+        /// </summary>
+        public void SetArmDimensions(double arm, double stick, double armBase, double stickBase)
+        {
+            armLength = arm;
+            stickLength = stick;
+            baseHeight = armBase;
+            baseOffsetX = stickBase;
+        }
+
+        /// <summary>
+        /// Set antenna offset from bucket/stick endpoint
+        /// </summary>
+        public void SetAntennaOffset(double offsetX, double offsetY, double offsetZ)
+        {
+            antennaOffsetX = offsetX;
+            antennaOffsetY = offsetY;
+            antennaOffsetZ = offsetZ;
+        }
+
+        /// <summary>
+        /// Validate arm angles are within physical limits
+        /// </summary>
+        public bool ValidateAngles(double armAngleDeg, double stickAngleDeg)
+        {
+            // Arm angle typically: -10 to 90 degrees
+            if (armAngleDeg < -20 || armAngleDeg > 100)
+                return false;
+
+            // Stick angle typically: -90 to 30 degrees
+            if (stickAngleDeg < -120 || stickAngleDeg > 50)
+                return false;
+
+            return true;
+        }
+    }
+}