mica2.cc

机器人仿真软件

			break;
		}
    }
    // Set the baudrate to the given portSpeed
    cfsetispeed (&options, port_speed);
    cfsetospeed (&options, port_speed);

    // Activate the settings for the port
    if (tcsetattr (fd, TCSANOW, &options) < 0)
    {
		PLAYER_ERROR (">> Unable to set serial port attributes !");
		return (-1);
    }

    // Make sure queues are empty before we begin
    tcflush (fd, TCIFLUSH);

    // Start the device thread
    StartThread ();

    return (0);
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int Mica2::Shutdown ()
{
    // Stop the driver thread
    StopThread ();
	
    // Close the serial port
    close (fd);
	
    PLAYER_MSG0 (1, "> Mica2 driver shutting down... [done]");
    return (0);
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void Mica2::Main () 
{
    timespec sleepTime = {0, 0};

    // The main loop; interact with the device here
    while (true)
    {
		// test if we are supposed to cancel
		pthread_testcancel ();

		// Process any pending messages
		ProcessMessages ();

		// Interact with the device, and push out the resulting data.
		if (base_node_status != 0) // if the base node is asleep, no serial
	        	            	   // data can be read
		    	RefreshData ();

		nanosleep (&sleepTime, NULL);
    }
}

////////////////////////////////////////////////////////////////////////////////
// ProcessMessage function
int Mica2::ProcessMessage (MessageQueue* resp_queue, 
			   player_msghdr * hdr,
			   void * data)
{	
    assert (hdr);
    assert (data);
	
    if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD, 
	PLAYER_WSN_CMD_DEVSTATE, wsn_addr))
    {
    	// Actuate various devices on the node
		player_wsn_cmd_t *command = (player_wsn_cmd_t*)data;

		ChangeNodeState (command->node_id, command->group_id, 
			 2, command->device, command->enable, -1);

		return 0;
    }
    else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
	 PLAYER_WSN_REQ_POWER, wsn_addr))
    {
		// Put the node in sleep mode (0) or wake it up (1)
		player_wsn_power_config *powerconfig = 
			(player_wsn_power_config*)data;
		
		// Only allow 0/1 values here
		if ((powerconfig->value != 0) && (powerconfig->value != 1))
		{
			Publish (wsn_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK, 
		     hdr->subtype);
			return -1;
		}
			
		ChangeNodeState (powerconfig->node_id, powerconfig->group_id, 
			 powerconfig->value, -1, -1, -1);
		
		Publish (wsn_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
		return 0;
    }
    else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
	 PLAYER_WSN_REQ_DATATYPE, wsn_addr))
    {
		// Change the data type to RAW or converted metric units
		player_wsn_datatype_config *datatype = 
			(player_wsn_datatype_config*)data;

		unsigned int val = datatype->value;
		if ((val >= 0) && (val < 3))
		{
			raw_or_converted = val;
			Publish (wsn_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
                	 hdr->subtype);
		}
		else
			Publish (wsn_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
                	 hdr->subtype);
		return 0;
    }
    else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
	 PLAYER_WSN_REQ_DATAFREQ, wsn_addr))
    {
		// Change the data frequency rate
		player_wsn_datafreq_config *datafreq = 
			(player_wsn_datafreq_config*)data;
		ChangeNodeState (datafreq->node_id, datafreq->group_id, 
			 3, -1, -1, datafreq->frequency);

		Publish (wsn_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, 
		 hdr->subtype);
		return 0;
    }
    else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
	 PLAYER_RFID_REQ_READTAG, rfid_addr))
    {
		// to be implemented
		Publish (rfid_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, 
		 hdr->subtype);
		return 0;
    }
    else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
	 PLAYER_RFID_REQ_WRITETAG, rfid_addr))
    {
		// to be implemented
		Publish (rfid_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, 
		 hdr->subtype);
		return 0;
    }
    else
    {
		return -1;
    }
	
    return 0;
}

////////////////////////////////////////////////////////////////////////////////
// calcByte function
int Mica2::calcByte (int crc, int b)
{
    crc = crc ^ (int)b << 8;

    for (int i = 0; i < 8; i++)
    {
		if ((crc & 0x8000) == 0x8000)
			crc = crc << 1 ^ 0x1021;
		else
			crc = crc << 1;
    }

    return (crc & 0xFFFF);
}

////////////////////////////////////////////////////////////////////////////////
// calcCRC function
void Mica2::calcCRC (unsigned char *packet, int length)
{
    int crc   = 0;
    int index = 1;
    int count = length - 3;
	
    while (count > 0) {
    	crc = calcByte (crc, packet[index++]);
	count--;
    }
    packet[length-2] = (char)(crc & 0xFF);
    packet[length-1] = (char)((crc >> 8) & 0xFF);
}

