Adafruit LSM303 accelerometer and magnetometer via I2C-bus from Raspberry PI
accelerometer, err := NewAccelerometer()
xa, ya, za, err := accelerometer.Sense()
// Configuration
accelerometer.SetRange(ACCELEROMETER_RANGE_16G)
accelerometer.SetMode(ACCELEROMETER_MODE_LOW_POWER)
// The periph.io has units defined for many things, but not for
// magnetometer flux, so we only have SenseRaw
xm, ym, zm, err := magnetometer.SenseRaw()
// Configuration
magnetometer.SetGain(MAGNETOMETER_GAIN_5_6)
magnetometer.SetRate(MAGNETOMETER_RATE_75)
// The magnetometer also has a relative temperature sensor. It's not
// calibrated, but adding 20 should give the approximate temperature.
temp, err := magnetometer.SenseRelativeTemperature()
With these sensors, you can compute the tilt-compensated heading. Note that the magnetometer will have some hard interference from locally mounted metal objects. To compensate, you will need to log the maximum and minimum magnetometer readings while moving the object through all orientations.
type Axes struct {
Pitch Radians
Roll Radians
Yaw Radians
}
xa, ya, za, err := accelerometer.Sense()
xm, ym, zm, err := magnetometer.SenseRaw()
axes := computeAxes(xa, ya, za, xm, ym, zm)
func computeAxes(xRawA, yRawA, zRawA, xRawM, yRawM, zRawM int16) Axes {
// Avoid divide by zero problems
if zRawA == 0 {
zRawA = 1
}
// The roll calculation assumes that +y is forward, x is right, and
// +z is up
x2 := int32(xRawA) * int32(xRawA)
z2 := int32(zRawA) * int32(zRawA)
// Tilt compensated compass readings
pitch_r := math.Atan2(float64(yRawA), math.Sqrt(float64(x2+z2)))
roll_r := -math.Atan2(float64(xRawA), float64(zRawA))
pitch_r -= configuration.PitchOffset
for pitch_r < ToRadians(-180.0) {
pitch_r += ToRadians(360.0)
}
for pitch_r > ToRadians(180.0) {
pitch_r -= ToRadians(360.0)
}
roll_r -= configuration.RollOffset
for roll_r < ToRadians(-180.0) {
roll_r += ToRadians(360.0)
}
for roll_r > ToRadians(180.0) {
roll_r -= ToRadians(360.0)
}
temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
// Here you will need to set the maximum and minimum magnetometer
// readings from calibration as described above
const int16 maxXM = 0
const int16 maxYM = 0
const int16 maxZM = 0
const int16 minXM = 0
const int16 minYM = 0
const int16 minZM = 0
xM := float64(xRawM - (maxXM - minXM) / 2)
yM := float64(yRawM - (maxYM - minYM) / 2)
zM := float64(zRawM - (maxZM - minZM) / 2)
xHorizontal := xM*math.Cos(-pitch_r) + yM*math.Sin(roll_r)*math.Sin(-pitch_r) - zM*math.Cos(roll_r)*math.Sin(-pitch_r)
yHorizontal := yM*math.Cos(roll_r) + zM*math.Sin(roll_r)
yaw_r := math.Atan2(yHorizontal, xHorizontal)
return Axes{
Pitch: pitch_r,
Roll: roll_r,
Yaw: yaw_r,
}
}
This uses periph.io to access peripherals. periph.io made some major updates in 2020, (see Periph's blog post) but I haven't had a chance to test the new version yet. Pull requests welcome!
For best results, you will need to calibrate the magnetometer as described above.