diff --git a/attitude/data.go b/attitude/attitude.go similarity index 60% rename from attitude/data.go rename to attitude/attitude.go index 760c5bf..71a9c95 100644 --- a/attitude/data.go +++ b/attitude/attitude.go @@ -4,14 +4,16 @@ import ( "fmt" ) -type Data struct { +// Attitude describes the drones position relative to the ground. +type Attitude struct { Roll float64 // degree Pitch float64 // degree Yaw float64 // degree Altitude float64 // meters } -func (d Data) String() string { +// String returns a human readable version of the Attitude. +func (d Attitude) String() string { return fmt.Sprintf( "Roll: %+6.1f Pitch: %+6.1f Yaw: %+6.1f Altitude: %+6.1f", d.Roll, diff --git a/attitude/complementary.go b/attitude/complementary.go index b2a4022..c4004e9 100644 --- a/attitude/complementary.go +++ b/attitude/complementary.go @@ -6,43 +6,46 @@ import ( "time" ) +const rad2Deg = 180 / math.Pi + +// NewComplementary returns a new Complementary filter. func NewComplementary() *Complementary { return &Complementary{gGain: 0.98, aGain: 0.02} - //return &Complementary{gGain: 1, aGain: 0} } +// Implements a simple complementation filter. +// see http://www.pieter-jan.com/node/11 type Complementary struct { - data Data + state Attitude updated time.Time aGain float64 gGain float64 } -const rad2Deg = 180 / math.Pi - -func (c *Complementary) Update(d imu.Data) Data { +// Update processes the given imu.Data and returns the new Attitude. +func (c *Complementary) Update(d imu.Data) Attitude { var ( - a Data - now = time.Now() + accel Attitude + now = time.Now() ) - a.Pitch = angle(d.Ax, d.Az) - a.Roll = angle(d.Ay, d.Az) + accel.Pitch = angle(d.Ax, d.Az) + accel.Roll = angle(d.Ay, d.Az) if !c.updated.IsZero() { dt := now.Sub(c.updated).Seconds() - c.data.Roll += d.Gx * dt - c.data.Pitch += d.Gy * dt - c.data.Yaw += d.Gz * dt + c.state.Roll += d.Gx * dt + c.state.Pitch += d.Gy * dt + c.state.Yaw += d.Gz * dt } - c.data.Pitch = c.data.Pitch*c.gGain + a.Pitch*c.aGain - c.data.Roll = c.data.Roll*c.gGain + a.Roll*c.aGain - c.data.Altitude = d.UsAltitude + c.state.Pitch = c.state.Pitch*c.gGain + accel.Pitch*c.aGain + c.state.Roll = c.state.Roll*c.gGain + accel.Roll*c.aGain + c.state.Altitude = d.UsAltitude c.updated = now - return c.data + return c.state } func angle(a, b float64) float64 { diff --git a/attitude/doc.go b/attitude/doc.go new file mode 100644 index 0000000..800e059 --- /dev/null +++ b/attitude/doc.go @@ -0,0 +1,3 @@ +// Package attitude contains algorithm for determining the drones position +// relative to the ground. +package attitude diff --git a/control/control.go b/control/control.go index 70c69a9..d9226ed 100644 --- a/control/control.go +++ b/control/control.go @@ -30,7 +30,7 @@ type Control struct { throttle float64 } -func (c *Control) Set(s attitude.Data, throttle float64) { +func (c *Control) Set(s attitude.Attitude, throttle float64) { c.l.Lock() defer c.l.Unlock() @@ -40,7 +40,7 @@ func (c *Control) Set(s attitude.Data, throttle float64) { c.throttle = throttle } -func (c *Control) Update(a attitude.Data) (speeds [4]float64) { +func (c *Control) Update(a attitude.Attitude) (speeds [4]float64) { c.l.Lock() defer c.l.Unlock() diff --git a/http/handler.go b/http/handler.go index 329e04e..3d018cc 100644 --- a/http/handler.go +++ b/http/handler.go @@ -26,11 +26,11 @@ type Handler struct { type update struct { NavData navboard.Data - AttitudeData attitude.Data + AttitudeData attitude.Attitude } type setpoint struct { - attitude.Data + attitude.Attitude Throttle float64 } @@ -78,7 +78,7 @@ func (h *Handler) handleWebsocket(conn *websocket.Conn) { return } case s := <-setCh: - h.config.Control.Set(s.Data, s.Throttle) + h.config.Control.Set(s.Attitude, s.Throttle) case err := <-setErrCh: log.Warn("WebSocket error. err=%s ip=%s", err, ip) return @@ -94,7 +94,7 @@ func (h *Handler) ServeHTTP(w http.ResponseWriter, r *http.Request) { h.fileHandler.ServeHTTP(w, r) } -func (h *Handler) Update(n navboard.Data, a attitude.Data) { +func (h *Handler) Update(n navboard.Data, a attitude.Attitude) { h.pub(update{n, a}) }