Diff: uart.cpp

Revision:

7:bfbbf605be43

Parent:

1:15ab74f0d0f1

Child:

8:1ca49cb18290

diff -r 15ab74f0d0f1 -r bfbbf605be43 uart.cpp
--- a/uart.cpp	Sat Nov 24 11:35:48 2018 +0000
+++ b/uart.cpp	Sat Nov 24 11:49:51 2018 +0000
@@ -2,6 +2,11 @@
 #include "uart.h"
 #include "command.h"
 
+#include <vector>
+#include <string>
+#include <sstream>
+using namespace std;
+
 //#define WIREED_SERIAL
 
 //シリアルポートインスタンス
@@ -12,7 +17,10 @@
     Serial  sp(p9, p10);//TWILITEをモジュールを接続したポート
     #define BAUD_RATE   115200
 #endif
-    
+
+Serial sp46Axis(p28,p27);
+#define BAUD_RATE4_6AXIS   115200
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // UART 受信割り込みハンドラ
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
@@ -25,6 +33,18 @@
     commandPush(buf);
 }
 
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+void uartRxIntHndler46Axis(){
+    //1Byte抜き出す
+    UCHAR buf = sp46Axis.getc();
+    //エコーバック
+    sp46Axis.putc(buf);
+    //スルーするためのバッファに突っ込む
+    PushBuf(buf);
+}
+
 //=============================================================
 //初期設定
 //=============================================================
@@ -46,4 +66,69 @@
     sp.printf("long  = %d\r\n" ,sizeof(unsigned long) );
     sp.printf("***********\r\n");
 
+    ////////////////////////////
+    // for 6Axis Sensor
+    //ボーレート設定
+    sp46Axis.baud(BAUD_RATE4_6AXIS);
+    
+    //受信割り込みハンドラ登録
+    sp46Axis.attach
+        (uartRxIntHndler46Axis    //登録するハンドラ
+        ,Serial::RxIrq           //割り込み要因
+        );
+        
+    sp46Axis.printf("***********\r\n");
+    sp46Axis.printf("UART open!!\r\n");    
+    sp46Axis.printf("-----------\r\n");
+    sp46Axis.printf("short = %d\r\n" ,sizeof(unsigned short) );
+    sp46Axis.printf("int   = %d\r\n" ,sizeof(unsigned int) );
+    sp46Axis.printf("long  = %d\r\n" ,sizeof(unsigned long) );
+    sp46Axis.printf("***********\r\n");
+    
+    // bufferのクリア
+    InitBuf();
+    ////////////////////////////
 }
+
+static std::vector<std::string> split(const std::string &str, char sep)
+{
+    std::vector<std::string> v;
+    std::stringstream ss(str);
+    std::string buffer;
+    while( std::getline(ss, buffer, sep) ) {
+        v.push_back(buffer);
+    }
+    return v;
+}
+
+void InitBuf()
+{
+    idxBufByG = 0;
+    memset(bufDataByGyro, 0x00, sizeof(bufDataByGyro));
+    accZ = gyroZ = yaw = 0.0f;
+}
+
+void PushBuf(UCHAR rxChar)
+{
+    bufDataByGyro[idxBufByG] = rxChar;
+    ++idxBufByG;
+    if(0x0A != rxChar){
+        return;
+    }
+    else
+    {
+        //std::size_t sz;
+        std::string str((char*)bufDataByGyro);
+        std::vector<std::string> list = split(str,0x09);
+        if(list.size() > 0 && list.size() < 4){
+            yaw = atof(list.at(0).c_str());
+            accZ = atof(list.at(1).c_str());
+            gyroZ = atof(list.at(2).c_str());
+            sp.printf("yaw = %f " ,yaw );
+            sp.printf("acc = %f" ,accZ );
+            sp.printf("gyro = %f\r\n" ,gyroZ );
+        }
+        idxBufByG = 0;
+        memset(bufDataByGyro, 0x00, sizeof(bufDataByGyro));
+    }
+}