mica2.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2006 Radu Bogdan Rusu ([e-mail])
 *
 *  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
 *
 */
/*
 Desc: Driver for the Crossbow Mica2/Mica2Dot mote
 Author: Radu Bogdan Rusu
 Date: 09 Mar 2006
 Portions borrowed from the TinyOS project (http://www.tinyos.net), distributed
 according to the Intel Open Source License.
*/

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_mica2 mica2
 * @brief Crossbow Mica2/Mica2Dot mote sensor node

The mica2 driver controls the Crossbow Mica2/Mica2DOT mote sensor node. The 
MTS310 and MTS510 boards are supported, as well as the M1-mini RFID reader 
board.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_wsn
- @ref interface_rfid

@par Requires

- none

@par Configuration requests

- PLAYER_WSN_REQ_POWER
- PLAYER_WSN_REQ_DATATYPE
- PLAYER_WSN_REQ_DATAFREQ
- PLAYER_RFID_REQ_READTAG
- PLAYER_RFID_REQ_WRITETAG

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
  - Serial port to which the Crossbow MIB510 is attached.  If you are
    using a USB/232 or USB/422 converter, this will be "/dev/ttyUSBx".

- speed (integer)
  - Default: 57600
  - Baud rate. Valid values are 19200 for MICA2DOT and 57600 for MICA2.

- node (integer tupple)
  - These are the calibration values for -1G/+1G for the ADXL202JE accelerometer
    sensor (see the appropriate data sheet on how to obtain it). Each sepparate
    board *MUST* be calibrated! Only usable when requesting data in converted
    metric units mode.
  - The tuple means: [node_id
                      group_id
                      calibration_negative_1g_x_axis
                      calibration_positive_1g_x_axis
                      calibration_negative_1g_y_axis
                      calibration_positive_1g_y_axis
                      calibration_negative_1g_z_axis
                      calibration_positive_1g_z_axis
                     ]

- converted (integer)
  - Default: 1.
  - Fill the data buffer with converted engineering units (1) or RAW (0) values.

- filterbasenode (integer)
  - Default: 0.
  - Filter the base node (ID 0) in case there is another application != TOSBase
    installed on it.
  
@par Example 

@verbatim
driver
(
  name "mica2"
  provides ["wsn:0" "rfid:0"]
  port "/dev/ttyS0"
  speed "57600"
  # Calibrate node 0 from group 125 (default) with X={419,532} and Y={440,552}
  node [0 125 419 532 440 552 0 0]
  # Calibrate node 2 from group 125 (default) with X={447,557} and Y={410,520}
  node [2 125 447 557 410 520 0 0]
  # Use RAW values
  converted 0
  # Filter the base node (in case TOSBase is not installed on it)
  filterbasenode 1
)
@endverbatim

@author Radu Bogdan Rusu

*/
/** @} */

#include "mica2.h"

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <math.h>
#include <vector>

// Includes needed for player
#include <libplayercore/playercore.h>

// The Mica2 device class.
class Mica2 : public Driver
{
    public:
    	// Constructor
		Mica2 (ConfigFile* cf, int section);

		// Destructor
		~Mica2 ();

		// Implementations of virtual functions
		virtual int Setup       ();
		virtual int Shutdown    ();
		virtual int Subscribe   (player_devaddr_t id);
		virtual int Unsubscribe (player_devaddr_t id);

		// This method will be invoked on each incoming message
		virtual int ProcessMessage (MessageQueue* resp_queue, 
				    player_msghdr * hdr,
				    void * data);
    private:

		// Main function for device thread.
		virtual void Main ();
		void RefreshData  ();

		// Port file descriptor
		int                fd;

		// Does the user want RAW or converted values?
		int                raw_or_converted;

		// Is the base node awake or sleeping?
		int                base_node_status;

		// Interfaces that we might be using
		// WSN interface
		player_devaddr_t   wsn_addr;
		player_wsn_cmd_t   wsn_cmd;
		bool               provideWSN;
		int                wsn_subscriptions;

		// RFID interface
		player_devaddr_t   rfid_addr;
		player_rfid_cmd_t  rfid_cmd;
		bool               provideRFID;
		int                rfid_subscriptions;

		const char*        port_name;
		int                port_speed;

		// Filter base node from readings ?
		int                filterbasenode;

		// Calibration values
		int                nodes_count;
		NCV                ncv;
		
		// Calibration values
		int                calibration_values[6];
		// Calibration node ID
		int                calibration_node_id;
		
		int ReadSerial   (unsigned char *buffer);
		int WriteSerial  (unsigned char *buffer, int length, unsigned char ack);
		NodeCalibrationValues FindNodeValues (unsigned int nodeID);
		int DecodeSerial (unsigned char *buffer, int length);

		int BuildXCommandHeader (unsigned char* buffer, int command, 
				 int node_id, int group_id, 
				 int device, int state, 
				 int rate);
		int BuildRFIDHeader (unsigned char command, unsigned char* buf, 
							 unsigned short len, int node_id, int group_id);
		int  calcByte (int crc, int b);
		char getDigit (char c);
		void calcCRC  (unsigned char *packet, int length);
		
