File content as of revision 106:36458fb9b5b7:

#include "global.hpp"

void send2center()
{
    valuePack& vp = posValues;
    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.altitude = -palt;
    vp.de = de;
    vp.deobj = deobj;
    Matrix pihat = eskf.getPihat();
    vp.pi[0] = pihat(1, 1);
    vp.pi[1] = pihat(2, 1);
    vp.pi[2] = pihat(3, 1);
    Matrix vihat = eskf.getVihat();
    vp.vi[0] = vihat(1, 1);
    vp.vi[1] = vihat(2, 1);
    vp.vi[2] = vihat(3, 1);
    if (prefligt_finished)
    {
        vp.readyFlag = 1;
    }
    else
    {
        vp.readyFlag = 0;
    }
    if(updateValues.calibrationFlag == 1111){
       vp.calibEndFlag = 2;
    }else{ 
        vp.calibEndFlag = 0;
    }
    for(int i = 0 ; i<5 ; i++){
        vp.rc[i] = rc[i];
    }
    vp.att_dt = att_dt;
    
    tail.Send(tail_address[pos_tail], &(posValues));
    pc.printf("%f %f %f %d %d %f %f %f \r\n",rpy.x/M_PI*180.0f,rpy.y/M_PI*180.0f,rpy.z/M_PI*180.0f,vp.readyFlag,updateValues.calibrationFlag,updateValues.vx_opt,updateValues.vy_opt,updateValues.dist_ir);
}

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