Update of /cvsroot/playerstage/code/player/server/drivers/laser
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/laser
Modified Files:
Tag: release-2-0-patches
sicklms200.cc urglaserdriver.cc
Log Message:
backported lots of stuff from HEAD
Index: urglaserdriver.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/laser/urglaserdriver.cc,v
retrieving revision 1.7
retrieving revision 1.7.2.1
diff -C2 -d -r1.7 -r1.7.2.1
*** urglaserdriver.cc 16 Mar 2006 01:55:23 -0000 1.7
--- urglaserdriver.cc 7 Jun 2006 16:12:48 -0000 1.7.2.1
***************
*** 245,248 ****
--- 245,254 ----
Publish(device_addr,resp_queue,
PLAYER_MSGTYPE_RESP_ACK,hdr->subtype,&Geom,sizeof(Geom),NULL);
}
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_LASER_REQ_GET_CONFIG,
+ this->device_addr))
+ {
+ Publish(device_addr,resp_queue,
PLAYER_MSGTYPE_RESP_ACK,hdr->subtype,&Conf,sizeof(Conf),NULL);
+ }
else
{
Index: sicklms200.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/laser/sicklms200.cc,v
retrieving revision 1.58
retrieving revision 1.58.2.1
diff -C2 -d -r1.58 -r1.58.2.1
*** sicklms200.cc 6 Mar 2006 23:05:38 -0000 1.58
--- sicklms200.cc 7 Jun 2006 16:12:48 -0000 1.58.2.1
***************
*** 60,64 ****
USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".
! - rate (integer)
- Default: 38400
- Baud rate. Valid values are 9600, 38400 (RS232 or RS422) and
--- 60,71 ----
USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".
! - connect_rate (integer)
! - Rate used when stablishing connection with the laser.
! - Default: 9600
! - Baud rate. Valid values are 9600, 38400 (RS232 or RS422) and
! 500000 (RS422 only).
!
! - transfer_rate (integer)
! - Rate desired for data transfers, negotiated after connection
- Default: 38400
- Baud rate. Valid values are 9600, 38400 (RS232 or RS422) and
***************
*** 72,76 ****
- delay (integer)
- Default: 0
! - Delay (in seconds) before laser is initialized (set this to 35 if
you have a newer generation Pioneer whose laser is switched on
when the serial port is open).
--- 79,83 ----
- delay (integer)
- Default: 0
! - Delay (in seconds) before laser is initialized (set this to 32-35 if
you have a newer generation Pioneer whose laser is switched on
when the serial port is open).
***************
*** 154,158 ****
#define DEFAULT_LASER_PORT "/dev/ttyS1"
! #define DEFAULT_LASER_PORT_RATE 38400
#define DEFAULT_LASER_RETRIES 3
--- 161,166 ----
#define DEFAULT_LASER_PORT "/dev/ttyS1"
! #define DEFAULT_LASER_CONNECT_RATE 9600
! #define DEFAULT_LASER_TRANSFER_RATE 38400
#define DEFAULT_LASER_RETRIES 3
***************
*** 276,281 ****
bool can_do_hi_speed;
! int port_rate;
! int current_rate;
int scan_id;
--- 284,290 ----
bool can_do_hi_speed;
! int connect_rate; // Desired rate for first connection
! int transfer_rate; // Desired rate for operation
! int current_rate; // Current rate
int scan_id;
***************
*** 327,332 ****
// Serial rate
! this->port_rate = cf->ReadInt(section, "rate", DEFAULT_LASER_PORT_RATE);
! this->current_rate = this->port_rate;
this->retry_limit = cf->ReadInt(section, "retry", 0) + 1;
--- 336,342 ----
// Serial rate
! this->connect_rate = cf->ReadInt(section, "connect_rate",
DEFAULT_LASER_CONNECT_RATE);
! this->transfer_rate = cf->ReadInt(section, "transfer_rate",
DEFAULT_LASER_TRANSFER_RATE);
! this->current_rate = 0;
this->retry_limit = cf->ReadInt(section, "retry", 0) + 1;
***************
*** 337,344 ****
#endif
! if (!this->can_do_hi_speed && this->port_rate > 38400)
{
! PLAYER_ERROR("sicklms200: requested hi speed serial, but no support
compiled in. Defaulting to 38400 bps.");
! this->port_rate = 38400;
}
--- 347,362 ----
#endif
! if (!this->can_do_hi_speed && this->connect_rate > 38400)
{
! PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support
compiled in. Defaulting to %d bps.",
! DEFAULT_LASER_CONNECT_RATE);
! this->connect_rate = DEFAULT_LASER_CONNECT_RATE;
! }
!
! if (!this->can_do_hi_speed && this->transfer_rate > 38400)
! {
! PLAYER_ERROR1("sicklms200: requested hi speed serial, but no support
compiled in. Defaulting to %d bps.",
! DEFAULT_LASER_TRANSFER_RATE);
! this->connect_rate = DEFAULT_LASER_TRANSFER_RATE;
}
***************
*** 374,379 ****
int SickLMS200::Setup()
{
- int rate = 0;
-
PLAYER_MSG1(2, "Laser initialising (%s)", this->device_name);
--- 392,395 ----
***************
*** 389,436 ****
{
// Try connecting at the given rate
! PLAYER_MSG1(2, "connecting at %d", this->port_rate);
! if (ChangeTermSpeed(this->port_rate))
return 1;
if (SetLaserMode() == 0)
! rate = this->port_rate;
else if (SetLaserMode() == 0)
! rate = this->port_rate;
!
! // Try connecting at 9600
! if (rate == 0 && this->port_rate != 9600)
! {
! PLAYER_MSG0(2, "connecting at 9600");
! if (ChangeTermSpeed(9600))
! return 1;
! if (SetLaserMode() == 0)
! rate = 9600;
! }
!
! // Try connecting at 38400
! if (rate == 0 && this->port_rate >= 38400)
! {
! PLAYER_MSG0(2, "connecting at 38400");
! if (ChangeTermSpeed(38400))
! return 1;
! if (SetLaserMode() == 0)
! rate = 38400;
! }
!
! // Try connecting at 500000
! if (rate == 0 && this->port_rate >= 500000 && this->can_do_hi_speed)
! {
! PLAYER_MSG0(2, "connecting at 500000");
! if (ChangeTermSpeed(500000))
! return 1;
! if (SetLaserMode() == 0)
! rate = 500000;
! }
! if(rate != 0)
break;
}
// Could not find the laser
! if (rate == 0)
{
PLAYER_ERROR("unable to connect to laser");
--- 405,422 ----
{
// Try connecting at the given rate
! PLAYER_MSG1(2, "connecting at %d", this->connect_rate);
! if (ChangeTermSpeed(this->connect_rate))
return 1;
if (SetLaserMode() == 0)
! this->current_rate = this->connect_rate;
else if (SetLaserMode() == 0)
! this->current_rate = this->connect_rate;
! if(this->current_rate != 0)
break;
}
// Could not find the laser
! if (this->current_rate == 0)
{
PLAYER_ERROR("unable to connect to laser");
***************
*** 438,449 ****
}
! // Jump up to 38400
! if (rate != 38400 && this->port_rate == 38400)
{
! PLAYER_MSG2(2, "laser operating at %d; changing to %d", rate,
this->port_rate);
! if (SetLaserSpeed(38400))
return 1;
sleep(1);
! if (ChangeTermSpeed(38400))
return 1;
sleep(1);
--- 424,435 ----
}
! // Jump up to 38400 rate
! if (this->current_rate != this->transfer_rate && this->transfer_rate ==
38400)
{
! PLAYER_MSG2(2, "laser operating at %d; changing to %d",
this->current_rate, this->transfer_rate);
! if (SetLaserSpeed(this->transfer_rate))
return 1;
sleep(1);
! if (ChangeTermSpeed(this->transfer_rate))
return 1;
sleep(1);
***************
*** 451,461 ****
// Jump up to 500000
! else if (rate != 500000 && this->port_rate == 500000 &&
this->can_do_hi_speed)
{
! PLAYER_MSG2(2, "laser operating at %d; changing to %d", rate,
this->port_rate);
! if (SetLaserSpeed(this->port_rate))
return 1;
sleep(1);
! if (ChangeTermSpeed(this->port_rate))
return 1;
sleep(1);
--- 437,447 ----
// Jump up to 500000
! else if (this->current_rate != 500000 && this->transfer_rate == 500000 &&
this->can_do_hi_speed)
{
! PLAYER_MSG2(2, "laser operating at %d; changing to %d",
this->current_rate, this->transfer_rate);
! if (SetLaserSpeed(this->transfer_rate))
return 1;
sleep(1);
! if (ChangeTermSpeed(this->transfer_rate))
return 1;
sleep(1);
***************
*** 463,469 ****
// Dont know this rate
! else if (rate != this->port_rate)
{
! PLAYER_ERROR1("unsupported rate %d", this->port_rate);
return 1;
}
--- 449,455 ----
// Dont know this rate
! else if (this->current_rate != this->transfer_rate)
{
! PLAYER_ERROR1("unsupported transfer rate %d", this->transfer_rate);
return 1;
}
***************
*** 474,478 ****
if (GetLaserType(type, sizeof(type)))
return 1;
! PLAYER_MSG3(2, "SICK laser type [%s] at [%s:%d]", type, this->device_name,
this->port_rate);
// Configure the laser
--- 460,464 ----
if (GetLaserType(type, sizeof(type)))
return 1;
! PLAYER_MSG3(2, "SICK laser type [%s] at [%s:%d]", type, this->device_name,
this->transfer_rate);
// Configure the laser
***************
*** 499,502 ****
--- 485,493 ----
// shutdown laser device
StopThread();
+
+ // switch to connect rate just in case
+ if (this->connect_rate != this->current_rate)
+ if (SetLaserSpeed(this->connect_rate))
+ PLAYER_WARN1("Cannot throttle back to %d bauds", this->connect_rate);
CloseTerm();
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
