-
Notifications
You must be signed in to change notification settings - Fork 2
/
test_IMU_cal.go
73 lines (59 loc) · 1.42 KB
/
test_IMU_cal.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
package main
import (
"fmt"
"os"
"os/signal"
"syscall"
"time"
"github.com/kpeu3i/bno055"
)
func main() {
sensor, err := bno055.NewSensor(0x28, 3)
if err != nil {
panic(err)
}
err = sensor.UseExternalCrystal(true)
if err != nil {
panic(err)
}
var (
isCalibrated bool
calibrationOffsets bno055.CalibrationOffsets
calibrationStatus *bno055.CalibrationStatus
)
signals := make(chan os.Signal, 1)
signal.Notify(signals, syscall.SIGINT, syscall.SIGTERM)
err = sensor.setOperationMode(prevMode)
if err != nil {
return err
}
for !isCalibrated {
select {
case <-signals:
err := sensor.Close()
if err != nil {
panic(err)
}
default:
calibrationOffsets, calibrationStatus, err = sensor.Calibration()
if err != nil {
panic(err)
}
isCalibrated = calibrationStatus.IsCalibrated()
fmt.Printf(
"\r*** Calibration status (0..3): system=%v, accelerometer=%v, gyroscope=%v, magnetometer=%v",
calibrationStatus.System,
calibrationStatus.Accelerometer,
calibrationStatus.Gyroscope,
calibrationStatus.Magnetometer,
)
// fmt.Printf("Calibration offsets: %v\n", calibrationOffsets)
}
time.Sleep(100 * time.Millisecond)
}
fmt.Printf("*** Done! Calibration offsets: %v\n", calibrationOffsets)
// 3 = fully calibrated
// Output
// *** Calibration status (0..3): system=3, accelerometer=3, gyroscope=3, magnetometer=3
// *** Done! Calibration offsets: [...]
}