tinygo-org/drivers
1
0
Fork 0
mirror of https://github.com/tinygo-org/drivers.git synced 2026-08-03 14:37:46 +00:00
Code Issues Packages Projects Releases 10 Wiki Activity

mpu6050: properly scale the outputs of the accel/gyro

Browse Source 
Units were unspecified before but were in practice the raw output from
the sensor. They have now been changed to sensible outputs
understandable to humans, like the other accelerometers.
This commit is contained in:
Ayke van Laethem
2019-07-10 16:17:44 +02:00
committed by Ron Evans
parent cb49783f18
commit 78fdaad9c0
1 changed files with 33 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mpu6050/mpu6050.go
+33 -10
View File
@@ -39,22 +39,45 @@ func (d Device) Configure() {
}
// ReadAcceleration reads the current acceleration from the device and returns
// it.
func (d Device) ReadAcceleration() (x int16, y int16, z int16) {
// it in µg (micro-gravity). When one of the axes is pointing straight to Earth
// and the sensor is not moving the returned value will be around 1000000 or
// -1000000.
func (d Device) ReadAcceleration() (x int32, y int32, z int32) {
data := make([]byte, 6)
d.bus.ReadRegister(uint8(d.Address), ACCEL_XOUT_H, data)
x = int16((uint16(data[0]) << 8) | uint16(data[1]))
y = int16((uint16(data[2]) << 8) | uint16(data[3]))
z = int16((uint16(data[4]) << 8) | uint16(data[5]))
// Now do two things:
// 1. merge the two values to a 16-bit number (and cast to a 32-bit integer)
// 2. scale the value to bring it in the -1000000..1000000 range.
// This is done with a trick. What we do here is essentially multiply by
// 1000000 and divide by 16384 to get the original scale, but to avoid
// overflow we do it at 1/64 of the value:
// 1000000 / 64 = 15625
// 16384 / 64 = 256
x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256
y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256
z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256
return
}
// ReadRotation reads the current rotation from the device and returns it.
func (d Device) ReadRotation() (x int16, y int16, z int16) {
// ReadRotation reads the current rotation from the device and returns it in
// µ°/s (micro-degrees/sec). This means that if you were to do a complete
// rotation along one axis and while doing so integrate all values over time,
// you would get a value close to 360000000.
func (d Device) ReadRotation() (x int32, y int32, z int32) {
data := make([]byte, 6)
d.bus.ReadRegister(uint8(d.Address), GYRO_XOUT_H, data)
x = int16((uint16(data[0]) << 8) | uint16(data[1]))
y = int16((uint16(data[2]) << 8) | uint16(data[3]))
z = int16((uint16(data[4]) << 8) | uint16(data[5]))
// First the value is converted from a pair of bytes to a signed 16-bit
// value and then to a signed 32-bit value to avoid integer overflow.
// Then the value is scaled to µ°/s (micro-degrees per second).
// This is done in the following steps:
// 1. Multiply by 250 * 1000_000
// 2. Divide by 32768
// The following calculation (x * 15625 / 2048 * 1000) is essentially the
// same but avoids overflow. First both operations are divided by 16 leading
// to multiply by 15625000 and divide by 2048, and then part of the multiply
// is done after the divide instead of before.
x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000
y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000
z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000
return
}