func handleNav(f *godrone.Firmware, addr *net.TCPAddr) {
log.Println("Remote ", addr.String())
remoteAddr := &net.TCPAddr{addr.IP, 7777, ""}
conn, err := net.DialTCP("tcp", nil, remoteAddr)
log.Println("Dialed")
if err != nil {
log.Println(err)
return
}
encoder := gob.NewEncoder(conn)
for {
f.Observe()
encoder.Encode(f.Actual)
//time.Sleep(time.Millisecond * 1000)
}
}
func main() {
f, err := os.OpenFile("/var/log/firmware", os.O_CREATE|os.O_RDWR|os.O_TRUNC, 0666)
if err != nil {
panic(err)
}
defer f.Close()
log.SetOutput(f)
cutoutReason := "not calibrated"
flag.Parse()
log.SetFlags(log.Ldate | log.Ltime | log.Lmicroseconds)
log.Printf("Godrone started")
var firmware *godrone.Firmware
if *dummy {
firmware, _ = godrone.NewCustomFirmware(&mockNavboard{}, &mockMotorboard{})
} else {
var err error
firmware, err = godrone.NewFirmware()
if err != nil {
log.Fatalf("%s", err)
}
defer firmware.Close()
}
// This is the channel to send requests to the main control loop.
// It is the only way to send and receive info the the
// godrone.Firmware object (which is non-concurrent).
//reqCh := make(chan Request)
motorCh := make(chan *godrone.MotorPWM)
// Autonomy/guard goroutines.
//go monitorAngles(reqCh)
//go lander(reqCh)
// The websocket input goroutine.
//go serveHttp(reqCh)
calibrate := func() {
for {
// @TODO The LEDs don't seem to turn off when this is called again after
// a calibration errors, instead they just blink. Not sure why.
firmware.Motorboard.WriteLeds(godrone.Leds(godrone.LedOff))
log.Printf("Calibrating sensors")
err := firmware.Calibrate()
if err != nil {
firmware.Motorboard.WriteLeds(godrone.Leds(godrone.LedRed))
time.Sleep(time.Second)
} else {
log.Printf("Finished calibration")
firmware.Motorboard.WriteLeds(godrone.Leds(godrone.LedGreen))
cutoutReason = ""
break
}
}
}
calibrate()
println("Success.")
log.Print("Up, up and away!")
var q *lane.Queue = lane.NewQueue()
q.Enqueue([4]float64{0.0, 0.0, 0.0, 0.0})
go handleExternal(motorCh, firmware)
for {
select {
case packet := <-motorCh:
if packet.Speed < godrone.MAX_MOTOR {
q.Enqueue([4]float64{
packet.Speed, packet.Speed, packet.Speed, packet.Speed})
q.Dequeue()
//log.Println(q.Head())
}
default:
}
time.Sleep(time.Millisecond * 10)
v := q.Head()
switch v := v.(type) {
case [4]float64:
if err := firmware.Motorboard.WriteSpeeds(v); err != nil {
log.Printf("Failed to write speeds: %s", err)
}
default:
log.Println("ERROR")
}
}
//go rotate(queue, motorCh)
// This is the main control loop.
/*
flying := false
for {
select {
case req := <-reqCh:
var res Response
res.Cutout = cutoutReason
res.Actual = firmware.Actual
res.Desired = firmware.Desired
res.Time = time.Now()
if req.SetDesired != nil {
// Log changes to desired
if Verbose() && firmware.Desired != *req.SetDesired {
log.Print("New desired attitude:", firmware.Desired)
}
firmware.Desired = *req.SetDesired
}
if req.Cutout != "" {
cutoutReason = req.Cutout
log.Print("Cutout: ", cutoutReason)
firmware.Desired.Altitude = 0
}
if req.Calibrate {
calibrate()
}
req.response <- res
if reallyVerbose() {
log.Print("Request: ", req, "Response: ", res)
}
default:
}
var err error
if firmware.Desired.Altitude > 0 {
err = firmware.Control()
flying = true
} else {
// Something subtle to note here: When the motors are running,
// but then desired altitude goes to zero (for instance due to
// the emergency command in the UI) we end up here.
//
// We never actually send a motors=0 command. Instead we count on
// the failsafe behavior of the motorboard, which puts the motors to
// zero if it does not receive a new motor command soon enough.
err = firmware.Observe()
if flying {
log.Print("Motor cutoff.")
flying = false
}
}
if err != nil {
log.Printf("%s", err)
}
}
*/
}