Support for sweep firmware v1.1

Author: dcyoung

README.md

@@ -5,7 +5,7 @@ Arduino Library for Scanse Sweep LiDAR
 ### Arduino
 Currently the library has only been tested with an `Arduino Mega 2560`.
 ### Sweep Firmware
-Currently the library only supports sweep firmware `v1.0`. Firmware `v1.1` introduced a calibration routine and new commands that change the device's behavior. Firmware `v1.1` is significantly more robust for arduino applications, so support for `v1.1` will be coming soon.
+Currently the library only supports Sweep firmware `v1.1`, available [here](http://scanse.io/downloads). Earlier verisons (<=`v1.0`) did not support the communication protocol used by the latest firmware (`v1.1`) and will not work properly with this library.
 
 # Installation
 Copy the entire `Sweep/` folder to your `.../Arduino/libraries/` directory.
@@ -98,15 +98,17 @@ True if the device is currently scanning.
 ```c++
 bool startScanning()
 ```
-Signals the sweep device to start scanning. Initiates an indefinite stream of individual sensor readings until `stopScanning()` is called. 
+Blocks until the sweep device is ready, then signals the device to start scanning. Initiates an indefinite stream of individual sensor readings until `stopScanning()` is called. 
 During an active data stream, any attempt to communicate with the device other than `stopScanning()` will fail.
 
+To avoid blocking, you can use the `getMotorReady()` command to query the current state of the device , and do work in the meantime if it isn't ready.
+
 
 ```c++
 bool stopScanning()
 ```
 
-Signals the sweep device to stop scanning.
+Signals the sweep device to stop scanning. Will block for ~500ms to flush leftover data stream from the buffer and validate a second response from the sensor.
 
 ```c++
 bool getReading(ScanPacket &reading)
@@ -126,6 +128,19 @@ struct ScanPacket
 
 Structure representing a single sensor reading (ranging). ie: a full 360deg scan would be composed of many such readings.
 
+```c++
+bool getMotorReady()
+```
+
+Returns true if the device is ready. A device is ready when the motor speed has stabilized to the current setting and the calibration routine is complete. If a device was just powered on, or the motor speed was just changed, it can take up to 6 seconds for the device to get ready. For visual confirmation, the blue LED on the face of the sweep will blink until the device is ready.
+
+```c++
+bool waitUntilMotorReady()
+```
+
+Blocks until the device is ready. Returns true if the device is ready, and false if the check timed out (max 8 seconds).
+
+
 ```c++
 int32_t getMotorSpeed()
 ```
@@ -148,7 +163,9 @@ MOTOR_SPEED_CODE_9_HZ
 MOTOR_SPEED_CODE_10_HZ
 ```
 
-Adjusts the motor speed setting to the provided code. Recommend users pass one of the const codes defined by library:
+Blocks until the device is ready, then adjusts the motor speed setting to the provided code. Recommend users pass one of the const codes defined by library.
+
+To avoid blocking, you can use the `getMotorReady()` command to query the current state of the device , and do work in the meantime if it isn't ready.
 
 ```c++
 int32_t getSampleRate()
@@ -165,4 +182,4 @@ SAMPLE_RATE_CODE_750_HZ
 SAMPLE_RATE_CODE_1000_HZ
 ```
 
-Adjusts the sample rate setting to the provided code. Recommend users pass one of the const codes defined by library:
+Adjusts the sample rate setting to the provided code. Recommend users pass one of the const codes defined by library.

Sweep/Examples/MegaSerialPrinter/MegaSerialPrinter.ino

@@ -4,26 +4,22 @@
   
   MegaSerialPrinter:
       - Example sketch for using the Scanse Sweep with the Arduino Mega 2560.
-        Collects at least 3 complete scans, and then prints the sensor readings
+        Collects 3 complete scans, and then prints the sensor readings
       - Assumes Sweep sensor is physically connected to Serial #1 (RX1 & TX1)
         - For the sweep's power, ground, RX & TX pins, follow the connector 
           pinouts in the sweep user manual located here: 
           http://scanse.io/downloads
         - Be sure to connect RX_device -> TX_arduino & TX_device -> RX_arduino
-      - The sketch WILL work with the arduino running off USB power alone, 
-        however, the quantity and quality of sensor readings will drop. 
-      - The behavior of the sketch can be unreliable with both USB & powerjack
-        powering the arduino. If the sketch doesn't seem to to work with both
-        connected, try following sequence:
-          - upload the sketch
-          - unplug the USB connector
-          - unplug/remove power to the power jack
-          - plug in the USB connector
-          - open the serial terminal from the arduino IDE
-          - plug in the power to the power jack
-          - send "start" over serial
-
-  Created by David C. Young, February 21, 2017.
+      - You should run the sketch off USB power alone! Using both power sources 
+        (USB + power jack) can cause issues. 
+      - Note that running off of USB power is not entirely adequate for the sweep, 
+        so the quantity and qaulity of sensor readings will drop. This is OK for
+        this example , as it is only meant to provide some visual feedback over 
+        the serial monitor.
+      - In your own projects that do not involve the serial monitor, be sure to use
+        dedicated power instead of the USB.
+
+  Created by Scanse LLC, February 21, 2017.
   Released into the public domain.
 */
 
@@ -167,8 +163,8 @@ bool verifyCurrentDeviceSettings()
   }
 
   // Report the motor speed and sample rate to the computer terminal
