Revision: 7080 http://playerstage.svn.sourceforge.net/playerstage/?rev=7080&view=rev Author: thjc Date: 2008-10-07 11:27:57 +0000 (Tue, 07 Oct 2008)
Log Message: ----------- applied patch 2136885 sicklms200 read() bug fix Modified Paths: -------------- code/player/trunk/server/drivers/laser/sicklms200.cc Modified: code/player/trunk/server/drivers/laser/sicklms200.cc =================================================================== --- code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-07 11:16:53 UTC (rev 7079) +++ code/player/trunk/server/drivers/laser/sicklms200.cc 2008-10-07 11:27:57 UTC (rev 7080) @@ -273,12 +273,29 @@ // Set the laser configuration // Returns 0 on success - int SetLaserConfig(bool intensity); + int SetLaserConfig(bool intensity, bool high_avail); + // Get the current configuration of the laser + // Returns 0 on success + int GetLaserConfig(uint8_t* packet, ssize_t packet_size); + + // Compare the laser configs to the desired configs + // Returns 0 if they are equal + int CheckLaserConfig(uint8_t* packet, ssize_t packet_size, + int intensity, int high_avail); + // Change the resolution of the laser int SetLaserRes(int angle, int res); + // Get the current status of the laser + // Returns 0 on success + int GetLaserStatus(uint8_t* packet, ssize_t packet_size); + // Compare the laser resolution to the desired resolution + // Returns 0 if they are equal + int CheckLaserRes(uint8_t* packet, ssize_t packet_size, + int width, int res); + // RequestLaserStopStream() // Returns 0 on success int RequestLaserStopStream(); @@ -341,6 +358,10 @@ // Turn intensity data on/off bool intensity; + // Set high availability mode -- if the unit continues after being + // dazzled by sunlight + bool high_avail; + // Is the laser upside-down? (if so, we'll reverse the ordering of the // readings) int invert; @@ -486,6 +507,7 @@ this->intensity = true; this->range_res = cf->ReadInt(section, "range_res", 1); this->invert = cf->ReadInt(section, "invert", 0); + this->high_avail = cf->ReadInt(section, "high_avail", 0); if (this->CheckScanConfig() != 0) PLAYER_ERROR("invalid scan configuration"); @@ -571,7 +593,8 @@ // Configure the laser if (SetLaserRes(this->scan_width, this->scan_res)) return 1; - if (SetLaserConfig(this->intensity)) + + if (SetLaserConfig(this->intensity, this->high_avail)) return 1; this->scan_id = 0; @@ -595,6 +618,8 @@ // switch to connect rate just in case if (this->connect_rate != this->current_rate) + // PBeeson -- This doesn't do anything. Once Setup() completes, + // these are equal, and if setup() fails, Shutdown() isn't run. if (SetLaserSpeed(this->connect_rate)) PLAYER_WARN1("Cannot throttle back to %d bauds", this->connect_rate); @@ -620,8 +645,6 @@ { player_laser_config_t * config = reinterpret_cast<player_laser_config_t *> (data); - int old_scan_width = this->scan_width; - int old_scan_res = this->scan_res; this->intensity = config->intensity; this->scan_res = (int) rint(RTOD(config->resolution)*100); @@ -639,14 +662,11 @@ PLAYER_ERROR("request for config mode failed"); else { - if(old_scan_width != this->scan_width || old_scan_res != this->scan_res) { - if (SetLaserRes(this->scan_width, this->scan_res) != 0) - PLAYER_ERROR("failed setting resolution [SetLaserRes()]"); - /* This call fails for me, but I've only tested with one laser - BPG - * */ - } - if(SetLaserConfig(this->intensity) != 0) - PLAYER_ERROR("failed setting intensity [SetLaserConfig()]"); + if (SetLaserRes(this->scan_width, this->scan_res) != 0) + PLAYER_ERROR("failed setting resolution [SetLaserRes()]"); + + if(SetLaserConfig(this->intensity, this->high_avail) != 0) + PLAYER_ERROR("failed setting configuration [SetLaserConfig()]"); } // Issue a new request for data @@ -1227,14 +1247,55 @@ //////////////////////////////////////////////////////////////////////////////// -// Set the laser configuration +// Compare Laser configuration to desired configuration // Returns 0 on success // -int SickLMS200::SetLaserConfig(bool intensity) +int SickLMS200::CheckLaserConfig(uint8_t* packet, ssize_t packet_size, + int intensity, int high_avail) { + + // Set high-availability level 3 mode + if (packet[5] != high_avail) + return 1; + + // Return intensity in top 3 data bits + if (packet[6] != intensity) + return 1; + + // Set the units for the range reading + if (this->range_res == 1) + { + if (packet[7] != 0x01) + return 1; + } + else if (this->range_res == 10) + { + if (packet[7] != 0x00) + return 1; + } + else if (this->range_res == 100) + { + if (packet[7] != 0x02) + return 1; + } + else + if (packet[7] != 0x01) + return 1; + + return 0; + +} + + + +//////////////////////////////////////////////////////////////////////////////// +// Get the laser configuration +// Returns 0 on success +// +int SickLMS200::GetLaserConfig(uint8_t* packet, ssize_t packet_size) +{ int tries; ssize_t len; - uint8_t packet[512]; for (tries = 0; tries < DEFAULT_LASER_RETRIES; tries++) { @@ -1247,37 +1308,65 @@ // Wait for laser to return data //PLAYER_MSG0(2, "waiting for reply"); - len = ReadFromLaser(packet, sizeof(packet), false, 25000, 5000); + len = ReadFromLaser(packet, packet_size, false, 25000, 5000); if (len < 0) return 1; else if (len < 1) { - PLAYER_WARN("SetLaserConfig(): timeout in ReadFromLaser() [1]"); + PLAYER_WARN("GetLaserConfig(): timeout in ReadFromLaser() [1]"); continue; } else if (packet[0] == NACK) { - PLAYER_ERROR("SetLaserConfig(): request denied by laser [1]"); + PLAYER_ERROR("GetLaserConfig(): request denied by laser [1]"); return 1; } else if (packet[0] != 0xF4) { - PLAYER_ERROR("SetLaserConfig(): unexpected packet type [1]"); + PLAYER_ERROR("GetLaserConfig(): unexpected packet type [1]"); return 1; } break; } - if (tries >= DEFAULT_LASER_RETRIES) + + return (tries >= DEFAULT_LASER_RETRIES); + +} + + +//////////////////////////////////////////////////////////////////////////////// +// Set the laser configuration +// Returns 0 on success +// +int SickLMS200::SetLaserConfig(bool intensity, bool high_avail) +{ + int tries; + ssize_t len; + uint8_t packet[512]; + + if (GetLaserConfig(packet,sizeof(packet))) return 1; + if (CheckLaserConfig(packet,sizeof(packet), + intensity,high_avail) == 0) + { + PLAYER_MSG0(2,"No need to flash redundant configuration to device."); + return 0; + } + //PLAYER_MSG0(2, "get configuration request ok"); //PLAYER_TRACE1("laser units [%d]", (int) packet[7]); + PLAYER_MSG0(2,"Flashing new configuration to device."); + for (tries = 0; tries < DEFAULT_LASER_RETRIES; tries++) { // Modify the configuration and send it back packet[0] = 0x77; + // Set high-availability level 3 mode + packet[5] = (high_avail ? 0x01 : 0x00); + // Return intensity in top 3 data bits packet[6] = (intensity ? 0x01 : 0x00); @@ -1291,6 +1380,8 @@ else packet[7] = 0x01; + len=8; + //PLAYER_MSG0(2, "sending set configuration request to laser"); if (WriteToLaser(packet, len) < 0) return 1; @@ -1325,9 +1416,76 @@ //////////////////////////////////////////////////////////////////////////////// +// Compare Laser Resolution to desired resolution +// Returns 0 on success +// +int SickLMS200::CheckLaserRes(uint8_t* packet, ssize_t packet_size, + int width, int res) +{ + + if ((packet[107] != (width & 0xFF)) || + (packet[108] != (width >> 8))) + return 1; + + if ((packet[109] != (res & 0xFF)) || + (packet[110] != (res >> 8))) + return 1; + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Get the laser status +// Returns 0 on success +// +int SickLMS200::GetLaserStatus(uint8_t* packet, ssize_t packet_size) +{ + int tries; + ssize_t len; + + for (tries = 0; tries < DEFAULT_LASER_RETRIES; tries++) + { + packet[0] = 0x31; + len = 1; + + //PLAYER_MSG0(2, "sending get status request to laser"); + if (WriteToLaser(packet, len) < 0) + return 1; + + // Wait for laser to return data + //PLAYER_MSG0(2, "waiting for reply"); + + len = ReadFromLaser(packet, packet_size, false, 25000, 5000); + if (len < 0) + return 1; + else if (len < 1) + { + PLAYER_WARN("GetLaserStatus(): timeout in ReadFromLaser() [1]"); + continue; + } + else if (packet[0] == NACK) + { + PLAYER_ERROR("GetLaserStatus(): request denied by laser [1]"); + return 1; + } + else if (packet[0] != 0xB1) + { + PLAYER_ERROR("GetLaserStatus(): unexpected packet type [1]"); + return 1; + } + break; + } + + return (tries >= DEFAULT_LASER_RETRIES); + +} + + +//////////////////////////////////////////////////////////////////////////////// // Change the resolution of the laser // Valid widths are: 100, 180 (degrees) -// Valid resolitions are: 25, 50, 100 (1/100 degree) +// Valid resolutions are: 25, 50, 100 (1/100 degree) // int SickLMS200::SetLaserRes(int width, int res) { @@ -1335,6 +1493,18 @@ ssize_t len; uint8_t packet[512]; + if (GetLaserStatus(packet,sizeof(packet))) + return 1; + + if (CheckLaserRes(packet,sizeof(packet), + width, res) == 0) + { + PLAYER_MSG0(2,"No need to send redundant resolution to device."); + return 0; + } + + PLAYER_MSG0(2,"Sending new resolution to device."); + for (tries = 0; tries < DEFAULT_LASER_RETRIES; tries++) { len = 0; @@ -1406,12 +1576,12 @@ // This could take a while... // PLAYER_MSG0(2, "waiting for acknowledge"); - len = ReadFromLaser(packet, sizeof(packet), true, 2500, 500); + len = ReadFromLaser(packet, sizeof(packet), true, 3000, 1000); if (len < 0) return 1; else if (len < 1) { - PLAYER_WARN("RequestLaserStopStream(): timeout in ReadFromLaser"); + PLAYER_MSG0(2,"RequestLaserStopStream(): timeout in ReadFromLaser -- may be using wrong baud rate."); continue; } else if (packet[0] == NACK) @@ -1690,6 +1860,8 @@ int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); if (retval < 0) { + if (errno == EINTR) + continue; PLAYER_ERROR("error on select (1)"); return 0; } @@ -1704,6 +1876,8 @@ bytes = ::read(this->laser_fd, header + sizeof(header) - 1, 1); if (bytes < 0) { + if (errno == EINTR) + continue; PLAYER_ERROR("error on read (1)"); return 0; } @@ -1732,7 +1906,7 @@ // Check for buffer overflows // if (len > maxlen) - RETURN_ERROR(0, "buffer overflow (len > max_len)"); + RETURN_ERROR(0, "buffer overflow (len > maxlen)"); // Read in the data // Note that we smooge the packet type from the header @@ -1757,6 +1931,8 @@ int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); if (retval < 0) { + if (errno == EINTR) + continue; PLAYER_ERROR("error on select (3)"); return 0; } @@ -1769,7 +1945,9 @@ int retval = ::read(this->laser_fd, data + bytes, len - bytes); if (retval < 0) { - PLAYER_ERROR("error on read (3)"); + if (errno == EINTR) + continue; + PLAYER_ERROR("error on read (3)"); return 0; } if (!retval) @@ -1800,6 +1978,8 @@ int retval = ::select(this->laser_fd + 1, &rfds, 0, &efds, &tv); if (retval < 0) { + if (errno == EINTR) + continue; PLAYER_ERROR("error on select (4)"); return 0; } @@ -1812,6 +1992,8 @@ int retval = ::read(this->laser_fd, footer + bytes, 3 - bytes); if (retval < 0) { + if (errno == EINTR) + continue; PLAYER_ERROR("error on read (4)"); return 0; } 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