Merge remote-tracking branch 'krylovsk/master' into sphero_enhancements

Conflicts:
	platforms/sphero/sphero_driver.go

platforms/sphero/sphero_driver.go

@@ -11,6 +11,14 @@ import (
 
 var _ gobot.Driver = (*SpheroDriver)(nil)
 
+const (
+	SensorFrequencyDefault = 10
+	SensorFrequencyMax     = 420
+	ChannelSensordata      = "sensordata"
+	ChannelCollisions      = "collision"
+	Error                  = "error"
+)
+
 type packet struct {
 	header   []uint8
 	body     []uint8
@@ -30,19 +38,6 @@ type SpheroDriver struct {
 	gobot.Commander
 }
 
-type Collision struct {
-	// Normalized impact components (direction of the collision event):
-	X, Y, Z int16
-	// Thresholds exceeded by X (1h) and/or Y (2h) axis (bitmask):
-	Axis byte
-	// Power that cross threshold Xt + Xs:
-	XMagnitude, YMagnitude int16
-	// Sphero's speed when impact detected:
-	Speed uint8
-	// Millisecond timer
-	Timestamp uint32
-}
-
 // NewSpheroDriver returns a new SpheroDriver given a SpheroAdaptor and name.
 //
 // Adds the following API Commands:
@@ -52,6 +47,7 @@ type Collision struct {
 // 	"SetBackLED" - See SpheroDriver.SetBackLED
 // 	"SetHeading" - See SpheroDriver.SetHeading
 // 	"SetStabilization" - See SpheroDriver.SetStabilization
+//  "SetDataStreaming" - See SpheroDriver.SetDataStreaming
 func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 	s := &SpheroDriver{
 		name:            name,
@@ -62,8 +58,9 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 		responseChannel: make(chan []uint8, 1024),
 	}
 
-	s.AddEvent("error")
-	s.AddEvent("collision")
+	s.AddEvent(Error)
+	s.AddEvent(ChannelCollisions)
+	s.AddEvent(ChannelSensordata)
 	s.AddCommand("SetRGB", func(params map[string]interface{}) interface{} {
 		r := uint8(params["r"].(float64))
 		g := uint8(params["g"].(float64))
@@ -99,12 +96,24 @@ func NewSpheroDriver(a *SpheroAdaptor, name string) *SpheroDriver {
 		s.SetHeading(heading)
 		return nil
 	})
+
 	s.AddCommand("SetStabilization", func(params map[string]interface{}) interface{} {
 		on := params["enable"].(bool)
 		s.SetStabilization(on)
 		return nil
 	})
 
+	s.AddCommand("SetDataStreaming", func(params map[string]interface{}) interface{} {
+		freq := params["freq"].(uint16)
+		frames := params["frames"].(uint16)
+		mask := params["mask"].(uint32)
+		count := params["count"].(uint8)
+		mask2 := params["mask2"].(uint32)
+
+		s.SetDataStreaming(freq, frames, mask, count, mask2)
+		return nil
+	})
+
 	return s
 }
 
@@ -126,7 +135,7 @@ func (s *SpheroDriver) Start() (errs []error) {
 			packet := <-s.packetChannel
 			err := s.write(packet)
 			if err != nil {
-				gobot.Publish(s.Event("error"), err)
+				gobot.Publish(s.Event(Error), err)
 			}
 		}
 	}()
@@ -165,6 +174,8 @@ func (s *SpheroDriver) Start() (errs []error) {
 				evt, s.asyncResponse = s.asyncResponse[len(s.asyncResponse)-1], s.asyncResponse[:len(s.asyncResponse)-1]
 				if evt[2] == 0x07 {
 					s.handleCollisionDetected(evt)
+				} else if evt[2] == 0x03 {
+					s.handleDataStreaming(evt)
 				}
 			}
 			time.Sleep(100 * time.Millisecond)
@@ -227,8 +238,37 @@ func (s *SpheroDriver) Roll(speed uint8, heading uint16) {
 	s.packetChannel <- s.craftPacket([]uint8{speed, uint8(heading >> 8), uint8(heading & 0xFF), 0x01}, 0x02, 0x30)
 }
 
-// Stop sets the Sphero to a roll speed of 0
+// Enables sensor data streaming
+func (s *SpheroDriver) SetDataStreaming(freq uint16, frames uint16, mask uint32, count uint8, mask2 uint32) {
+	var n uint16
+
+	if freq == 0 || freq >= SensorFrequencyMax {
+		fmt.Printf("Invalid data streaming frequency. Setting to default (%v Hz)\n", SensorFrequencyDefault)
+		n = SensorFrequencyMax / SensorFrequencyDefault
+	} else {
+		n = SensorFrequencyMax / freq
+	}
+	if frames == 0 {
+		frames = 1
+	}
+
+	cmd := DataStreamingSetting{
+		N:     n,
+		M:     frames,
+		Pcnt:  count,
+		Mask:  mask,
+		Mask2: mask2,
+	}
+
+	buf := new(bytes.Buffer)
+	binary.Write(buf, binary.BigEndian, cmd)
+
+	s.packetChannel <- s.craftPacket(buf.Bytes(), 0x02, 0x11)
+}
+
+// Stop sets the Sphero to a roll speed of 0 and disables data streaming
 func (s *SpheroDriver) Stop() {
+	s.SetDataStreaming(SensorFrequencyDefault, 0, 0, 0, 0)
 	s.Roll(0, 0)
 }
 
@@ -245,10 +285,21 @@ func (s *SpheroDriver) handleCollisionDetected(data []uint8) {
 	if len(data) != 22 || data[4] != 17 {
 		return
 	}
-	var collision Collision
+	var collision CollisionPacket
 	buffer := bytes.NewBuffer(data[5:]) // skip header
 	binary.Read(buffer, binary.BigEndian, &collision)
-	gobot.Publish(s.Event("collision"), collision)
+	gobot.Publish(s.Event(ChannelCollisions), collision)
+}
+
+func (s *SpheroDriver) handleDataStreaming(data []uint8) {
+	// ensure data is the right length:
+	if len(data) != 90 {
+		return
+	}
+	var dataPacket DataStreamingPacket
+	buffer := bytes.NewBuffer(data[5:]) // skip header
+	binary.Read(buffer, binary.BigEndian, &dataPacket)
+	gobot.Publish(s.Event(ChannelSensordata), dataPacket)
 }
 
 func (s *SpheroDriver) getSyncResponse(packet *packet) []byte {

platforms/sphero/sphero_packets.go

@@ -0,0 +1,112 @@
+package sphero
+
+type CollisionPacket struct {
+	// Normalized impact components (direction of the collision event):
+	X, Y, Z int16
+	// Thresholds exceeded by X (1h) and/or Y (2h) axis (bitmask):
+	Axis byte
+	// Power that cross threshold Xt + Xs:
+	XMagnitude, YMagnitude int16
+	// Sphero's speed when impact detected:
+	Speed uint8
+	// Millisecond timer
+	Timestamp uint32
+}
+
+type DataStreamingSetting struct {
+	// Divisor of the maximum sensor sampling rate
+	N uint16
+	// Number of sample frames emitted per packet
+	M uint16
+	// Bitwise selector of data sources to stream
+	Mask uint32
+	// Packet count 1-255 (or 0 for unlimited streaming)
+	Pcnt uint8
+	// Bitwise selector of more data sources to stream (optional)
+	Mask2 uint32
+}
+
+type DataStreamingPacket struct {
+	// 8000 0000h	accelerometer axis X, raw	-2048 to 2047	4mG
+	RawAccX int16
+	// 4000 0000h	accelerometer axis Y, raw	-2048 to 2047	4mG
+	RawAccY int16
+	// 2000 0000h	accelerometer axis Z, raw	-2048 to 2047	4mG
+	RawAccZ int16
+	// 1000 0000h	gyro axis X, raw	-32768 to 32767	0.068 degrees
+	RawGyroX int16
+	// 0800 0000h	gyro axis Y, raw	-32768 to 32767	0.068 degrees
+	RawGyroY int16
+	// 0400 0000h	gyro axis Z, raw	-32768 to 32767	0.068 degrees
+	RawGyroZ int16
+	// 0200 0000h	Reserved
+	Rsrv1 int16
+	// 0100 0000h	Reserved
+	Rsrv2 int16
+	// 0080 0000h	Reserved
+	Rsrv3 int16
+	// 0040 0000h	right motor back EMF, raw	-32768 to 32767	22.5 cm
+	RawRMotorBack int16
+	// 0020 0000h	left motor back EMF, raw	-32768 to 32767	22.5 cm
+	RawLMotorBack int16
+	// 0010 0000h	left motor, PWM, raw	-2048 to 2047	duty cycle
+	RawLMotor int16
+	// 0008 0000h	right motor, PWM raw	-2048 to 2047	duty cycle
+	RawRMotor int16
+	// 0004 0000h	IMU pitch angle, filtered	-179 to 180	degrees
+	FiltPitch int16
+	// 0002 0000h	IMU roll angle, filtered	-179 to 180	degrees
+	FiltRoll int16
+	// 0001 0000h	IMU yaw angle, filtered	-179 to 180	degrees
+	FiltYaw int16
+	// 0000 8000h	accelerometer axis X, filtered	-32768 to 32767	1/4096 G
+	FiltAccX int16
+	// 0000 4000h	accelerometer axis Y, filtered	-32768 to 32767	1/4096 G
+	FiltAccY int16
+	// 0000 2000h	accelerometer axis Z, filtered	-32768 to 32767	1/4096 G
+	FiltAccZ int16
+	// 0000 1000h	gyro axis X, filtered	-20000 to 20000	0.1 dps
+	FiltGyroX int16
+	// 0000 0800h	gyro axis Y, filtered	-20000 to 20000	0.1 dps
+	FiltGyroY int16
+	// 0000 0400h	gyro axis Z, filtered	-20000 to 20000	0.1 dps
+	FiltGyroZ int16
+	// 0000 0200h	Reserved
+	Rsrv4 int16
+	// 0000 0100h	Reserved
+	Rsrv5 int16
+	// 0000 0080h	Reserved
+	Rsrv6 int16
+	// 0000 0040h	right motor back EMF, filtered	-32768 to 32767	22.5 cm
+	FiltRMotorBack int16
+	// 0000 0020h	left motor back EMF, filtered	-32768 to 32767	22.5 cm
+	FiltLMotorBack int16
+	// 0000 0010h	Reserved 1
+	Rsrv7 int16
+	// 0000 0008h	Reserved 2
+	Rsrv8 int16
+	// 0000 0004h	Reserved 3
+	Rsrv9 int16
+	// 0000 0002h	Reserved 4
+	Rsrv10 int16
+	// 0000 0001h	Reserved 5
+	Rsrv11 int16
+	// 8000 0000h	Quaternion Q0	-10000 to 10000	1/10000 Q
+	Quat0 int16
+	// 4000 0000h	Quaternion Q1	-10000 to 10000	1/10000 Q
+	Quat1 int16
+	// 2000 0000h	Quaternion Q2	-10000 to 10000	1/10000 Q
+	Quat2 int16
+	// 1000 0000h	Quaternion Q3	-10000 to 10000	1/10000 Q
+	Quat3 int16
+	// 0800 0000h	Odometer X	-32768 to 32767	cm
+	OdomX int16
+	// 0400 0000h	Odometer Y	-32768 to 32767	cm
+	OdomY int16
+	// 0200 0000h	AccelOne	0 to 8000	1 mG
+	AccelOne int16
+	// 0100 0000h	Velocity X	-32768 to 32767	mm/s
+	VeloX int16
+	// 0080 0000h	Velocity Y	-32768 to 32767	mm/s
+	VeloY int16
+}