Revision: 6995 http://playerstage.svn.sourceforge.net/playerstage/?rev=6995&view=rev Author: thjc Date: 2008-08-30 02:22:34 +0000 (Sat, 30 Aug 2008)
Log Message: ----------- Applied the mica2 fixes from [ 1956512 ] Mica2 fix and two new drivers. Modified Paths: -------------- code/player/branches/release-2-1-patches/server/drivers/wsn/mica2.cc Modified: code/player/branches/release-2-1-patches/server/drivers/wsn/mica2.cc =================================================================== --- code/player/branches/release-2-1-patches/server/drivers/wsn/mica2.cc 2008-08-30 02:19:04 UTC (rev 6994) +++ code/player/branches/release-2-1-patches/server/drivers/wsn/mica2.cc 2008-08-30 02:22:34 UTC (rev 6995) @@ -129,7 +129,13 @@ #include <sys/ioctl.h> #include <math.h> #include <vector> +#include <iostream> +//For nanosleep: +#include <time.h> +#include <sys/time.h> + + // Includes needed for player #include <libplayercore/playercore.h> @@ -159,6 +165,17 @@ virtual void Main (); void RefreshData (); + //!Time between samples (in mS) + float rfidsamplingrate; + //!Alarm time (mS) + float alarmtime; + //Need two timers: one for calculating the sleep time to keep a desired framerate. + // The other for measuring the real elapsed time. (And maybe give an alarm) + struct timeval tv_realtime_start; + struct timeval tv_realtime_end; + float real_elapsed; + bool send_rfidcmd; + // Port file descriptor int fd; @@ -233,6 +250,13 @@ table->AddDriver ("mica2", Mica2_Init); } +//This function returns the difference in mS between two timeval structures +inline float timediffms(struct timeval start, struct timeval end) { + return(end.tv_sec*1000.0 + end.tv_usec/1000.0 - (start.tv_sec*1000.0 + start.tv_usec/1000.0)); +} + + + //////////////////////////////////////////////////////////////////////////////// // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. @@ -270,6 +294,7 @@ // Filter base node from readings ? filterbasenode = cf->ReadInt (section, "filterbasenode", 0); + rfidsamplingrate = cf->ReadInt (section, "rfidsamplingrate", 500); // Do we create a WSN interface? if (cf->ReadDeviceAddr (&wsn_addr, section, "provides", @@ -351,8 +376,9 @@ // Set up the device. Return 0 if things go well, and -1 otherwise. int Mica2::Setup () { + real_elapsed=0; // Open serial port - fd = open (port_name, O_RDWR | O_NOCTTY); + fd = open (port_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (fd < 0) { PLAYER_ERROR2 ("> Connecting to MIB510 on [%s]; [%s]...[failed!]", @@ -425,7 +451,7 @@ // Main function for device thread void Mica2::Main () { - timespec sleepTime = {0, 0}; + gettimeofday( &tv_realtime_start, NULL ); // NULL -> don't want timezone information // The main loop; interact with the device here while (true) @@ -440,8 +466,6 @@ if (base_node_status != 0) // if the base node is asleep, no serial // data can be read RefreshData (); - - nanosleep (&sleepTime, NULL); } } @@ -798,16 +822,31 @@ // RefreshData function void Mica2::RefreshData () { + + + int length; unsigned char buffer[255]; // Get the time at which we started reading // This will be a pretty good estimate of when the phenomena occured - struct timeval time; - GlobalTime->GetTime (&time); + + //find out the real elapsed time + gettimeofday( &tv_realtime_end, NULL ); + //calculate the time in mS + real_elapsed=timediffms(tv_realtime_start,tv_realtime_end)+real_elapsed; + //restart the timer + gettimeofday( &tv_realtime_start, NULL ); + //check if the time was too long + if (real_elapsed > rfidsamplingrate) { + send_rfidcmd=true; + real_elapsed=0; + } // In case the RFID interface is enabled, send a "select_tag" command first - if ((provideRFID) && (this->rfid_subscriptions > 0)) + if (((provideRFID) && (this->rfid_subscriptions > 0)) && send_rfidcmd) { + send_rfidcmd=false; BuildRFIDHeader (0, NULL, 0, 0xFFFF, 1); + } // Reading from UART length = ReadSerial (buffer); @@ -826,24 +865,33 @@ // ReadSerial function - reads one XSensorPacket from the serial port int Mica2::ReadSerial (unsigned char *buffer) { +// printf("leyendo\n"); unsigned char c; int err, i = 0; buffer[i] = 0x7e; // serial start byte - while (1) { + int no_read=0; + + while (no_read<100) { err = read (fd, &c, 1); +// printf("Terminando de leer\n"); if (err < 0) { - PLAYER_ERROR (">> Error reading from serial port !"); - return (-1); +// no_read++; + if (errno!=EAGAIN) { + PLAYER_ERROR (">> Error reading from serial port !"); + return (-1); + } else { no_read++;} } if (err == 1) { + no_read=0; if (++i > 255) return i; buffer[i] = c; if (c == 0x7e) return i; } } + return 0; } //////////////////////////////////////////////////////////////////////////////// @@ -904,6 +952,8 @@ NodeCalibrationValues node_values; player_wsn_data_t wsn_data; player_rfid_data_t rfid_data; + rfid_data.tags = new player_rfid_tag_t[1]; + rfid_data.tags[0].guid = new char[8]; bool rfidPacket = FALSE; bool wsnPacket = FALSE; int i = 0, o = 2; // index and offset @@ -912,8 +962,8 @@ return -1; // Zero data - memset (&wsn_data, 0, sizeof (player_wsn_data_t)); - memset (&rfid_data, 0, sizeof (player_rfid_data_t)); + //memset (&wsn_data, 0, sizeof (player_wsn_data_t)); + //memset (&rfid_data, 0, sizeof (player_rfid_data_t)); while (i < length) { @@ -1062,8 +1112,8 @@ rfidPacket = TRUE; - player_rfid_tag_t RFIDtag; - memset (&RFIDtag, 0, sizeof (RFIDtag)); + //player_rfid_tag_t RFIDtag; + //memset (&RFIDtag, 0, sizeof (RFIDtag)); RFIDMsg *rmsg = (RFIDMsg *)buffer; int dataoffset; @@ -1075,6 +1125,9 @@ response_code <<= 4; response_code &= 0xF0; response_code |= getDigit (rmsg->data[1]); + rfid_data.tags_count = 0; + rfid_data.tags[0].guid_count=0; + rfid_data.tags[0].type=0; if (response_code == 0x14) // SELECT TAG pass { @@ -1082,10 +1135,13 @@ tag_type <<= 4; tag_type &= 0xF0; tag_type |= getDigit (rmsg->data[3]); - RFIDtag.type = tag_type; + //RFIDtag.type = tag_type; + rfid_data.tags_count = 1; + rfid_data.tags[0].type = tag_type; dataoffset = 4; - RFIDtag.guid_count = 8; + //RFIDtag.guid_count = 8; + rfid_data.tags[0].guid_count = 8; int x = 0, cc = 0; int xlength = 23 - (29 - buffer[4]); @@ -1098,13 +1154,13 @@ { char str[3]; sprintf (str, "%c%c", rmsg->data[x], rmsg->data[x+1]); - sscanf (str, "%x", (unsigned int*)&RFIDtag.guid[cc]); + sscanf (str, "%x", (unsigned int*)&rfid_data.tags[0].guid[cc]); cc++; } } - rfid_data.tags_count = 1; - rfid_data.tags[0] = RFIDtag; + //rfid_data.tags_count = 1; + //rfid_data.tags[0] = RFIDtag; } } break; @@ -1123,7 +1179,10 @@ // Write the RFID data Publish (rfid_addr, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA_TAGS, &rfid_data, sizeof (player_rfid_data_t), NULL); + delete [] rfid_data.tags[0].guid; + delete [] rfid_data.tags; + if ((provideWSN) && (wsnPacket)) // Write the WSN data Publish (wsn_addr, PLAYER_MSGTYPE_DATA, PLAYER_WSN_DATA_STATE, @@ -1151,3 +1210,4 @@ return acceleration; } //------------------------------------------------------------------------------ + 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