File content as of revision 86:6b8e797306b6:

#include "global.hpp"

void send2center()
{
    valuePack& vp = posValues;
    eskf.getGyroBias();
    //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]);
    pc.printf("%f %f \r\n",rpy.x/M_PI*180.0f,rpy.y/M_PI*180.0f);
}

// 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);
}