Diff: r1307.cpp

Revision:

1:59efa65938c7

Parent:

0:1356771603cf

Child:

2:deb20a97e035

Child:

3:5655017c6806

--- a/r1307.cpp	Fri Sep 01 06:12:50 2017 +0000
+++ b/r1307.cpp	Sat Sep 02 19:30:23 2017 +0900
@@ -1,1 +1,42 @@
-#include "r1307.h"
\ No newline at end of file
+#include "r1307.h"
+
+
+R1307::R1307(PinName tx_,PinName rx_): serial(tx_,rx_)
+{
+  ticker.attach(this,&R1307::update,0.01);
+  serial.baud(115200);
+}
+
+
+void R1307::update()
+{
+  uint8_t index;
+  int16_t angle;
+  int16_t rate;
+  int16_t x_acc;
+  int16_t y_acc;
+  int16_t z_acc;
+  uint8_t check_sum;
+  //Verify packet heading information
+  while(serial.getc() != 0xAA){
+    if(!serial.readable())
+      return;
+  }
+  if(serial.getc() != 0x00)
+    return;
+   //Assemble data
+
+  index = serial.getc();
+  rate = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0xFF00);
+  angle = (serial.getc() & 0xFF) | ((serial.getc()<< 8) & 0XFF00);
+  x_acc = (serial.getc() & 0xFF) | ((serial.getc()<< 8) & 0xFF00);
+  y_acc = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0XFF00);
+  z_acc = (serial.getc() & 0xFF) | ((serial.getc() << 8) & 0xFF00);
+  //Scale and store data
+  gRate = rate / 100.0;
+  gAngle = angle / 100.0;
+  gX_acc = x_acc;
+  gY_acc = y_acc;
+  gZ_acc = z_acc;
+  return;
+}