Revision: 6992 http://playerstage.svn.sourceforge.net/playerstage/?rev=6992&view=rev Author: thjc Date: 2008-08-30 02:06:38 +0000 (Sat, 30 Aug 2008)
Log Message: ----------- applied [ 2016923 ] Error checking and better timeouts in SICK LMS200 Modified Paths: -------------- code/player/branches/release-2-1-patches/server/drivers/laser/sicklms200.cc Modified: code/player/branches/release-2-1-patches/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/branches/release-2-1-patches/server/drivers/laser/sicklms200.cc 2008-08-30 01:56:14 UTC (rev 6991) +++ code/player/branches/release-2-1-patches/server/drivers/laser/sicklms200.cc 2008-08-30 02:06:38 UTC (rev 6992) @@ -1661,43 +1661,6 @@ if(timeout_header == -1) timeout_header = timeout; - // If the timeout is infinite, - // go to blocking io - // - if (timeout < 0) - { - //PLAYER_MSG0(2, "using blocking io"); - int flags = ::fcntl(this->laser_fd, F_GETFL); - if (flags < 0) - { - PLAYER_ERROR("unable to get device flags"); - return 0; - } - if (::fcntl(this->laser_fd, F_SETFL, flags & (~O_NONBLOCK)) < 0) - { - PLAYER_ERROR("unable to set device flags"); - return 0; - } - } - // - // Otherwise, use non-blocking io - // - else - { - //PLAYER_MSG0(2, "using non-blocking io"); - int flags = ::fcntl(this->laser_fd, F_GETFL); - if (flags < 0) - { - PLAYER_ERROR("unable to get device flags"); - return 0; - } - if (::fcntl(this->laser_fd, F_SETFL, flags | O_NONBLOCK) < 0) - { - PLAYER_ERROR("unable to set device flags"); - return 0; - } - } - int64_t start_time = GetTime(); int64_t stop_time = start_time + timeout; int64_t stop_time_header = start_time + timeout_header; @@ -1714,10 +1677,42 @@ while (true) { if (timeout >= 0) - usleep(1000); + { + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time_header - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (1)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (1)"); + return 0; + } + } //printf("reading %d\n", timeout); fflush(stdout); bytes = ::read(this->laser_fd, header + sizeof(header) - 1, 1); + if (bytes < 0) + { + PLAYER_ERROR("error on read (1)"); + return 0; + } + if (!bytes) + { + PLAYER_MSG0(2, "eof on read (1)"); + return 0; + } //printf("bytes read %d\n", bytes); fflush(stdout); if (header[0] == STX && header[1] == 0x80) @@ -1728,12 +1723,6 @@ break; } memmove(header, header + 1, sizeof(header) - 1); - if (timeout >= 0 && GetTime() >= stop_time_header) - { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - PLAYER_MSG0(2, "timeout on read (1)"); - return 0; - } } // Determine data length @@ -1755,13 +1744,41 @@ while (bytes < len) { if (timeout >= 0) - usleep(1000); - bytes += ::read(this->laser_fd, data + bytes, len - bytes); - if (timeout >= 0 && GetTime() >= stop_time) { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - RETURN_ERROR(0, "timeout on read (3)"); + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (3)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (3)"); + return 0; + } } + int retval = ::read(this->laser_fd, data + bytes, len - bytes); + if (retval < 0) + { + PLAYER_ERROR("error on read (3)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "eof on read (3)"); + return 0; + } + bytes += retval; } // Read in footer @@ -1770,13 +1787,41 @@ while (bytes < 3) { if (timeout >= 0) - usleep(1000); - bytes += ::read(this->laser_fd, footer + bytes, 3 - bytes); - if (timeout >= 0 && GetTime() >= stop_time) { - //PLAYER_MSG2(2, "%Ld %Ld", GetTime(), stop_time); - RETURN_ERROR(0, "timeout on read (4)"); + fd_set rfds, efds; + FD_ZERO(&rfds); + FD_SET(this->laser_fd, &rfds); + FD_ZERO(&efds); + FD_SET(this->laser_fd, &efds); + int64_t delay = stop_time - GetTime(); + if (delay < 0) delay = 0; + struct timeval tv; + tv.tv_usec = (delay % 1000) * 1000; + tv.tv_sec = delay / 1000; + int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); + if (retval < 0) + { + PLAYER_ERROR("error on select (4)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "timeout on select (4)"); + return 0; + } } + int retval = ::read(this->laser_fd, footer + bytes, 3 - bytes); + if (retval < 0) + { + PLAYER_ERROR("error on read (4)"); + return 0; + } + if (!retval) + { + PLAYER_MSG0(2, "eof on read (4)"); + return 0; + } + bytes += retval; } // Construct entire packet This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit