forked from blinken/paradar
-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.go
64 lines (54 loc) · 1.36 KB
/
main.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
package main
import (
"fmt"
"time"
"github.com/blinken/paradar/sensor"
"github.com/blinken/paradar/sensor/ahrs"
"github.com/blinken/paradar/sensor/altimeter"
)
/*
import _ "net/http/pprof"
import "net/http"
// Profiling
go func() {
fmt.Println(http.ListenAndServe("localhost:6060", nil))
}()
*/
func main() {
sb := sensor.NewBus()
alt := altimeter.NewAltimeter(sb)
orientation := ahrs.NewAHRS(sb)
if alt.SelfTest() {
fmt.Printf("altimeter is connected\n")
} else {
fmt.Printf("altimeter not connected\n")
}
go alt.Track()
go orientation.Track()
var count float64 = 1.0
for {
if orientation.GetRoll() == 0.0 {
time.Sleep(100 * time.Millisecond)
continue
}
/*
// Detailed debugging
offset := orientation.GetIMUYawOffset()
fmt.Printf("%04.3f %04d my %04.3f iy %04.3f | r %9.4f p %7.4f (only gyro=%5t offset=%3.3f c=%04d rate=%2.2f)\n",
orientation.GetYaw(),
alt.GetAltitude(),
orientation.GetIMUYawUncorrected(),
orientation.GetMagYawUncorrected(),
orientation.GetRoll(),
orientation.GetPitch(),
orientation.UsingIMU(),
offset,
orientation.GetIMUCount(),
float64(orientation.GetIMUCount())/count*20,
)
*/
count += 1
fmt.Printf("%.3f %04d ft\n", orientation.GetYaw(), int(alt.GetAltitude()))
time.Sleep(50 * time.Millisecond)
}
}