-
Notifications
You must be signed in to change notification settings - Fork 0
/
ground_control.go
129 lines (117 loc) · 3.25 KB
/
ground_control.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
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
package main
import (
"image"
"log"
"time"
"github.com/davecgh/go-spew/spew"
"golang.org/x/image/font"
"golang.org/x/image/font/basicfont"
"golang.org/x/image/math/fixed"
"periph.io/x/conn/v3/i2c/i2creg"
"periph.io/x/devices/v3/ssd1306"
"periph.io/x/devices/v3/ssd1306/image1bit"
"periph.io/x/host/v3"
)
const telemetryTimeout time.Duration = 10 * time.Second // time to wait for a telemetry message from the rover
const ackTimeout time.Duration = 1000 * time.Millisecond // time to wait for an ack message from the rover
const msgDelay time.Duration = 100 * time.Millisecond // time to wait between Rx and Tx, to give the other side time to switch from Tx to Rx
func initHost() {
// Make sure periph is initialized.
if _, err := host.Init(); err != nil {
log.Fatal(err)
}
}
func testDisplay() {
b, err := i2creg.Open("/dev/i2c-1")
if err != nil {
log.Fatal(err)
}
defer b.Close()
opts := ssd1306.DefaultOpts
opts.Sequential = true
dev, err := ssd1306.NewI2C(b, &opts)
if err != nil {
log.Fatalf("failed to initialize ssd1306: %v", err)
}
// Draw on it.
img := image1bit.NewVerticalLSB(dev.Bounds())
f := basicfont.Face7x13
drawer := font.Drawer{
Dst: img,
Src: &image.Uniform{image1bit.On},
Face: f,
Dot: fixed.P(0, img.Bounds().Dy()-1-f.Descent),
}
drawer.DrawString("GROUND STATION READY")
if err := dev.Draw(dev.Bounds(), img, image.Point{}); err != nil {
log.Fatal(err)
}
}
func main() {
initHost()
testDisplay()
r := initRadioEcc1()
//defer spiPort.Close()
// main loop - receive telemetry packets
messageCount := 0
groundRssi := 0.0
roverRssi := 0.0
retries := 0.0
for {
msgType, buf, rssi, err := receiveMessage(r, telemetryTimeout)
if err != nil {
log.Fatal(err)
}
switch msgType {
case messageTypeTelemetry:
log.Print("Received TELEMETRY message:")
log.Printf("RSSI: %d", rssi)
tm, err := receiveTelemetryMessage(buf)
spew.Dump(tm)
// accumulate stats
messageCount++
if rssi != 0 {
groundRssi = ((groundRssi * float64(messageCount-1)) + float64(rssi)) / float64(messageCount)
}
if tm.signalStrength != 0 {
roverRssi = ((roverRssi * float64(messageCount-1)) + float64(tm.signalStrength)) / float64(messageCount)
}
retries = ((retries * float64(messageCount-1)) + float64(tm.retries)) / float64(messageCount)
log.Printf("***Avg ground RSSI: %.2f Avg rover RSSI: %.2f Avg retries: %.2f", groundRssi, roverRssi, retries)
if err != nil {
log.Fatal(err)
}
// brief pause to give the otehr side time to switch from Tx to Rx
time.Sleep(msgDelay)
// send the ack
ta := telemetryAck{
timestamp: currentTimestamp(),
ack: true,
commandWaiting: false,
}
log.Print("Sending TELEMETRY_ACK:")
err = sendMessage(r, messageTypeTelemetryAck, ta)
if err != nil {
// TODO retry sending the ACK
log.Fatal(err)
}
default:
log.Printf("Unexpected message type: %s", getMessageType(msgType))
}
}
}
func currentTimestamp() roverTimestamp {
now := time.Now().Local()
t := roverTimestamp{
year: byte(now.Year() - 2000),
month: byte(now.Month()),
day: byte(now.Day()),
hour: byte(now.Hour()),
minute: byte(now.Minute()),
second: byte(now.Second()),
}
return t
}
// TODO
// func setup_display() {
// }