가속도 센서로 pitch, roll 알아내기
생각을 정리하기 위해서 기록으로 남긴다..
처음에 3축 가속도 센서가 있으면 pitch, roll, yaw를 알 수 있다고 생각했다. 그러나 이는 불가능하다..
아래 문서를 읽어보니 pitch와 roll 2개의 값만 알 수 있다. 나머지 yaw는 compass 센서로 확인이 가능하다.
AN3461
위 문서를 읽다 보면, 아래와 같은 식이 어떻게 나왔는지 파악하는데 좀 걸렸다.
x축으로 돌렸을 경우(roll), y,z 평면으로 보면 위와 같은 그림이 된다. 나머지는 안해봤는데, 아마도 이런 식으로 하면 되지 않을까 한다. roll, pitch, yaw 적용하는 순서에 따라 6개의 방정식이 나오는데, 4개는 못쓰는 식이다. 경우의 수가 너무 많아서 그런 듯 싶다. 나머지 2개는 사용할 수 있는데, 두 개의 식이 다른 결과를 갖는다.
tan(pi)는 cos(theta)sin(pi)/cos(theta)cos(pi)로 간단하게 구할 수 있는데, tan(theta)는 3개의 식을 G_px, G_py, G_pz에 대해서 풀어써야 된다.
암튼 내가 가진 gyro+accelemeter는 배치가 아래와 같이 되어있다.
x에서 y로 갈 경우, z가 나와야 되는데, 들어가니 left handed coordinate system인듯 한데..
X표시가 들어가는 방향. ●가 나오는 방향이다.
left handed, right handed coordinate
배치된 그림의 z축을 흔들리게 설치를 했다. z축이 회전이라 yaw를 알아야 한다고 처음에 판단 했으나, z축을 x축으로 변환을 하고, roll을 구하는 방법으로 해야 겠다. 난 한개의 값만 필요하니..
몇번의 삽질로..아래 식을 참조하여…대략 구할 수 있었다.
accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (-accelerationY/accelerationZ) / M_PI;
yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;
위 식이 틀린 부분도 있어 보인다.
MMA7455L 가속도 센서를 사용하는데, 첨부로 data sheet를 넣어둔다..
MMA7455L
여기서 더 나가면, 센서 드라이버를 어떻게 구현했는지도 찾아봐야 할 것도 같다..