Background scan worker thread accumulates entire ordered scan

libsweep/README.md

@@ -187,6 +187,7 @@ void sweep_device_start_scanning(sweep_device_s device, sweep_error_s* error)
 Signals the `sweep_device_s` to start scanning.
 If the motor is stationary (0Hz), will automatically set motor speed to default 5Hz.
 Will block until the the motor speed is stable (uses `sweep_device_wait_until_motor_ready` internally).
+Starts internal background thread to accumulate and queue up scans. Scans can then be retrieved using `sweep_device_get_scan`.
 In case of error a `sweep_error_s` will be written into `error`.
 
 ```c++
@@ -196,7 +197,8 @@ void sweep_device_attempt_start_scanning(sweep_device_s device, sweep_error_s* e
 Non-blocking alternative to `sweep_device_start_scanning`. 
 Signals the `sweep_device_s` to start scanning. 
 Will only succeed if the motor speed is \>0Hz and stable.
-User is responsible for checking these conditions before calling. User can check that motor speed has stabilized using `sweep_device_get_motor_ready` and that motor speed is \> 0Hz using `sweep_device_get_speed`. 
+User is responsible for checking these conditions before calling. User can check that motor speed has stabilized using `sweep_device_get_motor_ready` and that motor speed is \> 0Hz using `sweep_device_get_speed`.
+Will NOT start an internal background thread. User is responsible for keeping up with incoming scans by calling `sweep_device_get_scan_direct`.
 In case of error a `sweep_error_s` will be written into `error`. This method will error on legitimate failures (ex: the motor speed is stationary or has not yet stabilized).
 
 ```c++
@@ -287,7 +289,16 @@ Opaque type representing a single full 360 degree scan from a `sweep_device_s`.
 sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error)
 ```
 
-Blocks waiting for the `sweep_device_s` to accumulate a full 360 degree scan into `sweep_scan_s`.
+Returns the ordered readings (1st to last) from a single scan.
+Retrieves the oldest scan from a queue of scans accumulated in a background thread. Blocks until a scan is available. To be used after calling `sweep_device_start_scanning`, NOT after `sweep_device_attempt_start_scanning`.
+In case of error a `sweep_error_s` will be written into `error`.
+
+```c++
+sweep_scan_s sweep_device_get_scan_direct(sweep_device_s device, sweep_error_s* error)
+```
+
+Returns the ordered readings from the 2nd reading of the current scan through the 1st reading of the subsequent scan.
+Blocks waiting for the `sweep_device_s` to accumulate a full 360 degree scan into `sweep_scan_s`. To be used after calling `sweep_device_attempt_start_scanning`, NOT after `sweep_device_start_scanning`.
 In case of error a `sweep_error_s` will be written into `error`.
 
 ```c++

libsweep/examples/example.cc

@@ -24,6 +24,7 @@ int main(int argc, char* argv[]) try {
   for (auto n = 0; n < 10; ++n) {
     const sweep::scan scan = device.get_scan();
 
+    std::cout << "Scan #" << n << ":" << std::endl;
     for (const sweep::sample& sample : scan.samples) {
       std::cout << "angle " << sample.angle << " distance " << sample.distance << " strength " << sample.signal_strength << "\n";
     }

libsweep/src/sweep.cc

@@ -2,6 +2,7 @@
 #include "protocol.h"
 #include "serial.h"
 
+#include <atomic>
 #include <chrono>
 #include <condition_variable>
 #include <mutex>
@@ -67,7 +68,7 @@ typedef struct sweep_device {
   sweep::serial::device_s serial; // serial port communication
   bool is_scanning;
   ScanQueue* scan_queue;
-  bool stop_thread;
+  std::atomic<bool> stop_thread;
 } sweep_device;
 
 #define SWEEP_MAX_SAMPLES 4096
@@ -172,9 +173,11 @@ void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) {
     return;
   }
 
-  // Wait until device stopped sending
-  std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  // Wait some time, for the device to register the stop cmd and stop sending data blocks
+  std::this_thread::sleep_for(std::chrono::milliseconds(35));
 
+  // Flush the left over data blocks, received after sending the stop cmd
+  // This will also flush the response to the stop cmd
   sweep::serial::error_s serialerror = nullptr;
   sweep::serial::device_flush(device->serial, &serialerror);
 
@@ -184,6 +187,7 @@ void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) {
     return;
   }
 
