mica2.cc

机器人仿真软件

	case 1:                            // wake up (XCOMMAND_WAKEUP)
	{
		index += BuildXCommandHeader 
			(buffer+index, 0x12, node_id, group_id, -1, -1, -1);
		if (node_id == 0)             // base node?
			base_node_status = 1;
		break;
	}
	case 2:                            // actuate (XCOMMAND_ACTUATE)
	{
		index += BuildXCommandHeader 
			(buffer+index, 0x40, node_id, group_id, device, enable, -1);
		break;
	}
	case 3:                            // set_rate (XCOMMAND_SET_RATE)
	{
		index += BuildXCommandHeader 
			(buffer+index, 0x20, node_id, group_id, -1, -1, node_sleep);
		break;
	}
    }
	
    index += 2;
    calcCRC (buffer, index);               // calculate and add CRC
	
    WriteSerial (buffer, index, 0x41);		// Write with ACK
}

////////////////////////////////////////////////////////////////////////////////
// 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);
	
    // In case the RFID interface is enabled, send a "select_tag" command first
    if ((provideRFID) && (this->rfid_subscriptions > 0))
        BuildRFIDHeader (0, NULL, 0, 0xFFFF, 1);

	// Reading from UART
    length = ReadSerial (buffer);
    if (length < 16)        // minimum valid packet size
	return;             // ignore partial packets

    // Decoding and publishing the UART data
    if (DecodeSerial (buffer, length) == -1)
	return;

    return;
}

//------------------------------------------------------------------------------
////////////////////////////////////////////////////////////////////////////////
// ReadSerial function - reads one XSensorPacket from the serial port
int Mica2::ReadSerial (unsigned char *buffer)
{
    unsigned char c;
    int err, i = 0;

    buffer[i] = 0x7e;          // serial start byte
    while (1) {
    	err = read (fd, &c, 1);
		if (err < 0)
		{
			PLAYER_ERROR (">> Error reading from serial port !");
			return (-1);
		}
		if (err == 1)
		{
			if (++i > 255) return i;
			buffer[i] = c;
			if (c == 0x7e) return i;
		}
    }
}

////////////////////////////////////////////////////////////////////////////////
// WriteSerial function - write one XSensorPacket to the serial port
int Mica2::WriteSerial (unsigned char *buffer, int length, unsigned char ack)
{
    unsigned char c;
    int err, i = 0;

    c = 0x7e;					// serial start byte
    write (fd, &c , 1);
    c = ack;					// P_PACKET_ACK or P_PACKET_NOACK
    write (fd, &c , 1); 

    c = buffer[0];
    while (1)
    {
		if (i>= length)
			return length;
		c = buffer[i++];
		err = write (fd, &c, 1);

		if (err < 0)
		{
			PLAYER_ERROR (">> Error writing to serial port !");
			return (-1);
		}
    }
    c = 0x7e;					// serial SYNC_BYTE
    write (fd, &c , 1);

    return 0;
}

////////////////////////////////////////////////////////////////////////////////
// FindNodeValues function - find the appropriate calibration values for nodeID
NodeCalibrationValues Mica2::FindNodeValues (unsigned int nodeID)
{
    NodeCalibrationValues n;
	
    unsigned int i = 0;
	
    for (i = 0; i < ncv.size (); i++)
    {
    	n = ncv.at (i);
	
	if (n.node_id == nodeID)
		break;
    }
	
    return n;
}

