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 ReadGSensor(void)
{
unsigned char d1,d2,d3,d4,d5,d6;
if(i2c0master_StartN(G_SENSOR, I2C_WRITE, 1) == false)
{
printf("Gsensor : %s !!\n",roboio_GetErrMsg());
return false;
}
i2c0master_SetRestartN(I2C_READ, 6);
i2c0master_WriteN(0x32); //Read from X register (Address : 0x32)
/* write stop and start read */
d1 = i2c0master_ReadN(); //X MSB
d2 = i2c0master_ReadN(); //X LSB
d3 = i2c0master_ReadN(); //Y MSB
d4 = i2c0master_ReadN(); //Y LSB
d5 = i2c0master_ReadN(); //Z MSB
d6 = i2c0master_ReadN(); //Z LSB
G_AXIS_VALUE[0] = (d2 & 0x02) ? ~(0xFFFF ^ (d2*256+d1)) : d2*256+d1;
G_AXIS_VALUE[1] = (d4 & 0x02) ? ~(0xFFFF ^ (d4*256+d3)) : d4*256+d3;
G_AXIS_VALUE[2] = (d6 & 0x02) ? ~(0xFFFF ^ (d6*256+d5)) : d6*256+d5;
if(G_AXIS_VALUE[0] == 0 && G_AXIS_VALUE[1] == 0 && G_AXIS_VALUE[2] == 0)
return false;
return true;
}
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);
}
}
void Compass_Sensor::read()
{
oldAzimuth=currentAzimuth;
oldTicks=currentTicks;
currentTicks=GetTickCount();
i2c0master_StartN(i2c_address, I2C_WRITE, 1);
i2c0master_SetRestartN(I2C_READ, 6);
i2c0master_WriteN(0x03); //Read from data register (Address : 0x03)
d1 = i2c0master_ReadN();//X MSB
d2 = i2c0master_ReadN();//X LSB
d3 = i2c0master_ReadN();//Y MSB
d4 = i2c0master_ReadN();//Y LSB
d5 = i2c0master_ReadN();//Z MSB
d6 = i2c0master_ReadN();//Z LSB
//std::cout<<d1<<", "<<d2<<", "<<d3<<", "<<d4<<", "<<d5<<", "<<d6<<"\n";
x=((d1 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d1<<8)+d2): (d1<<8)+d2;
y=((d3 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d3<<8)+d4): (d3<<8)+d4;
z=((d5 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d5<<8)+d6): (d5<<8)+d6;
//std::cout<<x<<" "<<y<<": ";//" "<<z<<"\n";
result=atan((float)y/(float)x)*180.0/PI+90;
//result=(180*(atan((double)(-1*(y/x))/PI)))+180;
int quadrant;
if(x>=0 && y>=0)quadrant=1;
if(x<0 && y>=0)quadrant=2;
if(x<0 && y<0)quadrant=3;
if(x>=0 && y<0)quadrant=4;
if(quadrant==4 || quadrant==1)result+=180;
//don't need this if I'm using atan2
/*if(quadrant==1)result=90-result;
else result=450-result;
result=(int)result%360;
//bearing = (450-theta) mod 360*/
//std::cout<<(int)result<<"\n";//<<quadrant<<"\n";
currentAzimuth=result;
if(x==0 && y==0 && z==0)currentAzimuth= -999;
}
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;
}
void Altimeter_Sensor::read()
{
//read SHT21
i2c0master_StartN(SHT21_ADDR, I2C_WRITE,1);
i2c0master_WriteN(0xf5);
//delay_ms(100);
//wait_ms(100);
i2c0master_StartN(SHT21_ADDR, I2C_READ,3);
sht21_humi[1] = i2c0master_ReadN();
sht21_humi[0] = i2c0master_ReadN();
// checksum
i2c0master_ReadN();
i2c0master_StartN(SHT21_ADDR, I2C_WRITE,1);
i2c0master_WriteN(0xf3);
//wait_ms(100);
i2c0master_StartN(SHT21_ADDR, I2C_READ,3);
sht21_temp[1] = i2c0master_ReadN();
sht21_temp[0] = i2c0master_ReadN();
// checksum
i2c0master_ReadN();
// LPS331AP
// read LPS331AP
i2c_SensorRead(LPS331AP_ADDR, 0x27, &lps331ap_state,1);
//wait_ms(100);
if(lps331ap_state == 0xff)
{
printf("LPS331AP error:%s !!\n",roboio_GetErrMsg());
}
// check error
if(lps331ap_state& 0x02)
{
if(i2c_SensorRead(LPS331AP_ADDR, 0x28, &lps331ap_press[0],1) == false)
{
printf("LPS331AP fail to read pressure XLB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x29, &lps331ap_press[1],1) == false)
{
printf("LPS331AP fail to read pressure LB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x2a, &lps331ap_press[2],1) == false)
{
printf("LPS331AP fail to read pressure MSB (%s)!\n", roboio_GetErrMsg());
}
}
if(lps331ap_state& 0x01)
{
if(i2c_SensorRead(LPS331AP_ADDR, 0x2b, lps331ap_temp,1) == false)
{
printf("LPS331AP fail to read temp LSB (%s)!\n", roboio_GetErrMsg());
}
if(i2c_SensorRead(LPS331AP_ADDR, 0x2c, lps331ap_temp+1,1) == false)
{
printf("LPS331AP fail to read temp MSB (%s)!\n", roboio_GetErrMsg());
}
}
// SHT21
sht21_rh = (double) (((long)sht21_humi[1]<< 8) + (long)sht21_humi[0]);
sht21_rh = sht21_rh*125/65536 - 6;
//printf("== SHT21 ==\n\nhumidity:%f RH ",sht21_rh);
sht21_deg = (double) (((long)sht21_temp[1]<< 8L) + (long)sht21_temp[0]);
sht21_deg = sht21_deg*175.72/65535 - 46.85;
//printf("temp:%f deg\n\n",sht21_deg);
//LPS331AP
lps331ap_mbar = (double) ((((long)lps331ap_press[2])<<16L) + (((long)lps331ap_press[1])<< 8L) + (long)lps331ap_press[0]);
lps331ap_mbar = lps331ap_mbar/4096;
//printf("== LPS331AP ==\n\npressure:%f mbar ",lps331ap_mbar);
/**************************************************
*
PSVL = P*10^M
M = StationHigh/(18400*(1+Temp/273))
*
**************************************************/
lps331ap_deg = (double) ((((int)lps331ap_temp[1])<< 8L) + (int)lps331ap_temp[0]);
lps331ap_deg = 42.5 + lps331ap_deg/480;
//printf("temp:%f deg\n\n",lps331ap_deg);
height = log10(1013.25/lps331ap_mbar)*8400*(1+lps331ap_deg/273.0);
//printf("height:%f",height);
rawHeightFromSensor=(int)height;
}
Altimeter_Sensor::Altimeter_Sensor()
{
lps331ap_state = 0;
lps331ap_press[0] = 0;
lps331ap_press[1] = 0;
lps331ap_press[2] = 0;
lps331ap_temp[0] = 0;
lps331ap_temp[1] = 0;
count[0] = 0;
count[1] = 0;
count[2] = 0;
count[3] = 0;
i2c_Init(I2CMODE_AUTO, 400000L); // init I2C lib to 400Kbps
i2c0master_StartN(SHT21_ADDR, I2C_WRITE,2);
//write user reg
i2c0master_WriteN(0xe6);
i2c0master_WriteN(0x01);
// init LPS331AP
// set CTRL_REG
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
//set REG2
i2c0master_WriteN(0x21);
i2c0master_WriteN(0x00);
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
// set REG1
i2c0master_WriteN(0x20);
i2c0master_WriteN(0xe0);
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
//set REG3
i2c0master_WriteN(0x22);
i2c0master_WriteN(0x00);
// set resolution mode
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
i2c0master_WriteN(0x10);
i2c0master_WriteN(0x6a);
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
i2c0master_WriteN(0x25);
i2c0master_WriteN(0x00);
i2c0master_StartN(LPS331AP_ADDR, I2C_WRITE,2);
i2c0master_WriteN(0x26);
i2c0master_WriteN(0x00);
rawHeightFromSensor=-1;
}
