diff --git a/libsweep/README.md b/libsweep/README.md index 8e0aa21..5c26ae5 100644 --- a/libsweep/README.md +++ b/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++ diff --git a/libsweep/examples/example.cc b/libsweep/examples/example.cc index 298f420..2139389 100644 --- a/libsweep/examples/example.cc +++ b/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"; } diff --git a/libsweep/src/sweep.cc b/libsweep/src/sweep.cc index 8c74626..0f056b8 100644 --- a/libsweep/src/sweep.cc +++ b/libsweep/src/sweep.cc @@ -2,6 +2,7 @@ #include "protocol.h" #include "serial.h" +#include #include #include #include @@ -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 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(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;