Menu
▾
▴
[Playerstage-commit] SF.net SVN: playerstage:[6998] code/player/trunk/server/drivers
|
From: <gb...@us...> - 2008-08-31 19:22:28
|
Revision: 6998
http://playerstage.svn.sourceforge.net/playerstage/?rev=6998&view=rev
Author: gbiggs
Date: 2008-09-01 02:22:32 +0000 (Mon, 01 Sep 2008)
Log Message:
-----------
Applied patch 2084625
Modified Paths:
--------------
code/player/trunk/server/drivers/rfid/CMakeLists.txt
code/player/trunk/server/drivers/wsn/CMakeLists.txt
code/player/trunk/server/drivers/wsn/mica2.cc
Added Paths:
-----------
code/player/trunk/server/drivers/rfid/acr120u.cc
code/player/trunk/server/drivers/wsn/phidgetAcc.cc
Modified: code/player/trunk/server/drivers/rfid/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997)
+++ code/player/trunk/server/drivers/rfid/CMakeLists.txt 2008-09-01 02:22:32 UTC (rev 6998)
@@ -23,3 +23,7 @@
PLAYERDRIVER_ADD_DRIVER (phidgetRFID build_phidgetRFID
LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}"
SOURCES phidgetRFID.cc)
+
+PLAYERDRIVER_OPTION (acr120u build_acr120u ON)
+PLAYERDRIVER_REQUIRE_HEADER (acr120u build_acr120u "usb.h")
+PLAYERDRIVER_ADD_DRIVER (acr120u build_acr120u LINKFLAGS "-lusb" SOURCES acr120u.cc)
Added: code/player/trunk/server/drivers/rfid/acr120u.cc
===================================================================
--- code/player/trunk/server/drivers/rfid/acr120u.cc (rev 0)
+++ code/player/trunk/server/drivers/rfid/acr120u.cc 2008-09-01 02:22:32 UTC (rev 6998)
@@ -0,0 +1,415 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2007
+ * Federico Ruiz Ugalde ruizf /at/ cs.tum.edu, memeruiz /at/ gmail.com
+ * Lorenz Moesenlechner moesenle /at/ in.tum.de
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_acr120u acr_120u
+ * @brief ACR120U RFID reader
+
+The acr120u driver communicates with the ACR120U (Part# ACR120U-TK-R, Firmware V2.2U) reader. (13.56MHz Read-Write multitag, anti-collision and USB Powered).
+
+@par Compile-time dependencies
+
+- none
+
+@par Provides
+
+- @ref interface_rfid
+
+@par Requires
+
+- libusb should be installed.
+
+@par Configuration requests
+
+- none
+
+@par Configuration file options
+
+- sampling_rate (integer)
+ - Default: 200
+ - How often (in mS) should the phidget produce data. Minimum is around 100mS
+
+- alarmtime (integer)
+ - Default: 210
+ - If the data acquisition cycle takes longer than this time (in mS), a warning will be printed.
+
+- provides
+ - The driver supports the "rfid" interface.
+ - No support for the buzzer and led yet.
+
+@par Example
+
+@verbatim
+driver
+(
+ name "acr120u"
+ provides ["rfid:0"]
+ alwayson 0
+ samplingrate 200
+ alarmtime 210
+)
+@endverbatim
+
+@author Federico Ruiz Ugalde, Lorenz Moesenlechner
+
+ */
+/** @} */
+
+
+
+#include <libplayercore/playercore.h>
+#include <usb.h>
+
+#include <unistd.h>
+#include <string.h>
+#include <iostream>
+#include <sstream>
+
+#include <time.h>
+#include <sys/time.h>
+#include <errno.h>
+
+//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));
+}
+
+class Acr120u : public Driver {
+ public:
+
+ Acr120u(ConfigFile* cf, int section);
+ ~Acr120u();
+
+ virtual int Setup();
+ virtual int Shutdown();
+
+ virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
+
+ private:
+
+ // Main function for device thread.
+ virtual void Main();
+
+ int intFromHexTuple( char c1, char c2 ) const;
+ int intFromHexTuple( const char *c ) const { return intFromHexTuple( c[0], c[1] ); }
+ //!Time between samples (in mS)
+ float samplingrate;
+ //!Alarm time (mS)
+ float alarmtime;
+
+ //!Libusb data
+ struct usb_bus *busses,*bus,*bus_temp;
+ usb_dev_handle *HANDLE;
+ struct usb_device *dev,*dev_temp;
+
+ typedef enum Acr120uCmds { RESET=0, TURN_ON_RADIO, LIST_TAGS };
+ static const int Acr120uCmdLength=14;
+ static const char Acr120uCmdStrings[][Acr120uCmdLength ];
+ static const int Acr120uResponseLength=3*8;
+ static const int vendorId = 0x072f;
+ static const int productId = 0x8003;
+ static const int tag_count_position = 20;
+ static const int tag_startoffset = 1;
+
+ //! Player Interfaces
+ player_devaddr_t rfid_id;
+ //player_devaddr_t dio_id; //Use this for the buzzer and led latter
+};
+
+const char Acr120u::Acr120uCmdStrings[][Acr120u::Acr120uCmdLength ] =
+{
+ { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x30, 0x35, 0x30, 0x30, 0x45, 0x36, 0x03 }, // Reset
+ { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x31, 0x33, 0x30, 0x30, 0x46, 0x30, 0x03 }, // Turn on Radio
+ { 0x02, 0x30, 0x31, 0x45, 0x30, 0x30, 0x32, 0x30, 0x33, 0x30, 0x30, 0x45, 0x30, 0x03 } // List Tags
+};
+
+int Acr120u::intFromHexTuple( char c1, char c2 ) const
+{
+ char c_str[] = { c1, c2, 0x00 };
+ std::istringstream strm( c_str );
+ int num;
+
+ strm.setf( std::ios::hex, std::ios::basefield );
+ strm >> num;
+
+ return num;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor. Retrieve options from the configuration file and do any
+// pre-Setup() setup.
+Acr120u::Acr120u(ConfigFile* cf, int section)
+ : Driver(cf, section) {
+ //! Start with a clean device
+ memset(&rfid_id,0,sizeof(player_devaddr_t));
+ //memset(&dio_id,0,sizeof(player_devaddr_t)); //For the buzzer and led
+
+ // Creating the rfid interface
+ if (cf->ReadDeviceAddr(&(rfid_id), section, "provides", PLAYER_RFID_CODE, -1, NULL) == 0) {
+ if (AddInterface(rfid_id) != 0) {
+ SetError(-1);
+ return;
+ }
+ } else {
+ PLAYER_WARN("rfid interface not created for acr120u driver");
+ }
+ /*
+ //For the buzzer and led
+ if (cf->ReadDeviceAddr(&(dio_id), section, "provides", PLAYER_DIO_CODE, -1, NULL) == 0) {
+ if (AddInterface(dio_id) != 0) {
+ SetError(-1);
+ return;
+ }
+ } else {
+ PLAYER_WARN("dio interface not created for phidgetrfid driver");
+ }*/
+
+
+ // Set the libusb pointers to NULL
+ busses=0;
+ dev=0;
+ dev_temp=0;
+
+ // Read an option from the configuration file
+ //serial = cf->ReadInt(section, "serial", -1);
+
+ //Sampling rate and alarm time in mS
+ samplingrate = cf->ReadFloat(section, "samplingrate", 200.0);
+ alarmtime = cf->ReadFloat(section, "alarmtime", 210.0);
+
+ return;
+}
+
+Acr120u::~Acr120u() {}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set up the device. Return 0 if things go well, and -1 otherwise.
+int Acr120u::Setup() {
+ PLAYER_MSG0(1,"ACR120U driver initialising");
+ usb_init();
+ usb_find_busses();
+ usb_find_devices();
+ busses=usb_get_busses();
+ PLAYER_MSG0(1,"Searching for the device...");
+ int found=0;
+ while ((!found) && busses) {
+ dev_temp=busses->devices;
+ while ((!found) && dev_temp) {
+ if (dev_temp->descriptor.idVendor == 0x072f && dev_temp->descriptor.idProduct == 0x8003) {
+ dev=dev_temp;
+ HANDLE=usb_open(dev);
+ if (usb_claim_interface(HANDLE,0) == 0) {
+ found=1;
+ } else {
+ usb_close(HANDLE);
+ }
+ }
+ dev_temp=dev_temp->next;
+ }
+ busses=busses->next;
+ }
+
+ if (found==1) {
+ PLAYER_MSG0(1,"Device found. Connection granted to the ACR120U.");
+ } else {
+ PLAYER_ERROR("There was a problem connecting to the ACR120u. You don't have device access permissions?");
+ return(-1);
+ }
+
+ //Setting up the device
+ char acrResponse[Acr120uResponseLength];
+ char acrCommand[Acr120uCmdLength];
+ //Reset command
+ memcpy(acrCommand, Acr120uCmdStrings[RESET], Acr120uCmdLength);
+ usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,Acr120uCmdLength,0);
+ for (int i=0;i<3;i++) {
+ usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0);
+ }
+ //Turn on the Radio
+ memcpy(acrCommand, Acr120uCmdStrings[TURN_ON_RADIO], Acr120uCmdLength);
+ usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,Acr120uCmdLength,0);
+ for (int i=0;i<3;i++) {
+ usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0);
+ }
+
+ PLAYER_MSG0(1,"ACR120U driver ready");
+ // Start the device thread; spawns a new thread and executes
+ // Phidgetrfid::Main(), which contains the main loop for the driver.
+ StartThread();
+
+ return(0);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Shutdown the device
+int Acr120u::Shutdown() {
+
+ PLAYER_MSG0(1,"Shutting ACR120U driver down");
+ // Stop and join the driver thread
+ StopThread();
+ //Turn off the device and delete the ACR120U objects
+ //Turn power off. This is missing yet
+ //Release interface
+ usb_clear_halt(HANDLE,0);
+ usb_clear_halt(HANDLE,1);
+ usb_release_interface(HANDLE,0);
+ //Close device
+ usb_reset(HANDLE);
+ usb_close(HANDLE);
+ busses=0;
+ dev=0;
+ dev_temp=0;
+
+ PLAYER_MSG0(1,"ACR120U driver has been shutdown");
+
+ return(0);
+}
+
+int Acr120u::ProcessMessage(QueuePointer &resp_queue,
+ player_msghdr * hdr,
+ void * data) {
+ return(0);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Main function for device thread
+void Acr120u::Main() {
+
+ //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_framerate_start;
+ struct timeval tv_framerate_end;
+ struct timeval tv_realtime_start;
+ struct timeval tv_realtime_end;
+
+ gettimeofday( &tv_framerate_start, NULL ); // NULL -> don't want timezone information
+ tv_realtime_start = tv_framerate_start;
+
+ // The main loop; interact with the device here
+ while (true) {
+ //find out the real elapsed time
+ gettimeofday( &tv_realtime_end, NULL );
+ //calculate the time in mS
+ float real_elapsed=timediffms(tv_realtime_start,tv_realtime_end);
+ //restart the timer
+ gettimeofday( &tv_realtime_start, NULL );
+
+ //check if the time was too long
+ static bool gavewarning(false);
+ if ((!gavewarning) && (real_elapsed > alarmtime)) {
+ PLAYER_WARN2("Cycle took %d mS instead of the desired %d mS. (Only warning once)\n",real_elapsed , samplingrate);
+ gavewarning=true;
+ }
+
+ // test if we are supposed to cancel
+ pthread_testcancel();
+
+ // Process incoming messages. Phidgetrfid::ProcessMessage() is
+ // called on each message.
+ ProcessMessages();
+
+ //Get the Tags
+ player_rfid_data_t data_rfid;
+ char acrCommand[Acr120uCmdLength];
+ char acrResponse[Acr120uResponseLength];
+ //ListTag
+ memcpy(acrCommand, Acr120uCmdStrings[LIST_TAGS], Acr120uCmdLength);
+ usb_control_msg(HANDLE,0x40,0x00,0x00,0x00,acrCommand,14,0);
+ for (int i=0;i<3;i++) {
+ usb_interrupt_read(HANDLE,0x01,&acrResponse[i*8],8,0);
+ }
+ data_rfid.tags_count=acrResponse[tag_count_position]-0x30;
+ data_rfid.tags = new player_rfid_tag_t[data_rfid.tags_count+1];
+
+ //Receiving Tags
+ for (unsigned int i=0;i<data_rfid.tags_count;i++) {
+ data_rfid.tags[i].guid = new char[8];
+ data_rfid.tags[i].type=1;
+ data_rfid.tags[i].guid_count=8;
+ for (int j=0;j<3;j++) {
+ usb_interrupt_read(HANDLE,0x01,&acrResponse[j*8],8,0);
+ }
+ for (int j=0;j<8;j++) {
+ data_rfid.tags[i].guid[7-j] = intFromHexTuple(&acrResponse[j*2+tag_startoffset]);
+ }
+ }
+
+ //Publishing data.
+ if (rfid_id.interf !=0) {
+ Publish(rfid_id, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA_TAGS, (unsigned char*)&data_rfid, sizeof(player_rfid_data_t), NULL);
+ }
+ for (unsigned int i=0;i<data_rfid.tags_count;i++){
+ delete [] data_rfid.tags[i].guid;
+ }
+ delete [] data_rfid.tags;
+
+ //point to calculate how much to sleep, call nanosleep, after sleep restart the timer
+ //Get the ammount of time passed:
+ gettimeofday( &tv_framerate_end, NULL );
+ // figure out how much to sleep
+ long usecs = tv_framerate_end.tv_usec - tv_framerate_start.tv_usec;
+ long secs = tv_framerate_end.tv_sec - tv_framerate_start.tv_sec;
+ long elapsed_usecs = 1000000*secs + usecs;
+ long us_tosleep = static_cast<long>(samplingrate*1000) - elapsed_usecs;
+ struct timespec ts;
+ ts.tv_sec = 0;
+ ts.tv_nsec = us_tosleep*1000;
+ int done=nanosleep(&ts, NULL);
+
+ //restart the counter
+ gettimeofday( &tv_framerate_start, NULL );
+
+ if (done != 0) {
+ std::cout << "Error in nanosleep! ERRNO: " << errno << " ";
+ if (errno == EINTR) {
+ std::cout << "EINTR" ;
+ } else if (errno == EINVAL) {
+ std::cout << "EINVAL" ;
+ }
+ std::cout << std::endl;
+ }
+
+ }
+}
+
+// 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*
+Acr120u_Init(ConfigFile* cf, int section) {
+ // Create and return a new instance of this driver
+ return((Driver*)(new Acr120u(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 acr120u_Register(DriverTable* table) {
+ table->AddDriver("acr120u", Acr120u_Init);
+}
+
Modified: code/player/trunk/server/drivers/wsn/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/wsn/CMakeLists.txt 2008-09-01 00:57:06 UTC (rev 6997)
+++ code/player/trunk/server/drivers/wsn/CMakeLists.txt 2008-09-01 02:22:32 UTC (rev 6998)
@@ -19,3 +19,20 @@
PLAYERDRIVER_OPTION (accel_calib build_accel_calib OFF "STL not found.")
ENDIF (HAVE_STL)
PLAYERDRIVER_ADD_DRIVER (accel_calib build_accel_calib SOURCES accel_calib.cc)
+
+PLAYERDRIVER_OPTION (phidgetAcc build_phidgetAcc ON)
+SET (PHIDGETACC_DIR "" CACHE STRING "Directory containing the Phidget Acc headers and libraries")
+MARK_AS_ADVANCED (PHIDGETACC_DIR)
+IF ("${PHIDGETACC_DIR}" STREQUAL "")
+ SET (phidgetReqHeader "phidget21.h")
+ SET (phidgetExtraFlags "")
+ SET (phidgetExtraLibs "-lphidget21")
+ELSE ("${PHIDGETACC_DIR}" STREQUAL "")
+ SET (phidgetReqHeader "${PHIDGETACC_DIR}/phidget21.h")
+ SET (phidgetExtraFlags "-I${PHIDGETACC_DIR}/include")
+ SET (phidgetExtraLibs "-L${PHIDGETACC_DIR}/lib -lphidget21")
+ENDIF ("${PHIDGETACC_DIR}" STREQUAL "")
+PLAYERDRIVER_REQUIRE_HEADER (phidgetAcc build_phidgetAcc ${phidgetReqHeader})
+PLAYERDRIVER_ADD_DRIVER (phidgetAcc build_phidgetAcc
+ LINKFLAGS ${phidgetExtraLibs} CFLAGS "${phidgetExtraFlags}"
+ SOURCES phidgetAcc.cc)
Modified: code/player/trunk/server/drivers/wsn/mica2.cc
===================================================================
--- code/player/trunk/server/drivers/wsn/mica2.cc 2008-09-01 00:57:06 UTC (rev 6997)
+++ code/player/trunk/server/drivers/wsn/mica2.cc 2008-09-01 02:22:32 UTC (rev 6998)
@@ -129,7 +129,12 @@
#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 +164,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 +249,12 @@
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 +292,8 @@
// 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 +375,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,8 +450,8 @@
// 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 +465,6 @@
if (base_node_status != 0) // if the base node is asleep, no serial
// data can be read
RefreshData ();
-
- nanosleep (&sleepTime, NULL);
}
}
@@ -802,12 +825,24 @@
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);
@@ -830,20 +865,26 @@
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);
if (err < 0)
{
- PLAYER_ERROR (">> Error reading from serial port !");
- return (-1);
+ 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;
}
////////////////////////////////////////////////////////////////////////////////
@@ -883,11 +924,12 @@
NodeCalibrationValues Mica2::FindNodeValues (unsigned int nodeID)
{
NodeCalibrationValues n;
- NCV::iterator it;
- for (it = ncv.begin (); it != ncv.end (); it++)
+ unsigned int i = 0;
+
+ for (i = 0; i < ncv.size (); i++)
{
- n = *it;
+ n = ncv.at (i);
if (n.node_id == nodeID)
break;
}
@@ -902,6 +944,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
@@ -909,10 +953,6 @@
if ((this->wsn_subscriptions < 1) && (this->rfid_subscriptions < 1))
return -1;
- // Zero data
- memset (&wsn_data, 0, sizeof (player_wsn_data_t));
- memset (&rfid_data, 0, sizeof (player_rfid_data_t));
-
while (i < length)
{
if (buffer[o] == 0x7d) // handle escape characters
@@ -1060,9 +1100,6 @@
rfidPacket = TRUE;
- player_rfid_tag_t RFIDtag;
- memset (&RFIDtag, 0, sizeof (RFIDtag));
-
RFIDMsg *rmsg = (RFIDMsg *)buffer;
int dataoffset;
@@ -1073,6 +1110,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
{
@@ -1080,10 +1120,11 @@
tag_type <<= 4;
tag_type &= 0xF0;
tag_type |= getDigit (rmsg->data[3]);
- RFIDtag.type = tag_type;
+ rfid_data.tags_count = 1;
+ rfid_data.tags[0].type = tag_type;
dataoffset = 4;
- RFIDtag.guid_count = 8;
+ rfid_data.tags[0].guid_count = 8;
int x = 0, cc = 0;
int xlength = 23 - (29 - buffer[4]);
@@ -1096,13 +1137,10 @@
{
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;
}
}
break;
@@ -1121,6 +1159,8 @@
// 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
Added: code/player/trunk/server/drivers/wsn/phidgetAcc.cc
===================================================================
--- code/player/trunk/server/drivers/wsn/phidgetAcc.cc (rev 0)
+++ code/player/trunk/server/drivers/wsn/phidgetAcc.cc 2008-09-01 02:22:32 UTC (rev 6998)
@@ -0,0 +1,351 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2007 Federico Ruiz Ugalde ruizf /at/ cs.tum.edu
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_phidgetACC phidgetACC
+ * @brief Phidget ACC
+
+The phidgetACC driver communicates with the PhidgetACC (Part# 1059) accelerometer.
+
+@par Compile-time dependencies
+
+- none
+
+@par Provides
+
+- @ref interface_wsn
+
+@par Requires
+
+- libphidget from www.phidgets.com should be installed.
+
+@par Configuration requests
+
+- none
+
+@par Configuration file options
+
+- serial (integer)
+ - Default: -1
+ - This defines which phidget will be controlled if there is more than one connected to the USB bus.
+ You can obtain the number with lsusb, like this: "lsusb -v |grep iSerial".
+ The default is -1 , and it will connect to the first phidget available.
+
+- sampling_rate (integer)
+ - Default: 40
+ - How often (in mS) should the phidget produce data. 40mS produces ACC data at a rate of 25Hz.
+
+- alarmtime (integer)
+ - Default: 45
+ - If the data acquisition cycle takes longer than this time (in mS), a warning will be printed.
+
+- provides
+ - The "wsn" interface with the 3 accelerometers data
+
+@par Example
+
+@verbatim
+driver
+(
+ name "phidgetAcc"
+ provides ["wsn:0"]
+ serial -1
+ alwayson 1
+ samplingrate 40
+ alarmtime 45
+)
+@endverbatim
+
+@author Federico Ruiz Ugalde
+
+ */
+/** @} */
+
+
+
+#include "phidget21.h"
+#include <libplayercore/playercore.h>
+
+#include <unistd.h>
+#include <string.h>
+#include <iostream>
+
+//For nanosleep:
+#include <time.h>
+#include <sys/time.h>
+//To catch the errors of nanosleep
+#include <errno.h>
+
+//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));
+}
+
+class PhidgetAcc : public Driver {
+ public:
+
+ // Constructor;
+ PhidgetAcc(ConfigFile* cf, int section);
+
+ //Destructor
+ ~PhidgetAcc();
+
+ virtual int Setup();
+ virtual int Shutdown();
+
+ virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
+
+ private:
+
+ // Main function for device thread.
+ virtual void Main();
+
+ //!Time between samples (in mS)
+ float samplingrate;
+ //!Alarm time (mS)
+ float alarmtime;
+
+ // WSN interface
+ player_wsn_data_t data;
+
+ //! Pointer to the ACC Phidget Handle
+ CPhidgetAccelerometerHandle accel;
+
+ //! Player Interfaces
+ player_devaddr_t wsn_id;
+
+ //!Serial number of the phidget
+ int serial;
+
+// NodeCalibrationValues FindNodeValues (unsigned int nodeID);
+ player_wsn_data_t DecodePacket (struct p_packet *pkt);
+ float ConvertAccel (unsigned short raw_accel, int neg_1g, int pos_1g, int converted);
+
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor. Retrieve options from the configuration file and do any
+// pre-Setup() setup.
+PhidgetAcc::PhidgetAcc(ConfigFile* cf, int section)
+ : Driver(cf, section) {
+ //! Start with a clean device
+ memset(&wsn_id,0,sizeof(player_devaddr_t));
+
+ // Creating the wsn interface
+ if (cf->ReadDeviceAddr(&(wsn_id), section, "provides", PLAYER_WSN_CODE, -1, NULL) == 0) {
+ if (AddInterface(wsn_id) != 0) {
+ SetError(-1);
+ return;
+ }
+ } else {
+ PLAYER_WARN("wsn interface not created for phidgetAccel driver");
+ }
+
+ // Set the phidgetwsn pointer to NULL
+ accel=0;
+
+ // Read an option from the configuration file
+ serial = cf->ReadInt(section, "serial", -1);
+
+ //Sampling rate and alarm time in mS
+ samplingrate = cf->ReadFloat(section, "samplingrate", 40.0);
+ alarmtime = cf->ReadFloat(section, "alarmtime", 45.0);
+
+ return;
+}
+
+PhidgetAcc::~PhidgetAcc() {}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set up the device. Return 0 if things go well, and -1 otherwise.
+int PhidgetAcc::Setup() {
+ PLAYER_MSG0(1,"PhidgetAccel driver initialising");
+
+ //Use the Phidgets library to communicate with the devices
+ CPhidgetAccelerometer_create(&accel);
+ CPhidget_open((CPhidgetHandle)accel,serial);
+
+ PLAYER_MSG0(1,"Waiting for Attachment.");
+
+ int status(-1);
+
+ //Wait for attachment 1s or aborts.
+ status=CPhidget_waitForAttachment((CPhidgetHandle)accel, 1000);
+
+ if (status != 0) {
+ PLAYER_ERROR("There was a problem connecting to the PhidgetAccelerometer.");
+ return(1);
+ } else {
+ PLAYER_MSG0(1,"Connection granted to the PhidgetAccelerometer.");
+ }
+
+ PLAYER_MSG0(1,"PhidgetAcc driver ready");
+
+ // Start the device thread; spawns a new thread and executes
+ // PhidgetAcc::Main(), which contains the main loop for the driver.
+ StartThread();
+
+ return(0);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Shutdown the device
+int PhidgetAcc::Shutdown() {
+ PLAYER_MSG0(1,"Shutting PhidgetAcc driver down");
+
+ // Stop and join the driver thread
+ StopThread();
+
+ // Turn of the device and delete the Phidget objects
+ CPhidget_close((CPhidgetHandle)accel);
+ CPhidget_delete((CPhidgetHandle)accel);
+ accel=0;
+
+ PLAYER_MSG0(1,"PhidgetAcc driver has been shutdown");
+
+ return(0);
+}
+
+int PhidgetAcc::ProcessMessage(QueuePointer &resp_queue,
+ player_msghdr * hdr,
+ void * data) {
+ return(0);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Main function for device thread
+void PhidgetAcc::Main() {
+
+ //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_framerate_start;
+ struct timeval tv_framerate_end;
+ struct timeval tv_realtime_start;
+ struct timeval tv_realtime_end;
+
+ gettimeofday( &tv_framerate_start, NULL ); // NULL -> don't want timezone information
+ tv_realtime_start = tv_framerate_start;
+
+ // The main loop; interact with the device here
+ while (true) {
+ //find out the real elapsed time
+ gettimeofday( &tv_realtime_end, NULL );
+ //calculate the time in mS
+ float real_elapsed=timediffms(tv_realtime_start,tv_realtime_end);
+ //restart the timer
+ gettimeofday( &tv_realtime_start, NULL );
+
+ //check if the time was too long
+ static bool gavewarning(false);
+ if ((!gavewarning) && (real_elapsed > alarmtime)) {
+ PLAYER_WARN2("Cycle took %d mS instead of the desired %d mS. (Only warning once)\n",real_elapsed , samplingrate);
+ gavewarning=true;
+ }
+ //std::cout << real_elapsed << "mS\n";
+
+
+ // test if we are supposed to cancel
+ pthread_testcancel();
+
+ // Process incoming messages. PhidgetAcc::ProcessMessage() is
+ // called on each message.
+ ProcessMessages();
+
+ data.node_type=1;
+ data.node_id=1;
+ data.node_parent_id=1;
+ data.data_packet.light = -1;
+ data.data_packet.mic = -1;
+ data.data_packet.magn_x = -1;
+ data.data_packet.magn_y = -1;
+ data.data_packet.magn_z = -1;
+ data.data_packet.temperature = -1;
+ data.data_packet.battery = -1;
+ int n_axis;
+ if(CPhidgetAccelerometer_getNumAxis(accel,&n_axis)) return;
+ double *p_accel;
+ p_accel= new double[n_axis];
+ for (int i=0;i<n_axis;++i) {
+ if(CPhidgetAccelerometer_getAcceleration(accel, i, &p_accel[i])) return;
+ }
+ data.data_packet.accel_x=p_accel[0];
+ data.data_packet.accel_y=p_accel[1];
+ data.data_packet.accel_z=p_accel[2];
+ delete [] p_accel;
+
+ //Publishing data.
+ if (wsn_id.interf !=0) {
+ Publish(wsn_id, PLAYER_MSGTYPE_DATA, PLAYER_WSN_DATA_STATE, (unsigned char*)&data, sizeof(player_wsn_data_t), NULL);
+ }
+
+ //point to calculate how much to sleep, call nanosleep, after sleep restart the timer
+ //Get the ammount of time passed:
+ gettimeofday( &tv_framerate_end, NULL );
+ // figure out how much to sleep
+ long usecs = tv_framerate_end.tv_usec - tv_framerate_start.tv_usec;
+ long secs = tv_framerate_end.tv_sec - tv_framerate_start.tv_sec;
+ long elapsed_usecs = 1000000*secs + usecs;
+
+ long us_tosleep = static_cast<long>(samplingrate*1000) - elapsed_usecs;
+ //cout << "usec to sleep: " << us_tosleep << endl;
+
+ struct timespec ts;
+ ts.tv_sec = 0;
+ ts.tv_nsec = us_tosleep*1000;
+ int done=nanosleep(&ts, NULL);
+
+ //restart the counter
+ gettimeofday( &tv_framerate_start, NULL );
+
+ if (done != 0) {
+ std::cout << "Error in nanosleep! ERRNO: " << errno << " ";
+ if (errno == EINTR) {
+ std::cout << "EINTR" ;
+ } else if (errno == EINVAL) {
+ std::cout << "EINVAL" ;
+ }
+ std::cout << std::endl;
+ }
+
+ }
+}
+
+// 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*
+PhidgetAcc_Init(ConfigFile* cf, int section) {
+ // Create and return a new instance of this driver
+ return((Driver*)(new PhidgetAcc(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 phidgetAcc_Register(DriverTable* table) {
+ table->AddDriver("phidgetAcc", PhidgetAcc_Init);
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|