Fly baby, fly

Author: felixge

Diff for: cmd/godrone/main.go

@@ -1,27 +1,122 @@
 package main
 
 import (
-	"github.com/felixge/godrone"
+	"github.com/felixge/godrone/attitude"
+	"github.com/felixge/godrone/control"
+	"github.com/felixge/godrone/drivers/motorboard"
+	"github.com/felixge/godrone/drivers/navboard"
 	"github.com/felixge/godrone/http"
+	"github.com/felixge/log"
 	gohttp "net/http"
 	"os"
 	"os/signal"
 	"syscall"
+	"time"
 )
 
+type Config struct {
+	NavboardTTY   string
+	MotorboardTTY string
+	RollPID       [3]float64
+	PitchPID      [3]float64
+	YawPID        [3]float64
+	HttpAddr      string
+}
+
+var (
+	defaultRollPitchPID = [3]float64{0.02, 0, 0}
+
+	green  = motorboard.Leds(motorboard.LedGreen)
+	orange = motorboard.Leds(motorboard.LedOrange)
+	red    = motorboard.Leds(motorboard.LedRed)
+)
+
+var DefaultConfig = Config{
+	NavboardTTY:   "/dev/ttyO1",
+	MotorboardTTY: "/dev/ttyO0",
+	RollPID:       defaultRollPitchPID,
+	PitchPID:      defaultRollPitchPID,
+	YawPID:        [3]float64{1, 0, 0},
+	HttpAddr:      ":80",
+}
+
+type Instances struct {
+	log        *log.Logger
+	navboard   *navboard.Navboard
+	motorboard *motorboard.Motorboard
+	attitude   *attitude.Complementary
+	control    *control.Control
+	http       gohttp.Handler
+}
+
 func main() {
-	log := godrone.DefaultConfig.Log
-	firmware := godrone.DefaultFirmware
-	if err := firmware.Start(); err != nil {
+	config := DefaultConfig
+	i, err := NewInstances(config)
+	if err != nil {
 		panic(err)
 	}
-	defer firmware.Stop()
+	i.log.Info("Starting godrone")
+	defer i.motorboard.Close()
 
-	h := http.NewHandler(firmware, log)
-	go gohttp.ListenAndServe(":80", h)
+	i.motorboard.SetLeds(green)
+	time.Sleep(500 * time.Millisecond)
+	i.motorboard.SetLeds(red)
+
+	i.log.Info("Calibrating sensors")
+	for {
+		if err := i.navboard.Calibrate(); err == nil {
+			break
+		}
+	}
+	i.motorboard.SetLeds(green)
+
+	navDataCh := make(chan navboard.Data)
+	go readNavData(i.navboard, navDataCh)
 
 	sigCh := make(chan os.Signal, 1)
 	signal.Notify(sigCh, syscall.SIGINT)
-	sig := <-sigCh
-	log.Info("Received signal=%s, shutting down", sig)
+
+	go gohttp.ListenAndServe(config.HttpAddr, i.http)
+
+	i.log.Info("Entering main loop")
+mainLoop:
+	for {
+		select {
+		case navData := <-navDataCh:
+			attitudeData := i.attitude.Update(navData.Data)
+			motorSpeeds := i.control.Update(attitudeData)
+			if err := i.motorboard.SetSpeeds(motorSpeeds); err != nil {
+				i.log.Error("Could not set motor speeds. err=%s", err)
+			}
+		case sig := <-sigCh:
+			i.log.Info("Received signal=%s, shutting down", sig)
+			break mainLoop
+		}
+	}
+}
+
+func readNavData(board *navboard.Navboard, ch chan<- navboard.Data) {
+	for {
+		navData, err := board.NextData()
+		if err != nil {
+			continue
+		}
+		select {
+		case ch <- navData:
+		default:
+		}
+	}
+}
+
+func NewInstances(c Config) (i Instances, err error) {
+	i.log = log.DefaultLogger
+	i.navboard = navboard.NewNavboard(c.NavboardTTY, i.log)
+	i.motorboard, err = motorboard.NewMotorboard(c.MotorboardTTY)
+	if err != nil {
+		return
+	}
+	i.attitude = attitude.NewComplementary()
+	i.control = control.NewControl(c.RollPID, c.PitchPID, c.YawPID)
+	i.http = http.NewHandler(i.control, i.log)
+	return
 }

Diff for: control/control.go

@@ -0,0 +1,62 @@
+package control
+
+import (
+	"github.com/felixge/godrone/attitude"
+	"github.com/felixge/pidctrl"
+	"math"
+	"sync"
+)
+
+const (
+	throttleMax = 0.85
+	adjMax      = 1 - throttleMax
+)
+
+func NewControl(rollPID, pitchPID, yawPID [3]float64) *Control {
+	return &Control{
+		roll:  pidctrl.NewPIDController(rollPID[0], rollPID[1], rollPID[2]),
+		pitch: pidctrl.NewPIDController(pitchPID[0], pitchPID[1], pitchPID[2]),
+		yaw:   pidctrl.NewPIDController(yawPID[0], yawPID[1], yawPID[2]),
+	}
+}
+
+type Control struct {
+	l                sync.Mutex
+	throttle         float64
+	roll, pitch, yaw *pidctrl.PIDController
+}
+
+func (c *Control) Set(s attitude.Data, throttle float64) {
+	c.l.Lock()
+	defer c.l.Unlock()
+
+	c.roll.Set(s.Roll)
+	c.pitch.Set(s.Pitch)
+	c.yaw.Set(s.Yaw)
+	c.throttle = throttle
+}
+
+func (c *Control) Update(a attitude.Data) (speeds [4]float64) {
+	c.l.Lock()
+	defer c.l.Unlock()
+
+	var (
+		adjRoll  = c.roll.Update(a.Roll)
+		adjPitch = c.pitch.Update(a.Pitch)
+		//adjYaw   = c.yaw.Update(a.Yaw)
+		adjYaw   = 0.0
+		throttle = math.Min(c.throttle, 1) * throttleMax
+	)
+
+	if throttle <= 0 {
+		return
+	}
+
+	speeds = [4]float64{
+		throttle + math.Max(math.Min(+adjRoll+adjPitch+adjYaw, adjMax), -adjMax),
+		throttle + math.Max(math.Min(-adjRoll+adjPitch-adjYaw, adjMax), -adjMax),
+		throttle + math.Max(math.Min(-adjRoll-adjPitch+adjYaw, adjMax), -adjMax),
+		throttle + math.Max(math.Min(+adjRoll-adjPitch-adjYaw, adjMax), -adjMax),
+	}
+	return
+}

Diff for: defaults.go

@@ -0,0 +1,14 @@
+package motorboard
+
+type LedColor int
+
+const (
+	LedOff    LedColor = iota
+	LedRed             = 1
+	LedGreen           = 2
+	LedOrange          = 3
+)
+
+func Leds(c LedColor) [4]LedColor {
+	return [4]LedColor{c, c, c, c}
+}