Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200 In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv25284/server/drivers/position/nav200 Modified Files: nav200.cc nav200.h sicknav200.cc test.cpp test_program.cpp test_program_init.cpp Log Message: added serial stream opaque driver and modified s3000 and nav200 drivers to use it Index: nav200.h =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/nav200.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** nav200.h 25 Sep 2006 15:53:56 -0000 1.2 --- nav200.h 6 Dec 2007 02:35:36 -0000 1.3 *************** *** 26,32 **** //#include <stdint.h> - #define DEFAULT_PORT "/dev/ttyS0" - #define DEFAULT_RATE B19200 - #define STX 0x02 --- 26,29 ---- *************** *** 91,98 **** public: Nav200(); ~Nav200(); ! int Initialise(const char * port = DEFAULT_PORT, int rate = DEFAULT_RATE); int Terminate(); --- 88,96 ---- public: + friend class SickNAV200; Nav200(); ~Nav200(); ! int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id); int Terminate(); *************** *** 156,159 **** --- 154,165 ---- int WriteCommand(char mode, char function, int dataLength, uint8_t * data); uint8_t CreateCRC(uint8_t* data, ssize_t len); + + // SickNav200 Driver info + Driver *sn200; + + // Opaque info - for setting filter + Device *opaque; + player_devaddr_t opaque_id; + }; Index: test_program_init.cpp =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/test_program_init.cpp,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** test_program_init.cpp 2 Dec 2007 09:11:12 -0000 1.2 --- test_program_init.cpp 6 Dec 2007 02:35:36 -0000 1.3 *************** *** 1,3 **** ! #include <stdlib.h> #include <time.h> #include "nav200.h" --- 1,3 ---- ! /*#include <stdlib.h> #include <time.h> #include "nav200.h" *************** *** 103,104 **** --- 103,108 ---- } + */ + + int main() + {} \ No newline at end of file Index: nav200.cc =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/nav200.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** nav200.cc 14 Mar 2007 23:19:28 -0000 1.2 --- nav200.cc 6 Dec 2007 02:35:36 -0000 1.3 *************** *** 11,54 **** } ! int Nav200::Initialise(const char * port, int rate) { bytesReceived = 0; - fd = -1; - - // open the serial port - fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK); - if ( fd<0 ) - { - fprintf(stderr, "Could not open serial device %s\n",port); - return -1; - } - - // save the current io settings - tcgetattr(fd, &oldtio); - - // set up new settings - struct termios newtio; - memset(&newtio, 0,sizeof(newtio)); - newtio.c_cflag = CS8 | CREAD | PARENB; - newtio.c_iflag = INPCK; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - - // activate new settings - tcflush(fd, TCIFLUSH); - if (cfsetispeed(&newtio, rate) < 0 || cfsetospeed(&newtio, rate) < 0) - { - fprintf(stderr,"Failed to set serial baud rate: %d\n", rate); - tcsetattr(fd, TCSANOW, &oldtio); - close(fd); - fd = -1; - return -1; - } - tcsetattr(fd, TCSANOW, &newtio); - tcflush(fd, TCIOFLUSH); - // clear the input buffer in case junk data is on the port - usleep(10000); - tcflush(fd, TCIFLUSH); return 0; } --- 11,21 ---- } ! int Nav200::Initialise(Driver* sn2002, Device* opaque2, player_devaddr_t opaque_id2 ) { + this->sn200 = sn2002; + this->opaque_id = opaque_id2; + this->opaque = opaque2; bytesReceived = 0; return 0; } *************** *** 56,65 **** int Nav200::Terminate() { - // restore old port settings - if (fd > 0) - { - tcsetattr(fd, TCSANOW, &oldtio); - close(fd); - } return 0; } --- 23,26 ---- *************** *** 68,71 **** --- 29,33 ---- * Standby mode *******************/ + bool Nav200::EnterStandby() { *************** *** 589,594 **** int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t * data) { ! if (fd < 0) ! return -1; int length = dataLength+5; --- 551,556 ---- int Nav200::WriteCommand(char mode, char function, int dataLength, uint8_t * data) { ! /* if (fd < 0) ! return -1;*/ int length = dataLength+5; *************** *** 607,614 **** buffer[length-1] = CreateCRC(buffer, 4 + dataLength); ! // Make sure both input and output queues are empty ! tcflush(fd, TCIOFLUSH); ! // switch to blocking IO for the write int flags = fcntl(fd, F_GETFL); if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0) --- 569,576 ---- buffer[length-1] = CreateCRC(buffer, 4 + dataLength); ! /* // Make sure both input and output queues are empty ! tcflush(fd, TCIOFLUSH);*/ ! /* // switch to blocking IO for the write int flags = fcntl(fd, F_GETFL); if (flags < 0 || fcntl(fd,F_SETFL,flags &~O_NONBLOCK) < 0) *************** *** 620,625 **** return -1; } ! ! if((length && (write(fd, buffer, length)) < length)) { fprintf(stderr,"Error writing to FOB (%d - %s), disabling\n",errno,strerror(errno)); --- 582,592 ---- return -1; } ! */ ! player_opaque_data_t mData; ! mData.data_count = length; ! mData.data = buffer; ! opaque->PutMsg(sn200->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData), 0, NULL); ! ! /* if((length && (write(fd, buffer, length)) < length)) { fprintf(stderr,"Error writing to FOB (%d - %s), disabling\n",errno,strerror(errno)); *************** *** 628,634 **** delete [] buffer; return -1; ! } ! // restore flags if (fcntl(fd,F_SETFL,flags) < 0) { --- 595,601 ---- delete [] buffer; return -1; ! }*/ ! /* // restore flags if (fcntl(fd,F_SETFL,flags) < 0) { *************** *** 638,642 **** delete [] buffer; return -1; ! } delete [] buffer; --- 605,609 ---- delete [] buffer; return -1; ! }*/ delete [] buffer; *************** *** 650,654 **** int Nav200::ReadFromNav200(int timeout_usec) { ! int ret; int dataLength = 0; --- 617,621 ---- int Nav200::ReadFromNav200(int timeout_usec) { ! int dataLength = 0; *************** *** 656,659 **** --- 623,628 ---- gettimeofday(&start,NULL); + sn200->InQueue->SetFilter(opaque_id.host, opaque_id.robot, opaque_id.interf, opaque_id.index, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE); + for (;;) { *************** *** 664,688 **** return -1; } ! ! ret = read(fd, &receivedBuffer[bytesReceived], BUFFER_SIZE - bytesReceived); ! if (ret < 0) ! { ! fprintf(stderr,"Got error while reading %d %s\n",errno, strerror(errno)); ! return ret; ! } ! ! bytesReceived += ret; ! ! if (ret == 0) ! { ! usleep(1000); ! continue; ! } ! // do we have enough for a header? while (bytesReceived > 4) { if (receivedBuffer[0] != STX) ! { // bad thing, we dont have the correct start to a message for(int i=1; i<bytesReceived; i++) --- 633,646 ---- return -1; } ! //puts("waiting for data"); ! sn200->ProcessMessages(); ! // do we have enough for a header? while (bytesReceived > 4) { + //PLAYER_MSG4(2, "recieved STX %d data len %d mode %c fun %c", receivedBuffer[0], receivedBuffer[1], receivedBuffer[2], receivedBuffer[3]); if (receivedBuffer[0] != STX) ! { ! bool found = false; // bad thing, we dont have the correct start to a message for(int i=1; i<bytesReceived; i++) *************** *** 692,697 **** --- 650,662 ---- memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i); bytesReceived-=i; + found = true; + break; } } + // If none of the data points are STX then all are (essentially) removed + if (!found) + { + bytesReceived = 0; + } continue; } *************** *** 711,714 **** --- 676,680 ---- {// bad thing, we dont have the correct start to a message fprintf(stderr,"bad CRC!!!\n"); + bool found = false; for(int i=1; i<bytesReceived; i++) { // find where STX is *************** *** 717,722 **** --- 683,695 ---- memmove(receivedBuffer, receivedBuffer+i, bytesReceived-i); bytesReceived-=i; + found = true; + break; } } + // If none of the data points are STX then all are (essentially) removed + if (!found) + { + bytesReceived = 0; + } continue; } *************** *** 732,736 **** //check out what the error is and it out PrintErrorMsg(); ! return -2; } --- 705,709 ---- //check out what the error is and it out PrintErrorMsg(); ! sn200->InQueue->ClearFilter(); return -2; } *************** *** 746,753 **** --- 719,728 ---- memmove(receivedBuffer, receivedBuffer+dataLength, bytesReceived-dataLength); bytesReceived-=dataLength; + sn200->InQueue->ClearFilter(); return 1; } } } + sn200->InQueue->ClearFilter(); return 0; } Index: sicknav200.cc =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/sicknav200.cc,v retrieving revision 1.8 retrieving revision 1.9 diff -C2 -d -r1.8 -r1.9 *** sicknav200.cc 21 Sep 2007 03:31:50 -0000 1.8 --- sicknav200.cc 6 Dec 2007 02:35:36 -0000 1.9 *************** *** 48,52 **** @par Requires ! - none @par Configuration requests --- 48,52 ---- @par Requires ! - @ref opaque @par Configuration requests *************** *** 56,64 **** @par Configuration file options - - port (string) - - Default: "/dev/ttyS0" - - Serial port to which laser is attached. If you are using a - USB/232 or USB/422 converter, this will be "/dev/ttyUSBx". - - pose (length tuple) - Default: [0.0 0.0 0.0] --- 56,59 ---- *************** *** 77,82 **** name "sicknav200" provides ["position2d:0"] ! port "/dev/ttyS0" ) @endverbatim --- 72,84 ---- name "sicknav200" provides ["position2d:0"] ! requires ["opaque:0"] ) + driver + ( + name "serialstream" + provides ["opaque:0"] + port "/dev/ttyS0" + ) + @endverbatim *************** *** 107,111 **** #include <libplayercore/playercore.h> ! #include <replace/replace.h> extern PlayerTime* GlobalTime; --- 109,113 ---- #include <libplayercore/playercore.h> ! // #include <replace/replace.h> extern PlayerTime* GlobalTime; *************** *** 140,144 **** // Name of device used to communicate with the laser ! char *device_name; // storage for outgoing data --- 142,148 ---- // Name of device used to communicate with the laser ! //char *device_name; ! ! bool nchanged; // storage for outgoing data *************** *** 149,152 **** --- 153,159 ---- int min_radius, max_radius; + // Opaque Driver info + Device *opaque; + player_devaddr_t opaque_id; }; *************** *** 161,164 **** --- 168,172 ---- void SickNAV200_Register(DriverTable* table) { + puts("Registering driver"); table->AddDriver("sicknav200", SickNAV200_Init); } *************** *** 181,191 **** this->size[1] = 0.15; ! // Serial port ! this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT)); // nav200 parameters, convert to cm this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius", 1) * 100); this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius", 30) * 100); ! return; } --- 189,213 ---- this->size[1] = 0.15; ! nchanged = true; ! ! // Serial port - done in the opaque driver ! //this->device_name = strdup(cf->ReadString(section, "port", DEFAULT_PORT)); // nav200 parameters, convert to cm this->min_radius = static_cast<int> (cf->ReadLength(section, "min_radius", 1) * 100); this->max_radius = static_cast<int> (cf->ReadLength(section, "max_radius", 30) * 100); ! ! this->opaque = NULL; ! // Must have an opaque device ! PLAYER_MSG0(2, "reading opaque id now"); ! if (cf->ReadDeviceAddr(&this->opaque_id, section, "requires", ! PLAYER_OPAQUE_CODE, -1, NULL) != 0) ! { ! PLAYER_MSG0(2, "No opaque driver specified"); ! this->SetError(-1); ! return; ! } ! PLAYER_MSG0(2, "reading opaque id now"); ! return; } *************** *** 193,197 **** SickNAV200::~SickNAV200() { ! free(device_name); } --- 215,219 ---- SickNAV200::~SickNAV200() { ! //free(device_name); } *************** *** 201,218 **** int SickNAV200::Setup() { ! PLAYER_MSG1(2, "NAV200 initialising (%s)", this->device_name); ! ! // Open the terminal ! Laser.Initialise(this->device_name); ! if (!Laser.EnterStandby() || !Laser.EnterPositioning()) { ! PLAYER_ERROR("unable to enter standby or position mode\n"); ! return -1;; } ! if (!Laser.SetActionRadii(min_radius, max_radius)) { ! PLAYER_ERROR("failed to set action radii\n"); ! return -1;; } PLAYER_MSG0(2, "NAV200 ready"); --- 223,251 ---- int SickNAV200::Setup() { ! PLAYER_MSG0(2, "NAV200 initialising"); ! ! // Subscribe to the opaque device. ! if(Device::MatchDeviceAddress(this->opaque_id, this->device_addr)) { ! PLAYER_ERROR("attempt to subscribe to self"); ! return(-1); } ! ! if(!(this->opaque = deviceTable->GetDevice(this->opaque_id))) { ! PLAYER_ERROR("unable to locate suitable opaque device"); ! return(-1); } + + if(this->opaque->Subscribe(this->InQueue) != 0) + { + PLAYER_ERROR("unable to subscribe to opaque device"); + return(-1); + } + + // Open the terminal + Laser.Initialise(this, opaque, opaque_id); + puts("Laser initilised"); + PLAYER_MSG0(2, "NAV200 ready"); *************** *** 225,228 **** --- 258,262 ---- + //////////////////////////////////////////////////////////////////////////////// // Shutdown the device *************** *** 232,235 **** --- 266,271 ---- StopThread(); + opaque->Unsubscribe(InQueue); + PLAYER_MSG0(2, "laser shutdown"); *************** *** 243,246 **** --- 279,290 ---- void * data) { + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, opaque_id)) + { + player_opaque_data_t * recv = reinterpret_cast<player_opaque_data_t * > (data); + memmove(&Laser.receivedBuffer[Laser.bytesReceived], recv->data, recv->data_count); + Laser.bytesReceived += recv->data_count; + return 0; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, *************** *** 279,282 **** --- 323,340 ---- ProcessMessages(); + if (nchanged) + { + if (!Laser.EnterStandby() || !Laser.EnterPositioning()) + { + PLAYER_ERROR("unable to enter standby or position mode\n"); + return ; + } + if (!Laser.SetActionRadii(min_radius, max_radius)) + { + PLAYER_ERROR("failed to set action radii\n"); + return ; + } + nchanged = false; + } // get update and publish result if(Laser.GetPositionAuto(Reading)) *************** *** 309,311 **** - --- 367,368 ---- Index: test_program.cpp =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/test_program.cpp,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** test_program.cpp 1 Dec 2006 02:47:52 -0000 1.3 --- test_program.cpp 6 Dec 2007 02:35:36 -0000 1.4 *************** *** 1,3 **** ! #include <stdlib.h> #include "nav200.h" --- 1,3 ---- ! /*#include <stdlib.h> #include "nav200.h" *************** *** 36,37 **** --- 36,41 ---- return 0; } + */ + + int main() + {} \ No newline at end of file Index: test.cpp =================================================================== RCS file: /cvsroot/playerstage/code/player/server/drivers/position/nav200/test.cpp,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** test.cpp 18 Dec 2006 12:17:22 -0000 1.2 --- test.cpp 6 Dec 2007 02:35:36 -0000 1.3 *************** *** 1,3 **** ! #include <stdlib.h> #include <time.h> #include "nav200.h" --- 1,3 ---- ! /*#include <stdlib.h> #include <time.h> #include "nav200.h" *************** *** 9,13 **** PositionXY reflector; Nav200 testing; ! testing.Initialise("/dev/ttyS1"); if (testing.EnterStandby()) { --- 9,13 ---- PositionXY reflector; Nav200 testing; ! testing.Initialise("/dev/ttyS0"); if (testing.EnterStandby()) { *************** *** 220,221 **** --- 220,225 ---- // bool rotateDirection(uint8_t direction); //absolutely not working for some unknown reason // bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector); <-- return incorrect X value + */ + + int main() + {} \ No newline at end of file |