mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-07-26 10:38:41 +00:00
Neil Davis
96 lines
3.3 KiB
Go
96 lines
3.3 KiB
Go
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
|
|
}
|