		void ChangeNodeState (int node_id, int group_id, unsigned char state, 
			      int device, int enable, double rate);
		float ConvertAccel (unsigned short raw_accel, int neg_1g, int pos_1g,
		              int converted);
};

////////////////////////////////////////////////////////////////////////////////
//Factory creation function. This functions is given as an argument when
// the driver is added to the driver table
Driver* Mica2_Init (ConfigFile* cf, int section)
{
    // Create and return a new instance of this driver
    return ((Driver*)(new Mica2 (cf, section)));
}

////////////////////////////////////////////////////////////////////////////////
//Registers the driver in the driver table. Called from the 
// player_driver_init function that the loader looks for
void Mica2_Register (DriverTable* table)
{
    table->AddDriver ("mica2", Mica2_Init);
}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
Mica2::Mica2 (ConfigFile* cf, int section)
	: Driver (cf, section)
{
    int i = 0;
    int j = 0;
    this->wsn_subscriptions = this->rfid_subscriptions = 0;
    provideRFID = FALSE;
    provideWSN  = FALSE;
    base_node_status = 1;

    memset(&this->wsn_addr,  0, sizeof (player_devaddr_t));
    memset(&this->rfid_addr, 0, sizeof (player_devaddr_t));

    port_name   = cf->ReadString (section, "port", DEFAULT_MICA2_PORT);
    port_speed  = cf->ReadInt (section, "speed", DEFAULT_MICA2_RATE);
    nodes_count = cf->ReadInt (section, "nodes", 0);
	
    for (i = 0; i < nodes_count; i++)
    {
    	char node_nr[7];
    	sprintf (node_nr, "node%d", (i+1));
	NodeCalibrationValues n;
	n.node_id  = cf->ReadTupleInt (section, node_nr, 0, 0);
	n.group_id = cf->ReadTupleInt (section, node_nr, 1, 0);
	for (j = 0; j < 6; j++)
		n.c_values[j] = cf->ReadTupleInt (section, node_nr, j+2, 0);
   	ncv.push_back (n);
    }

    // Defaults to converted values
    raw_or_converted = cf->ReadInt (section, "converted", 1);

    // Filter base node from readings ?
    filterbasenode   = cf->ReadInt (section, "filterbasenode", 0);

    // Do we create a WSN interface?
    if (cf->ReadDeviceAddr (&wsn_addr, section, "provides",
       PLAYER_WSN_CODE, -1, NULL) == 0)
    {
        if (this->AddInterface (this->wsn_addr))
        {
            this->SetError(-1);
            return;
        }
	provideWSN = TRUE;
    }

    // Do we create a RFID interface?
    if (cf->ReadDeviceAddr (&rfid_addr, section, "provides",
       PLAYER_RFID_CODE, -1, NULL) == 0)
    {
        if (this->AddInterface (this->rfid_addr))
        {
            this->SetError(-1);
            return;
        }
        provideRFID = TRUE;
    }

    return;
}


////////////////////////////////////////////////////////////////////////////////
// Destructor.
Mica2::~Mica2()
{
}

////////////////////////////////////////////////////////////////////////////////
// Subscribe
int Mica2::Subscribe (player_devaddr_t id)
{
	int setupResult;
	
	// do the subscription
	if ((setupResult = Driver::Subscribe (id)) == 0)
	{
		// also increment the appropriate subscription counter
		if (Device::MatchDeviceAddress (id, this->wsn_addr))
			this->wsn_subscriptions++;
		else if (Device::MatchDeviceAddress (id, this->rfid_addr))
			this->rfid_subscriptions++;
	}
	return setupResult;
}

////////////////////////////////////////////////////////////////////////////////
// Unsubscribe
int Mica2::Unsubscribe (player_devaddr_t id)
{
	int (shutdownResult);
	
	// do the unsubscription
	if ((shutdownResult = Driver::Unsubscribe (id)) == 0)
	{
		// also increment the appropriate subscription counter
		if (Device::MatchDeviceAddress (id, this->wsn_addr))
		{
			this->wsn_subscriptions--;
			assert(this->wsn_subscriptions >= 0);
		}
		else if (Device::MatchDeviceAddress (id, this->rfid_addr))
		{
			this->rfid_subscriptions--;
			assert(this->rfid_subscriptions >= 0);
		}
	}
	return (shutdownResult);
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int Mica2::Setup ()
{
    // Open serial port
    fd = open (port_name, O_RDWR | O_NOCTTY);
    if (fd < 0)
    {
		PLAYER_ERROR2 ("> Connecting to MIB510 on [%s]; [%s]...[failed!]",
		      (char*) port_name, strerror (errno));
		return (-1);
    }
    PLAYER_MSG0 (1, "> Connecting to MIB510... [done]");

    // Change port settings
    struct termios options;
    memset (&options, 0, sizeof (options));  // clear the struct for new settings
    // read satisfied if one char received
    options.c_cc[VMIN] = 1;
    options.c_cflag = CS8 | CLOCAL | CREAD;
    options.c_iflag = IGNBRK | IGNPAR;

    switch (port_speed)
    {
		case 19200:
		{
			port_speed = B19200;           // mica2dot
			break;
		}
		case 57600:
		{
			port_speed = B57600;           // mica2
			break;
		}
		default:
		{
			port_speed = B57600;