From: Brandon S. <si...@sp...> - 2006-09-12 01:14:05
|
Yes, I am calling Publish for both interfaces with the proper addresses filled in. Here is the main code for the driver: ***Note that it is calling Publish in the RefreshData() function, but it is filling in the struct in the decode_OEM4() function.*** /* This is a Player driver for the Novatel GPS OEM4. */ /********************************************************************** ******/ /** **/ /** HEADERS USED **/ /** **/ /********************************************************************** ******/ // ONLY if you need something that was #define'd as a result of configure // (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so: /* #if HAVE_CONFIG_H #include <config.h> #endif */ /* C Standard Library */ #include <stdio.h> #include <stdlib.h> #include <string.h> #include <errno.h> #include <fcntl.h> #include <limits.h> #include <math.h> #include <netinet/in.h> #include <sys/stat.h> #include <sys/types.h> #include <sys/signal.h> #include <time.h> #include <sys/time.h> #include <termios.h> #include <unistd.h> #include "../util/utils.h" #include "../util/real.h" #include "novatel_oem4.h" /* Player Library */ #include <libplayercore/playercore.h> /********************************************************************** ******/ /** **/ /** DEFINITIONS AND MACROS **/ /** **/ /********************************************************************** ******/ /* Connect at 115200 bps. */ #define BAUDRATE B115200 #define PORT "/dev/ttyS0" #define GPS_CMD_RETRIES 3 #define GPS_CMD_TIMEOUT 0.5 #define GPS_CMD_BUFSIZE 64 #define GPS_CMD_SLEEPMS 250 #define GPS_BAUD_SLEEPMS 500 #define GPS_DATA_BUFSIZE 1024 #define GPS_DATA_SLEEPMS 10 #define GPS_PKT_LENGTH (255+65535+4) #define GPS_TIMEOUT 5.0 #define GPS_LOG_PREFIX "[GPS] " #define GPS_MIN_SWVERSION "2.300" #define MSGID_BESTPOS 42 #define MSGID_BESTVEL 99 #define MSGID_PSRDOP 174 #define MSGID_VERSION 37 #define MSGID_BESTUTM 726 #define CRC32_POLYNOMIAL 0xedb88320L #define OEM4SYNC1 0xAA #define OEM4SYNC2 0x44 #define OEM4SYNC3 0x12 #define PACKET_SYNC 0 #define PACKET_HEADER 1 #define PACKET_DATA 2 // gps initialization commands (OK required) static const char *gps_init[] = { "UNLOGALL THISPORT\r", "DGPSEPHEMDELAY 300\r", "DYNAMICS LAND\r", "DGPSTIMEOUT 22\r", "DATUM WGS84\r", "FIX AUTO\r", "WAASCORRECTION DISABLE 0 WAASTESTMODE\r", "UTMZONE CURRENT\r", // all logs should be binary "log THISPORT psrdopb onchanged\r", "log THISPORT bestposb ontime 0.05\r", "log THISPORT bestvelb ontime 0.05\r", "log THISPORT bestutmb ontime 0.05\r", NULL }; // gps log commands (OK not required) static const char *gps_logs[] = { "log THISPORT versionb once\r", NULL }; /********************************************************************** ******/ /** **/ /** TYPEDEFS AND STRUCTURES **/ /** **/ /********************************************************************** ******/ struct novatel_gps_data { struct _OEM4_header header; struct _OEM4_bestpos bestpos; struct _OEM4_bestvel bestvel; struct _OEM4_bestutm bestutm; struct _OEM4_psrdop psrdop; char hw_version[17]; char sw_version[17]; char boot_version[17]; struct timeval timestamp; }; /********************************************************************** ******/ class NovatelGPSDriver : public Driver /********************************************************************** ******/ { // Constructor; need that public: NovatelGPSDriver(ConfigFile* cf, int section); // Must implement the following methods. public: int Setup(); public: int Shutdown(); // Main function for device thread. private: virtual void Main(); public: virtual int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data); private: int RefreshData(); //Open the serial port private: int Open_Serial_Port (const char *port); private: void decode_OEM4_bestpos(struct _OEM4_header *hdr, const unsigned char *raw); private: void decode_OEM4_bestvel(struct _OEM4_header *hdr, const unsigned char *raw); private: void decode_OEM4_bestutm(struct _OEM4_header *hdr, const unsigned char *raw); private: void decode_OEM4_psrdop(struct _OEM4_header *hdr, const unsigned char *raw); private: void decode_OEM4_version(struct _OEM4_header *hdr, const unsigned char *raw); private: void decode_OEM4(struct _OEM4_header *hdr, const unsigned char *raw); private: int send_cmd(int fd, const char *cmd) const; private: unsigned long CRC32Value(int i) const; private: unsigned long CalculateBlockCRC32(unsigned long ulCount, const unsigned char *ucBuffer) const; private: int sync_search(const unsigned char *buf, int *idx, int cnt, int *found) const; // Player position3d data type struct. Used to send the data back to Player. private: player_devaddr_t m_position3d_addr; private: player_position3d_data_t position_data; // Player GPS data type struct. Used to send the data back to Player. private: player_devaddr_t m_gps_addr; private: player_gps_data_t gps_data; //Variables //the desired port private: const char *port; private: int fd; private: int baud; //Communication Variables private: unsigned char buf[GPS_DATA_BUFSIZE]; private: unsigned char msg[GPS_PKT_LENGTH]; private: unsigned char *pos; private: int packet; private: int found; private: int i, cnt, idx; private: unsigned int msgcrc, crc; private: int msglen; private: double temp; private: int oldstate; private: enum update { UPDATE_NULL, UPDATE_BESTPOS, UPDATE_BESTVEL, UPDATE_BESTUTM, UPDATE_PSRDOP, UPDATE_VERSION }; //Local GPS Data struct private: struct novatel_gps_data data; private: struct _OEM4_header *oem4_hdr; protected: unsigned long packets; }; /********************************************************************** ******/ /** **/ /** PROTOTYPES OF LOCAL FUNCTIONS **/ /** **/ /********************************************************************** ******/ /********************************************************************** ******/ /** **/ /** EXPORTED VARIABLES **/ /** **/ /********************************************************************** ******/ /********************************************************************** ******/ /** **/ /** GLOBAL VARIABLES **/ /** **/ /********************************************************************** ******/ /********************************************************************** ******/ /** **/ /** EXPORTED FUNCTIONS **/ /** **/ /********************************************************************** ******/ // A factory creation function, declared outside of the class so that it // can be invoked without any object context (alternatively, you can // declare it static in the class). In this function, we create and return // (as a generic Driver*) a pointer to a new instance of this driver. Driver* NovatelGPSDriver_Init(ConfigFile* cf, int section) { // Create and return a new instance of this driver return ((Driver*) (new NovatelGPSDriver(cf, section))); } // A driver registration function, again declared outside of the class so // that it can be invoked without object context. In this function, we add // the driver into the given driver table, indicating which interface the // driver can support and how to create a driver instance. void NovatelGPSDriver_Register(DriverTable* table) { table->AddDriver("player_novatel_gps", NovatelGPSDriver_Init); //the name must match the name in the config file including case } // Extra stuff for building a shared object. // Need access to the global driver table //#include <player/drivertable.h> /* need the extern to avoid C++ name-mangling */ extern "C" { int player_driver_init(DriverTable* table) { puts("plugin init"); NovatelGPSDriver_Register(table); return(0); } } /********************************************************************** ******/ /** **/ /** LOCAL FUNCTIONS **/ /** **/ /********************************************************************** ******/ /********************************************************************** ******/ NovatelGPSDriver::NovatelGPSDriver(ConfigFile* cf, int section) : Driver(cf, section) /********************************************************************** ******/ { /* Create the position3d interface. */ if (cf->ReadDeviceAddr(&(this->m_position3d_addr), section, "provides", PLAYER_POSITION3D_CODE, -1, NULL) != 0) { this->SetError(-1); return; } if (this->AddInterface(this->m_position3d_addr)) { this->SetError(-1); return; } /* Create the GPS interface. */ if (cf->ReadDeviceAddr(&(this->m_gps_addr), section, "provides", PLAYER_GPS_CODE, -1, NULL) != 0) { this->SetError(-1); return; } if (this->AddInterface(this->m_gps_addr)) { this->SetError(-1); return; } /* Read the configuration options from the config file. */ this->port = cf->ReadString(section, "port", PORT); /* Initialize all variables */ fd = -1; baud = 115200; packets = 0; packet = 0; found = 0; i = 0; cnt = 0; idx = 0; msgcrc = 0; crc = 0; msglen = 0; temp = 0.0; oldstate = 0; return; } /********************************************************************** ******/ int NovatelGPSDriver::Setup() /********************************************************************** ******/ { puts("NovatelGPSDriver initialising"); /* Open the serial port. Or USB or TCP/IP etc... */ this->fd = Open_Serial_Port(this->port); printf("fd:%d\n\n", fd); /* TODO: Any sensor or actuator initialization here */ memset(&buf, 0, sizeof(buf)); memset(&msg, 0, sizeof(msg)); memset(&gps_data, 0 , sizeof(gps_data)); memset(&position_data, 0 , sizeof(gps_data)); // make search variable types are the correct size if ((sizeof(gps_char) != 1) || (sizeof(gps_uchar) != 1) || (sizeof(gps_short) != 2) || (sizeof(gps_ushort) != 2) || (sizeof(gps_long) != 4) || (sizeof(gps_ulong) != 4) || (sizeof(gps_double) != 8) || (sizeof(gps_float) != 4)) { dbg_printf(DEBUG_CRITICAL, GPS_LOG_PREFIX "gps variable type mismatch!\n"); exit(-1); } // send initialization commands for (i = 0; gps_init[i]; i++) { if (send_cmd(fd, gps_init[i]) < 0) { dbg_printf(DEBUG_ERROR, GPS_LOG_PREFIX "initialization failure: %s\n", gps_init[i]); return 0; } // if (killthread) return(0); } // send log commands for (i = 0; gps_logs[i]; i++) { dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "requesting log: %s\n", gps_logs[i]); write(fd, gps_logs[i], strlen(gps_logs[i])); tcdrain(fd); sleep_ms(GPS_CMD_SLEEPMS); // if (killthread) return(0); } dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "waiting...\n"); gettimeofday(&data.timestamp, NULL); oem4_hdr = (struct _OEM4_header *)msg; found = 0; pos = msg; packet = PACKET_SYNC; puts("NovatelGPSDriver Ready"); // Start the device thread; spawns a new thread and executes // NovatelGPSDriver::Main(), which contains the main loop for the driver. this->StartThread(); return 0; } /********************************************************************** ******/ int NovatelGPSDriver::Shutdown() /********************************************************************** ******/ { puts("Shutting NovatelGPSDriver down"); // Stop and join the driver thread this->StopThread(); /* TODO: Put the sensor or actuator in a safe state */ /* Close the serial, USB, etc... port */ close(this->fd); puts("NovatelGPSDriver has been shutdown\n\n"); return 0; } /********************************************************************** ******/ void NovatelGPSDriver::Main() /********************************************************************** ******/ { // The main loop; interact with the device here while(true) { // test if we are supposed to cancel pthread_testcancel(); //Check for configuration requests this->ProcessMessages(); // Write outgoing data this->RefreshData(); } return; } /********************************************************************** ******/ int NovatelGPSDriver::ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data) /********************************************************************** ******/ { //Process messages here. Send a response if necessary, using Publish (). // If you handle the message successfully, return 0. Otherwise, // return -1, and a NACK will be sent for you, if a response is required. //Members of player_msghdr are: // player_devaddr_t addr // uint8_t type // uint8_t subtype // double timestamp // uint32_t seq // uint32_t size //First, check the hdr. if( hdr->type == PLAYER_MSGTYPE_REQ) { //We have received a request. switch (hdr->subtype) { //We do not support any configuration requests. default: return (-1); break; } } else { return (-1); } return 0; } /********************************************************************** ******/ int NovatelGPSDriver::RefreshData() /********************************************************************** ******/ { if ((cnt = read(fd, buf, GPS_DATA_BUFSIZE)) < 0) { dbg_printf(DEBUG_ERROR, GPS_LOG_PREFIX "read: %s\n", strerror (errno)); return 0; } if (cnt == 0) //Nothing read { sleep_ms(GPS_DATA_SLEEPMS); temp = time_get(&data.timestamp); if (temp > GPS_TIMEOUT) { dbg_printf(DEBUG_WARNING, GPS_LOG_PREFIX "no data received in %.2f seconds!\n", temp); return 0; } } // TODO: Get all the GPS data // AND Fill the data into the proper struct, as follows: idx = 0; while (idx != cnt) { switch (packet) { case PACKET_SYNC: if (sync_search(buf, &idx, cnt, &found)) { pos = msg; *pos++ = OEM4SYNC1; *pos++ = OEM4SYNC2; *pos++ = OEM4SYNC3; *pos++ = buf[idx++]; // HeaderLength byte packet = PACKET_HEADER; } break; case PACKET_HEADER: for (; idx < cnt && pos-msg < oem4_hdr->HeaderLength; idx++) { *pos++ = buf[idx]; } if (pos-msg < oem4_hdr->HeaderLength) break; packet = PACKET_DATA; if (oem4_hdr->MessageLength < 1 || oem4_hdr->MessageLength > 4096) { dbg_printf(DEBUG_WARNING, GPS_LOG_PREFIX "bad message length: %d\n", oem4_hdr->MessageLength); packet = PACKET_SYNC; } break; case PACKET_DATA: msglen = oem4_hdr->HeaderLength + oem4_hdr->MessageLength + 4; for (; idx < cnt && pos-msg < msglen; idx++) { *pos++ = buf[idx]; } if (pos-msg < msglen) break; msgcrc = *((unsigned int *)(msg+oem4_hdr- >HeaderLength+oem4_hdr->MessageLength)); crc = CalculateBlockCRC32(oem4_hdr->HeaderLength+oem4_hdr- >MessageLength, msg); if (msgcrc == crc) { memset(&gps_data, 0 , sizeof(gps_data)); packets++; decode_OEM4(oem4_hdr, msg+oem4_hdr->HeaderLength); // I think (Paul left no comments) this means we have received data, so fill it in // And send it back. BMS. // All other data is filled in in the decode_OEM4() functions unsigned long sec, msec; sec = data.header.Milliseconds / 1000; msec = data.header.Milliseconds - sec*1000; //Fill in the Player data structure gps_data.time_sec = (sec); gps_data.time_usec = msec*1000; // We do not have this information ? // gps_data.err_horz = htonl((uint32_t) ); // gps_data.err_vert = htonl((uint32_t) ); //fprintf(stderr, "Publishing Data.\n\r"); /* fprintf(stderr, "Lat/Lon: (%d, %d) Heading(deg): %.2f Vel (m/s): %.2f Sats: %d\n\n\r", this->gps_data.latitude, this->gps_data.longitude, this->position_data.pos.pyaw, this->position_data.vel.px, this->gps_data.num_sats); */ /* Write data to the server. */ this->Publish(this->m_gps_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_GPS_DATA_STATE, (void*)&gps_data, sizeof(gps_data), NULL); this->Publish(this->m_position3d_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, (void*)&position_data, sizeof(position_data), NULL); } else { dbg_printf(DEBUG_WARNING, GPS_LOG_PREFIX "CRC error: expected %#08x, calculated %#08x\n", msgcrc, crc); } packet = PACKET_SYNC; break; default: packet = PACKET_SYNC; break; } } return 1; } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4_bestpos(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { if (sizeof(struct _OEM4_bestpos) > hdr->MessageLength) return; memcpy(&data.bestpos,raw,sizeof(struct _OEM4_bestpos)); return; } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4_bestvel(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { if (sizeof(struct _OEM4_bestvel) > hdr->MessageLength) return; memcpy(&data.bestvel,raw,sizeof(struct _OEM4_bestvel)); return; } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4_bestutm(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { if (sizeof(struct _OEM4_bestutm) > hdr->MessageLength) return; memcpy(&data.bestutm,raw,sizeof(struct _OEM4_bestutm)); return; } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4_psrdop(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { if (sizeof(struct _OEM4_psrdop) > hdr->MessageLength) return; memcpy(&data.psrdop,raw,sizeof(struct _OEM4_psrdop)); return; } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4_version(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { struct _OEM4_version_comp *ver; int comps, i; unsigned int length; char *pos; char str[16+1]; char str2[16+1]; comps = *((gps_long *)raw); pos = (char *)raw + 4; length = hdr->MessageLength - 4; for (i = 0; i < comps; i++) { if (sizeof(struct _OEM4_version_comp) > length) break; ver = (struct _OEM4_version_comp *)pos; if (i == 0) { memcpy(data.hw_version,ver->hw_version,16); data.hw_version[16] = '\0'; memcpy(data.sw_version,ver->sw_version,16); data.sw_version[16] = '\0'; memcpy(data.boot_version,ver->boot_version,16); data.boot_version[16] = '\0'; } memcpy(str,ver->hw_version,16); str[16] = '\0'; dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "%d: H/W version = %s\n", i, str); memcpy(str,ver->sw_version,16); str[16] = '\0'; dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "%d: S/W version = %s\n", i, str); if (strcmp(str, GPS_MIN_SWVERSION) < 0) { dbg_printf(DEBUG_WARNING, GPS_LOG_PREFIX "WARNING!!! S/W version %s is obsolete! Please upgrade the firmware!\n", str); } memcpy(str,ver->boot_version,16); str[16] = '\0'; dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "%d: Boot version = %s\n", i, str); memcpy(str,ver->comp_date,12); str[12] = '\0'; memcpy(str2,ver->comp_time,8); str2[8] = '\0'; dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "%d: Firmware compiled at %s on %s\n", i, str2, str); length -= 108; pos += 108; } } /********************************************************************** *******************/ void NovatelGPSDriver::decode_OEM4(struct _OEM4_header *hdr, const unsigned char *raw) /********************************************************************** *******************/ { enum update update = UPDATE_NULL; memcpy(&data.header,hdr,sizeof(struct _OEM4_header)); gettimeofday(&data.timestamp, NULL); packets++; switch (hdr->MessageID) { case MSGID_BESTPOS: update = UPDATE_BESTPOS; decode_OEM4_bestpos(hdr,raw); break; case MSGID_BESTVEL: update = UPDATE_BESTVEL; decode_OEM4_bestvel(hdr,raw); break; case MSGID_BESTUTM: update = UPDATE_BESTUTM; decode_OEM4_bestutm(hdr,raw); break; case MSGID_PSRDOP: update = UPDATE_PSRDOP; decode_OEM4_psrdop(hdr,raw); break; case MSGID_VERSION: update = UPDATE_VERSION; decode_OEM4_version(hdr,raw); break; } #ifdef TESTPROG printf("Lat/Lon: (%f, %f) Heading(deg): %.2f Vel(m/s): %.2f Sats: %ld\n", data.bestpos.lat, data.bestpos.lon, data.bestvel.trk_gnd, data.bestvel.hor_spd, data.psrdop.num_prn); #endif this->gps_data.latitude = (int32_t)(data.bestpos.lat * 1e7); //degrees / 1e7 this->gps_data.longitude = (int32_t)(data.bestpos.lon * 1e7); //degrees / 1e7 this->gps_data.altitude = (int32_t)(data.bestpos.hgt * 1000); //mm //if(data.bestpos.sol_status > 0) // gps_data.quality = 1; //else // gps_data.quality = 0; gps_data.quality = (uint32_t)data.bestpos.sol_status; this->position_data.vel.px = data.bestvel.hor_spd; //radians this->position_data.vel.pz = data.bestvel.vert_spd; //radians this->position_data.pos.pyaw = (M_PI/180.00) *data.bestvel.trk_gnd; //radians this->gps_data.utm_e = ( data.bestutm.easting); //meters this->gps_data.utm_n = ( data.bestutm.northing); //meters. this->gps_data.num_sats = (uint32_t)(data.psrdop.num_prn); this->gps_data.hdop = (uint32_t)(100.0 * data.psrdop.hdop); //From the Novatel GPS manual, vdop = pdop^2 - hdop^2 (pg.226) this->gps_data.vdop = (uint32_t)( 100.0 * (data.psrdop.pdop*data.psrdop.pdop - data.psrdop.hdop*data.psrdop.hdop )); this->gps_data.err_horz = (data.bestutm.easting_sd); //meters. this->gps_data.err_vert = (data.bestutm.northing_sd); //meters //You cannot (yet) access the err_horz and err_vert members using //the C++ GPSProxy class. Maybe someday soon they will include all // standard deviation measurements in the GPS data struct. /* fprintf(stderr, "Lat/Lon: (%d, %d) Heading(deg): %.2f Vel(m/s): %.2f Sats: %d\n\r", this->gps_data.latitude, this->gps_data.longitude, this->position_data.pos.pyaw, this->position_data.vel.px, this->gps_data.num_sats); */ // exec_hooks(update, &data, sizeof(struct novatel_gps_data), &data.timestamp); } /********************************************************************** *******************/ int NovatelGPSDriver::send_cmd(int fd, const char *cmd) const /********************************************************************** *******************/ { unsigned char buf[GPS_CMD_BUFSIZE]; int cnt, i; int found = 0; int attempt; struct timeval start; tcflush(fd, TCIFLUSH); for (attempt = 0; attempt < GPS_CMD_RETRIES; attempt++) { dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "sending command: %s\n", cmd); if (write(fd, cmd, strlen(cmd)) < 0) { perror(GPS_LOG_PREFIX "write"); break; } tcdrain(fd); found = 0; gettimeofday(&start, NULL); sleep_ms(GPS_CMD_SLEEPMS); while (found != 3 && time_get(&start) < GPS_CMD_TIMEOUT) { if ((cnt = read(fd, buf, GPS_CMD_BUFSIZE)) < 0) { perror(GPS_LOG_PREFIX "read"); break; } if (cnt == 0) { sleep_ms(GPS_CMD_SLEEPMS); continue; } for (i = 0; i < cnt; i++) { if (buf[i] == '<') { found = 1; } else if (buf[i] == 'O' && found == 1) { found = 2; } else if (buf[i] == 'K' && found == 2) { found = 3; dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "received OK\n"); break; } else { found = 0; } } } if (found == 3) break; } return (found == 3) ? 0 : -1; } /********************************************************************** *******************/ unsigned long NovatelGPSDriver::CRC32Value(int i) const /********************************************************************** *******************/ { int j; unsigned long ulCRC; ulCRC = i; for (j = 8; j > 0; j--) { if (ulCRC & 1) ulCRC = (ulCRC >> 1) ^ CRC32_POLYNOMIAL; else ulCRC >>= 1; } return ulCRC; } /********************************************************************** *******************/ unsigned long NovatelGPSDriver::CalculateBlockCRC32(unsigned long ulCount, const unsigned char *ucBuffer) const /********************************************************************** *******************/ { unsigned long ulTemp1; unsigned long ulTemp2; unsigned long ulCRC = 0; while (ulCount--) { ulTemp1 = (ulCRC >> 8) & 0x00ffffffL; ulTemp2 = CRC32Value(((int)ulCRC ^ *ucBuffer++) & 0xff); ulCRC = ulTemp1 ^ ulTemp2; } return (ulCRC); } /********************************************************************** *******************/ int NovatelGPSDriver::sync_search(const unsigned char *buf, int *idx, int cnt, int *found) const /********************************************************************** *******************/ { int headerlen; for (; *idx < cnt; (*idx)++) { if (buf[*idx] == OEM4SYNC1) { *found = 1; } else if (buf[*idx] == OEM4SYNC2 && *found == 1) { *found = 2; } else if (buf[*idx] == OEM4SYNC3 && *found == 2) { *found = 3; } else if (*found == 3) { *found = 0; headerlen = buf[*idx]; if (headerlen < 1 || headerlen > 100) { dbg_printf(DEBUG_WARNING, GPS_LOG_PREFIX "bad header length: %d\n", headerlen); } else { return 1; } } else { *found = 0; } } return 0; } /********************************************************************** ******/ int NovatelGPSDriver::Open_Serial_Port (const char *port) /********************************************************************** ******/ { int fd; /* file descriptor of serial port */ struct termios settings, old_settings; /* serial port settings */ //struct sigaction saio; /*definition of signal action*/ /* Print a nice message. */ printf("\n\nInitializing serial port %s...\n", port); /* Open the serial port. */ if((fd = open(port, O_RDWR | O_NDELAY )) < 0) /*O_RDWR | O_NOCTTY | O_NDELAY*/ { perror("open()"); return -1; } /*allow the process to receive SIGIO */ /*fcntl(fd, F_SETOWN, getpid());*/ /*Make the file descriptor asynchronous*/ // fcntl(fd, F_SETFL, FASYNC); /* Get the current terminal settings. */ if (tcgetattr(fd, &old_settings) < 0) { perror("tcgetattr()"); printf("Serial Port not opened"); close(fd); return -1; } bzero (&settings, sizeof(settings)); settings.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD; /*CRTSCTS*/ settings.c_iflag = IGNPAR; settings.c_oflag = 0; /*set input mode (non-canonical, no echo...)*/ settings.c_lflag = 0; // settings.c_cc[VTIME] = 5; /*Wait 2 seconds for a return*/ // settings.c_cc[VMIN] = 16; /*blocking read until 5 characters are present*/ // try to determine current baud rate for (i = 0, found = 0; baudrates[i] > 0 /*&& !killthread*/; i++) { dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "trying %d baud...\n", baudrates[i]); tcflush(fd, TCIOFLUSH); cfsetspeed(&settings, baud_cflag(baudrates[i])); tcsetattr(fd, TCSANOW, &settings); sleep_ms(GPS_BAUD_SLEEPMS); if (send_cmd(fd, "UNLOGALL THISPORT\r") >= 0) { sleep_ms(GPS_CMD_SLEEPMS); sprintf((char *)buf, "COM THISPORT %d\r", baud); dbg_printf(DEBUG_NOISE, GPS_LOG_PREFIX "setting baud: % s\n", buf); if (write(fd, buf, strlen((char *)buf)) < 0) { dbg_printf(DEBUG_ERROR, GPS_LOG_PREFIX "write: %s\n", strerror(errno)); return 0; } tcdrain(fd); sleep_ms(GPS_BAUD_SLEEPMS); if(tcflush(fd, TCIOFLUSH ) < 0) { perror("tcflush()"); printf("Serial Port not opened"); close(fd); return -1; } //Set to the desired baud rate cfsetspeed(&settings, baud_cflag(baud)); /* Write the new terminal settings. */ if(tcsetattr(fd, TCSANOW, &settings) < 0) /*second argument = TCSAFLUSH?*/ { perror("tcsetattr()"); printf("Serial Port not opened"); close(fd); return -1; } sleep_ms(GPS_BAUD_SLEEPMS); found = 1; break; } } if (!found) { dbg_printf(DEBUG_CRITICAL, GPS_LOG_PREFIX "unable to communicate with device\n"); return 0; } return fd; } /********************************************************************** ******/ /** **/ /** EOF **/ /** **/ /********************************************************************** ******/ ----- Original Message ----- From: Brian Gerkey <br...@ge...> Date: Monday, September 11, 2006 4:33 pm Subject: Re: [Playerstage-users] GpsProxy , no data? > > On Sep 8, 2006, at 2:26 PM, Brandon Sights wrote: > > > I finally upgraded the Novatel GPS driver to Player 2.0, and have > > tried to test it. Currently, I have two interfaces: gps, and > > position3d. The gps proxy provides the normal gps data and the > > position3d proxy provides the yaw and velocity readings from the GPS > > unit. I never receive data when I subscribe to the GpsProxy, even > > though I can read data from the Position3dProxy. I checked to make > > sure I did not have any problems with mixing up the addresses > between> the two proxies when instantiating the interfaces and > when sending > > data. Has anyone else had a problem with this? > > Are you calling Publish() for each interface? > > brian. > > ------------------------------------------------------------------- > ------ > Using Tomcat but need to do more? Need to support web services, > security?Get stuff done quickly with pre-integrated technology to > make your job easier > Download IBM WebSphere Application Server v.1.0.1 based on Apache > Geronimohttp://sel.as- > us.falkag.net/sel? cmd=lnk&kid=120709&bid=263057&dat=121642_______________________________ ________________ > Playerstage-users mailing list > Pla...@li... > https://lists.sourceforge.net/lists/listinfo/playerstage-users > |
From: Brandon S. <si...@sp...> - 2006-09-12 18:17:10
|
Yes, the debug print in RefreshData() is giving me the right output, which is when I decided to email you guys. It appears that NO messages are ever sent from the GPS interface. If I try to Read() in Push mode it will block forever. I will add the debug statements into that function and let you know what I see. Thanks, Brandon ----- Original Message ----- From: Brian Gerkey <br...@ge...> Date: Tuesday, September 12, 2006 11:02 am Subject: Re: [Playerstage-users] GpsProxy , no data? > > Yes, I am calling Publish for both interfaces with the proper > > addresses filled in. > > > > Here is the main code for the driver: > > ***Note that it is calling Publish in the RefreshData() function, > > but it is filling in the struct in the decode_OEM4() > function.*** > Is the debug print in RefreshData() giving you the right output, > right before you Publish()? > > When you say that you don't get GPS data, do you mean that you > don't > receive any messages from that device, or that you get all zeros? > > The problem may be on the client side, perhaps an integer > truncation > issue. Throw some debug statements in client_libs/libplayerc/ > dev_gps.c, in playerc_gps_putmsg(). That's where incoming > messages > are processed on the client side. > > brian. > > ------------------------------------------------------------------- > ------ > Using Tomcat but need to do more? Need to support web services, > security?Get stuff done quickly with pre-integrated technology to > make your job easier > Download IBM WebSphere Application Server v.1.0.1 based on Apache > Geronimohttp://sel.as- > us.falkag.net/sel? cmd=lnk&kid=120709&bid=263057&dat=121642_______________________________ ________________ > Playerstage-users mailing list > Pla...@li... > https://lists.sourceforge.net/lists/listinfo/playerstage-users > |
From: Brandon S. <si...@sp...> - 2006-09-12 21:47:19
|
I added some debug prints in playerc_gps_putmsg(), and it does not seem the function is ever being called. -Brandon ----- Original Message ----- From: "Brandon Sights" <si...@sp...> Date: Tuesday, September 12, 2006 11:17 am Subject: Re: [Playerstage-users] GpsProxy , no data? > Yes, the debug print in RefreshData() is giving me the right > output, > which is when I decided to email you guys. > > It appears that NO messages are ever sent from the GPS interface. > If I > try to Read() in Push mode it will block forever. > > I will add the debug statements into that function and let you > know > what I see. > > Thanks, > Brandon > > ----- Original Message ----- > From: Brian Gerkey <br...@ge...> > Date: Tuesday, September 12, 2006 11:02 am > Subject: Re: [Playerstage-users] GpsProxy , no data? > > > > Yes, I am calling Publish for both interfaces with the proper > > > addresses filled in. > > > > > > Here is the main code for the driver: > > > ***Note that it is calling Publish in the RefreshData() function, > > > but it is filling in the struct in the decode_OEM4() > > function.*** > > Is the debug print in RefreshData() giving you the right output, > > > right before you Publish()? > > > > When you say that you don't get GPS data, do you mean that you > > don't > > receive any messages from that device, or that you get all zeros? > > > > The problem may be on the client side, perhaps an integer > > truncation > > issue. Throw some debug statements in client_libs/libplayerc/ > > dev_gps.c, in playerc_gps_putmsg(). That's where incoming > > messages > > are processed on the client side. > > > > brian. > > > > ----------------------------------------------------------------- > -- > > ------ > > Using Tomcat but need to do more? Need to support web services, > > security?Get stuff done quickly with pre-integrated technology > to > > make your job easier > > Download IBM WebSphere Application Server v.1.0.1 based on > Apache > > Geronimohttp://sel.as- > > us.falkag.net/sel? > cmd=lnk&kid=120709&bid=263057&dat=121642_______________________________ > ________________ > > Playerstage-users mailing list > > Pla...@li... > > https://lists.sourceforge.net/lists/listinfo/playerstage-users > > > > > ------------------------------------------------------------------- > ------ > Using Tomcat but need to do more? Need to support web services, > security?Get stuff done quickly with pre-integrated technology to > make your job easier > Download IBM WebSphere Application Server v.1.0.1 based on Apache > Geronimohttp://sel.as- > us.falkag.net/sel? cmd=lnk&kid=120709&bid=263057&dat=121642_______________________________ ________________ > Playerstage-users mailing list > Pla...@li... > https://lists.sourceforge.net/lists/listinfo/playerstage-users > |
From: Brian G. <br...@ge...> - 2006-09-16 23:19:19
|
On Sep 12, 2006, at 2:47 PM, Brandon Sights wrote: > I added some debug prints in playerc_gps_putmsg(), and it does not > seem the function is ever being called. Any progress on this problem? I wasn't able to see anything wrong in your code or any of the Player infrastructure (XDR marshaling, libplayerc/libplayerc++ proxies). brian. |
From: Brandon S. <si...@sp...> - 2006-09-21 17:52:50
|
The problem is that my "player_devaddr_t m_gps_addr" struct is somehow being cleared. It is being properly filled when I call ReadDeviceAddr (), but before I call Publish() it is somehow being set to all zeros. My position3d interface does not have this same problem. My work- around for now has been to save the members of the m_gps_addr struct into seperate uint's after I call ReadDeviceAddr() and then copy those back into the m_gps_addr struct before I publish. This seems to work. I wonder if this problem is the hardware on my robot, or somewhere in the code? I would assume it is in the code somewhere since it only affects one interface. -Brandon ----- Original Message ----- From: Brian Gerkey <br...@ge...> Date: Saturday, September 16, 2006 4:18 pm Subject: Re: [Playerstage-users] GpsProxy , no data? > > On Sep 12, 2006, at 2:47 PM, Brandon Sights wrote: > > > I added some debug prints in playerc_gps_putmsg(), and it does not > > seem the function is ever being called. > > Any progress on this problem? I wasn't able to see anything wrong > in > your code or any of the Player infrastructure (XDR marshaling, > libplayerc/libplayerc++ proxies). > > brian. > > > ------------------------------------------------------------------- > ------ > Using Tomcat but need to do more? Need to support web services, > security?Get stuff done quickly with pre-integrated technology to > make your job easier > Download IBM WebSphere Application Server v.1.0.1 based on Apache > Geronimohttp://sel.as- > us.falkag.net/sel? cmd=lnk&kid=120709&bid=263057&dat=121642_______________________________ ________________ > Playerstage-users mailing list > Pla...@li... > https://lists.sourceforge.net/lists/listinfo/playerstage-users > |
From: Brian G. <br...@ge...> - 2006-09-21 18:22:53
|
On Sep 21, 2006, at 10:52 AM, Brandon Sights wrote: > The problem is that my "player_devaddr_t m_gps_addr" struct is somehow > being cleared. It is being properly filled when I call ReadDeviceAddr > (), but before I call Publish() it is somehow being set to all zeros. > My position3d interface does not have this same problem. Aha! When that kind of thing happens, I always check on what resides in memory just before the things that's being corrupted. In your case, this is 'position_data'. Looking closely at your Setup() method, I find this line: memset(&position_data, 0 , sizeof(gps_data)); This should be: memset(&position_data, 0 , sizeof(position_data)); Looks like a little cut-and-paste typo. The 'gps_data' structure is quite a big bigger that 'position_data', so the memset call is writing zeros past 'position_data' and into 'm_gps_addr'. brian. |
From: Brandon S. <si...@sp...> - 2006-09-21 20:35:31
|
That was the fix. Thanks Brian. -Brandon ----- Original Message ----- From: Brian Gerkey <br...@ge...> Date: Thursday, September 21, 2006 11:22 am Subject: Re: [Playerstage-users] GpsProxy , no data? > > On Sep 21, 2006, at 10:52 AM, Brandon Sights wrote: > > > The problem is that my "player_devaddr_t m_gps_addr" struct is > somehow> being cleared. It is being properly filled when I call > ReadDeviceAddr> (), but before I call Publish() it is somehow > being set to all zeros. > > My position3d interface does not have this same problem. > > Aha! When that kind of thing happens, I always check on what > resides > in memory just before the things that's being corrupted. In your > case, this is 'position_data'. Looking closely at your Setup() > method, I find this line: > memset(&position_data, 0 , sizeof(gps_data)); > This should be: > memset(&position_data, 0 , sizeof(position_data)); > Looks like a little cut-and-paste typo. The 'gps_data' structure > is > quite a big bigger that 'position_data', so the memset call is > writing zeros past 'position_data' and into 'm_gps_addr'. > > brian. > > ------------------------------------------------------------------- > ------ > Take Surveys. Earn Cash. Influence the Future of IT > Join SourceForge.net's Techsay panel and you'll get the chance to > share your > opinions on IT & business topics through brief surveys -- and earn > cashhttp://www.techsay.com/default.php? page=join.php&p=sourceforge&CID=DEVDEV > _______________________________________________ > Playerstage-users mailing list > Pla...@li... > https://lists.sourceforge.net/lists/listinfo/playerstage-users > |
From: Brian G. <br...@ge...> - 2006-09-12 18:02:21
|
> Yes, I am calling Publish for both interfaces with the proper > addresses filled in. > > Here is the main code for the driver: > ***Note that it is calling Publish in the RefreshData() function, > but it is filling in the struct in the decode_OEM4() function.*** Is the debug print in RefreshData() giving you the right output, right before you Publish()? When you say that you don't get GPS data, do you mean that you don't receive any messages from that device, or that you get all zeros? The problem may be on the client side, perhaps an integer truncation issue. Throw some debug statements in client_libs/libplayerc/ dev_gps.c, in playerc_gps_putmsg(). That's where incoming messages are processed on the client side. brian. |