Add support for ams AS560x on-axis magnetic rotary position sensors

2026-08-22 15:39:01 +00:00 · 2023-02-01 17:50:53 +00:00
parent b0a971573b
commit c59b5a909b
9 changed files with 933 additions and 0 deletions
@@ -0,0 +1,95 @@
+package as560x // import tinygo.org/x/drivers/ams560x
+
+import "math"
+
+// convertFromNativeAngle converts and scales an angle from the device's native 12-bit range to the requested units
+func convertFromNativeAngle(angle uint16, maxAngle uint16, units AngleUnit) (uint16, float32) {
+	// MANG == 0 & MANG == NATIVE_ANGLE_RANGE (1 << 12) mean the same thing: use full circle range
+	// but the latter makes the maths/code simpler
+	if 0 == maxAngle {
+		maxAngle = NATIVE_ANGLE_RANGE
+	}
+	switch units {
+	case ANGLE_NATIVE:
+		// For native angles, scaling has already been done by the device
+		return angle, float32(angle)
+	case ANGLE_DEGREES_INT:
+		// Convert to degrees using integer arithmetic. Less accuracy but faster
+		var deg int = 0
+		if NATIVE_ANGLE_RANGE == maxAngle {
+			// Simplify the conversion when using the full range
+			deg = int(angle) * 360 >> 12
+		} else {
+			// Using an integer degrees scale with a narrower native range is pointless since we don't
+			// benefit at all from the increase in native resolution, in fact we LOSE precision.
+			// Alas, we have to return something
+			// First get maxAngle on the degrees scale
+			degMang, _ := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
+			// Now scale angle
+			deg = int(angle) * int(degMang) / NATIVE_ANGLE_RANGE
+		}
+		return uint16(deg), float32(deg)
+	case ANGLE_DEGREES_FLOAT:
+		// Convert to degrees using floating point. More accuracy at expense of speed
+		var degF float32 = 0.0
+		if NATIVE_ANGLE_RANGE == maxAngle {
+			// Simplify the conversion when using the full range
+			degF = float32(angle) * 360.0 / NATIVE_ANGLE_RANGE
+		} else {
+			// Scale to degrees using a narrower native range
+			// First get maxAngle on the degrees scale
+			_, degMangF := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
+			// Now scale angle
+			degF = float32(angle) * degMangF / NATIVE_ANGLE_RANGE
+		}
+		return uint16(degF), degF
+	case ANGLE_RADIANS:
+		// Convert to radians. Can only be done using floating point.
+		var rad float32 = 0.0
+		if NATIVE_ANGLE_RANGE == maxAngle {
+			// Simplify the conversion when using the full range
+			rad = float32(angle) * 2 * math.Pi / NATIVE_ANGLE_RANGE
+		} else {
+			// Scale to radians using a narrower native range
+			// First get maxAngle on the radians scale
+			_, radMang := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
+			// Now scale angle
+			rad = float32(angle) * radMang / NATIVE_ANGLE_RANGE
+		}
+		return uint16(rad), rad
+	default:
+		panic("Unknown angle measurement unit")
+	}
+}
+
+// convertToNativeAngle converts an angle from the requested units to the device's native 12-bit range.
+func convertToNativeAngle(angle float32, units AngleUnit) uint16 {
+	var pos uint16 = 0
+	switch units {
+	case ANGLE_NATIVE:
+		pos = uint16(angle)
+	case ANGLE_DEGREES_INT:
+		fallthrough
+	case ANGLE_DEGREES_FLOAT:
+		// Convert from degrees
+		angle = float32(math.Mod(float64(angle), 360.0))
+		if angle < 0.0 {
+			angle += 360.0
+		}
+		pos = uint16(math.Round(float64(angle) * NATIVE_ANGLE_RANGE / 360.0))
+	case ANGLE_RADIANS:
+		// Convert from radians
+		const circRad = 2.0 * math.Pi
+		angle = float32(math.Mod(float64(angle), circRad))
+		if angle < 0.0 {
+			angle += circRad
+		}
+		pos = uint16(math.Round(float64(angle) * NATIVE_ANGLE_RANGE / circRad))
+	default:
+		panic("Unknown angle measurement unit")
+	}
+	if pos > NATIVE_ANGLE_MAX {
+		pos = NATIVE_ANGLE_MAX
+	}
+	return pos
+}