Update lidar implementation

platforms/i2c/lidarlite_driver.go

@@ -37,27 +37,42 @@ func (h *LIDARLiteDriver) Start() (errs []error) {
 // Halt returns true if devices is halted successfully
 func (h *LIDARLiteDriver) Halt() (errs []error) { return }
 
-// Distance returns the current distance
+// Distance returns the current distance in cm
 func (h *LIDARLiteDriver) Distance() (distance int, err error) {
 	if err = h.connection.I2cWrite(lidarliteAddress, []byte{0x00, 0x04}); err != nil {
 		return
 	}
 	<-time.After(20 * time.Millisecond)
 
-	if err = h.connection.I2cWrite(lidarliteAddress, []byte{0x8f}); err != nil {
+	if err = h.connection.I2cWrite(lidarliteAddress, []byte{0x0F}); err != nil {
 		return
 	}
-	<-time.After(20 * time.Millisecond)
 
-	ret, err := h.connection.I2cRead(lidarliteAddress, 2)
+	upper, err := h.connection.I2cRead(lidarliteAddress, 1)
 	if err != nil {
 		return
 	}
-	if len(ret) == 2 {
-		distance = (int(ret[1]) + int(ret[0])*256)
+
+	if len(upper) != 1 {
+		err = ErrNotEnoughBytes
+		return
+	}
+
+	if err = h.connection.I2cWrite(lidarliteAddress, []byte{0x10}); err != nil {
 		return
-	} else {
+	}
+
+	lower, err := h.connection.I2cRead(lidarliteAddress, 1)
+	if err != nil {
+		return
+	}
+
+	if len(lower) != 1 {
 		err = ErrNotEnoughBytes
+		return
 	}
+
+	distance = ((int(upper[0]) & 0xff) << 8) | (int(lower[0]) & 0xff)
+
 	return
 }

platforms/i2c/lidarlite_driver_test.go

@@ -54,24 +54,31 @@ func TestLIDARLiteDriverHalt(t *testing.T) {
 }
 
 func TestLIDARLiteDriverDistance(t *testing.T) {
-	// when len(data) is 2
+	// when everything is happy
 	hmc, adaptor := initTestLIDARLiteDriverWithStubbedAdaptor()
 
+	first := true
 	adaptor.i2cReadImpl = func() ([]byte, error) {
-		return []byte{99, 1}, nil
+		if first {
+			first = false
+			return []byte{99}, nil
+		}
+		return []byte{1}, nil
 	}
 
-	distance, _ := hmc.Distance()
+	distance, err := hmc.Distance()
+
+	gobot.Assert(t, err, nil)
 	gobot.Assert(t, distance, int(25345))
 
-	// when len(data) is not 2
+	// when insufficient bytes have been read
 	hmc, adaptor = initTestLIDARLiteDriverWithStubbedAdaptor()
 
 	adaptor.i2cReadImpl = func() ([]byte, error) {
-		return []byte{99}, nil
+		return []byte{}, nil
 	}
 
-	distance, err := hmc.Distance()
+	distance, err = hmc.Distance()
 	gobot.Assert(t, distance, int(0))
 	gobot.Assert(t, err, ErrNotEnoughBytes)