From fab0612f5ec63fc006279384bebaeb06efd0b7b9 Mon Sep 17 00:00:00 2001 From: dcyoung Date: Mon, 3 Apr 2017 17:26:52 -0700 Subject: [PATCH 1/2] Updated Protocol for firmware v1.1, and accumulate scans in background thread --- libsweep/README.md | 69 +++- libsweep/doc/serial_protocol_spec.md | 124 ++++++-- libsweep/examples/example.c | 7 +- libsweep/examples/example.cc | 8 +- libsweep/examples/net.cc | 1 + libsweep/examples/viewer.cc | 1 + libsweep/include/protocol.h | 22 +- libsweep/include/sweep/sweep.h | 32 +- libsweep/include/sweep/sweep.hpp | 11 +- libsweep/src/dummy.cc | 103 ++++-- libsweep/src/protocol.cc | 37 ++- libsweep/src/sweep.cc | 456 ++++++++++++++++++++++----- sweepjs/README.md | 8 + sweepjs/index.js | 3 +- sweepjs/sweepjs.cc | 24 ++ sweepjs/sweepjs.h | 2 + sweeppy/README.md | 6 +- sweeppy/sweeppy/__init__.py | 36 ++- sweeppy/sweeppy/__main__.py | 1 + 19 files changed, 783 insertions(+), 168 deletions(-) diff --git a/libsweep/README.md b/libsweep/README.md index f6c78a9..df9ba6d 100644 --- a/libsweep/README.md +++ b/libsweep/README.md @@ -185,20 +185,42 @@ 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++ void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) ``` Signals the `sweep_device_s` to stop scanning. +Blocks for ~35ms to allow time for the trailing data stream to collect and flush internally, before sending a second stop command and validate the response. +In case of error a `sweep_error_s` will be written into `error`. + +```c++ +void sweep_device_wait_until_motor_ready(sweep_device_s device, sweep_error_s* error) +``` + +Blocks until the `sweep_device_s` is ready, or the method times out (after 8 seconds). A device is ready when the motor speed has stabilized to the current setting, and the calibration routine is complete. The worst case wait time is around 6 seconds, which includes both motor stabilization and calibration. For visual reference, the blue LED on the device will blink unil the device is ready. This method is useful when the device is powered on, or when adjusting motor speed. If the device is NOT ready, it will respond to certain commands (`DS` or `MS`) with a status code indicating a failure to execute the command. Therefore, it is best practice to avoid this entirely by calling `sweep_device_wait_until_motor_ready` before calling a command that requires a ready device. +In case of error a `sweep_error_s` will be written into `error`. + +```c++ +bool sweep_device_get_motor_ready(sweep_device_s device, sweep_error_s* error) +``` + +Returns `true` if the device is ready. A device is ready if the motor speed has stabilized to the current setting, and the calibration routine is complete. +This method can be used to create a non-blocking alternative to `sweep_device_wait_until_motor_ready` in user programs. +For visual reference, the blue LED on the device will blink during calibration/speed stabilization, and stop blinking when the device is ready. In case of error a `sweep_error_s` will be written into `error`. ```c++ int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error) ``` -Returns the `sweep_device_s`'s motor speed in Hz. +Returns the `sweep_device_s`'s motor speed setting in Hz. +If the motor speed is currently changing, the returned motor speed is the target speed at which the device will stabilize. In case of error a `sweep_error_s` will be written into `error`. ```c++ @@ -206,9 +228,11 @@ void sweep_device_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error ``` Sets the `sweep_device_s`'s motor speed in Hz. -The device supports speeds of 1 Hz to 10 Hz. +Blocks until prior motor speed has stabilized. +The device supports speeds of 0 Hz to 10 Hz, but be careful that the device is not set at 0Hz before calling `sweep_device_start_scanning`. In case of error a `sweep_error_s` will be written into `error`. + ```c++ int32_t sweep_device_get_sample_rate(sweep_device_s device, sweep_error_s* error) ``` @@ -221,8 +245,8 @@ void sweep_device_set_sample_rate(sweep_device_s device, int32_t hz, sweep_error ``` Sets the `sweep_device_s`'s sample rate in Hz. -The device supports sample rates of 500 Hz, 750 Hz and 1000 Hz. -The device guarantees for those sample rates but they can be slightly higher by a maximum of roughly 50-100 Hz. +The device supports setting sample rate to the following values: 500 Hz, 750 Hz and 1000 Hz. +These sample rates are not exact. They are general ballpark values. The actual sample rate may differ slightly. In case of error a `sweep_error_s` will be written into `error`. ```c++ @@ -245,9 +269,11 @@ 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`. In case of error a `sweep_error_s` will be written into `error`. + ```c++ void sweep_scan_destruct(sweep_scan_s scan) ``` @@ -278,7 +304,40 @@ int32_t sweep_scan_get_signal_strength(sweep_scan_s scan, int32_t sample) Returns the signal strength (0 low -- 255 high) for the `sample`th sample in the `sweep_scan_s`. + ### License Copyright © 2016 Daniel J. Hofmann diff --git a/libsweep/doc/serial_protocol_spec.md b/libsweep/doc/serial_protocol_spec.md index 12cf465..cdac7a4 100644 --- a/libsweep/doc/serial_protocol_spec.md +++ b/libsweep/doc/serial_protocol_spec.md @@ -75,6 +75,7 @@ Command Symbol (2 bytes) | Parameter (2 bytes) | Line Feed(LF) | Status (2 bytes **LR** - Adjust LiDAR Sample Rate **LI** - LiDAR Info **MI** - Motor Information +**MZ** - Motor Ready **IV** - Version Info **ID** - Device Info **RR** - Reset Device @@ -97,6 +98,14 @@ Header response D | S | Status | SUM | LF | --- | --- | ---| --- | --- | +DS command is not guaranteed to succeed. There are a few conditions where it will fail. In the event of a failure, the two status bytes are used to communicate the failure. + +Status Code (2 byte ASCII code): +- `'00'`: Successfully processed command. Data acquisition effectively initiated. +- `'12'`: Failed to process command. Motor speed has not yet stabilized. Data acquisition NOT initiated. Wait until motor speed has stabilized before trying again. +- `'13'`: Failed to process command. Motor is currently stationary (0Hz). Data acquisition NOT initiated. Adjust motor speed before trying again. + +### (SENSOR -> HOST) Data Block (7 bytes) - repeat indefinitely sync/error (1byte) | Azimuth - degrees(float) (2bytes) | Distance - cm(int) (2bytes) | Signal Strength (1byte) | Checksum (1byte) @@ -136,50 +145,73 @@ D | X | Status | SUM | LF --- #### MS - Adjust Motor Speed -* Adjusts motor speed to integer value between 0Hz and 10Hz (Default Speed - 5Hz) +Adjusts motor speed setting to the specified code indicating a motor speed between 0Hz and 10Hz. This sets the target speed setting, but the motor will take time (~6 seconds) to adjust and stabilize to the new setting. The blue LED on the device will flash while the speed is stabilizing. #### (HOST -> SENSOR) -M | S | Speed Parameter (2 bytes) | LF +M | S | Speed Code (2 bytes) | LF | --- | --- | ---| --- | -Speed Parameter: -00 - 10 : 10 different speed levels according to Hz, increments of 1. ie: 01,02,.. -00 = Motor stopped -(Note: ASCII encoded, ie: '05' = 0x3035) - #### (SENSOR -> HOST) -M | S | Speed(Hz) (2 bytes) | LF | Status | Sum | LF +M | S | Speed Code (2 bytes) | LF | Status | Sum | LF | --- | --- | ---| --- | --- | ---| --- | +Speed Code (2 byte ASCII code): +- `'00'` = 0Hz +- `'01'` = 1Hz +- `'02'` = 2Hz +- `'03'` = 3Hz +- `'04'` = 4Hz +- `'05'` = 5Hz +- `'06'` = 6Hz +- `'07'` = 7Hz +- `'08'` = 8Hz +- `'09'` = 9Hz + - `'10'`= 10Hz + +(Note: codes are ASCII encoded, ie: in '05' = 0x3035) + +MS command is not guaranteed to succeed. There are a few conditions where it will fail. In the event of a failure, the two status bytes are used to communicate the failure. + +Status Code (2 byte ASCII code): +- `'00'`: Successfully processed command. Motor speed setting effectively changed to new value. +- `'11'`: Failed to process command. The command was sent with an invalid parameter. Use a valid parameter when trying again. +- `'12'`: Failed to process command. Motor speed has not yet stabilized to the previous setting. Motor speed setting NOT changed to new value. Wait until motor speed has stabilized before trying to adjust it again. + --- #### LR - Adjust LiDAR Sample Rate Default Sample Rate - 500-600Hz #### (HOST -> SENSOR) -L | R | Speed Parameter (2 bytes) | LF +L | R | Sample Rate Code (2 bytes) | LF | --- | --- | ---| --- | -Sample Rate Parameter Code: -01 = 500-600Hz -02 = 750-800Hz -03 = 1000-1050Hz -(Note: codes are ASCII encoded, ie: '02' = 0x3032) #### (SENSOR -> HOST) L | R | Sample Rate Code (2 bytes) | LF | Status | Sum | LF | --- | --- | ---| --- | ---| --- | --- | +Sample Rate Code (2 byte ASCII code): +- `'01'` = 500-600Hz +- `'02'` = 750-800Hz +- `'03'` = 1000-1050Hz + +(Note: codes are ASCII encoded, ie: '02' = 0x3032) + +LR command is not guaranteed to succeed. There are a few conditions where it will fail. In the event of a failure, the two status bytes are used to communicate the failure. + +Status Code (2 byte ASCII code): +- `'00'`: Successfully processed command. Sample Rate setting effectively changed to new value. +- `'11'`: Failed to process command. The command was sent with an invalid parameter. Use a valid parameter when trying again. + +(Note: codes are ASCII encoded, ie: '11' = 0x3131) + --- #### LI - LiDAR Information -Returns current LiDAR Sample Rate Code: -01 = 500-600Hz -02 = 750-800Hz -03 = 1000-1050Hz -(Note: codes are ASCII encoded, ie: '02' = 0x3032) +Returns the current LiDAR Sample Rate Code. #### (HOST -> SENSOR) @@ -191,10 +223,16 @@ L | I | LF L | I | Sample Rate Code (2 bytes) | LF | | --- | --- | --- | --- | +Sample Rate Code (2 byte ASCII code): +- `'01'` = 500-600Hz +- `'02'` = 750-800Hz +- `'03'` = 1000-1050Hz + +(Note: codes are ASCII encoded, ie: '02' = 0x3032) + --- #### MI - Motor Information -* Returns current motor speed code 00 - 10. (ie: rotation frequency in Hz) -(Note: motor speed code is ASCII encoded, ie: in '05' = 0x3035) +Returns current motor speed code representing the rotation frequency (in Hz) of the current target motor speed setting. This does not mean that the motor speed is stabilized yet. #### (HOST -> SENSOR) @@ -203,11 +241,50 @@ M | I | LF #### (SENSOR -> HOST) -M | I | Speed(Hz) (2 bytes) | LF | +M | I | Speed Code (Hz) (2 bytes) | LF | | --- | --- | ---| --- | +Speed Code (2 byte ASCII code): +- `'00'` = 0Hz +- `'01'` = 1Hz +- `'02'` = 2Hz +- `'03'` = 3Hz +- `'04'` = 4Hz +- `'05'` = 5Hz +- `'06'` = 6Hz +- `'07'` = 7Hz +- `'08'` = 8Hz +- `'09'` = 9Hz + - `'10'`= 10Hz + +(Note: codes are ASCII encoded, ie: in '05' = 0x3035) + +--- +#### MZ - Motor Ready/Stabilized +Returns a ready code representing whether or not the motor speed has stabilized. + +#### (HOST -> SENSOR) + +M | Z | LF +| --- | --- | ---| + +#### (SENSOR -> HOST) + +M | Z | Ready Code (2 bytes) | LF | +| --- | --- | --- | --- | + +Ready Code (2 byte ASCII code): + +- `'00'` = motor speed has stabilized. +- `'01'` = motor speed has not yet stabilized. + +(Note: codes are ASCII encoded, ie: '01' = 0x3031) + +While adjusting motor speed, the sensor will NOT be able to accomplish certain actions such as `DS` or `MS`. After powering on the device or adjusting motor speed, the device will allow ~6 seconds for the motor speed to stabilize. The `MZ` command allows the user to repeatedly query the motor speed state until the return code indicates the motor speed has stabilized. After the motor speed is noted as stable, the user can safely send commands like `DS` or `MS`. + --- #### IV - Version Details +Returns details about the device's version information. * Model * Protocol Version * Firmware Version @@ -231,6 +308,7 @@ Example: --- #### ID - Device Info +Returns details about the device's current state/settings. * Bit Rate * Laser State * Mode @@ -254,7 +332,7 @@ Example: --- #### RR - Reset Device -* Reset Scanner +Resets the device. Green LED indicates the device is resetting and cannot receive commands. When the LED turns blue, the device has successfully reset. #### (HOST -> SENSOR) diff --git a/libsweep/examples/example.c b/libsweep/examples/example.c index 7cf4710..ccb1add 100644 --- a/libsweep/examples/example.c +++ b/libsweep/examples/example.c @@ -37,7 +37,7 @@ int main(int argc, char* argv[]) { // All functions which can potentially fail write into an error object sweep_error_s error = NULL; - // Create a Sweep device from default USB serial port; there is a second constructor for advanced usage + // Create a Sweep device from the specified USB serial port; there is a second constructor for advanced usage sweep_device_s sweep = sweep_device_construct_simple(port, &error); check(error); @@ -45,15 +45,16 @@ int main(int argc, char* argv[]) { int32_t speed = sweep_device_get_motor_speed(sweep, &error); check(error); - fprintf(stdout, "Motor Speed: %" PRId32 " Hz\n", speed); + fprintf(stdout, "Motor Speed Setting: %" PRId32 " Hz\n", speed); // The Sweep's sample rate in Hz int32_t rate = sweep_device_get_sample_rate(sweep, &error); check(error); - fprintf(stdout, "Sample Rate: %" PRId32 " Hz\n", rate); + fprintf(stdout, "Sample Rate Setting: %" PRId32 " Hz\n", rate); // Capture scans + fprintf(stdout, "Beginning data acquisition as soon as motor speed stabilizes...\n"); sweep_device_start_scanning(sweep, &error); check(error); diff --git a/libsweep/examples/example.cc b/libsweep/examples/example.cc index dba74f6..2139389 100644 --- a/libsweep/examples/example.cc +++ b/libsweep/examples/example.cc @@ -12,23 +12,25 @@ int main(int argc, char* argv[]) try { return EXIT_FAILURE; } + std::cout << "Constructing sweep device..." << std::endl; sweep::sweep device{argv[1]}; - std::cout << "Motor Speed: " << device.get_motor_speed() << " Hz" << std::endl; - std::cout << "Sample Rate: " << device.get_sample_rate() << " Hz" << std::endl; + std::cout << "Motor Speed Setting: " << device.get_motor_speed() << " Hz" << std::endl; + std::cout << "Sample Rate Setting: " << device.get_sample_rate() << " Hz" << std::endl; + std::cout << "Beginning data acquisition as soon as motor speed stabilizes..." << std::endl; device.start_scanning(); 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"; } } device.stop_scanning(); - } catch (const sweep::device_error& e) { std::cerr << "Error: " << e.what() << std::endl; } diff --git a/libsweep/examples/net.cc b/libsweep/examples/net.cc index 62fc6a2..3b6fb82 100644 --- a/libsweep/examples/net.cc +++ b/libsweep/examples/net.cc @@ -53,6 +53,7 @@ void publisher(const std::string& dev) try { pub.bind("tcp://127.0.0.1:5555"); sweep::sweep device{dev.c_str()}; + // Begins data acquisition as soon as motor speed stabilizes device.start_scanning(); std::cout << "Publishing. Each dot is a full 360 degree scan." << std::endl; diff --git a/libsweep/examples/viewer.cc b/libsweep/examples/viewer.cc index 9a7473a..7f49375 100644 --- a/libsweep/examples/viewer.cc +++ b/libsweep/examples/viewer.cc @@ -73,6 +73,7 @@ int main(int argc, char* argv[]) try { // Now start scanning in the second thread, swapping in new points for every scan sweep::sweep device{argv[1]}; + // Begins data acquisition as soon as motor is ready device.start_scanning(); sweep::scan scan; diff --git a/libsweep/include/protocol.h b/libsweep/include/protocol.h index 558a120..2e6f77f 100644 --- a/libsweep/include/protocol.h +++ b/libsweep/include/protocol.h @@ -24,6 +24,7 @@ void error_destruct(error_s error); extern const uint8_t DATA_ACQUISITION_START[2]; extern const uint8_t DATA_ACQUISITION_STOP[2]; extern const uint8_t MOTOR_SPEED_ADJUST[2]; +extern const uint8_t MOTOR_READY[2]; extern const uint8_t MOTOR_INFORMATION[2]; extern const uint8_t SAMPLE_RATE_ADJUST[2]; extern const uint8_t SAMPLE_RATE_INFORMATION[2]; @@ -133,14 +134,23 @@ typedef struct { static_assert(sizeof(response_info_version_s) == 21, "response info version size mismatch"); +typedef struct { + uint8_t cmdByte1; + uint8_t cmdByte2; + uint8_t motor_ready[2]; + uint8_t term; +} response_info_motor_ready_s; + +static_assert(sizeof(response_info_motor_ready_s) == 5, "response info motor ready size mismatch"); + typedef struct { uint8_t cmdByte1; uint8_t cmdByte2; uint8_t motor_speed[2]; uint8_t term; -} response_info_motor_s; +} response_info_motor_speed_s; -static_assert(sizeof(response_info_motor_s) == 5, "response info motor size mismatch"); +static_assert(sizeof(response_info_motor_speed_s) == 5, "response info motor speed size mismatch"); typedef struct { uint8_t cmdByte1; @@ -149,7 +159,7 @@ typedef struct { uint8_t term; } response_info_sample_rate_s; -static_assert(sizeof(response_info_sample_rate_s) == 5, "response info sample rate siye mismatch"); +static_assert(sizeof(response_info_sample_rate_s) == 5, "response info sample rate size mismatch"); // Done with in-memory representations for packets we send over the wire. #pragma pack(pop) @@ -166,7 +176,11 @@ void read_response_param(sweep::serial::device_s serial, const uint8_t cmd[2], r void read_response_scan(sweep::serial::device_s serial, response_scan_packet_s* scan, error_s* error); -void read_response_info_motor(sweep::serial::device_s serial, const uint8_t cmd[2], response_info_motor_s* info, error_s* error); +void read_response_info_motor_ready(sweep::serial::device_s serial, const uint8_t cmd[2], response_info_motor_ready_s* info, + error_s* error); + +void read_response_info_motor_speed(sweep::serial::device_s serial, const uint8_t cmd[2], response_info_motor_speed_s* info, + error_s* error); void read_response_info_sample_rate(sweep::serial::device_s serial, const uint8_t cmd[2], response_info_sample_rate_s* info, error_s* error); diff --git a/libsweep/include/sweep/sweep.h b/libsweep/include/sweep/sweep.h index 2ccac3e..8349b31 100644 --- a/libsweep/include/sweep/sweep.h +++ b/libsweep/include/sweep/sweep.h @@ -52,25 +52,45 @@ SWEEP_API sweep_device_s sweep_device_construct_simple(const char* port, sweep_e SWEEP_API sweep_device_s sweep_device_construct(const char* port, int32_t bitrate, sweep_error_s* error); SWEEP_API void sweep_device_destruct(sweep_device_s device); +// Blocks until device is ready to start scanning, then starts scanning SWEEP_API void sweep_device_start_scanning(sweep_device_s device, sweep_error_s* error); +// Stops stream, blocks while leftover stream is flushed, and sends stop once more to validate response SWEEP_API void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error); -SWEEP_API sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error); -SWEEP_API void sweep_scan_destruct(sweep_scan_s scan); +// Blocks until the device is ready (calibration complete and motor speed stabilized) +SWEEP_API void sweep_device_wait_until_motor_ready(sweep_device_s device, sweep_error_s* error); -SWEEP_API int32_t sweep_scan_get_number_of_samples(sweep_scan_s scan); -SWEEP_API int32_t sweep_scan_get_angle(sweep_scan_s scan, int32_t sample); -SWEEP_API int32_t sweep_scan_get_distance(sweep_scan_s scan, int32_t sample); -SWEEP_API int32_t sweep_scan_get_signal_strength(sweep_scan_s scan, int32_t sample); +// Retrieves a scan from the queue (will block until scan is available) +SWEEP_API 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); +SWEEP_API bool sweep_device_get_motor_ready(sweep_device_s device, sweep_error_s* error); SWEEP_API int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error); +// Blocks until device is ready to adjust motor speed, then adjusts motor speed SWEEP_API void sweep_device_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error); SWEEP_API int32_t sweep_device_get_sample_rate(sweep_device_s device, sweep_error_s* error); SWEEP_API void sweep_device_set_sample_rate(sweep_device_s device, int32_t hz, sweep_error_s* error); +SWEEP_API int32_t sweep_scan_get_number_of_samples(sweep_scan_s scan); +SWEEP_API int32_t sweep_scan_get_angle(sweep_scan_s scan, int32_t sample); +SWEEP_API int32_t sweep_scan_get_distance(sweep_scan_s scan, int32_t sample); +SWEEP_API int32_t sweep_scan_get_signal_strength(sweep_scan_s scan, int32_t sample); + +SWEEP_API void sweep_scan_destruct(sweep_scan_s scan); + SWEEP_API void sweep_device_reset(sweep_device_s device, sweep_error_s* error); +//------- Alternative methods for Low Level development (can error on failure) ------ // +// Not yet part of the public API +// Attempts to start scanning without waiting for motor ready, does NOT start background thread to accumulate scans +void sweep_device_attempt_start_scanning(sweep_device_s device, sweep_error_s* error); +// Read incoming scan directly (not retrieving from the queue) +sweep_scan_s sweep_device_get_scan_direct(sweep_device_s device, sweep_error_s* error); +// Attempts to set motor speed without waiting for motor ready +void sweep_device_attempt_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error); + #ifdef __cplusplus } #endif diff --git a/libsweep/include/sweep/sweep.hpp b/libsweep/include/sweep/sweep.hpp index 2b41bd0..ea8b787 100644 --- a/libsweep/include/sweep/sweep.hpp +++ b/libsweep/include/sweep/sweep.hpp @@ -44,18 +44,15 @@ class sweep { public: sweep(const char* port); sweep(const char* port, std::int32_t bitrate); - void start_scanning(); void stop_scanning(); - + void wait_until_motor_ready(); + bool get_motor_ready(); std::int32_t get_motor_speed(); void set_motor_speed(std::int32_t speed); - std::int32_t get_sample_rate(); void set_sample_rate(std::int32_t speed); - scan get_scan(); - void reset(); private: @@ -90,6 +87,10 @@ void sweep::start_scanning() { ::sweep_device_start_scanning(device.get(), detai void sweep::stop_scanning() { ::sweep_device_stop_scanning(device.get(), detail::error_to_exception{}); } +void sweep::wait_until_motor_ready() { ::sweep_device_wait_until_motor_ready(device.get(), detail::error_to_exception{}); } + +bool sweep::get_motor_ready() { return ::sweep_device_get_motor_ready(device.get(), detail::error_to_exception{}); } + std::int32_t sweep::get_motor_speed() { return ::sweep_device_get_motor_speed(device.get(), detail::error_to_exception{}); } void sweep::set_motor_speed(std::int32_t speed) { diff --git a/libsweep/src/dummy.cc b/libsweep/src/dummy.cc index 887a4f7..c47c14d 100644 --- a/libsweep/src/dummy.cc +++ b/libsweep/src/dummy.cc @@ -62,26 +62,35 @@ void sweep_device_start_scanning(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); SWEEP_ASSERT(!device->is_scanning); - (void)device; (void)error; + if (device->is_scanning) + return; + device->is_scanning = true; } void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); - (void)device; (void)error; device->is_scanning = false; } +void sweep_device_wait_until_motor_ready(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + + (void)device; + (void)error; +} + sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); SWEEP_ASSERT(device->is_scanning); - (void)device; (void)error; auto out = new sweep_scan{/*count=*/device->is_scanning ? 16 : 0, /*nth=*/device->nth_scan_request}; @@ -94,6 +103,55 @@ sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error) return out; } +bool sweep_device_get_motor_ready(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + (void)device; + (void)error; + + return true; +} + +int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + (void)device; + (void)error; + + return device->motor_speed; +} + +void sweep_device_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(hz >= 0 && hz <= 10); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + (void)error; + + device->motor_speed = hz; +} + +int32_t sweep_device_get_sample_rate(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + (void)error; + + return device->sample_rate; +} + +void sweep_device_set_sample_rate(sweep_device_s device, int32_t hz, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(hz == 500 || hz == 750 || hz == 1000); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + (void)error; + + device->sample_rate = hz; +} + int32_t sweep_scan_get_number_of_samples(sweep_scan_s scan) { SWEEP_ASSERT(scan); @@ -149,49 +207,48 @@ void sweep_scan_destruct(sweep_scan_s scan) { delete scan; } -int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error) { +void sweep_device_reset(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); SWEEP_ASSERT(!device->is_scanning); (void)device; (void)error; - - return device->motor_speed; } -void sweep_device_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error) { +void sweep_device_attempt_start_scanning(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); - SWEEP_ASSERT(hz >= 0 && hz <= 10); SWEEP_ASSERT(error); SWEEP_ASSERT(!device->is_scanning); (void)error; - device->motor_speed = hz; + if (device->is_scanning) + return; + + device->is_scanning = true; } -int32_t sweep_device_get_sample_rate(sweep_device_s device, sweep_error_s* error) { +sweep_scan_s sweep_device_get_scan_direct(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); - SWEEP_ASSERT(!device->is_scanning); + SWEEP_ASSERT(device->is_scanning); (void)error; - return device->sample_rate; -} + auto out = new sweep_scan{/*count=*/device->is_scanning ? 16 : 0, /*nth=*/device->nth_scan_request}; -void sweep_device_set_sample_rate(sweep_device_s device, int32_t hz, sweep_error_s* error) { - SWEEP_ASSERT(device); - SWEEP_ASSERT(hz == 500 || hz == 750 || hz == 1000); - SWEEP_ASSERT(error); - SWEEP_ASSERT(!device->is_scanning); - (void)error; + device->nth_scan_request += 1; - device->sample_rate = hz; + // Artificially introduce slowdown, to simulate device rotation + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + return out; } -void sweep_device_reset(sweep_device_s device, sweep_error_s* error) { +void sweep_device_attempt_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error) { SWEEP_ASSERT(device); + SWEEP_ASSERT(hz >= 0 && hz <= 10); SWEEP_ASSERT(error); SWEEP_ASSERT(!device->is_scanning); - (void)device; (void)error; -} + + device->motor_speed = hz; +} \ No newline at end of file diff --git a/libsweep/src/protocol.cc b/libsweep/src/protocol.cc index 36740cd..e4d8567 100644 --- a/libsweep/src/protocol.cc +++ b/libsweep/src/protocol.cc @@ -1,5 +1,7 @@ +#include #include #include +#include #include "protocol.h" @@ -9,6 +11,7 @@ namespace protocol { const uint8_t DATA_ACQUISITION_START[2] = {'D', 'S'}; const uint8_t DATA_ACQUISITION_STOP[2] = {'D', 'X'}; const uint8_t MOTOR_SPEED_ADJUST[2] = {'M', 'S'}; +const uint8_t MOTOR_READY[2] = {'M', 'Z'}; const uint8_t MOTOR_INFORMATION[2] = {'M', 'I'}; const uint8_t SAMPLE_RATE_ADJUST[2] = {'L', 'R'}; const uint8_t SAMPLE_RATE_INFORMATION[2] = {'L', 'I'}; @@ -75,8 +78,10 @@ void write_command(serial::device_s serial, const uint8_t cmd[2], error_s* error packet.cmdByte2 = cmd[1]; packet.cmdParamTerm = '\n'; - serial::error_s serialerror = nullptr; + // pause for 2ms, so the device is never bombarded with back to back commands + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + serial::error_s serialerror = nullptr; serial::device_write(serial, &packet, sizeof(cmd_packet_s), &serialerror); if (serialerror) { @@ -195,7 +200,33 @@ void read_response_scan(serial::device_s serial, response_scan_packet_s* scan, e } } -void read_response_info_motor(serial::device_s serial, const uint8_t cmd[2], response_info_motor_s* info, error_s* error) { +void read_response_info_motor_ready(serial::device_s serial, const uint8_t cmd[2], response_info_motor_ready_s* info, + error_s* error) { + SWEEP_ASSERT(serial); + SWEEP_ASSERT(cmd); + SWEEP_ASSERT(info); + SWEEP_ASSERT(error); + + serial::error_s serialerror = nullptr; + + serial::device_read(serial, info, sizeof(response_info_motor_ready_s), &serialerror); + + if (serialerror) { + *error = error_construct("unable to read response motor ready"); + serial::error_destruct(serialerror); + return; + } + + bool ok = info->cmdByte1 == cmd[0] && info->cmdByte2 == cmd[1]; + + if (!ok) { + *error = error_construct("invalid motor ready response commands"); + return; + } +} + +void read_response_info_motor_speed(serial::device_s serial, const uint8_t cmd[2], response_info_motor_speed_s* info, + error_s* error) { SWEEP_ASSERT(serial); SWEEP_ASSERT(cmd); SWEEP_ASSERT(info); @@ -203,7 +234,7 @@ void read_response_info_motor(serial::device_s serial, const uint8_t cmd[2], res serial::error_s serialerror = nullptr; - serial::device_read(serial, info, sizeof(response_info_motor_s), &serialerror); + serial::device_read(serial, info, sizeof(response_info_motor_speed_s), &serialerror); if (serialerror) { *error = error_construct("unable to read response motor info"); diff --git a/libsweep/src/sweep.cc b/libsweep/src/sweep.cc index a18afd4..ed62bcd 100644 --- a/libsweep/src/sweep.cc +++ b/libsweep/src/sweep.cc @@ -2,9 +2,59 @@ #include "protocol.h" #include "serial.h" +#include #include +#include +#include +#include #include +// A threadsafe-queue to store and retrieve scans +class ScanQueue { +public: + ScanQueue(int32_t max) : the_queue(), the_mutex(), the_cond_var() { max_size = max; } + + // empty the queue + void flush() { + std::unique_lock lock(the_mutex); + while (!the_queue.empty()) { + the_queue.pop(); + } + } + + // Add an element to the queue. + void enqueue(sweep_scan_s scan) { + std::lock_guard lock(the_mutex); + + // if necessary, remove the oldest scan to make room for new + if (the_queue.size() >= max_size) + the_queue.pop(); + + the_queue.push(scan); + the_cond_var.notify_one(); + } + + // If the queue is empty, wait till an element is avaiable. + sweep_scan_s dequeue() { + std::unique_lock lock(the_mutex); + // wait until queue is not empty + while (the_queue.empty()) { + // the_cond_var could wake up the thread spontaneously, even if the queue is still empty... + // so put this wakeup inside a while loop, such that the empty check is performed whenever it wakes up + the_cond_var.wait(lock); // release lock as long as the wait and reaquire it afterwards. + } + sweep_scan_s scan = the_queue.front(); + the_queue.pop(); + return scan; + } + +private: + int32_t max_size; + std::queue the_queue; + mutable std::mutex the_mutex; + std::condition_variable the_cond_var; +}; + int32_t sweep_get_version(void) { return SWEEP_VERSION; } bool sweep_is_abi_compatible(void) { return sweep_get_version() >> 16u == SWEEP_VERSION_MAJOR; } @@ -15,6 +65,8 @@ typedef struct sweep_error { typedef struct sweep_device { sweep::serial::device_s serial; // serial port communication bool is_scanning; + std::unique_ptr scan_queue; + std::atomic stop_thread; } sweep_device; #define SWEEP_MAX_SAMPLES 4096 @@ -66,14 +118,16 @@ sweep_device_s sweep_device_construct(const char* port, int32_t bitrate, sweep_e return nullptr; } - auto out = new sweep_device{serial, /*is_scanning=*/true}; + // initialize assuming the device is scanning + auto out = + new sweep_device{serial, /*is_scanning=*/true, std::unique_ptr(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; sweep_device_stop_scanning(out, &stoperror); - if (stoperror) { - *error = stoperror; - sweep_device_destruct(out); + sweep_error_destruct(stoperror); + *error = sweep_error_construct("Failed to create sweep device."); return nullptr; } @@ -100,33 +154,60 @@ void sweep_device_start_scanning(sweep_device_s device, sweep_error_s* error) { if (device->is_scanning) return; - sweep::protocol::error_s protocolerror = nullptr; - sweep::protocol::write_command(device->serial, sweep::protocol::DATA_ACQUISITION_START, &protocolerror); - - if (protocolerror) { - *error = sweep_error_construct("unable to send start scanning command"); - sweep::protocol::error_destruct(protocolerror); + // Get the current motor speed setting + sweep_error_s speederror = nullptr; + int32_t speed = sweep_device_get_motor_speed(device, &speederror); + if (speederror) { + *error = sweep_error_construct("unable to start scanning. could not verify motor speed."); + sweep_error_destruct(speederror); return; } + // Check that the motor is not stationary + if (speed == 0 /*Hz*/) { + // If the motor is stationary, adjust it to default 5Hz + sweep_device_set_motor_speed(device, 5 /*Hz*/, &speederror); + if (speederror) { + *error = sweep_error_construct("unable to start scanning. failed to start motor."); + sweep_error_destruct(speederror); + return; + } + } - sweep::protocol::response_header_s response; - sweep::protocol::read_response_header(device->serial, sweep::protocol::DATA_ACQUISITION_START, &response, &protocolerror); + // Make sure the motor is stabilized so the DS command doesn't fail + sweep_error_s stabilityerror = nullptr; + sweep_device_wait_until_motor_ready(device, &stabilityerror); + if (stabilityerror) { + *error = sweep_error_construct("unable to start scanning. motor stability could not be verified."); + sweep_error_destruct(stabilityerror); + return; + } - if (protocolerror) { - *error = sweep_error_construct("unable to receive start scanning command response"); - sweep::protocol::error_destruct(protocolerror); + // Attempt to start scanning + sweep_error_s starterror = nullptr; + sweep_device_attempt_start_scanning(device, &starterror); + if (starterror) { + *error = sweep_error_construct("unable to start scanning."); + sweep_error_destruct(starterror); return; } + // Start SCAN WORKER + device->scan_queue->flush(); device->is_scanning = true; + // START background worker thread + device->stop_thread = false; + // create a thread + std::thread th = std::thread(sweep_device_accumulate_scans, device); + // detach the thread so that it runs in the background and cleans itself up + th.detach(); } void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); - if (!device->is_scanning) - return; + // STOP the background thread from accumulating scans + device->stop_thread = true; sweep::protocol::error_s protocolerror = nullptr; sweep::protocol::write_command(device->serial, sweep::protocol::DATA_ACQUISITION_STOP, &protocolerror); @@ -137,9 +218,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); @@ -149,6 +232,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) { @@ -157,6 +241,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); @@ -169,24 +254,53 @@ void sweep_device_stop_scanning(sweep_device_s device, sweep_error_s* error) { device->is_scanning = false; } +void sweep_device_wait_until_motor_ready(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + + sweep_error_s readyerror = nullptr; + bool motor_ready; + // Only check for 8 seconds (16 iterations with 500ms pause) + for (auto i = 0; i < 16; ++i) { + motor_ready = sweep_device_get_motor_ready(device, &readyerror); + if (readyerror) { + *error = readyerror; + return; + } + if (motor_ready) { + return; + } + // only check every 500ms, to avoid unecessary processing if this is executing in a dedicated thread + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + *error = sweep_error_construct("timed out waiting for motor to stabilize"); +} + +// Retrieves a scan from the queue (will block until scan is available) sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error) { SWEEP_ASSERT(device); SWEEP_ASSERT(error); SWEEP_ASSERT(device->is_scanning); - sweep::protocol::error_s protocolerror = nullptr; + auto out = device->scan_queue->dequeue(); + return out; +} - sweep::protocol::response_scan_packet_s responses[SWEEP_MAX_SAMPLES]; +// Accumulates scans in the queue (method to be used by background thread) +void sweep_device_accumulate_scans(sweep_device_s device) { + 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 (received < SWEEP_MAX_SAMPLES) { + while (!device->stop_thread && received < SWEEP_MAX_SAMPLES) { sweep::protocol::read_response_scan(device->serial, &responses[received], &protocolerror); - if (protocolerror) { - *error = sweep_error_construct("unable to receive sweep scan response"); sweep::protocol::error_destruct(protocolerror); - return nullptr; + break; } const bool is_sync = responses[received].sync_error & sweep::protocol::response_scan_packet_sync::sync; @@ -197,57 +311,59 @@ sweep_scan_s sweep_device_get_scan(sweep_device_s device, sweep_error_s* error) } if (is_sync) { - break; + 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; } } - - auto out = new sweep_scan; - - out->count = received; - - for (int32_t it = 0; it < received; ++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; - } - - return out; } -int32_t sweep_scan_get_number_of_samples(sweep_scan_s scan) { - SWEEP_ASSERT(scan); - SWEEP_ASSERT(scan->count >= 0); - - return scan->count; -} - -int32_t sweep_scan_get_angle(sweep_scan_s scan, int32_t sample) { - SWEEP_ASSERT(scan); - SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); +bool sweep_device_get_motor_ready(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); - return scan->angle[sample]; -} + sweep::protocol::error_s protocolerror = nullptr; -int32_t sweep_scan_get_distance(sweep_scan_s scan, int32_t sample) { - SWEEP_ASSERT(scan); - SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); + sweep::protocol::write_command(device->serial, sweep::protocol::MOTOR_READY, &protocolerror); - return scan->distance[sample]; -} + if (protocolerror) { + *error = sweep_error_construct("unable to send motor ready command"); + sweep::protocol::error_destruct(protocolerror); + return false; + } -int32_t sweep_scan_get_signal_strength(sweep_scan_s scan, int32_t sample) { - SWEEP_ASSERT(scan); - SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); + sweep::protocol::response_info_motor_ready_s response; + sweep::protocol::read_response_info_motor_ready(device->serial, sweep::protocol::MOTOR_READY, &response, &protocolerror); - return scan->signal_strength[sample]; -} + if (protocolerror) { + *error = sweep_error_construct("unable to receive motor ready command response"); + sweep::protocol::error_destruct(protocolerror); + return false; + } -void sweep_scan_destruct(sweep_scan_s scan) { - SWEEP_ASSERT(scan); + int32_t ready_code = sweep::protocol::ascii_bytes_to_integral(response.motor_ready); + SWEEP_ASSERT(ready_code >= 0); - delete scan; + return ready_code == 0; } int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error) { @@ -265,8 +381,8 @@ int32_t sweep_device_get_motor_speed(sweep_device_s device, sweep_error_s* error return 0; } - sweep::protocol::response_info_motor_s response; - sweep::protocol::read_response_info_motor(device->serial, sweep::protocol::MOTOR_INFORMATION, &response, &protocolerror); + sweep::protocol::response_info_motor_speed_s response; + sweep::protocol::read_response_info_motor_speed(device->serial, sweep::protocol::MOTOR_INFORMATION, &response, &protocolerror); if (protocolerror) { *error = sweep_error_construct("unable to receive motor speed command response"); @@ -286,27 +402,17 @@ void sweep_device_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error SWEEP_ASSERT(error); SWEEP_ASSERT(!device->is_scanning); - uint8_t args[2] = {0}; - sweep::protocol::integral_to_ascii_bytes(hz, args); - - sweep::protocol::error_s protocolerror = nullptr; - - sweep::protocol::write_command_with_arguments(device->serial, sweep::protocol::MOTOR_SPEED_ADJUST, args, &protocolerror); - - if (protocolerror) { - *error = sweep_error_construct("unable to send motor speed command"); - sweep::protocol::error_destruct(protocolerror); + // Make sure the motor is stabilized so the MS command doesn't fail + sweep_error_s stabilityerror = nullptr; + sweep_device_wait_until_motor_ready(device, &stabilityerror); + if (stabilityerror) { + *error = sweep_error_construct("unable to set motor speed. prior motor stability could not be verified."); + sweep_error_destruct(stabilityerror); return; } - sweep::protocol::response_param_s response; - sweep::protocol::read_response_param(device->serial, sweep::protocol::MOTOR_SPEED_ADJUST, &response, &protocolerror); - - if (protocolerror) { - *error = sweep_error_construct("unable to receive motor speed command response"); - sweep::protocol::error_destruct(protocolerror); - return; - } + // Attempt to set motor speed + sweep_device_attempt_set_motor_speed(device, hz, error); } int32_t sweep_device_get_sample_rate(sweep_device_s device, sweep_error_s* error) { @@ -399,6 +505,51 @@ void sweep_device_set_sample_rate(sweep_device_s device, int32_t hz, sweep_error sweep::protocol::error_destruct(protocolerror); return; } + + // Check the status bytes do not indicate failure + const uint8_t status_bytes[2] = {response.cmdStatusByte1, response.cmdStatusByte2}; + int32_t status_code = sweep::protocol::ascii_bytes_to_integral(status_bytes); + switch (status_code) { + case 11: + *error = sweep_error_construct("Failed to set motor speed because provided parameter was invalid."); + return; + default: + break; + } +} + +int32_t sweep_scan_get_number_of_samples(sweep_scan_s scan) { + SWEEP_ASSERT(scan); + SWEEP_ASSERT(scan->count >= 0); + + return scan->count; +} + +int32_t sweep_scan_get_angle(sweep_scan_s scan, int32_t sample) { + SWEEP_ASSERT(scan); + SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); + + return scan->angle[sample]; +} + +int32_t sweep_scan_get_distance(sweep_scan_s scan, int32_t sample) { + SWEEP_ASSERT(scan); + SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); + + return scan->distance[sample]; +} + +int32_t sweep_scan_get_signal_strength(sweep_scan_s scan, int32_t sample) { + SWEEP_ASSERT(scan); + SWEEP_ASSERT(sample >= 0 && sample < scan->count && "sample index out of bounds"); + + return scan->signal_strength[sample]; +} + +void sweep_scan_destruct(sweep_scan_s scan) { + SWEEP_ASSERT(scan); + + delete scan; } void sweep_device_reset(sweep_device_s device, sweep_error_s* error) { @@ -416,3 +567,138 @@ void sweep_device_reset(sweep_device_s device, sweep_error_s* error) { return; } } + +//------- Alternative methods for Low Level development (can error on failure) ------ // +// Attempts to start scanning without waiting for motor ready, does NOT start background thread to accumulate scans +void sweep_device_attempt_start_scanning(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + + if (device->is_scanning) + return; + + sweep::protocol::error_s protocolerror = nullptr; + sweep::protocol::write_command(device->serial, sweep::protocol::DATA_ACQUISITION_START, &protocolerror); + + if (protocolerror) { + *error = sweep_error_construct("unable to send start scanning command"); + sweep::protocol::error_destruct(protocolerror); + return; + } + + sweep::protocol::response_header_s response; + sweep::protocol::read_response_header(device->serial, sweep::protocol::DATA_ACQUISITION_START, &response, &protocolerror); + + if (protocolerror) { + *error = sweep_error_construct("unable to receive start scanning command response"); + sweep::protocol::error_destruct(protocolerror); + return; + } + + // Check the status bytes do not indicate failure + const uint8_t status_bytes[2] = {response.cmdStatusByte1, response.cmdStatusByte2}; + int32_t status_code = sweep::protocol::ascii_bytes_to_integral(status_bytes); + switch (status_code) { + case 12: + *error = sweep_error_construct("Failed to start scanning because motor speed has not stabilized."); + return; + case 13: + *error = sweep_error_construct("Failed to start scanning because motor is stationary."); + return; + default: + break; + } +} + +// Read incoming scan directly (not retrieving from the queue) +sweep_scan_s sweep_device_get_scan_direct(sweep_device_s device, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(error); + 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 (received < SWEEP_MAX_SAMPLES) { + sweep::protocol::read_response_scan(device->serial, &responses[received], &protocolerror); + + if (protocolerror) { + *error = sweep_error_construct("unable to receive sweep scan response"); + sweep::protocol::error_destruct(protocolerror); + return nullptr; + } + + 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) { + break; + } + } + + auto out = new sweep_scan; + + out->count = received; + + for (int32_t it = 0; it < received; ++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; + } + + return out; +} + +// Attempts to set motor speed without waiting for motor ready +void sweep_device_attempt_set_motor_speed(sweep_device_s device, int32_t hz, sweep_error_s* error) { + SWEEP_ASSERT(device); + SWEEP_ASSERT(hz >= 0 && hz <= 10); + SWEEP_ASSERT(error); + SWEEP_ASSERT(!device->is_scanning); + + uint8_t args[2] = {0}; + sweep::protocol::integral_to_ascii_bytes(hz, args); + + sweep::protocol::error_s protocolerror = nullptr; + + sweep::protocol::write_command_with_arguments(device->serial, sweep::protocol::MOTOR_SPEED_ADJUST, args, &protocolerror); + + if (protocolerror) { + *error = sweep_error_construct("unable to send motor speed command"); + sweep::protocol::error_destruct(protocolerror); + return; + } + + sweep::protocol::response_param_s response; + sweep::protocol::read_response_param(device->serial, sweep::protocol::MOTOR_SPEED_ADJUST, &response, &protocolerror); + + if (protocolerror) { + *error = sweep_error_construct("unable to receive motor speed command response"); + sweep::protocol::error_destruct(protocolerror); + return; + } + + // Check the status bytes do not indicate failure + const uint8_t status_bytes[2] = {response.cmdStatusByte1, response.cmdStatusByte2}; + int32_t status_code = sweep::protocol::ascii_bytes_to_integral(status_bytes); + switch (status_code) { + case 11: + *error = sweep_error_construct("Failed to set motor speed because provided parameter was invalid."); + return; + case 12: + *error = sweep_error_construct("Failed to set motor speed because prior speed has not yet stabilized."); + return; + default: + break; + } +} \ No newline at end of file diff --git a/sweepjs/README.md b/sweepjs/README.md index 6371ace..6d8aef2 100644 --- a/sweepjs/README.md +++ b/sweepjs/README.md @@ -42,10 +42,18 @@ sweep = new Sweep('/dev/ttyUSB0'); sweep.startScanning(); sweep.stopScanning(); +// waits until device is ready (calibration routine complete and motor speed stabilized) +sweep.waitUntilMotorReady(); +// ready === true if device is ready, false otherwise +ready = sweep.getMotorReady(); +// integer value between 0:10 (in HZ) speed = sweep.getMotorSpeed(); +// integer value between 0:10 (in HZ) sweep.setMotorSpeed(Number); +// integer value, either 500, 750 or 1000 (in HZ) rate = sweep.getSampleRate(); +// integer value, either 500, 750 or 1000 (in HZ) sweep.setSampleRate(Number); sweep.scan(function (err, samples) { diff --git a/sweepjs/index.js b/sweepjs/index.js index 6398a1c..a8a80a3 100644 --- a/sweepjs/index.js +++ b/sweepjs/index.js @@ -17,6 +17,7 @@ if (require.main === module) { console.log(util.format('Motor speed: %d Hz', speed)); console.log(util.format('Sample rate: %d Hz', rate)); + console.log('Starting data acquisition as soon as motor is ready...'); sweep.startScanning(); sweep.scan(function (err, samples) { @@ -26,7 +27,7 @@ if (require.main === module) { samples.forEach(function (sample) { var fmt = util.format('angle: %d distance %d signal strength: %d', - sample.angle, sample.distance, sample.signal); + sample.angle, sample.distance, sample.signal); console.log(fmt); }); }); diff --git a/sweepjs/sweepjs.cc b/sweepjs/sweepjs.cc index 67757e8..a19baa7 100755 --- a/sweepjs/sweepjs.cc +++ b/sweepjs/sweepjs.cc @@ -61,7 +61,9 @@ NAN_MODULE_INIT(Sweep::Init) { SetPrototypeMethod(fnTp, "startScanning", startScanning); SetPrototypeMethod(fnTp, "stopScanning", stopScanning); + SetPrototypeMethod(fnTp, "waitUntilMotorReady", waitUntilMotorReady); SetPrototypeMethod(fnTp, "scan", scan); + SetPrototypeMethod(fnTp, "getMotorReady", getMotorReady); SetPrototypeMethod(fnTp, "getMotorSpeed", getMotorSpeed); SetPrototypeMethod(fnTp, "setMotorSpeed", setMotorSpeed); SetPrototypeMethod(fnTp, "getSampleRate", getSampleRate); @@ -133,6 +135,16 @@ NAN_METHOD(Sweep::stopScanning) { ::sweep_device_stop_scanning(self->device.get(), ErrorToNanException{}); } +NAN_METHOD(Sweep::waitUntilMotorReady) { + auto* const self = Nan::ObjectWrap::Unwrap(info.Holder()); + + if (info.Length() != 0) { + return Nan::ThrowTypeError("No arguments expected"); + } + + ::sweep_device_wait_until_motor_ready(self->device.get(), ErrorToNanException{}); +} + class AsyncScanWorker final : public Nan::AsyncWorker { public: AsyncScanWorker(Nan::Callback* callback, std::shared_ptr<::sweep_device> device) @@ -200,6 +212,18 @@ NAN_METHOD(Sweep::scan) { Nan::AsyncQueueWorker(new AsyncScanWorker(callback, self->device)); } +NAN_METHOD(Sweep::getMotorReady) { + auto* const self = Nan::ObjectWrap::Unwrap(info.Holder()); + + if (info.Length() != 0) { + return Nan::ThrowTypeError("No arguments expected"); + } + + const auto ready = ::sweep_device_get_motor_ready(self->device.get(), ErrorToNanException{}); + + info.GetReturnValue().Set(Nan::New(ready)); +} + NAN_METHOD(Sweep::getMotorSpeed) { auto* const self = Nan::ObjectWrap::Unwrap(info.Holder()); diff --git a/sweepjs/sweepjs.h b/sweepjs/sweepjs.h index 5bc0bb6..3b842d2 100644 --- a/sweepjs/sweepjs.h +++ b/sweepjs/sweepjs.h @@ -16,9 +16,11 @@ class Sweep final : public Nan::ObjectWrap { static NAN_METHOD(startScanning); static NAN_METHOD(stopScanning); + static NAN_METHOD(waitUntilMotorReady); static NAN_METHOD(scan); + static NAN_METHOD(getMotorReady); static NAN_METHOD(getMotorSpeed); static NAN_METHOD(setMotorSpeed); diff --git a/sweeppy/README.md b/sweeppy/README.md index acdd947..8a47d73 100644 --- a/sweeppy/README.md +++ b/sweeppy/README.md @@ -62,10 +62,12 @@ class Sweep: def start_scanning(self) -> None def stop_scanning(self) -> None - def get_motor_speed(self) -> int + def wait_until_motor_ready(self) -> None + def get_motor_ready(self) -> bool + def get_motor_speed(self) -> int (Hz) def set_motor_speed(self, speed) -> None - def get_sample_rate(self) -> int + def get_sample_rate(self) -> int (Hz) def set_sample_rate(self, speed) -> None def get_scans(self) -> Iterable[Scan] diff --git a/sweeppy/sweeppy/__init__.py b/sweeppy/sweeppy/__init__.py index c7af5fd..aac3d4d 100644 --- a/sweeppy/sweeppy/__init__.py +++ b/sweeppy/sweeppy/__init__.py @@ -31,6 +31,9 @@ libsweep.sweep_device_stop_scanning.restype = None libsweep.sweep_device_stop_scanning.argtypes = [ctypes.c_void_p, ctypes.c_void_p] +libsweep.sweep_device_wait_until_motor_ready.restype = None +libsweep.sweep_device_wait_until_motor_ready.argtypes = [ctypes.c_void_p, ctypes.c_void_p] + libsweep.sweep_device_get_scan.restype = ctypes.c_void_p libsweep.sweep_device_get_scan.argtypes = [ctypes.c_void_p, ctypes.c_void_p] @@ -49,6 +52,9 @@ libsweep.sweep_scan_get_signal_strength.restype = ctypes.c_int32 libsweep.sweep_scan_get_signal_strength.argtypes = [ctypes.c_void_p, ctypes.c_int32] +libsweep.sweep_device_get_motor_ready.restype = ctypes.c_bool +libsweep.sweep_device_get_motor_ready.argtypes = [ctypes.c_void_p, ctypes.c_void_p] + libsweep.sweep_device_get_motor_speed.restype = ctypes.c_int32 libsweep.sweep_device_get_motor_speed.argtypes = [ctypes.c_void_p, ctypes.c_void_p] @@ -91,7 +97,7 @@ def __enter__(_): assert libsweep.sweep_is_abi_compatible(), 'Your installed libsweep is not ABI compatible with these bindings' - error = ctypes.c_void_p(); + error = ctypes.c_void_p() simple = not _.args[1] config = all(_.args) @@ -133,14 +139,34 @@ def start_scanning(_): raise _error_to_exception(error) def stop_scanning(_): - _._assert_scoped(); + _._assert_scoped() - error = ctypes.c_void_p(); + error = ctypes.c_void_p() libsweep.sweep_device_stop_scanning(_.device, ctypes.byref(error)) if error: raise _error_to_exception(error) + def wait_until_motor_ready(_): + _._assert_scoped() + + error = ctypes.c_void_p() + libsweep.sweep_device_wait_until_motor_ready(_.device, ctypes.byref(error)) + + if error: + raise _error_to_exception(error) + + def get_motor_ready(_): + _._assert_scoped() + + error = ctypes.c_void_p() + is_ready = libsweep.sweep_device_get_motor_ready(_.device, ctypes.byref(error)) + + if error: + raise _error_to_exception(error) + + return is_ready + def get_motor_speed(_): _._assert_scoped() @@ -205,9 +231,9 @@ def get_scans(_): def reset(_): - _._assert_scoped(); + _._assert_scoped() - error = ctypes.c_void_p(); + error = ctypes.c_void_p() libsweep.sweep_device_reset(_.device, ctypes.byref(error)) if error: diff --git a/sweeppy/sweeppy/__main__.py b/sweeppy/sweeppy/__main__.py index 2fd7dfa..9656c79 100644 --- a/sweeppy/sweeppy/__main__.py +++ b/sweeppy/sweeppy/__main__.py @@ -16,6 +16,7 @@ def main(): print('Motor Speed: {} Hz'.format(speed)) print('Sample Rate: {} Hz'.format(rate)) + # Starts scanning as soon as the motor is ready sweep.start_scanning() # get_scans is coroutine-based generator lazily returning scans ad infinitum From 2ba6d8ea384a8552fe41a4f99a721d22ee5bb1ac Mon Sep 17 00:00:00 2001 From: dcyoung Date: Fri, 21 Apr 2017 10:22:25 -0700 Subject: [PATCH 2/2] Incremented libsweep MAJOR version to reflect compatibility with sweep firmware v1.1 --- libsweep/CMakeLists.txt | 2 +- libsweep/README.md | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libsweep/CMakeLists.txt b/libsweep/CMakeLists.txt index fef0dc4..bfc1861 100644 --- a/libsweep/CMakeLists.txt +++ b/libsweep/CMakeLists.txt @@ -4,7 +4,7 @@ project(sweep C CXX) # Major for breaking ABI changes. Minor for features. Patch for bugfixes. -set(SWEEP_VERSION_MAJOR 0) +set(SWEEP_VERSION_MAJOR 1) set(SWEEP_VERSION_MINOR 1) set(SWEEP_VERSION_PATCH 0) diff --git a/libsweep/README.md b/libsweep/README.md index df9ba6d..e46add8 100644 --- a/libsweep/README.md +++ b/libsweep/README.md @@ -2,6 +2,8 @@ Low-level Scanse Sweep LiDAR library. Comes as C99 library `sweep.h` with optional C++11 header `sweep.hpp` on top of it. + + ### Quick Start ```bash @@ -87,6 +89,12 @@ Table of Contents: #### Version And ABI Management +Compatibility: + +| libsweep | sweep firmware | +| -------- | :------------: | +| v1.x.x | v1.1 | +| v0.x.x | v1.0 | ```c++ SWEEP_VERSION_MAJOR