func HandleReadConnection(conn net.Conn, readQueue *lane.Queue, writePort int, mapOperation SplitterMap) {
buffConn := bufio.NewReaderSize(conn, 1024)
buffer := make([]byte, 1024)
for {
logrus.Debug("Begining Read")
bytes, err := buffConn.Read(buffer)
// Output the content of the bytes to the queue
if bytes > 0 {
readQueue.Enqueue(buffer[:bytes])
bytesSeen += bytes
}
if bytes == 0 {
if err.Error() == "EOF" {
logrus.Error("End of individual transmission")
buffConn = nil
conn.Close()
return
}
}
if err != nil {
logrus.Error("Underlying network failure?")
logrus.Error(err)
break
}
}
}
func HandleWriteConnections(cList *ConnectionList, readQueue *lane.Queue) {
for {
data := readQueue.Dequeue()
if data != nil {
bytes := data.([]byte)
cList.Broadcast(bytes)
}
}
}
func MonitorServer(queue *lane.Queue) {
scheduler := time.NewTicker(5 * time.Second)
go func(queue *lane.Queue) {
for {
select {
case <-scheduler.C:
logrus.Infof("BYTES SEEN: %d", bytesSeen)
logrus.Infof("QUEUE SIZE: %d", queue.Size())
}
}
}(queue)
}
func callPeer(watchmetadata *Watchmedata, destination *utilities.OPData, queue *lane.Queue, verb string, wg *sync.WaitGroup) {
defer wg.Done()
target := "http://" + destination.Ovip + ":" + strconv.Itoa(destination.Announceport) + "/ovp"
url := target + "/" + verb
fmt.Println("Instructed as " + url)
e := struct {
Message string
Errors []struct {
Resource string
Field string
Code string
}
}{}
var j map[string]interface{}
s := napping.Session{}
resp, err := s.Post(url, watchmetadata, &j, &e)
if err != nil {
log.Printf("failed to " + verb + " on " + target + " because " + err.Error())
return
}
if resp.Status() != 200 {
log.Printf("failed to " + verb + " on " + target + " because " + strconv.Itoa(resp.Status()) + "..." + e.Message)
return
}
if verb == "withdraw" || verb == "update" {
source := watchmetadata.OPConfig
if j["Status"].(bool) {
queue.Enqueue(destination)
} else {
mesg := fmt.Sprintf("Not "+verb+" %+v from %+v", source, destination)
panic(mesg)
}
}
queue.Enqueue(*destination)
}
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)
}
}
*/
}
