센서값 보정

하드웨어를 다시 만들었기 때문에 센서가 기체에 얼마만큼 기울어져 있는가를 다시 측정 해야했다.

그래서 테스트 해보았는데.. 센서값이 희안하게 조금씩 증가하기도 하고... 뭔가가 반응이 느린것 같았다.

그래서 센서가 기울어진 정도를 측정 했을때 값이 이상하게 나왔다.

test1

test2

test3

test4

roll이든 pitch든 실제 IMU의 측정값인 alpha값이 어느 정도 오차 범위에서 비슷 하게 나와야 하는데 1테스당 5번 시행해서 1개씩 너무 오차가 크게 달랐다.

그래서 오차가 적은 test2의 결과를 받아들여 초기 roll의 setpoint는 -0.60으로 하고 pitch의 setpoint는 -0.67로 하였다.

보정하는데 쓴 코드는 아래에 있다.

/////////////////IMU Sensor/////////////////
//IMU Sensor variable
typedef union {
    signed short value;
    unsigned char byte[2];
} S2BYTE;
 
S2BYTE data_word;
 
unsigned char SBuf[100];
int SCnt = 0;
double  DegRoll, DegPitch, DegYaw;
double roll_alpha1[5], roll_alpha2[5], roll_beta[5];
double pitch_alpha1[5], pitch_alpha2[5], pitch_beta[5];
double roll_mean, pitch_mean;
int count = 0, n=0;
////////////////////////////////////////////
 
int receieve_y_from_computer(void)
{
    if (Serial.available()){
        char c = Serial.read();
        if (c == 'y'){
            Serial.println("recieve 'y'.");
            return 1;
        }
    }
 
    return 0;
}
 
 
//센서값을 읽어서 roll,pitch,yaw 값을 알아낸다.
int recieve_data(void)
{
    static int step_ = 0, check_all_recieve = 0;
    unsigned char data1byte;
    int n;
 
    data1byte = Serial1.read();
 
    switch (step_)
    {
    case 0:
        if (data1byte == 0x55) { step_ = 1, check_all_recieve = 0; }
        break;
 
    case 1:
        if (data1byte == 0x55) { step_ = 2;  SCnt = 0; }
        else step_ = 0;
        break;
 
    case 2:
        SBuf[SCnt++] = (unsigned char)data1byte;
        if (SCnt == 8)  // roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 
        {
            step_ = 0;
            SCnt = 0;
            check_all_recieve = 1;
        }
        break;
 
    default:
        break;
    }
 
    return check_all_recieve;
}
 
 
static int trans_data_to_degree(unsigned char *sdata)  // sdata : roll(2)+pitch(2)+yaw(2)+chk(2)
{
    int n;
    unsigned short chk = 0;
 
    ///////////// checksum /////////////////
    chk = 0x55 + 0x55;
    for (n = 0; n<6; n++) chk += sdata[n];
    if (chk != get_data_word(&sdata[6]))
    {
        Serial.println("checksum error!");
        return 0; //   
    }
    ////////////////////////////////////////
 
    DegRoll = get_data_word(&sdata[0]) / 100.;
    DegPitch = get_data_word(&sdata[2]) / 100. * -1;
    DegYaw = get_data_word(&sdata[4]) / 100.;
 
    return 1;
}
 
static signed short get_data_word(unsigned char *dat)
{
    // little endian..
    data_word.byte[0] = dat[1];
    data_word.byte[1] = dat[0];
 
    return data_word.value;
}
 
 
 
void setup() {
 
    Serial.begin(115200);
    Serial1.begin(57600);
 
 
}
 
void loop() {
 
    if (Serial1.available()){
        if (recieve_data() == 1)  //roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 모든 데이터 받았음
        {
            if (trans_data_to_degree(SBuf) == 1) //센서값을 읽어서 roll,pitch,yaw 값을 알아낸다. 
            {
                if (count == 0){
 
                    if (receieve_y_from_computer() == 1)
                    {
                        Serial.print("n = "); Serial.println(n);
 
                        roll_alpha1[n] = DegRoll; 
                        pitch_alpha1[n] = DegPitch;
                        Serial.print("roll_alpha1=  "); Serial.print(roll_alpha1[n]); 
                        Serial.print(" , pitch_alpha1= "); Serial.println(pitch_alpha1[n]);
                        count++;
                    }
                }
 
                Serial.println(DegPitch);
 
            
                if (count == 1){
                    if (receieve_y_from_computer() == 1)
                    {
                        roll_alpha2[n] = DegRoll; 
                        pitch_alpha2[n] = DegPitch;
                        Serial.print("roll_alpha2=  "); Serial.print(roll_alpha2[n]); 
                        Serial.print(" , pitch_alpha2= "); Serial.println(pitch_alpha2[n]);
                        count++;
                    }
                }
 
                if (count == 2){
                    if (receieve_y_from_computer() == 1 && count == 2)
                    {
                        roll_beta[n] = (roll_alpha1[n] + roll_alpha2[n]) / 2;
                        pitch_beta[n] = (pitch_alpha1[n] + pitch_alpha2[n]) / 2;
 
                        Serial.print("roll_beta=  "); Serial.print(roll_beta[n]); 
                        Serial.print(" , pitch_beta= "); Serial.println(pitch_beta[n]);
                        Serial.println("");
                    
                        count = 0;
                        n++;
                    }
                }
 
 
 
            }
        }
    }
 
 
    if (n >= 5)
    {
        Serial.print("roll_alpha1 = ");
        for (int i = 0; i < 5; i++){
            Serial.print(roll_alpha1[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.print("roll_alpha2 = ");
        for (int i = 0; i < 5; i++){
            Serial.print(roll_alpha2[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.print("roll_beta = ");
        for (int i = 0; i < 5; i++){
            Serial.print(roll_beta[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.print("pitch_alpha1 = ");
        for (int i = 0; i < 5; i++){
            Serial.print(pitch_alpha1[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.print("pitch_alpha2 = ");
        for (int i = 0; i < 5; i++){
            Serial.print(pitch_alpha2[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.print("pitch_beta = ");
        for (int i = 0; i < 5; i++){
            Serial.print(pitch_beta[i]);
            Serial.print(" ,  ");
        }
        Serial.println("");
 
        Serial.println("");
        Serial.println("");
 
        for (int i = 0; i < 5; i++)
        {
            roll_mean += roll_beta[i];
            pitch_mean += pitch_beta[i];
        }
        roll_mean = roll_mean / 5.0;
        pitch_mean = pitch_mean / 5.0;
 
        Serial.print("roll_mean = "); Serial.println(roll_mean);
        Serial.print("pitch_mean = "); Serial.println(pitch_mean);
        n = 0;
    }
}

근데 밤새서 왜 센서가 이런가에대해서 열받고 씨름하고 있었는데 또 해보니까 잘 되었다...뭐지..??