/*====================================================================================================*/
void MPU9250_getData( int16_t *dataIMU )
{
uint8_t tmpRead[22] = {0};
#ifdef _USE_MAG_AK8963
MPU9250_ReadRegs(MPU6500_ACCEL_XOUT_H, tmpRead, 22);
#else
MPU9250_ReadRegs(MPU6500_ACCEL_XOUT_H, tmpRead, 14);
#endif
dataIMU[0] = (Byte16(int16_t, tmpRead[6], tmpRead[7])); // Temp
dataIMU[1] = (Byte16(int16_t, tmpRead[0], tmpRead[1])); // Acc.X
dataIMU[2] = (Byte16(int16_t, tmpRead[2], tmpRead[3])); // Acc.Y
dataIMU[3] = (Byte16(int16_t, tmpRead[4], tmpRead[5])); // Acc.Z
dataIMU[4] = (Byte16(int16_t, tmpRead[8], tmpRead[9])); // Gyr.X
dataIMU[5] = (Byte16(int16_t, tmpRead[10], tmpRead[11])); // Gyr.Y
dataIMU[6] = (Byte16(int16_t, tmpRead[12], tmpRead[13])); // Gyr.Z
#ifdef _USE_MAG_AK8963
if(!(tmpRead[14] & AK8963_STATUS_DRDY) || (tmpRead[14] & AK8963_STATUS_DOR) || (tmpRead[21] & AK8963_STATUS_HOFL))
return;
dataIMU[7] = (Byte16(int16_t, tmpRead[16], tmpRead[15])); // Mag.X
dataIMU[8] = (Byte16(int16_t, tmpRead[18], tmpRead[17])); // Mag.Y
dataIMU[9] = (Byte16(int16_t, tmpRead[20], tmpRead[19])); // Mag.Z
#endif
}
/*=====================================================================================================*/
void Sensor_Read( u16 ReadFreg )
{
u8 ReadBuf[20] = {0};
static s8 ReadCount = 0;
static s32 Baro_Buf[2] = {0}; // 沒加 static 資料會有問題
MPU9150_Read(ReadBuf);
Acc.X = Byte16(s16, ReadBuf[0], ReadBuf[1]); // Acc.X
Acc.Y = Byte16(s16, ReadBuf[2], ReadBuf[3]); // Acc.Y
Acc.Z = Byte16(s16, ReadBuf[4], ReadBuf[5]); // Acc.Z
Gyr.X = Byte16(s16, ReadBuf[8], ReadBuf[9]); // Gyr.X
Gyr.Y = Byte16(s16, ReadBuf[10], ReadBuf[11]); // Gyr.Y
Gyr.Z = Byte16(s16, ReadBuf[12], ReadBuf[13]); // Gyr.Z
Mag.Z = Byte16(s16, ReadBuf[14], ReadBuf[15]); // Mag.X
Mag.Z = Byte16(s16, ReadBuf[16], ReadBuf[17]); // Mag.Y
Mag.Z = Byte16(s16, ReadBuf[18], ReadBuf[19]); // Mag.Z
Temp.T = Byte16(s16, ReadBuf[6], ReadBuf[7]); // Temp
if(ReadCount == 0) {
MS5611_Read(Baro_Buf, MS5611_D1_OSR_4096);
Baro.Temp = (fp32)(Baro_Buf[0]*0.01f);
Baro.Press = (fp32)(Baro_Buf[1]*0.01f);
Baro.Height = (fp32)((Baro_Buf[1]-101333)*9.5238f);
ReadCount = (u16)(ReadFreg/MS5611_RespFreq_4096)+1;
}
ReadCount--;
}
/*=====================================================================================================*/
u8 CmpArr_U8U16( uc8 *Arr1, uc16 *Arr2, u32 dataLen )
{
u32 ErrCount = 0;
while(dataLen) {
dataLen--;
if(Byte16(Arr1[(dataLen<<1)], Arr1[(dataLen<<1)+1]) != Arr2[dataLen])
ErrCount++;
}
return ((ErrCount==0) ? SUCCESS : ERROR);
}
/*====================================================================================================*/
uint8_t Cmp_ArrU8U16( const uint8_t *Arr1, const uint16_t *Arr2, uint32_t dataLen )
{
uint32_t ErrCount = 0;
while(dataLen) {
dataLen--;
if(Byte16(uint16_t, Arr1[(dataLen<<1)], Arr1[(dataLen<<1)+1]) != Arr2[dataLen])
ErrCount++;
}
return ((ErrCount==0) ? SUCCESS : ERROR);
}
/*====================================================================================================*/
void IMU_getData( IMU_DataTypeDef *pIMU )
{
uint8_t tmpRead[22] = {0};
if((pIMU->MPU_GyrAcc_Enable == MPU_GyrAcc_ENABLE) && (pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE)) {
MPU9250_ReadRegs(MPU6500_ACCEL_XOUT_H, tmpRead, 22); // Read Gyr, Acc, Mag
}
else if(pIMU->MPU_GyrAcc_Enable == MPU_GyrAcc_ENABLE) {
MPU9250_ReadRegs(MPU6500_ACCEL_XOUT_H, tmpRead, 14); // Read Gyr, Acc
}
else if(pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE) {
MPU9250_ReadRegs(MPU6500_EXT_SENS_DATA_00, tmpRead + 14, 8); // Read Mag
}
if(pIMU->MPU_GyrAcc_Enable == MPU_GyrAcc_ENABLE) {
pIMU->Gyr[0] = (Byte16(int16_t, tmpRead[8], tmpRead[9])); // Gyr.X
pIMU->Gyr[1] = (Byte16(int16_t, tmpRead[10], tmpRead[11])); // Gyr.Y
pIMU->Gyr[2] = (Byte16(int16_t, tmpRead[12], tmpRead[13])); // Gyr.Z
pIMU->Acc[0] = (Byte16(int16_t, tmpRead[0], tmpRead[1])); // Acc.X
pIMU->Acc[1] = (Byte16(int16_t, tmpRead[2], tmpRead[3])); // Acc.Y
pIMU->Acc[2] = (Byte16(int16_t, tmpRead[4], tmpRead[5])); // Acc.Z
pIMU->ICTemp = (Byte16(int16_t, tmpRead[6], tmpRead[7])); // Temp
}
if(pIMU->MPU_Mag_Enable == MPU_Mag_ENABLE) {
if(!(!(tmpRead[14] & AK8963_STATUS_DRDY) || (tmpRead[14] & AK8963_STATUS_DOR) || (tmpRead[21] & AK8963_STATUS_HOFL))) {
pIMU->Mag[0] = (Byte16(int16_t, tmpRead[16], tmpRead[15])); // Mag.X
pIMU->Mag[1] = (Byte16(int16_t, tmpRead[18], tmpRead[17])); // Mag.Y
pIMU->Mag[2] = (Byte16(int16_t, tmpRead[20], tmpRead[19])); // Mag.Z
}
}
if(pIMU->LPS_PresTemp_Enable == LPS_PresTemp_ENABLE) {
// LPS25H_ReadReg(LPS25H_WHO_AM_I, );
}
}
/*====================================================================================================*/
void MPU6050_getData( int16_t *dataIMU )
{
uint8_t tmpRead[22] = {0};
MPU6050_ReadRegs(MPU6050_ACCEL_XOUT_H, tmpRead, 14);
dataIMU[0] = (Byte16(int16_t, tmpRead[6], tmpRead[7])); // Temp
dataIMU[1] = (Byte16(int16_t, tmpRead[0], tmpRead[1])); // Acc.X
dataIMU[2] = (Byte16(int16_t, tmpRead[2], tmpRead[3])); // Acc.Y
dataIMU[3] = (Byte16(int16_t, tmpRead[4], tmpRead[5])); // Acc.Z
dataIMU[4] = (Byte16(int16_t, tmpRead[8], tmpRead[9])); // Gyr.X
dataIMU[5] = (Byte16(int16_t, tmpRead[10], tmpRead[11])); // Gyr.Y
dataIMU[6] = (Byte16(int16_t, tmpRead[12], tmpRead[13])); // Gyr.Z
}
/*=====================================================================================================*/
void Transport_Recv( u8* RecvBuf )
{
RF_RecvData.Packet = (u8)RecvBuf[0];
RF_RecvData.Time.Min = (u8)RecvBuf[1];
RF_RecvData.Time.Sec = (u8)RecvBuf[2];
RF_RecvData.Time.mSec = (u8)RecvBuf[3];
switch(RF_RecvData.Packet) {
case 0x01:
RF_RecvData.PID.KP = (u16)Byte16(RecvBuf[4], RecvBuf[5]);
RF_RecvData.PID.KI = (u16)Byte16(RecvBuf[6], RecvBuf[7]);
RF_RecvData.PID.KD = (u16)Byte16(RecvBuf[8], RecvBuf[9]);
RF_RecvData.Thr.CH1 = (u16)Byte16(RecvBuf[10], RecvBuf[11]);
RF_RecvData.Thr.CH2 = (u16)Byte16(RecvBuf[12], RecvBuf[13]);
RF_RecvData.Thr.CH3 = (u16)Byte16(RecvBuf[14], RecvBuf[15]);
RF_RecvData.Thr.CH4 = (u16)Byte16(RecvBuf[16], RecvBuf[17]);
RF_RecvData.Thr.CH5 = (u16)Byte16(RecvBuf[18], RecvBuf[19]);
RF_RecvData.Thr.CH6 = (u16)Byte16(RecvBuf[20], RecvBuf[21]);
RF_RecvData.Batery.Vol = (u8)RecvBuf[22];
RF_RecvData.Batery.Cur = (u8)RecvBuf[23];
RF_RecvData.Batery.Cap = (u16)Byte16(RecvBuf[24], RecvBuf[25]);
break;
case 0x02:
RF_RecvData.Acc.X = (s16)Byte16(RecvBuf[4], RecvBuf[5]);
RF_RecvData.Acc.Y = (s16)Byte16(RecvBuf[6], RecvBuf[7]);
RF_RecvData.Acc.Z = (s16)Byte16(RecvBuf[8], RecvBuf[9]);
RF_RecvData.Gyr.X = (s16)Byte16(RecvBuf[10], RecvBuf[11]);
RF_RecvData.Gyr.Y = (s16)Byte16(RecvBuf[12], RecvBuf[13]);
RF_RecvData.Gyr.Z = (s16)Byte16(RecvBuf[14], RecvBuf[15]);
RF_RecvData.Mag.X = (s16)Byte16(RecvBuf[16], RecvBuf[17]);
RF_RecvData.Mag.Y = (s16)Byte16(RecvBuf[18], RecvBuf[19]);
RF_RecvData.Mag.Z = (s16)Byte16(RecvBuf[20], RecvBuf[21]);
RF_RecvData.Baro.Temp = (u16)Byte16(RecvBuf[22], RecvBuf[23]);
RF_RecvData.Baro.Press = (u32)Byte32(0, RecvBuf[24], RecvBuf[25], RecvBuf[26]);
RF_RecvData.Baro.Height = (u32)Byte32(0, RecvBuf[27], RecvBuf[28], RecvBuf[29]);
break;
case 0x03:
RF_RecvData.Ang.X = (s16)Byte16(RecvBuf[4], RecvBuf[5]);
RF_RecvData.Ang.Y = (s16)Byte16(RecvBuf[6], RecvBuf[7]);
RF_RecvData.Ang.Z = (s16)Byte16(RecvBuf[8], RecvBuf[9]);
RF_RecvData.Vel.X = (s16)Byte16(RecvBuf[10], RecvBuf[11]);
RF_RecvData.Vel.Y = (s16)Byte16(RecvBuf[12], RecvBuf[13]);
RF_RecvData.Vel.Z = (s16)Byte16(RecvBuf[14], RecvBuf[15]);
RF_RecvData.Pos.X = (s16)Byte16(RecvBuf[16], RecvBuf[17]);
RF_RecvData.Pos.Y = (s16)Byte16(RecvBuf[18], RecvBuf[19]);
RF_RecvData.Pos.Z = (s16)Byte16(RecvBuf[20], RecvBuf[21]);
RF_RecvData.GPS.Lon = (u32)Byte32(0, RecvBuf[22], RecvBuf[23], RecvBuf[24]);
RF_RecvData.GPS.Lat = (u32)Byte32(0, RecvBuf[25], RecvBuf[26], RecvBuf[27]);
break;
default:
// ERROR
break;
}
}
bool parseimumsg(uint8 * dataPacket, sensor_msgs::Imu * imu_msg)
{
float m1=0.0,m2=0.0,m3=0.0;
//
if(dataPacket[0] == 0x55 && dataPacket[1] == 0xaa)
{
//检验校验和
unsigned int sum = 0;
for(int i = 0; i < PKGSIZE - 1; i++)
{
sum += dataPacket[i];
}
//std::cout<< sum << " " << imumsg[14] << std::endl;
//
if( sum % 256 == dataPacket[PKGSIZE - 1])
{
//解算加速度X,Y,Z
int16 AccX = Byte16(int16, dataPacket[2], dataPacket[3]);
int16 AccY = Byte16(int16, dataPacket[4], dataPacket[5]);
int16 AccZ = Byte16(int16, dataPacket[6], dataPacket[7]);
float Accscalar = MPU9250A_4g * GRAVITY_MSS;
float AccTrueX = AccX * Accscalar;
float AccTrueY = AccY * Accscalar;
float AccTrueZ = AccZ * Accscalar;
//解算角速度X,Y,Z
int16 GyrX = Byte16(int16, dataPacket[8], dataPacket[9]);
int16 GyrY = Byte16(int16, dataPacket[10], dataPacket[11]);
int16 GyrZ = Byte16(int16, dataPacket[12], dataPacket[13]);
float Gyrscalar = UserGyrResolution / 57.3;
float GyrTrueX = GyrX * Gyrscalar;
float GyrTrueY = GyrY * Gyrscalar;
float GyrTrueZ = GyrZ * Gyrscalar;
//解算Mag X,Y,Z
int16 MagX = Byte16(int16, dataPacket[15], dataPacket[14]);
int16 MagY = Byte16(int16, dataPacket[17], dataPacket[16]);
int16 MagZ = Byte16(int16, dataPacket[19], dataPacket[18]);
float Magscalar = MPU9250M_4800uT*Tesla2Gauss;
float magx=MagX*Magscalar;
float magy=MagY*Magscalar;
float magz=MagZ*Magscalar;
m1=magx+mag_offset[0]; m2=magy+mag_offset[1]; m3=magz+mag_offset[2];
MagTrueY=mag_sca[0]*m1+mag_sca[1]*m2+mag_sca[2]*m3;
MagTrueX=mag_sca[3]*m1+mag_sca[4]*m2+mag_sca[5]*m3;
MagTrueZ=-1.0*(mag_sca[6]*m1+mag_sca[7]*m2+mag_sca[8]*m3);
newDataMag =(bool) dataPacket[20];
//发送Imu数据
imu_msg->header.stamp = ros::Time::now();
imu_msg->header.frame_id = "imu";
imu_msg->angular_velocity.x = GyrTrueX;
imu_msg->angular_velocity.y = GyrTrueY;
imu_msg->angular_velocity.z = GyrTrueZ;
imu_msg->linear_acceleration.x = AccTrueX;
imu_msg->linear_acceleration.y = AccTrueY;
imu_msg->linear_acceleration.z = AccTrueZ;
return true;
}
else
{
ROS_INFO_STREAM("Checksum Error");
return false;
}
}
else
{
ROS_INFO_STREAM("Header Error");
return false;
}
}
