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; }