void I2CDeviceHMC6343::pollCB(const ros::TimerEvent& e)
{
unsigned char msb1, lsb1, msb2, lsb2, msb3, lsb3;
lockI2C();
{
i2c0_SetSpeed(I2CMODE_AUTO, speed_);
i2c0master_StartN(0x32>>1, I2C_WRITE, 1);
i2c0master_WriteN(0x50); // request data
}
unlockI2C();
usleep(1000); // 1 ms
lockI2C();
{
i2c0_SetSpeed(I2CMODE_AUTO, speed_);
i2c0master_StartN( 0x33>>1, I2C_READ, 6 );
msb1 = i2c0master_ReadN();
lsb1 = i2c0master_ReadN();
msb2 = i2c0master_ReadN();
lsb2 = i2c0master_ReadN();
msb3 = i2c0master_ReadN();
lsb3 = i2c0master_ReadN();
}
unlockI2C();
unsigned short head = msb1<<8 | lsb1;
short pitch = msb2<<8 | lsb2;
short roll = msb3<<8 | lsb3;
//ROS_INFO_STREAM( (float)head/10.0f << " " << (float)pitch/10.0f << " " << (float)roll/10.0f );
if (publish_raw_)
{
std_msgs::Int16MultiArray msg;
msg.data.resize(3);
msg.data[0] = head;
msg.data[1] = pitch;
msg.data[2] = roll;
std_msgs::MultiArrayDimension dim;
dim.label="Head,Pitch,Roll";
dim.size=msg.data.size();
dim.stride=msg.data.size();
msg.layout.dim.push_back(dim);
raw_pub_.publish(msg);
}
}
Compass_Sensor::Compass_Sensor()
{
i2c0_SetSpeed(I2CMODE_FAST, 400000L);
i2c0master_StartN(i2c_address,I2C_WRITE,2);//write 2 byte
i2c0master_WriteN(0x02); //mode register
i2c0master_WriteN(0x00); //continue-measureture mode
currentAzimuth=oldAzimuth=0;
oldTicks=currentTicks=0;
wait_ms(100);
}
int InitSensor(void)
{
if (i2c_Init2(0xffff,I2C_USEMODULE0+I2C_USEMODULE1,I2CIRQ_DISABLE,I2CIRQ_DISABLE) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0_SetSpeed(I2CMODE_AUTO, 400000L);
i2c1_SetSpeed(I2CMODE_AUTO, 400000L);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x2d); //mode register
i2c0master_WriteN(0x28); //Link and measure mode
delay_ms(100);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x31); //mode register
i2c0master_WriteN(0x08); //Full-resolution
delay_ms(100);
if(i2c0master_StartN(G_SENSOR,I2C_WRITE,2) == false)
{
printf("FALSE!! %s\n", roboio_GetErrMsg());
return false;
}
i2c0master_WriteN(0x38); //mode register
i2c0master_WriteN(0x00); //bypass mode
delay_ms(100);
return true;
}
