122 lines
2.9 KiB
Go
122 lines
2.9 KiB
Go
package main
|
|
|
|
import (
|
|
"encoding/json"
|
|
"fmt"
|
|
mqtt "github.com/eclipse/paho.mqtt.golang"
|
|
"log"
|
|
"time"
|
|
)
|
|
|
|
const broker = "tcp://mqtt.seeseepuff.be:1883"
|
|
const telemetryTopic = "spider/telemetry/"
|
|
|
|
func publishTelemetry(client mqtt.Client, topic string, payload any) {
|
|
token := client.Publish(telemetryTopic+topic, 0, false, payload)
|
|
token.Wait()
|
|
if token.Error() != nil {
|
|
log.Printf("Error publishing servo battery state: %v\n", token.Error())
|
|
}
|
|
}
|
|
|
|
func publishServoBatteryTelemetry(client mqtt.Client) {
|
|
value, err := GetBatteryServo()
|
|
if err != nil {
|
|
return
|
|
}
|
|
publishTelemetry(client, "servo_battery", fmt.Sprintf("%f", value))
|
|
}
|
|
|
|
func publishCpuBatteryTelemetry(client mqtt.Client) {
|
|
value, err := GetBatteryCPU()
|
|
if err != nil {
|
|
return
|
|
}
|
|
publishTelemetry(client, "cpu_battery", fmt.Sprintf("%f", value))
|
|
}
|
|
|
|
func publishSlowTelemetry(client mqtt.Client) {
|
|
go publishServoBatteryTelemetry(client)
|
|
go publishCpuBatteryTelemetry(client)
|
|
}
|
|
|
|
func onPing(client mqtt.Client, msg mqtt.Message) {
|
|
log.Print("Got ping")
|
|
publishTelemetry(client, "pong", msg.Payload())
|
|
}
|
|
|
|
func onSetCameraXY(client mqtt.Client, msg mqtt.Message) {
|
|
log.Print("Got move camera")
|
|
payload := make(map[string]float64)
|
|
err := json.Unmarshal(msg.Payload(), &payload)
|
|
if err != nil {
|
|
log.Printf("Error unmarshalling set_camera_xy payload: %v\n", err)
|
|
return
|
|
}
|
|
x, ok := payload["x"]
|
|
if !ok {
|
|
log.Printf("Missing x in set_camera_xy")
|
|
return
|
|
}
|
|
y, ok := payload["y"]
|
|
if !ok {
|
|
log.Printf("Missing y in set_camera_xy")
|
|
return
|
|
}
|
|
SetServoAngle(ServoHeadHorizontal, x)
|
|
SetServoAngle(ServoHeadVertical, y)
|
|
}
|
|
|
|
func subscribe(client mqtt.Client, topic string, handler mqtt.MessageHandler) {
|
|
token := client.Subscribe(topic, 0, handler)
|
|
token.Wait()
|
|
if token.Error() != nil {
|
|
log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
|
|
}
|
|
}
|
|
|
|
func onConnect(client mqtt.Client) {
|
|
log.Print("Subscribing to command topics")
|
|
subscribe(client, "spider/command/ping", onPing)
|
|
subscribe(client, "spider/command/set_camera_xy", onSetCameraXY)
|
|
}
|
|
|
|
func main() {
|
|
opts := mqtt.NewClientOptions()
|
|
opts.AddBroker(broker)
|
|
opts.SetClientID("spider-host-client")
|
|
opts.SetResumeSubs(true)
|
|
opts.SetOnConnectHandler(onConnect)
|
|
client := mqtt.NewClient(opts)
|
|
|
|
token := client.Connect()
|
|
token.Wait()
|
|
if token.Error() != nil {
|
|
log.Fatalf("Error connecting to MQTT broker: %v\n", token.Error())
|
|
}
|
|
|
|
InitBattery()
|
|
InitServo()
|
|
|
|
ServosOff()
|
|
|
|
slowTelemetry := time.NewTicker(3 * time.Second)
|
|
defer slowTelemetry.Stop()
|
|
|
|
//moveServo := time.NewTicker(100 * time.Millisecond)
|
|
//defer moveServo.Stop()
|
|
|
|
publishSlowTelemetry(client)
|
|
for {
|
|
select {
|
|
case <-slowTelemetry.C:
|
|
publishSlowTelemetry(client)
|
|
//case <-moveServo.C:
|
|
// seconds := time.Now().UnixMilli()
|
|
// angle := (math.Cos(float64(seconds)/2_500) + 1) * 0.5
|
|
// log.Printf("Target angle: %.1f\n", angle)
|
|
// SetServoAngle(ServoHeadHorizontal, angle)
|
|
}
|
|
}
|
|
}
|