Diff: PacketSerail.cpp

Revision:

0:56fce4868747

Child:

1:ee6809d3cee3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PacketSerail.cpp	Fri Jun 29 08:31:42 2018 +0000
@@ -0,0 +1,100 @@
+#include "PacketSerial.h"
+
+PacketSerial::PacketSerial(PinName tx, PinName rx, int32_t* registar) : Serial(tx, rx)
+{
+    _registar = registar;
+    Serial::baud(9600);
+    Serial::attach(this, &PacketSerial::readData, Serial::RxIrq);
+}
+
+void PacketSerial::writeData(int32_t data, uint8_t reg)
+{
+    uint8_t dataBytes[4] =
+    {
+        (data >> 24) & 0xFF,
+        (data >> 16) & 0xFF,
+        (data >>  8) & 0xFF,
+        (data >>  0) & 0xFF
+    };
+
+    Serial::putc(HEAD_BYTE);
+    Serial::putc(reg);
+    uint8_t checksum = 0;
+    for (uint8_t i = 0; i < 4; ++i)
+    {
+        if ((dataBytes[i] == ESCAPE_BYTE) || (dataBytes[i] == HEAD_BYTE))
+        {
+            Serial::putc(ESCAPE_BYTE);
+            checksum += ESCAPE_BYTE;
+            Serial::putc(dataBytes[i] ^ ESCAPE_MASK);
+            checksum += dataBytes[i] ^ ESCAPE_MASK;
+        }
+        else
+        {
+            Serial::putc(dataBytes[i]);
+            checksum += dataBytes[i];
+        }
+    }
+
+    // 末尾にチェックサムを追加で送信する
+    Serial::putc(checksum);
+}
+
+void PacketSerial::writeFloatData(float data, uint8_t reg)
+{
+  int integer = (int)data;
+  uint32_t decimal = (uint32_t)((data-integer)*1000000);
+  writeData(integer, reg);
+  writeData(decimal, reg+1);
+}
+
+float PacketSerial::getFloatData(uint8_t reg)
+{
+  return *(_registar + reg) + ((float)*(_registar + reg+1))/1000000;
+}
+
+void PacketSerial::readData()
+{
+    uint8_t bytes[4] = {0,0,0,0};
+    int8_t checksum = 0;
+    wait_us(500);
+    uint8_t data = Serial::getc();
+    wait_us(500);
+
+    if (data == HEAD_BYTE)
+    {
+        uint8_t reg = Serial::getc();
+        for (int i = 0; i < 4; ++i)
+        {
+            uint8_t d = Serial::getc();
+            wait_us(500);
+            if (d == ESCAPE_BYTE)
+            {
+                uint8_t nextByte = Serial::getc();
+                bytes[i] = nextByte ^ ESCAPE_MASK;
+                checksum += (d + nextByte);
+            }
+            else
+            {
+                bytes[i] = d;
+                checksum += d;
+            }
+        }
+        int8_t checksum_recv = Serial::getc();
+        wait_us(500);
+        int32_t DATA = 0x00;
+        for(int i = 0; i < 4; i++)
+        {
+            DATA |= (((int32_t)bytes[i]) << (24 - (i*8)));
+        }
+
+        if (checksum == checksum_recv)
+        {
+            *(_registar + reg) =  DATA;
+        }
+        else
+        {
+            // data error
+        }
+    }
+}
\ No newline at end of file