-  Serial.println("\nMotor Speed: " + String(currentMotorSpeed) + " HZ");
-  Serial.println("Sample Rate: " + String(currentSampleRate) + " HZ");
+  Serial.println("\nMotor Speed Setting: " + String(currentMotorSpeed) + " HZ");
+  Serial.println("Sample Rate Setting: " + String(currentSampleRate) + " HZ");
 
   return true;
 }
@@ -177,6 +173,7 @@ bool verifyCurrentDeviceSettings()
 bool beginDataCollectionPhase()
 {
   // Attempt to start scanning
+  Serial.println("\nWaiting for motor speed to stabilize and calibration routine to complete...");
   bool bSuccess = device.startScanning();
   Serial.println(bSuccess ? "\nSuccessfully initiated scanning..." : "\nFailed to start scanning.");
   return bSuccess;

Sweep/Sweep.cpp

@@ -14,6 +14,10 @@ bool Sweep::startScanning()
     if (bIsScanning)
         return false;
 
+    // wait until the device is ready (calibration complete + motor stabilized)
+    if (!waitUntilMotorReady())
+        return false;
+
     _writeCommand(_DATA_ACQUISITION_START);
     // wait for the receipt (possible timeout)
     if (_readResponseHeader())
@@ -66,6 +70,35 @@ bool Sweep::getReading(ScanPacket &reading)
     return false;
 }
 
+bool Sweep::getMotorReady()
+{
+    if (bIsScanning)
+        return false;
+    _writeCommand(_MOTOR_READY);
+    if (_readResponseInfoSetting())
+    {
+        // TODO: validate receipt (hold off until performance hit is determined)
+        const uint8_t readyCode[2] = {_responseInfoSetting[2], _responseInfoSetting[3]};
+        // readyCode == 0 indicates device is ready
+        return _ascii_bytes_to_integer(readyCode) == 0;
+    }
+}
+
+bool Sweep::waitUntilMotorReady()
+{
+    if (bIsScanning)
+        return false;
+    // only check for 8 seconds (16 iterations with 500ms pause)
+    for (uint8_t i = 0; i < 16; ++i)
+    {
+        if (getMotorReady())
+            return true;
+        delay(500);
+    }
+    // timeout after 8 seconds
+    return false;
+}
+
 int32_t Sweep::getMotorSpeed()
 {
     if (bIsScanning)
@@ -86,6 +119,10 @@ bool Sweep::setMotorSpeed(const uint8_t motorSpeedCode[2])
     if (bIsScanning)
         return false;
 
+    // wait until the device is ready (calibration complete + motor stabilized)
+    if (!waitUntilMotorReady())
+        return false;
+
     _writeCommandWithArgument(_MOTOR_SPEED_ADJUST, motorSpeedCode);
     // wait for the receipt (possible timeout)
     if (_readResponseParam())

Sweep/Sweep.h

@@ -56,7 +56,7 @@ class Sweep
     // Destructor
     ~Sweep();
 
-    // true if the device is currently scanning
+    // True if the device is currently scanning
     bool bIsScanning;
 
     /**
@@ -65,14 +65,14 @@ class Sweep
     *   stream. This data stream continues indefinitely until
     *   "stopScanning()" is called.
     *
-    *   @return True is start was successful, false otherwise.
+    *   @return True if start was successful
     */
     bool startScanning();
 
     /**
     *   Terminates an active data stream.
     *
-    *   @return True is stop was successful, false otherwise.
+    *   @return True if stop was successful
     */
     bool stopScanning();
 
@@ -86,6 +86,23 @@ class Sweep
     */
     bool getReading(ScanPacket &reading);
 
+    /**
+    *   Check if the device is ready. A device is ready if the
+    *   calibration routine is complete and the motor speed has 
+    *   stabilized to the current speed setting.
+    *
+    *   @return True if the device is ready
+    */
+    bool getMotorReady();
+
+    /**
+    *   Waits (blocks) until the device is ready, or the wait 
+    *   times out after 8 seconds. See getMotorReady for details.
+    *
+    *   @return True if motor is ready, false if the wait timed out
+    */
+    bool waitUntilMotorReady();
+
     // Returns the current motor speed setting in HZ
     int32_t getMotorSpeed();
 
@@ -118,6 +135,7 @@ class Sweep
     const uint8_t _COMMAND_TERMINATION = '\n';
     const uint8_t _DATA_ACQUISITION_START[2] = {'D', 'S'};
     const uint8_t _DATA_ACQUISITION_STOP[2] = {'D', 'X'};
+    const uint8_t _MOTOR_READY[2] = {'M', 'Z'};
     const uint8_t _MOTOR_SPEED_ADJUST[2] = {'M', 'S'};
     const uint8_t _MOTOR_INFORMATION[2] = {'M', 'I'};
     const uint8_t _SAMPLE_RATE_ADJUST[2] = {'L', 'R'};

Sweep/keywords.txt

@@ -15,6 +15,8 @@ bIsScanning	KEYWORD2
 startScanning	KEYWORD2
 stopScanning	KEYWORD2
 getReading	KEYWORD2
+getMotorReady	KEYWORD2
+waitUntilMotorReady	KEYWORD2
 getMotorSpeed	KEYWORD2
 setMotorSpeed	KEYWORD2
 getSampleRate	KEYWORD2