Ejemplo n.º 1
0
func HandleWriteConnections(cList *ConnectionList, readQueue *lane.Queue) {
	for {
		data := readQueue.Dequeue()
		if data != nil {
			bytes := data.([]byte)
			cList.Broadcast(bytes)
		}
	}
}
Ejemplo n.º 2
0
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)
			}
		}
	*/
}