센서 > 코드비젼 통신테스트 소스코드 (주기 변경)

TODAY243 TOTAL2,276,465
사이트 이용안내
Login▼/회원가입
최신글보기 질문게시판 기술자료 동영상강좌

아두이노 센서 ATMEGA128 PWM LED 초음파 AVR 블루투스 LCD UART 모터 적외선


BASIC4MCU | 센서 | 자이로센서 | 코드비젼 통신테스트 소스코드 (주기 변경)

페이지 정보

작성자 키트 작성일2017-08-21 16:29 조회1,310회 댓글0건

본문

#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz
위 선언은 함수 실행 주기(타이머인터럽트 주기)와 관계 됩니다.

 

 

 

 

 

 

 

void mpu6050_mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) {
 float norm;
 float halfvx, halfvy, halfvz;
 float halfex, halfey, halfez;
 float qa, qb, qc;

 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

  // Normalise accelerometer measurement
  norm = sqrt(ax * ax + ay * ay + az * az);
  ax /= norm;
  ay /= norm;
  az /= norm;

  // Estimated direction of gravity and vector perpendicular to magnetic flux
  halfvx = q1 * q3 - q0 * q2;
  halfvy = q0 * q1 + q2 * q3;
  halfvz = q0 * q0 - 0.5f + q3 * q3;

  // Error is sum of cross product between estimated and measured direction of gravity
  halfex = (ay * halfvz - az * halfvy);
  halfey = (az * halfvx - ax * halfvz);
  halfez = (ax * halfvy - ay * halfvx);

  // Compute and apply integral feedback if enabled
  if(mpu6050_mahonytwoKiDef > 0.0f) {
   integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq); // integral error scaled by Ki
   integralFBy += mpu6050_mahonytwoKiDef * halfey * (1.0f / mpu6050_mahonysampleFreq);
   integralFBz += mpu6050_mahonytwoKiDef * halfez * (1.0f / mpu6050_mahonysampleFreq);
   gx += integralFBx; // apply integral feedback
   gy += integralFBy;
   gz += integralFBz;
  } else {
   integralFBx = 0.0f; // prevent integral windup
   integralFBy = 0.0f;
   integralFBz = 0.0f;
  }

  // Apply proportional feedback
  gx += mpu6050_mahonytwoKpDef * halfex;
  gy += mpu6050_mahonytwoKpDef * halfey;
  gz += mpu6050_mahonytwoKpDef * halfez;
 }

 // Integrate rate of change of quaternion
 gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq));  // pre-multiply common factors
 gy *= (0.5f * (1.0f / mpu6050_mahonysampleFreq));
 gz *= (0.5f * (1.0f / mpu6050_mahonysampleFreq));
 qa = q0;
 qb = q1;
 qc = q2;
 q0 += (-qb * gx - qc * gy - q3 * gz);
 q1 += (qa * gx + qc * gz - q3 * gy);
 q2 += (qa * gy - qb * gz + q3 * gx);
 q3 += (qa * gz + qb * gy - qc * gx);

 // Normalise quaternion
 norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
 q0 /= norm;
 q1 /= norm;
 q2 /= norm;
 q3 /= norm;
}

 

원래 값이 61Hz (타이머 주기 16.384ms) 였으니 색칠한 코드의 상수값을 찾아서 수정 해줘야 합니다.

 

// Mahony update function (for 6DOF)
void mpu6050_mahonyUpdate(float gx,float gy,float gz,float ax,float ay,float az){
    float norm,halfvx,halfvy,halfvz,halfex,halfey,halfez,qa,qb,qc;
    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax==0.0f)&&(ay==0.0f)&&(az==0.0f))){
        // Normalise accelerometer measurement
        norm=sqrt(ax*ax+ay*ay+az*az);
        ax/=norm; ay/=norm; az/=norm;
        // Estimated direction of gravity and vector perpendicular to magnetic flux
        halfvx=q1*q3-q0*q2; halfvy=q0*q1+q2*q3; halfvz=q0*q0-0.5f+q3*q3;
        // Error is sum of cross product between estimated and measured direction of gravity
        halfex=(ay*halfvz-az*halfvy); halfey=(az*halfvx-ax*halfvz); halfez=(ax*halfvy-ay*halfvx);
        // Compute and apply integral feedback if enabled
        integralFBx+=halfex*0.0032786885245901639344262295081967; // integral error scaled by Ki //mpu6050_mahonysampleFreq=61.0f
        integralFBy+=halfey*0.0032786885245901639344262295081967;
        integralFBz+=halfez*0.0032786885245901639344262295081967;
        gx+=integralFBx; gy+=integralFBy; gz+=integralFBz; // apply integral feedback
        // Apply proportional feedback
        gx+=halfex; gy+=halfey; gz+=halfez; // mpu6050_mahonytwoKpDef=(2.0f*0.5f)
    }
    // Integrate rate of change of quaternion
    gx*=0.0081967213114754098360655737704918; // pre-multiply common factors  //mpu6050_mahonysampleFreq=61.0f
    gy*=0.0081967213114754098360655737704918;
    gz*=0.0081967213114754098360655737704918;
    qa=q0; qb=q1; qc=q2;
    q0+=(-qb*gx-qc*gy-q3*gz); q1+=(qa*gx+qc*gz-q3*gy); q2+=(qa*gy-qb*gz+q3*gx); q3+=(qa*gz+qb*gy-qc*gx);
    // Normalise quaternion
    norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
    q0/=norm; q1/=norm; q2/=norm; q3/=norm;
    //
    qw=q0; qx=q1; qy=q2; qz=q3; /* *get quaternion  */
    yaw=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1); // mpu6050_getRollPitchYaw
    pitch=-asin(2*q1*q3+2*q0*q2);
    roll=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1);
    flag=1;
}
//

integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq);

integralFBx+=halfex*0.0032786885245901639344262295081967;

 

gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq));

gx*=0.0081967213114754098360655737704918;

수정할 상수값이 두 곳입니다.

 

위 코드부터 보면

#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain
2.0f * 0.1f = 0.2 이므로 1/61에 곱하면 0.0032786885245901639344262295081967가 됩니다.

 

아래 코드는 0.5 에 1/61을 곱하면 0.0081967213114754098360655737704918가 됩니다.

//

타이머 주기를 20ms로 변경 한다면

61Hz는 50Hz가 되고

1/50=0.02

위 상수값은 0.02에 0.2를 곱하면 0.004

아래 상수값은 0.02에 0.5를 곱하면 0.01

 

동작시켜본 것은 아니니 참고하세요

댓글 0

조회수 1,310

등록된 댓글이 없습니다.

센서HOME > 센서 > 자이로센서 목록

게시물 검색

2022년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2021년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2020년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2019년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2018년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
Privacy Policy
MCU BASIC ⓒ 2020
모바일버전으로보기