tinygo.org/x/drivers@v0.27.1-0.20240509133757-7dbca2a54349/as560x/helpers.go (about)

     1  package as560x // import tinygo.org/x/drivers/ams560x
     2  
     3  import "math"
     4  
     5  // convertFromNativeAngle converts and scales an angle from the device's native 12-bit range to the requested units
     6  func convertFromNativeAngle(angle uint16, maxAngle uint16, units AngleUnit) (uint16, float32) {
     7  	// MANG == 0 & MANG == NATIVE_ANGLE_RANGE (1 << 12) mean the same thing: use full circle range
     8  	// but the latter makes the maths/code simpler
     9  	if 0 == maxAngle {
    10  		maxAngle = NATIVE_ANGLE_RANGE
    11  	}
    12  	switch units {
    13  	case ANGLE_NATIVE:
    14  		// For native angles, scaling has already been done by the device
    15  		return angle, float32(angle)
    16  	case ANGLE_DEGREES_INT:
    17  		// Convert to degrees using integer arithmetic. Less accuracy but faster
    18  		var deg int = 0
    19  		if NATIVE_ANGLE_RANGE == maxAngle {
    20  			// Simplify the conversion when using the full range
    21  			deg = int(angle) * 360 >> 12
    22  		} else {
    23  			// Using an integer degrees scale with a narrower native range is pointless since we don't
    24  			// benefit at all from the increase in native resolution, in fact we LOSE precision.
    25  			// Alas, we have to return something
    26  			// First get maxAngle on the degrees scale
    27  			degMang, _ := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
    28  			// Now scale angle
    29  			deg = int(angle) * int(degMang) / NATIVE_ANGLE_RANGE
    30  		}
    31  		return uint16(deg), float32(deg)
    32  	case ANGLE_DEGREES_FLOAT:
    33  		// Convert to degrees using floating point. More accuracy at expense of speed
    34  		var degF float32 = 0.0
    35  		if NATIVE_ANGLE_RANGE == maxAngle {
    36  			// Simplify the conversion when using the full range
    37  			degF = float32(angle) * 360.0 / NATIVE_ANGLE_RANGE
    38  		} else {
    39  			// Scale to degrees using a narrower native range
    40  			// First get maxAngle on the degrees scale
    41  			_, degMangF := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
    42  			// Now scale angle
    43  			degF = float32(angle) * degMangF / NATIVE_ANGLE_RANGE
    44  		}
    45  		return uint16(degF), degF
    46  	case ANGLE_RADIANS:
    47  		// Convert to radians. Can only be done using floating point.
    48  		var rad float32 = 0.0
    49  		if NATIVE_ANGLE_RANGE == maxAngle {
    50  			// Simplify the conversion when using the full range
    51  			rad = float32(angle) * 2 * math.Pi / NATIVE_ANGLE_RANGE
    52  		} else {
    53  			// Scale to radians using a narrower native range
    54  			// First get maxAngle on the radians scale
    55  			_, radMang := convertFromNativeAngle(maxAngle, NATIVE_ANGLE_RANGE, units)
    56  			// Now scale angle
    57  			rad = float32(angle) * radMang / NATIVE_ANGLE_RANGE
    58  		}
    59  		return uint16(rad), rad
    60  	default:
    61  		panic("Unknown angle measurement unit")
    62  	}
    63  }
    64  
    65  // convertToNativeAngle converts an angle from the requested units to the device's native 12-bit range.
    66  func convertToNativeAngle(angle float32, units AngleUnit) uint16 {
    67  	var pos uint16 = 0
    68  	switch units {
    69  	case ANGLE_NATIVE:
    70  		pos = uint16(angle)
    71  	case ANGLE_DEGREES_INT:
    72  		fallthrough
    73  	case ANGLE_DEGREES_FLOAT:
    74  		// Convert from degrees
    75  		angle = float32(math.Mod(float64(angle), 360.0))
    76  		if angle < 0.0 {
    77  			angle += 360.0
    78  		}
    79  		pos = uint16(math.Round(float64(angle) * NATIVE_ANGLE_RANGE / 360.0))
    80  	case ANGLE_RADIANS:
    81  		// Convert from radians
    82  		const circRad = 2.0 * math.Pi
    83  		angle = float32(math.Mod(float64(angle), circRad))
    84  		if angle < 0.0 {
    85  			angle += circRad
    86  		}
    87  		pos = uint16(math.Round(float64(angle) * NATIVE_ANGLE_RANGE / circRad))
    88  	default:
    89  		panic("Unknown angle measurement unit")
    90  	}
    91  	if pos > NATIVE_ANGLE_MAX {
    92  		pos = NATIVE_ANGLE_MAX
    93  	}
    94  	return pos
    95  }