드론을 제어를 하기 위해 Pitch, Roll은 mpu6050 자이로 센서를 사용하여 계산하지만 Yaw제어는 자이로 센서로도 가능은 하지만 완벽한 제어가 힘들다. 따라서 주로 지자기 센서를 사용하여 Yaw를 제어한다.
지자기 센서는 GPS 모듈에 함께 달려있는 hmc5883 ic를 사용한다.
모듈의 방향과 mpu6050의 방향이 일치해야 제대로 된 계산이 가능하다.
모듈의 빨간선은 VCC, 검은선은 GND, 초록선은 GPS RX, 노란선은 GPS TX, 하얀선은 SDA, 주황선은 SCL이다.
코드 생성 - (0)
hmc5883 ic는 I2C를 사용하여 통신한다. 1개의 I2C 통신에 여러 개의 칩들이 연결될 수 있지만 mpu6050만 따로 구분해 두기 위해 hmc5883은 다른 I2C를 사용한다. 또한 헥사콥터에서 회로가 들어갈 위치와 보드의 핀 위치를 고려하여 mpu6050의 경우 I2C2로 변경되고 hmc5883은 I2C3을 사용한다.
나중에 사용할 기압 센서 ms5611 ic 또한 I2C3에 연결할 생각이다.
HMC5883 - (1)
hmc5883의 레지스터 맵이다.
0x00, 0x01, 0x02 레지스터는 hmc5883의 기본적인 설정을 하는 데 사용하는 레지스터들이며, 0x03부터 0x08까지 지자기 데이터를 읽을 수 있다.
전원을 처음 인가하면 0x00, 0x01, 0x02 레지스터에 값을 써 초기화를 진행한 후 데이터를 읽어 들인다.
맨 처음 사용할 경우 0x0a, 0x0b, 0x0c 레지스터들을 읽어 제대로 된 id값이 나오는지 확인하여 I2C 통신이 제대로 작동하는지 확인을 먼저 해보는 게 좋다.
ic를 사용하기 전 데이터시트를 읽고 난 후 코드를 봤으면 좋겠다.
코드 - HMC5883.h (2)
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
|
#define HMC5883_ADDRESS 0b00011110<<1
#define mag_para_size 6
#define offset_x mag_para_arr[0]
#define offset_y mag_para_arr[1]
#define offset_z mag_para_arr[2]
#define scale_x mag_para_arr[3]
#define scale_y mag_para_arr[4]
#define scale_z mag_para_arr[5]
typedef struct{
I2C_struct i2c;
uint8_t magneto_address;
float mx, my, mz;
float mag_para_arr[mag_para_size];
}HMC5883;
HMC5883 hmc5883;
|
hmc5883의 i2c address와 hmc5883과 사용할 HMC5883 구조체가 정의되어있다.
mx, my, mz의 값은 처음 값을 받을 때 사용되고 계산하면서 칼리브레이션 된 값도 저장한다.
mag_para_arr의 경우 지자기 칼리브레이션에 사용할 변수들을 저장하고 있다.
코드 - HMC5883.c (3)
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
|
void init_HMC(HMC5883* hmc5883,I2C_TypeDef* i2c){
hmc5883->i2c.I2C=i2c;
hmc5883->i2c.i2c_IsRx=0;
hmc5883->i2c.i2c_busy=0;
hmc5883->magneto_address=HMC5883_ADDRESS;
Mag_Writebyte(hmc5883,0x00,0b00011000);
Mag_Writebyte(hmc5883,0x01,0b00100000);
Mag_Writebyte(hmc5883,0x02,0x00);
}
void HMC_Mag_Read(HMC5883* hmc5883){
uint8_t Receive[6];
uint8_t Trans[1];
int16_t output[3];
Trans[0]=0x03;
I2C_Transmit(&hmc5883->i2c,hmc5883->magneto_address,Trans,1);
I2C_Receive(&hmc5883->i2c,hmc5883->magneto_address,Receive,6);
output[0]=(int16_t)((Receive[0]<<8) | Receive[1]);
output[1]=(int16_t)((Receive[2]<<8) | Receive[3]);
output[2]=(int16_t)((Receive[4]<<8) | Receive[5]);
hmc5883->mx=(float)output[0];
hmc5883->my=(float)output[2];
hmc5883->mz=(float)output[1];
}
|
중요하다고 생각하는 2개의 함수에 대해 설명하겠다.
init_HMC() 는 HMC5883 구조체 변수를 초기화하고 hmc5883 ic를 초기화한다.
0x00 레지스터에 0x18, 0x01 레지스터에 0x20, 0x03 레지스터에 0x00의 값을 넣어 ic를 초기화한다.
HMC_Mag_Read() 는 데이터를 읽는 데 사용한다.
0x03 레지스터부터 0x08 레지스터까지의 레지스터가 지자기 센서가 측정한 값들이 들어가게 되는데 이를 읽어 mx, my, mz에 값을 저장한다.
코드 - main.c (4)
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
|
uint16_t mag_counter=0;
uint8_t first_data=1;
float mag_max[3];
float mag_min[3];
float avg_delta[4];
hmc5883.offset_x=70.31;
hmc5883.offset_y=36.49;
hmc5883.offset_z=16.47;
hmc5883.scale_x=0.99;
hmc5883.scale_y=1.0;
hmc5883.scale_z=1.01;
//========================================================================================================================= mag, baro
else if(condition_mag_baro){
if(mag_baro==0){
float radpitch, radroll;
float xh,yh;
HMC_Mag_Read(&hmc5883);
hmc5883.mx=(hmc5883.mx-hmc5883.offset_x)*hmc5883.scale_x;
hmc5883.my=(hmc5883.my-hmc5883.offset_y)*hmc5883.scale_y;
hmc5883.mz=(hmc5883.mz-hmc5883.offset_z)*hmc5883.scale_z;
radroll=-status_data.pitch*0.01745329252;
radpitch=-status_data.roll*0.01745329252;
xh=hmc5883.mx*cosf(radpitch) + hmc5883.my*sinf(radroll)*sinf(radpitch) + hmc5883.mz*sinf(radpitch)*cosf(radroll);
yh=hmc5883.my*cosf(radroll) + hmc5883.mz*sinf(radroll);
status_data.yaw=atan2f(-yh,-xh)*(57.29577951);
mag_counter++;
if(mag_counter>50){
printf("%.2f\n\n\r",status_data.yaw);
//printf("%.2f\t%.2f\t%.2f\r\n",hmc5883.mx,hmc5883.my,hmc5883.mz);
//printf("%.2f\t%.2f\t%.2f\r\n",hmc5883.offset_x,hmc5883.offset_y,hmc5883.offset_z);
//printf("%.2f\t%.2f\t%.2f\n\n\r",hmc5883.scale_x,hmc5883.scale_y,hmc5883.scale_z);
mag_counter=0;
}
/*if(first_data){
mag_max[0]=mag_min[0]=hmc5883.mx;
mag_max[1]=mag_min[1]=hmc5883.my;
mag_max[2]=mag_min[2]=hmc5883.mz;
first_data=0;
}
else{
if(mag_max[0]<hmc5883.mx){
mag_max[0]=hmc5883.mx;
}
else if(mag_min[0]>hmc5883.mx){
mag_min[0]=hmc5883.mx;
}
if(mag_max[1]<hmc5883.my){
mag_max[1]=hmc5883.my;
}
else if(mag_min[1]>hmc5883.my){
mag_min[1]=hmc5883.my;
}
if(mag_max[2]<hmc5883.mz){
mag_max[2]=hmc5883.mz;
}
else if(mag_min[2]>hmc5883.mz){
mag_min[2]=hmc5883.mz;
}
hmc5883.offset_x=(mag_max[0]+mag_min[0])/2;
hmc5883.offset_y=(mag_max[1]+mag_min[1])/2;
hmc5883.offset_z=(mag_max[2]+mag_min[2])/2;
avg_delta[0]=(mag_max[0]-mag_min[0])/2;
avg_delta[1]=(mag_max[1]-mag_min[1])/2;
avg_delta[2]=(mag_max[2]-mag_min[2])/2;
avg_delta[3]=(avg_delta[0]+avg_delta[1]+avg_delta[2])/3;
hmc5883.scale_x=avg_delta[3]/avg_delta[0];
hmc5883.scale_y=avg_delta[3]/avg_delta[1];
hmc5883.scale_z=avg_delta[3]/avg_delta[2];
}*/
mag_baro=1;
}
else{
mag_baro=0;
}
condition_mag_baro=0;
}
|
while 문에 들어가는 Yaw 각도를 계산하는 코드와 지자기 캘리브레이션을 하는 코드, 변수들을 보여준다.
캘리브레이션을 하고 나온 결과들과 지자기 센서가 기울어졌을 때 계산하는 코드가 있다.
긴 주석으로 처리되어있는 코드가 지자기 캘리브레이션을 하는 코드인데
appelsiini.net/2018/calibrate-magnetometer/
How to Calibrate a Magnetometer?
How to Calibrate a Magnetometer? Fri, Feb 23, 2018 Magnetometers are used to measure the strength of a magnetic field. They can also be used to determine orientation and to compensate gyro drift. Magnetometer provides the last three degrees of freedom in 9
appelsiini.net
에 자세한 이야기가 나와있다.
지자기 센서에서 칼리브레이션뿐만 아니라 지자기 센서가 기울어져있을 경우에 제대로 된 값을 얻기 위해서 pitch각도와 roll각도를 사용하여 추가적인 계산을 해줘야 한다.
pitch, roll에 따라서 값을 계산해 줘야 하므로 자이로 센서의 방향과 지자기 센서의 방향이 다를 경우 원하는 값을 얻지 못한다.
자세한 내용과 식은 magnetomter tilt compentation이라고 구글에 검색하면 정보를 찾을 수 있다.
기압 센서의 계산도 나중에 추가할 생각이어서 빈 else문을 추가해놨다.
결과 - (5)
실제 작동시켜보면 모듈이 바라보고 있는 방향을 나타내며 기울어져도 알맞은 값을 출력한다. 물론 mpu6050과 같은 각도로 기울어져야 한다.
-
[쿼드콥터] STM32 HAL 드론
올해 1월초부터 만들기 시작해 며칠전에 테스트 비행을 성공했다. 한달동안 공부하며 만든것 치고 괜찮게 날았다. 날씨가 아직 풀리지 않아 옥상에서만 날렸고 드론을 날리는게 처음이라 조종��
ddtxrx.tistory.com
처음 드론을 만들었을 때 Yaw 제어를 지자기 센서가 아닌 mpu6050의 각속도를 가지고 제어를 했었다.
어느 정도 비행은 가능하지만 진동에 의해 각속도가 완전한 0이 아닌 값이 나와 조금씩 회전하는 모습을 보였다.
https://ddtxrx.tistory.com/4?category=873972
[쿼드콥터] STM32 지자기 센서 추가
최근 대학동기 졸업과제 도와주면서 신경을 못쓰다 얼마전부터 다시 제작을 하고있다. IMU센서를 기존 가속도, 자이로 6축 센서인 mpu6050에서 ak8963 지자기 센서가 추가된 mpu9250센서로 바꿨다. YAW��
ddtxrx.tistory.com
mpu9250 내부의 지자기 센서를 달았을 때의 모습이다. Yaw 제어가 되고 있는 모습을 보여준다.
mpu9250에 탑재되어 있는 지자기 센서는 hmc5883과 다르지만 코드의 형태는 비슷하다.
[쿼드콥터] STM32 간단한 GPS제어
최근에 다시 드론을 만들기 시작해서 GPS를 추가했다. 아직 완벽한 호버링은 아니지만 어느 정도 위치를 유지하는 모습을 보여준다. 고도제어를 아직 만들지 않아 쓰로틀만 직접 만지고 한 손으�
ddtxrx.tistory.com
GPS의 사용과 지자기 센서와 모터, 회로들을 분리하기 위해 모듈에 장착되어 있는 hmc5883으로 변경한 후 비행 영상이다.
hmc5883의 내용과 코드가 너무 길어 센서의 자세한 코드들은 직접 파일로 확인하고 모르는 부분이 있으면 답글을 남겨주면 좋겠다.
모든 코드는 LL드라이버를 사용한다.
'드론' 카테고리의 다른 글
| [헥사콥터] F550 헥사콥터 제작 - 추락 (7) (0) | 2020.07.31 |
|---|---|
| [헥사콥터] F550 헥사콥터 제작 - 조립 (6) (0) | 2020.07.30 |
| [헥사콥터] F550 헥사콥터 제작 - PID (4) (0) | 2020.07.25 |
| [헥사콥터] F550 헥사콥터 제작 - UART PID gain (3) (0) | 2020.07.24 |
| [헥사콥터] F550 헥사콥터 제작 - PWM (2) (2) | 2020.07.20 |
