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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
