Diff: datatransfer.cpp

Revision:

73:be7a8b8188de

Child:

75:7fa924da9d32

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/datatransfer.cpp	Wed Aug 25 07:01:12 2021 +0000
@@ -0,0 +1,63 @@
+#include "global.hpp"
+
+void send2center()
+{
+    valuePack& vp = posValues;
+    vp.day = updateValues.day;
+    vp.ms = updateValues.ms;
+    ekf.getGyroBias(vp.gyroBias, vp.gyroBiasCov);
+    //ekf.getAccBias(vp.accBias, vp.accBiasCov);
+    vp.acc[0] = acc.x;
+    vp.acc[1] = acc.y;
+    vp.acc[2] = acc.z;
+    vp.gyro[0] = gyro.x;
+    vp.gyro[1] = gyro.y;
+    vp.gyro[2] = gyro.z;
+    vp.mag[0] = mag.x;
+    vp.mag[1] = mag.y;
+    vp.mag[2] = mag.z;
+    vp.rpy[0] = rpy.x;
+    vp.rpy[1] = rpy.y;
+    vp.rpy[2] = rpy.z;
+    vp.de = de;
+    vp.deobj = deobj;
+    for(int i = 0 ; i<5 ; i++){
+        vp.rc[i] = rc[i];
+    }
+    tail.Send(tail_address[pos_tail], &(posValues));
+    ekf.updateCenteredGyroBiasCorrection(updateValues.gyroBias);
+    //ekf.updateCenteredAccBiasCorrection(updateValues.accBias);
+    pc.printf("%f %f\n\r",rpy.x*180.0f/M_PI,updateValues.gyroBias[0]);
+}
+
+// eeprom書き込み・読み込みに必要な関数
+void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[size + 2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    for (int i = 0; i < size; i++) {
+        i2cBuffer[i + 2] = data[i];
+    }
+ 
+    int result = i2c.write(address, i2cBuffer, size + 2, false);
+    //sleep_ms(500);
+}
+
+// this function has no read limit
+void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    // Reset eeprom pointer address
+    int result = i2c.write(address, i2cBuffer, 2, false);
+    
+    //sleep_ms(500);
+ 
+    // Read eeprom
+    i2c.read(address, data, size);
+    //sleep_ms(500);
+}