添加温度输出

serial_6dof_imu/include/imu_data.h

@@ -39,6 +39,7 @@ float getAcc(int flag);
 float getAngular(int flag);
 float getAngle(int flag);
 float getQuat(int flag);
+float getTemp(void);
 
 int makeYawZero(void);
 int updateIICAddr(std::string input);

serial_6dof_imu/src/proc_serial_data.cpp

@@ -16,7 +16,7 @@ using namespace std;
 
 static unsigned char r_buf[BYTE_CNT];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1;  //串口打开时的文件描述符
-static float acce[3],angular[3],angle[3],quater[4];
+static float acce[3],angular[3],angle[3],quater[4],temp;
 
 static struct termios initial_settings, new_settings;
 
@@ -182,6 +182,7 @@ static void parse_serialData(unsigned char chr)
             angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGULAR_CONST;
             angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))*ANGULAR_CONST;
             angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))*ANGULAR_CONST;
+            temp       = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/100.0;
             break;
         case 0x53: //欧拉角度输出, roll, pitch, yaw
             angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))*ANGLE_CONST;
@@ -263,7 +264,7 @@ int updateIICAddr(std::string input)
  *******************************************************************/
 void show_imu_data(void)
 {
-    float yaw, pitch, roll;
+    float yaw, pitch, roll, temp;
     float degree2Rad = 3.1415926 / 180.0;
     float acc_factor = 9.806;
     init_keyboard();
@@ -278,9 +279,10 @@ void show_imu_data(void)
             close_keyboard();
             break;
         }  
-        roll = getAngle(0) * degree2Rad;
+        roll  = getAngle(0) * degree2Rad;
         pitch = getAngle(1) * degree2Rad;
-        yaw = getAngle(2) * degree2Rad;
+        yaw   = getAngle(2) * degree2Rad;
+        temp  = getTemp();
         if (yaw >= 3.1415926)
             yaw -= 6.2831852;
 
@@ -290,7 +292,7 @@ void show_imu_data(void)
         cout << "q_x:     " << setw(12) << getQuat(1) << "  q_y:     " << setw(12) << getQuat(2) << "  q_z:     " << setw(12) << getQuat(3) << "  q_w: " << setw(12) << getQuat(0) << endl;
         cout << "a_v_x:   " << setw(12) << getAngular(0) * degree2Rad << "  a_v_y:   " << setw(12) << getAngular(1) * degree2Rad << "  a_v_z:   " << setw(12) << getAngular(2) * degree2Rad << endl;
         cout << "l_acc_x: " << setw(12) << -getAcc(0) * acc_factor << "  l_acc_y: " << setw(12) << -getAcc(1) * acc_factor << "  l_acc_z: " << setw(12) << -getAcc(2) * acc_factor << endl;
-
+        cout << "temp:    " << setw(12) << temp << endl;
         //usleep(10 * 1000);
     }
 }
@@ -389,6 +391,14 @@ float getQuat(int flag)
     return quater[flag];
 }
 
+/********************************************
+ * Description:返回温度值.
+ ********************************************/
+float getTemp(void)
+{
+    return temp;
+}
+
 /*************************************************************
  * Description:从串口读取数据,然后解析出各有效数据段,这里
  *  一次性从串口中读取88个字节,然后需要从这些字节中进行

serial_6dof_imu/src/serial_imu.cpp

@@ -14,10 +14,10 @@ void show_direction() {
     cout << "----------------------------------------" << endl;
     cout << "                                        " << endl;
     cout << "                                        " << endl;
-    cout << "------------  0命令, 退出  -------------" << endl;
-    cout << "------  1命令，IMU模块yaw角归0  --------" << endl;
-    cout << "------  2命令，修改I2C地址      --------" << endl;
-    cout << "------  3命令，显示IMU模块数据  --------" << endl;
+    cout << "------------  0命令, 退出  -------------"  << endl;
+    cout << "------  1命令，IMU模块yaw角归0  --------"   << endl;
+    cout << "------  2命令，修改I2C地址      --------"   << endl;
+    cout << "------  3命令，显示IMU模块数据  --------"    << endl;
     cout << "                                        " << endl;
     cout << "                                        " << endl;
     cout << "----------------------------------------" << endl;