////////////////////////////////////////////////////////////////////////////////
// GetDigit function
char Mica2::getDigit (char c)
{
   if (( c >= '0' ) && (c <= '9'))
      return (c - '0');
   if (( c >= 'a') && (c <= 'f'))
      return (c - 'a' + 10 );
   if (( c >= 'A' ) && (c <= 'F'))
      return (c - 'A' + 10);
   return (-1);
}
////////////////////////////////////////////////////////////////////////////////
// BuildXCommandHeader function
int Mica2::BuildXCommandHeader (unsigned char* buffer, int command, 
				int node_id, int group_id, 
				int device, int state, 
				int rate)
{
    // TinyOS header
    XCommandMsg *msg = (XCommandMsg *)buffer;
    msg->tos.addr    = (unsigned short)node_id;// Broadcast address: 0xFFFF
    msg->tos.type    = 0x30;                   // AMTYPE_XCOMMAND
    msg->tos.group   = (unsigned char)group_id;// Broadcast: 0xFF
    msg->tos.length  = sizeof (XCommandMsg) - sizeof (TOSMsgHeader);

    // Data payload
    msg->seq_no      = 0xFF; // Broadcast sequence number
    msg->destination_id = (unsigned short)node_id; // Broadcast address: 0xFFFF
    msg->inst[0].cmd    = (unsigned short)command;
    if (device != -1)
    {
		msg->inst[0].param.actuate.device = (unsigned short)device;
		msg->inst[0].param.actuate.state  = (unsigned short)state;
		return sizeof (XCommandMsg);
    }
    if (rate != -1)
    {
		msg->inst[0].param.new_rate = (unsigned int)rate;
		return sizeof (XCommandMsg);
    }
    msg->inst[0].param.new_rate = 0xCCCCCCCC;  // Fill unused in known way
    return sizeof (XCommandMsg);
}

////////////////////////////////////////////////////////////////////////////////
// BuildRFIDHeader function - ASCII mode
int Mica2::BuildRFIDHeader (unsigned char command, unsigned char* buffer,
						 	unsigned short len, int node_id, int group_id)
{
    M1miniCommand  cmd;
    RFIDMsg        msg;
    int            msg_len = 23;
    int            cmd_len = 0;
    int            cmd_i   = 0;
    int            ptotal  = 1;
    int            pi      = 0;
    int            i       = 0;
    int            datalen;
    unsigned short crc;
    unsigned char *ptr;

	// If read/write tag command selected, buffer must hold the GUID values
    if ((buffer == NULL) && (command != 0))
	return -1;

    if (len > sizeof (M1miniCommand))
        return -1;

    // clear cmd buffer
    memset ((char *) &cmd, '0', sizeof (M1miniCommand));

    switch (command)
    {
		case 0:			// SELECT_TAG (0x14)
		{
			cmd.flags[0]   = '0';
			cmd.flags[1]   = '0';
			cmd.request[0] = '1';
			cmd.request[1] = '4';
		        cmd_len = 6;
			break;
		}
		case 1:			// READ_TAG (0x24)
		{
			cmd.flags[0]   = '4';
			cmd.flags[1]   = '0';
			cmd.request[0] = '2';
			cmd.request[1] = '4';
		        cmd_len = 26;
			memcpy (cmd.type, &buffer[0], len);
			break;
		}
		case 2:			// WRITE_TAG (0x44)
		{
			cmd.flags[0]   = '4';
			cmd.flags[1]   = '0';
			cmd.request[0] = '4';
			cmd.request[1] = '4';
	    		cmd_len = 34;
			memcpy (cmd.type, &buffer[0], len);
			break;
		}
		// Only allow already implemented commands
		default:
			return -1;
	}

	// Split the packet into several chunks for Read/Write tag commands
    if (cmd_len > msg_len)
    {
        ptotal = cmd_len / msg_len;
        if (cmd_len % msg_len)
            ptotal++; 
    }

    // TOS packet struct
    msg.tos.addr  = (unsigned short)node_id;// Broadcast address: 0xFFFF
    msg.tos.type  = 0x51;				     // AMTYPE_RFID
    msg.tos.group = (unsigned char)group_id;// Broadcast: 0xFF
    msg.ptotal = ptotal;
    msg.RID    = 0x1234;
    msg.SG     = 0x0022;

	// Data payload
    for (pi = 0; pi < ptotal; pi++)
    {
        if (pi == ptotal - 1)
            datalen = cmd_len - cmd_i;
        else
            datalen = msg_len;

        msg.tos.length = datalen + 6;		// PAYLOAD_DATA_INDEX
        msg.pi = pi;
		
	ptr = ((unsigned char*)&cmd) + cmd_i;

        for (i = 0; i < datalen; i++)
            msg.data[i] = ptr[i];

	ptr = (unsigned char*)&msg;

	// Calculate CRC for the entire packet
        crc = 0;
        crc = calcByte (crc, 0x42);		// PACKET_NOACK
        for (i = 0; i < (5 + msg.tos.length); i++)
            crc = calcByte (crc, ptr[i]);

        // Append CRC
        msg.data[datalen]   = 0;
        msg.data[datalen]  |= (unsigned char) crc;
        msg.data[datalen+1] = (unsigned char) (crc >> 8);

        WriteSerial ((unsigned char*)&msg, 7 + msg.tos.length, 0x42);	// NOACK

        cmd_i += datalen;

	// sleep a while so that TOSBase can forward all UART to RADIO
        usleep (50000);			// 50ms
    }
    return 0;
}

////////////////////////////////////////////////////////////////////////////////
// ChangeNodeState function
void Mica2::ChangeNodeState (int node_id, int group_id, unsigned char state, 
			     int device, int enable, double rate)
{
    unsigned char buffer[255];
    int index = 0;
	
    // Assign defaults to {node,group}_id
    if ((group_id == -1) || (group_id == 0))
    	group_id = 0xFF;
    if (node_id == -1)
	node_id = 0xFFFF;
		
    int node_sleep = 0;
    if (rate != -1)
	node_sleep =  static_cast<int> (1000 / rate);

    // Start constructing the TinyOS packet
	// (serial start byte and ACK are handled in the WriteSerial method) 
    buffer[index++] = 0xFF;                // Broadcast sequence number
	
    switch (state)
    {
    	case 0:                            // sleep (XCOMMAND_SLEEP)
	{
		index += BuildXCommandHeader 
			(buffer+index, 0x11, node_id, group_id, -1, -1, -1);
		if (node_id == 0)             // base node?
			base_node_status = 0;
		
		break;
	}