+  // Write another stop cmd so we can read a response
   sweep::protocol::write_command(device->serial, sweep::protocol::DATA_ACQUISITION_STOP, &protocolerror);
 
   if (protocolerror) {
@@ -192,6 +196,7 @@ void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) {
     return;
   }
 
+  // read the response
   sweep::protocol::response_header_s response;
   sweep::protocol::read_response_header(device->serial, sweep::protocol::DATA_ACQUISITION_STOP, &response, &protocolerror);
 
@@ -258,7 +263,7 @@ sweep_scan_s sweep_device_get_scan_direct(sweep_device_s device, sweep_error_s*
 
   int32_t received = 0;
 
-  while (received < SWEEP_MAX_SAMPLES && !device->stop_thread) {
+  while (received < SWEEP_MAX_SAMPLES) {
     sweep::protocol::read_response_scan(device->serial, &responses[received], &protocolerror);
 
     if (protocolerror) {
@@ -306,19 +311,50 @@ sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error)
 
 // Accumulates scans in the queue  (method to be used by background thread)
 void sweep_device_accumulate_scans(sweep_device_s device) {
-  while (!device->stop_thread) {
-    // gather a scan
-    sweep_error_s scanerror = nullptr;
-    auto scan = sweep_device_get_scan_direct(device, &scanerror);
-    if (scanerror) {
-      sweep_error_destruct(scanerror);
-      sweep_error_s stoperror = nullptr;
-      sweep_device_stop_scanning(device, &stoperror);
+  SWEEP_ASSERT(device);
+  SWEEP_ASSERT(device->is_scanning);
+
+  sweep::protocol::error_s protocolerror = nullptr;
+  sweep::protocol::response_scan_packet_s responses[SWEEP_MAX_SAMPLES];
+  int32_t received = 0;
+
+  while (!device->stop_thread && received < SWEEP_MAX_SAMPLES) {
+    sweep::protocol::read_response_scan(device->serial, &responses[received], &protocolerror);
+    if (protocolerror) {
+      sweep::protocol::error_destruct(protocolerror);
       break;
     }
 
-    // queue the scan
-    device->scan_queue->enqueue(scan);
+    const bool is_sync = responses[received].sync_error & sweep::protocol::response_scan_packet_sync::sync;
+    const bool has_error = (responses[received].sync_error >> 1) != 0; // shift out sync bit, others are errors
+
+    if (!has_error) {
+      received++;
+    }
+
+    if (is_sync) {
+      if (received <= 1)
+        continue;
+      // package the previous scan without the sync reading from the subsequent scan
+      auto out = new sweep_scan;
+      out->count = received - 1; // minus 1 to exclude sync reading
+      for (int32_t it = 0; it < received - 1; ++it) {
+        // Convert angle from compact serial format to float (in degrees).
+        // In addition convert from degrees to milli-degrees.
+        out->angle[it] = static_cast<int32_t>(sweep::protocol::u16_to_f32(responses[it].angle) * 1000.f);
+        out->distance[it] = responses[it].distance;
+        out->signal_strength[it] = responses[it].signal_strength;
+      }
+
+      // place the scan in the queue
+      device->scan_queue->enqueue(out);
+
+      // place the sync reading at the start for the next scan
+      responses[0] = responses[received - 1];
+
+      // reset received
+      received = 1;
+    }
   }
 }
 
@@ -345,7 +381,7 @@ sweep_device_s sweep_device_construct(const char* port, int32_t bitrate, sweep_e
   }
 
   // initialize assuming the device is scanning
-  auto out = new sweep_device{serial, /*is_scanning=*/true, new ScanQueue(20), /*stop_thread=*/false};
+  auto out = new sweep_device{serial, /*is_scanning=*/true, new ScanQueue(20), /*stop_thread=*/{false}};
 
   // send a stop scanning command in case the scanner was powered on and scanning
   sweep_error_s stoperror = nullptr;