Added reset command and simplified azimuth conversion

Author: dcyoung

Sweep/Sweep.cpp

@@ -60,12 +60,11 @@ bool Sweep::getReading(ScanPacket &reading)
     {
         // TODO: validate receipt
         reading.bIsSync = _responseScanPacket[0] % 2 == 1;
-        // convert the angle into a float according sweep protocol (in degrees),
-        // then store that value in a 16 bit unsigned int by converting it to millidegrees
-        reading.angle = _u16_to_f32((_responseScanPacket[2] << 8) + (_responseScanPacket[1]));
+        // convert the angle into a float in degrees
+        reading.angle = angle_raw_to_deg((_responseScanPacket[2] << 8) + (_responseScanPacket[1]));
         reading.distance = (_responseScanPacket[4] << 8) + (_responseScanPacket[3]);
         reading.signalStrength = _responseScanPacket[5];
-        return (reading.distance <= 1) ? false : true;
+        return true;
     }
     return false;
 }
@@ -173,6 +172,11 @@ bool Sweep::setSampleRate(const uint8_t sampleRateCode[2])
     return false;
 }
 
+void Sweep::reset()
+{
+    _writeCommand(_RESET_DEVICE);
+}
+
 bool Sweep::_writeCommand(const uint8_t cmd[2])
 {
     const uint8_t command[3] = {cmd[0], cmd[1], _COMMAND_TERMINATION};

Sweep/Sweep.h

@@ -127,6 +127,14 @@ class Sweep
     */
     bool setSampleRate(const uint8_t sampleRateCode[2]);
 
+    /**
+    *   Resets the device. 
+    *   Warning: attempting to communicate with the device while
+    *           it is resetting will cause issues. Device is ready
+    *           when the LED turns blue.
+    */
+    void reset();
+
   private:
     // The stream object connected to the sweep device.
     Stream &_serial;
@@ -166,8 +174,13 @@ class Sweep
     bool _readResponseInfoSetting();
     void _flushInputBuffer();
 
-    // converts a uint16_t representing a float into an f32
-    inline float _u16_to_f32(uint16_t v) { return ((float)(v >> 4u)) + (v & 15u) / 16.0f; }
+    // Some protocol conversion utilities
+    inline int angle_raw_to_deg(uint16_t v)
+    {
+        // angle is transmitted as fixed point integer with scaling factor of 16
+        return static_cast<int>(v / 16.0f);
+    }
+
     // converts a pair of ascii code (between '00':'10') into an integer
     inline const int32_t _ascii_bytes_to_integer(const uint8_t bytes[2])
     {

Sweep/keywords.txt

@@ -21,6 +21,7 @@ getMotorSpeed	KEYWORD2
 setMotorSpeed	KEYWORD2
 getSampleRate	KEYWORD2
 setSampleRate	KEYWORD2
+reset	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)