////////////////////////////////////////////////////////////////////////////////
// DecodeSerial function - decode a XSensorPacket or a M1MiniPacket
int Mica2::DecodeSerial (unsigned char *buffer, int length)
{
    NodeCalibrationValues node_values;
    player_wsn_data_t  wsn_data;
    player_rfid_data_t rfid_data;
    bool rfidPacket = FALSE;
    bool wsnPacket  = FALSE;
    int  i = 0, o = 2;    // index and offset

    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
		buffer[i++] = buffer[++o] ^ 0x20;
		++o;
	}
	else
	{
		buffer[i++] = buffer[o++];
	}
    }
	
    switch (buffer[2])
    {
	case 0x03:
	{       // a HEALTH packet
	    // Health offset to data payload
	    //SensorPacket *packet = (SensorPacket *)(buffer + 5);
	    //HealthData *data = (HealthData *)packet;
	    break;
	}
	case 0x33:
	{       // a MULTIHOP packet
	    if (wsn_subscriptions < 1)
		break;
	    // Multihop offset to data payload
	    SensorPacket *packet = (SensorPacket *)(buffer + 12);
	    switch (packet->board_id)
	    {
		case 0x02:
		{                       // MTS510
		    if (packet->packet_id == 1)
		    {
			wsnPacket = TRUE;
			MTS510Data *data = (MTS510Data *)packet->data;
			wsn_data.node_type      = packet->board_id;
			wsn_data.node_id        = packet->node_id;
			
			wsn_data.node_parent_id = packet->parent;
				
			wsn_data.data_packet.light       = data->light;
			int sound = (data->sound[0] + data->sound[1] + 
				data->sound[2] + data->sound[3] + 
				data->sound[4]) / 5;
			wsn_data.data_packet.mic         = sound;
						
			if (raw_or_converted != 0)
			{
				node_values = FindNodeValues (packet->node_id);
						
				wsn_data.data_packet.accel_x = ConvertAccel 
					(data->accelX, 
					node_values.c_values[0], 
					node_values.c_values[1], raw_or_converted);
				wsn_data.data_packet.accel_y = ConvertAccel 
					(data->accelY,
					node_values.c_values[2],
					node_values.c_values[3], raw_or_converted);
			} 
			else
			{
				wsn_data.data_packet.accel_x     = data->accelX;
				wsn_data.data_packet.accel_y     = data->accelY;
			}
			wsn_data.data_packet.accel_z     = -1;
			wsn_data.data_packet.magn_x      = -1;
			wsn_data.data_packet.magn_y      = -1;
			wsn_data.data_packet.magn_z      = -1;
			wsn_data.data_packet.temperature = -1;
			wsn_data.data_packet.battery     = -1;
		    }
		    break;
		}
		case 0x84:
		{                       // MTS310
		    if (packet->packet_id == 1)
		    {
			wsnPacket = TRUE;
			MTS310Data *data = (MTS310Data *)packet->data;
			wsn_data.node_type      = packet->board_id;
			wsn_data.node_id        = packet->node_id;
			wsn_data.node_parent_id = packet->parent;
	
			wsn_data.data_packet.mic         = data->mic;
	
			if (raw_or_converted != 0)
			{
				node_values = FindNodeValues (packet->node_id);
				
				wsn_data.data_packet.accel_x = ConvertAccel 
					(data->accelX,
					node_values.c_values[0],
					node_values.c_values[1], raw_or_converted);
				wsn_data.data_packet.accel_y = ConvertAccel 
					(data->accelY,
					node_values.c_values[2],
					node_values.c_values[3], raw_or_converted);
				// Convert battery to Volts
				wsn_data.data_packet.battery     =
					(1252352 / (float)data->vref) / 1000;
						
				// Convert temperature to degrees Celsius
				float thermistor = (float)data->thermistor;
				unsigned short rthr = (unsigned short)
					(10000 * (1023 - thermistor) / thermistor);
							
				wsn_data.data_packet.temperature = 
					(1 / (0.001307050f + 0.000214381f *
					log (rthr) + 0.000000093f *
					pow (log (rthr),3))) - 273.15;
							
				// Convert the magnetometer data to Gauss
				wsn_data.data_packet.magn_x      = 
					(data->magX / (1.023*2.262*3.2)) / 1000;
				wsn_data.data_packet.magn_y      = 
					(data->magY / (1.023*2.262*3.2)) / 1000;
							
				// Convert the light to mV
				wsn_data.data_packet.light       = (data->light *
					wsn_data.data_packet.battery / 1023);
			} 
			else
			{
				wsn_data.data_packet.accel_x     = data->accelX;
				wsn_data.data_packet.accel_y     = data->accelY;
				wsn_data.data_packet.battery     = data->vref;
				wsn_data.data_packet.temperature = 
					data->thermistor;
				wsn_data.data_packet.magn_x      = data->magX;
				wsn_data.data_packet.magn_y      = data->magY;
				wsn_data.data_packet.light       = data->light;
			}
			wsn_data.data_packet.accel_z     = -1;
			wsn_data.data_packet.magn_z      = -1;
		    }
		    break;
		}
	    }
	    break;
	}
	case 0x51:
	{                       // a RFID packet
		if (rfid_subscriptions < 1)
			break;
		
		rfidPacket = TRUE;
			
		player_rfid_tag_t RFIDtag;
		memset (&RFIDtag, 0, sizeof (RFIDtag));
			
		RFIDMsg *rmsg = (RFIDMsg *)buffer;
		int dataoffset;
			
		// Get tag information if first packet
		if ((rmsg->ptotal == 1) && (rmsg->pi == 0))
		{
		    unsigned char response_code = getDigit (rmsg->data[0]);
		    response_code <<= 4;
		    response_code &= 0xF0;
		    response_code |= getDigit (rmsg->data[1]);
				
		    if (response_code == 0x14)		// SELECT TAG pass
		    {
			unsigned char tag_type = getDigit (rmsg->data[2]);
			tag_type <<= 4;
			tag_type &= 0xF0;
			tag_type |= getDigit (rmsg->data[3]);
			RFIDtag.type = tag_type;
			dataoffset = 4;
					
			RFIDtag.guid_count = 8;
		
			int x = 0, cc = 0;
			int xlength = 23 - (29 - buffer[4]);
			for (x = dataoffset; x < xlength; x += 2)
			{
				if ((((rmsg->data[x]   > 0x2F) && (rmsg->data[x]   < 0x3A)) ||     // if p[i] is a digit
					((rmsg->data[x]    > 0x40) && (rmsg->data[x]   < 0x47)) ) &&   // if p[i] is a capital lette
					(((rmsg->data[x+1] > 0x2F) && (rmsg->data[x+1] < 0x3A)) ||     // if p[i+1] is a digit
					((rmsg->data[x+1]  > 0x40) && (rmsg->data[x+1] < 0x47)) ))     // if p[i+1] is a capital let
				{
					char str[3];
					sprintf (str, "%c%c", rmsg->data[x], rmsg->data[x+1]);
					sscanf (str, "%x", (unsigned int*)&RFIDtag.guid[cc]);
					cc++;
				}
			}
		
			rfid_data.tags_count = 1;
			rfid_data.tags[0] = RFIDtag;
		    }
		}
    		break;
	}
	default:
	{
		// we only handle RFID, HEALTH and MULTIHOP package types for now
		break;
	}
    }
	
    if ((wsn_data.node_id == 0) && (filterbasenode == 1) && (!rfidPacket))
        return -1;

    if ((provideRFID) && (rfidPacket))
	// Write the RFID data
	Publish (rfid_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_RFID_DATA,
		&rfid_data, sizeof (player_rfid_data_t), NULL);

    if ((provideWSN) && (wsnPacket))
	// Write the WSN data
	Publish (wsn_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_WSN_DATA,
		&wsn_data, sizeof (player_wsn_data_t), NULL);

    return 0;
}

////////////////////////////////////////////////////////////////////////////////
// ConvertAccel function - convert RAW accel. data to metric units (m/s^2)
float Mica2::ConvertAccel (unsigned short raw_accel, 
                           int neg_1g, int pos_1g, int converted)
{
    if (neg_1g == 0)
	neg_1g = 450;
    if (pos_1g == 0)
	pos_1g = 550;
	
    float sensitivity  = (pos_1g - neg_1g) / 2.0f;
    float offset       = (pos_1g + neg_1g) / 2.0f;
    float acceleration = (raw_accel - offset) / sensitivity;
    if (converted == 1)
        return acceleration * 9.81;
    else
        return acceleration;
}
//------------------------------------------------------------------------------