/*====================================================================================================*/
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);
}
Beispiel #5
0
/*====================================================================================================*/
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;
    }
}