154 lines
3.5 KiB
Go
154 lines
3.5 KiB
Go
package main
|
|
|
|
import (
|
|
"fmt"
|
|
"github.com/blackjack/webcam"
|
|
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 handleWebcam(client mqtt.Client) {
|
|
cam, err := webcam.Open("/dev/video0")
|
|
if err != nil {
|
|
log.Fatalf("Could not open webcam: %v\n", err)
|
|
}
|
|
defer cam.Close()
|
|
|
|
supportedFormat := cam.GetSupportedFormats()
|
|
//log.Printf("Supported formats:\n")
|
|
imageFormat := webcam.PixelFormat(0)
|
|
for format, description := range supportedFormat {
|
|
if description == "10-bit Bayer GBGB/RGRG" {
|
|
imageFormat = format
|
|
}
|
|
}
|
|
|
|
//width, height := 1296, 972
|
|
//width, height := uint32(2592), uint32(1944)
|
|
width, height := uint32(640), uint32(480)
|
|
|
|
if imageFormat != webcam.PixelFormat(0) {
|
|
_, _, _, err = cam.SetImageFormat(imageFormat, width, height)
|
|
if err != nil {
|
|
log.Fatalf("Could not set image format: %v\n", err)
|
|
}
|
|
}
|
|
|
|
supportedFramerates := cam.GetSupportedFramerates(imageFormat, width, height)
|
|
log.Printf("Supported frame rates:\n")
|
|
for _, format := range supportedFramerates {
|
|
log.Printf(" - %s\n", format)
|
|
}
|
|
|
|
controls := cam.GetControls()
|
|
log.Printf("Controls:\n")
|
|
for _, control := range controls {
|
|
log.Printf(" - %s\n", control)
|
|
}
|
|
|
|
//cam.SetAutoWhiteBalance(true)
|
|
//cam.SetControl()
|
|
|
|
//err = cam.SetFramerate(10)
|
|
//if err != nil {
|
|
// log.Fatalf("Could not set framerate: %v\n", err)
|
|
//}
|
|
|
|
err = cam.StartStreaming()
|
|
if err != nil {
|
|
log.Fatalf("Could not start streaming webcam: %v\n", err)
|
|
}
|
|
|
|
count := 0
|
|
for {
|
|
err = cam.WaitForFrame(100)
|
|
if err != nil {
|
|
log.Fatalf("Could not start streaming webcam: %v\n", err)
|
|
}
|
|
frame, err := cam.ReadFrame()
|
|
if err != nil {
|
|
log.Fatalf("Could not read frame: %v\n", err)
|
|
}
|
|
if len(frame) > 0 {
|
|
count++
|
|
if count > 30*4 {
|
|
print("Publishing frame\n")
|
|
publishTelemetry(client, "camfeed", frame)
|
|
return
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
func main() {
|
|
opts := mqtt.NewClientOptions()
|
|
opts.AddBroker(broker)
|
|
opts.SetClientID("spider-host-client")
|
|
client := mqtt.NewClient(opts)
|
|
|
|
token := client.Connect()
|
|
token.Wait()
|
|
if token.Error() != nil {
|
|
log.Fatalf("Error connecting to MQTT broker: %v\n", token.Error())
|
|
}
|
|
|
|
log.Print("Subscribing to command topics")
|
|
token = client.Subscribe("spider/command/ping", 0, onPing)
|
|
token.Wait()
|
|
if token.Error() != nil {
|
|
log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
|
|
}
|
|
|
|
InitBattery()
|
|
//go handleWebcam(client)
|
|
|
|
slowTelemetry := time.NewTicker(3 * time.Second)
|
|
defer slowTelemetry.Stop()
|
|
|
|
publishSlowTelemetry(client)
|
|
for {
|
|
select {
|
|
case <-slowTelemetry.C:
|
|
publishSlowTelemetry(client)
|
|
}
|
|
}
|
|
}
|