navboard: cleanup + docs

2013-12-15 14:10:59 +01:00
commit a02370c682
@@ -4,12 +4,15 @@ import (
 	"github.com/felixge/godrone/imu"
 )

+// Data holds the navboard data after adjusting for sensitivity / bias.
 type Data struct {
 	imu.Data
 	Raw RawData
 }

-// From https://github.com/RoboticaTUDelft/paparazzi/blob/minor1/sw/airborne/boards/ardrone/navdata.h
+// RawNavdata holds the navboard data as read from the tty file.
+// Based on https://github.com/RoboticaTUDelft/paparazzi/blob/minor1/sw/airborne/boards/ardrone/navdata.h
+// but adjusted a little (some values seem to be signed rather than unsigned)
 type RawData struct {
 	Seq uint16

@@ -23,7 +26,6 @@ type RawData struct {
 	Gy int16
 	Gz int16

-	// Everything below has not been confirmed to be correct yet
 	TemperatureAcc  uint16
 	TemperatureGyro uint16

@@ -55,6 +57,7 @@ type RawData struct {
 	Checksum uint16
 }

+// ImuData extracts the unadjusted imu.Data contained in the raw data.
 func (r RawData) ImuData() imu.Data {
 	return imu.Data{
 		Ax: float64(r.Ax),
@@ -8,17 +8,15 @@ import (
 	"os"
 )

-const (
-	DefaultTTY = "/dev/ttyO1"
-)
-
 // gyroGains are the measured gains for converting the gyroscope output into
 // deg/sec.
+// @TODO make these configurable
 //
 // from: /data/config.ini on drone
 // gyros_gains                    = { 1.0569232e-03 -1.0664322e-03 -1.0732636e-03 }
 var gyroGains = [3]float64{16, -16, -16}

+// NewNavboard returns a new Navboard reading from the given tty file.
 func NewNavboard(tty string, log log.Interface) *Navboard {
 	return &Navboard{
 		tty: tty,
@@ -26,6 +24,8 @@ func NewNavboard(tty string, log log.Interface) *Navboard {
 	}
 }

+// Navboard implements a driver for the navboard. It is meant to be used from
+// within a single goroutine.
 type Navboard struct {
 	reader      *reader
 	writer      *writer
@@ -35,7 +35,15 @@ type Navboard struct {
 	calibration calibration
 }

-// @TODO Turn into ReadData, taking a pointer
+// NextData reads and returns the next data frame from the tty file, or
+// returns an error if there was a problem. All errors should be considered
+// transient.
+//
+// @TODO Turn into ReadData, taking a pointer to avoid allocating too much?
+// @TODO We often get a few errors here before things start humming along, I
+// suspect we need a better way to drain the tty buffer initally, but my first
+// attempt at doing that using O_NONBLOCK failed. For now this is not an issue
+// as the issue sorts itself out after a few calls to NextData.
 func (n *Navboard) NextData() (data Data, err error) {
 	defer func() {
 		if err != nil {
@@ -67,6 +75,8 @@ type calibration struct {
 	Gains   imu.Data
 }

+// Calibrate tries to determine the bias and sensitivity of the sensors. It
+// expects that the drone is placed flat on the ground.
 func (n *Navboard) Calibrate() error {
 	var (
 		samples                      = make([]imu.Floats, 0, 40)
@@ -131,7 +141,6 @@ func (n *Navboard) Calibrate() error {
 	g[imu.Ax], g[imu.Ay], g[imu.Az] = ag, ag, -ag
 	o[imu.Az] -= ag

-	// @TODO Figure out gains for gyroscopes
 	g[imu.Gx], g[imu.Gy], g[imu.Gz] = gyroGains[0], gyroGains[1], gyroGains[2]

 	n.calibration.Offsets = imu.NewData(o)
@@ -160,14 +169,15 @@ func (n *Navboard) open() (err error) {
 	}
 	n.writer = newWriter(n.file)
 	n.reader = newReader(n.file)
-	n.log.Debug("Writing start command")
-	if err = n.writer.WriteCommand(start); err != nil {
+	n.log.Debug("Writing command to start aquisition")
+	if err = n.writer.WriteCommand(startAcq); err != nil {
 		return
 	}
 	n.log.Debug("Opened tty=%s", n.tty)
 	return
 }

+// Close closes the navboard tty file.
 func (n *Navboard) Close() (err error) {
 	n.log.Debug("Closing tty=%s", n.tty)
 	if n.file != nil {
@@ -1,32 +1,50 @@
 package navboard

 import (
-	"fmt"
 	"io"
 )

 type command byte

+// Navboard control commands. Found in Parrot SDK:
+// https://projects.ardrone.org/embedded/ardrone-api/d1/d4d/ardrone__common__config_8h.html#9fc16d3fe3d71ffea3841efded3132ee
+// https://projects.ardrone.org/embedded/ardrone-api/d3/d19/ardrone__common__config_8h-source.html
 const (
-	start command = 1 << iota
-	stop
-	resync
+	// command to start acquisition with ADC
+	startAcq command = 1
+	// command to stop acquisition with ADC
+	stopAcq = 2
+	// command to resync acquisition with ADC
+	resync = 3
+	// command to ADC send a test frame (123456)
+	test = 4
+	// command to ADC send his number : version (MSB) subversion (LSB)
+	version = 5
+	// set the ultrasound at 22,22Hz
+	selectUltrasound22hz = 7
+	// set the ultrasound at 25Hz
+	selectUltrasound25hz = 8
+	// command to ADC to send the calibration
+	sendCalibre = 13
+	// command to ADC to receved a new calibration
+	recevedCalibre = 14
+	// get the hard version of the navboard
+	getHardVersion = 15
+	// enabled the separation of sources ultrasound
+	activeSeparation = 16
+	// disables the ultrasound source separation
+	stopSeparation = 17
+	// command to ADC to receved the prod data
+	recevedProd = 18
+	// command to ADC to send the prod data
+	sendProd = 19
+	// command to ADC to send PWM ultrasond in continue
+	activeEtalonage = 20
+	// command to ADC to stop send PWM ultrasond in continue
+	activeUltrason     = 21
+	activeTestUltrason = 22
 )

-var commands = map[command]string{
-	start:  "Start",
-	stop:   "Stop",
-	resync: "Resync",
-}
-
-func (c command) String() string {
-	if s, ok := commands[c]; ok {
-		return s
-	} else {
-		return fmt.Sprintf("unkown cmd: %d", c)
-	}
-}
-
 func (c command) Bytes() []byte {
 	return []byte{byte(c)}
 }
@@ -39,6 +57,7 @@ type writer struct {
 	w io.Writer
 }

+// WriteCommand writes the given cmd.
 func (w *writer) WriteCommand(cmd command) (err error) {
 	_, err = w.w.Write(cmd.Bytes())
 	return