From b3097a40229ac7d31a71a9175aac8a05ce390441 Mon Sep 17 00:00:00 2001 From: huangda1982 Date: Thu, 19 Sep 2024 11:11:25 +0900 Subject: [PATCH] Add urg3d_high_blocking_stop_data API. Use blocking stop in high API samples. Wait 1s after stop in low API samples. --- include/urg3d_sensor.h | 12 +++++++ samples/high_1_field_data_file.c | 4 +-- samples/high_1_frame_4_field_data_file.c | 4 +-- ...high_1_frame_4_vfield_2_hfield_data_file.c | 4 +-- samples/high_1_line_data_file.c | 4 +-- samples/high_get_auxiliary.c | 4 +-- samples/low_get_ax.c | 9 ++++- samples/low_get_multi_data.c | 7 ++++ samples/low_get_ri.c | 7 ++++ samples/low_get_ro.c | 7 ++++ samples/low_get_ro_with_error_reboot.c | 7 ++++ src/urg3d_sensor.c | 34 +++++++++++++++++++ 12 files changed, 92 insertions(+), 11 deletions(-) diff --git a/include/urg3d_sensor.h b/include/urg3d_sensor.h index 122d46b..a82fb0c 100644 --- a/include/urg3d_sensor.h +++ b/include/urg3d_sensor.h @@ -398,6 +398,18 @@ extern "C" { */ extern int urg3d_high_stop_data(urg3d_t * const urg, urg3d_measurement_type_t meas); + /*! + \brief request to stop data + + \param[in,out] urg : urg3d session + \param[in] meas : urg3d_measurement_type_t sturct, i.e. meas = URG3D_DISTANCE_INTENSITY + + \retval 1 succeeded + \retval 0 nothing happens + */ + extern int urg3d_high_blocking_stop_data(urg3d_t* const urg, urg3d_measurement_type_t meas); + + /*! \brief get "range and intensity" or "range only" user format from revceived data buffer diff --git a/samples/high_1_field_data_file.c b/samples/high_1_field_data_file.c index 7694181..bf07db4 100644 --- a/samples/high_1_field_data_file.c +++ b/samples/high_1_field_data_file.c @@ -162,8 +162,8 @@ int main(int argc, char *argv[]) /* * stop the acquisition flow started earlier using the start acquisition mode. */ - if((ret = urg3d_high_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { - printf("error urg3d_high_stop_data %d\n", ret); + if((ret = urg3d_high_blocking_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { + printf("error urg3d_high_blocking_stop_data %d\n", ret); ret = urg3d_close(&urg); #if defined(URG3D_MSC) getchar(); diff --git a/samples/high_1_frame_4_field_data_file.c b/samples/high_1_frame_4_field_data_file.c index 98fe547..ccd5a40 100644 --- a/samples/high_1_frame_4_field_data_file.c +++ b/samples/high_1_frame_4_field_data_file.c @@ -185,8 +185,8 @@ int main(int argc, char *argv[]) /* * stop the acquisition flow started earlier using the start acquisition mode. */ - if((ret = urg3d_high_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { - printf("error urg3d_high_stop_data %d\n", ret); + if((ret = urg3d_high_blocking_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { + printf("error urg3d_high_blocking_stop_data %d\n", ret); ret = urg3d_close(&urg); #if defined(URG3D_MSC) getchar(); diff --git a/samples/high_1_frame_4_vfield_2_hfield_data_file.c b/samples/high_1_frame_4_vfield_2_hfield_data_file.c index 2ff1534..8ec403c 100644 --- a/samples/high_1_frame_4_vfield_2_hfield_data_file.c +++ b/samples/high_1_frame_4_vfield_2_hfield_data_file.c @@ -197,8 +197,8 @@ int main(int argc, char *argv[]) /* * stop the acquisition flow started earlier using the start acquisition mode. */ - if((ret = urg3d_high_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { - printf("error urg3d_high_stop_data %d\n", ret); + if((ret = urg3d_high_blocking_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { + printf("error urg3d_high_blocking_stop_data %d\n", ret); ret = urg3d_close(&urg); #if defined(URG3D_MSC) getchar(); diff --git a/samples/high_1_line_data_file.c b/samples/high_1_line_data_file.c index eedbc31..d0d3f1b 100644 --- a/samples/high_1_line_data_file.c +++ b/samples/high_1_line_data_file.c @@ -138,8 +138,8 @@ int main(int argc, char *argv[]) /* * stop the acquisition flow started earlier using the start acquisition mode. */ - if((ret = urg3d_high_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { - printf("error urg3d_high_stop_data %d\n", ret); + if((ret = urg3d_high_blocking_stop_data(&urg, URG3D_DISTANCE_INTENSITY)) < 0) { + printf("error urg3d_high_blocking_stop_data %d\n", ret); ret = urg3d_close(&urg); #if defined(URG3D_MSC) getchar(); diff --git a/samples/high_get_auxiliary.c b/samples/high_get_auxiliary.c index d744818..732a97f 100644 --- a/samples/high_get_auxiliary.c +++ b/samples/high_get_auxiliary.c @@ -295,8 +295,8 @@ int main(int argc, char *argv[]) } // stop data stream - if((ret = urg3d_high_stop_data(&urg, URG3D_AUXILIARY)) < 0) { - printf("error urg3d_high_stop_data %d\n", ret); + if((ret = urg3d_high_blocking_stop_data(&urg, URG3D_AUXILIARY)) < 0) { + printf("error urg3d_high_blocking_stop_data %d\n", ret); ret = urg3d_close(&urg); #if defined(URG3D_MSC) getchar(); diff --git a/samples/low_get_ax.c b/samples/low_get_ax.c index 0633c1b..6f7061c 100644 --- a/samples/low_get_ax.c +++ b/samples/low_get_ax.c @@ -104,7 +104,7 @@ int main(int argc, char *argv[]) } } } else { - #ifdef URG3D_WINDOWS_OS + #ifdef URG3D_WINDOWS_OS Sleep(10); #else usleep(10000); @@ -132,6 +132,13 @@ int main(int argc, char *argv[]) printf("send ok -> DAT:ax=0\n"); } + // wait stop + #ifdef URG3D_WINDOWS_OS + Sleep(1000); + #else + usleep(1000000); + #endif + /* * close the connection to the sensor */ diff --git a/samples/low_get_multi_data.c b/samples/low_get_multi_data.c index cf358c4..2a4b88b 100644 --- a/samples/low_get_multi_data.c +++ b/samples/low_get_multi_data.c @@ -141,6 +141,13 @@ int main(int argc, char *argv[]) printf("send ok -> DAT:ax=0\n"); } + // wait stop + #ifdef URG3D_WINDOWS_OS + Sleep(1000); + #else + usleep(1000000); + #endif + /* * close the connection to the sensor */ diff --git a/samples/low_get_ri.c b/samples/low_get_ri.c index 941203b..f724a6e 100644 --- a/samples/low_get_ri.c +++ b/samples/low_get_ri.c @@ -142,6 +142,13 @@ int main(int argc, char *argv[]) printf("send ok -> DAT:ri=0\n"); } + // wait stop + #ifdef URG3D_WINDOWS_OS + Sleep(1000); + #else + usleep(1000000); + #endif + /* * close the connection to the sensor */ diff --git a/samples/low_get_ro.c b/samples/low_get_ro.c index 3a1e2c4..790fad2 100644 --- a/samples/low_get_ro.c +++ b/samples/low_get_ro.c @@ -142,6 +142,13 @@ int main(int argc, char *argv[]) printf("send ok -> DAT:ro=0\n"); } + // wait stop + #ifdef URG3D_WINDOWS_OS + Sleep(1000); + #else + usleep(1000000); + #endif + /* * close the connection to the sensor */ diff --git a/samples/low_get_ro_with_error_reboot.c b/samples/low_get_ro_with_error_reboot.c index 2c7e6d7..d9253bc 100644 --- a/samples/low_get_ro_with_error_reboot.c +++ b/samples/low_get_ro_with_error_reboot.c @@ -211,6 +211,13 @@ int main(int argc, char *argv[]) printf("send success -> DAT:ro=0\n"); } + // wait stop + #ifdef URG3D_WINDOWS_OS + Sleep(1000); + #else + usleep(1000000); + #endif + // disconnect sensor if((ret = urg3d_close(&urg)) < 0) { printf("error urg3d_close %d\n", ret); diff --git a/src/urg3d_sensor.c b/src/urg3d_sensor.c index df2bf10..4d71d0b 100644 --- a/src/urg3d_sensor.c +++ b/src/urg3d_sensor.c @@ -37,6 +37,20 @@ int urg3d_open(urg3d_t* const urg return urg->last_errno; } + // stop the _ro/_ri data acquisition. + if ((ret = urg3d_high_stop_data(urg, URG3D_DISTANCE_INTENSITY)) < 0) { + urg3d_connection_close(&urg->connection); + urg->last_errno = URG3D_INVALID_RESPONSE; + return urg->last_errno; + } + + // stop the _ax data acquisition. + if ((ret = urg3d_high_stop_data(urg, URG3D_AUXILIARY)) < 0) { + urg3d_connection_close(&urg->connection); + urg->last_errno = URG3D_INVALID_RESPONSE; + return urg->last_errno; + } + urg->is_active = URG3D_TRUE; return 0; } @@ -790,6 +804,26 @@ int urg3d_high_stop_data(urg3d_t* const urg return urg3d_low_request_command(urg, command_list[meas]); } +int urg3d_high_blocking_stop_data(urg3d_t* const urg + , urg3d_measurement_type_t meas) +{ + urg3d_vssp_header_t header; + char data[URG3D_MAX_RX_LENGTH] = { 0 }; + const char command_list[4][10] = { "", "DAT:ro=0\n", "DAT:ri=0\n", "DAT:ax=0\n" }; + int ret = 0; + + if (meas == URG3D_NO_REQUEST) { + return 0; + } + + ret = urg3d_high_blocking_common(urg, &header, data, command_list[meas], "DAT", command_list[meas]); + if (ret < 0) { + return ret; + } + + return 1; +} + int urg3d_high_get_measurement_data(urg3d_t * const urg , urg3d_measurement_data_t *data) {