변경 사항
mpu6050 상보필터 코드를 main.c에서 MPU6050.c파일로 옮기고 약간의 수정을 했다.
작동은 동일하게 작동하며 코드를 정리하기 위해 나눴다.
I2C.c 코드는 기존
https://ddtxrx.tistory.com/15?category=913763
[STM32] CubeMX LL 드라이버 - I2C (3)
드론을 만들 때 mpu9250, mpu6050과 같은 자이로 센서와 지자기 센서, 기압 센서와 통신하기 위해서 I2C 통신을 사용해야 한다. HAL드라이버로 쿼드 콥터를 만들때는 주로 HAL_I2C_Master_Transmit(), HAL_I2C_Mas.
ddtxrx.tistory.com
글의 코드와 다른 점은 없다.
-
처음 쿼드콥터를 만들 때 중요하게 생각했던 부분 중 한 가지가 PID gain값이 상수값으로 고정되어 값을 변경하려 하면 코드를 그때마다 빌드해서 MCU에 프로그램해야 하는 방식이 아닌 flash에 값을 저장하고 컴퓨터와 통신을 통해 빌드를 하지 않아도 값을 원할 때 바꾸고 저장해야 하는 것이었다.
그래서 UART 통신을 사용하여 터미널 프로그램에서 특정한 데이터를 보내 알맞은 값이 들어오면 값을 변경하는 코드를 작성했다.
UART통신 코드는 interrupt만 사용했으며, DMA는 사용하지 않아 매우 간단하다.
하지만 받은 데이터가 알맞은 데이터인지를 판단하는 코드가 어렵진 않지만 길어서 작성하는데 시간이 좀 걸렸다.
받은 데이터를 SPI 통신을 통해 flash에 저장하는 코드 또한 있어
https://ddtxrx.tistory.com/16?category=913763
[STM32] CubeMX LL 드라이버 - SPI (4)
쿼드콥터를 만들 때 많은 센서들은 SPI가 아닌 I2C를 사용했다. 그렇기 때문에 많이 사용하지 않은 통신이다. 하지만 PID 값을 저장하기 위한 FLASH Memory제어, nrf24l01 통신 모듈을 사용하기 위해서 SPI
ddtxrx.tistory.com
글을 참조하면 좋을 것이다.
코드 생성 - (0)
기존에 printf의 출력으로 사용하던 USART1에 RXNE interrupt와 IDLE interrupt를 사용하기 위한 NVIC 설정만 바꿔주면 된다.
또한 이번에는 flash를 사용할 것이므로 SPI1 통신을 활성화 할 것이다. SPI 세팅은 위에 써둔 주소의 세팅값과 똑같다.
코드 - main.c (1)
|
1
2
3
4
5
6
|
typedef struct{
USART_TypeDef* UART;
uint8_t command_buf[30];
uint16_t command_length;
uint8_t command_receive;
}UART_COMMAND;
|
util.h에 정의되어 있는 UART_COMMAND
|
1
2
3
4
5
6
7
|
/* USER CODE BEGIN 2 */
//----------------------------------------------------------------------------------- usart1 command
init_COMMAND(&command,USART1);
//-----------------------------------------------------------------------------------
/* USER CODE END 2 */
|
util.h에 선언되어 있는 UART_COMMAND 의 command 구조체를 초기화하고 RXNE, IDLE interrupt를 활성화한다.
UART_COMMAND 구조체는 어떤 USART 장치를 사용하고 받은 데이터 버퍼, 받은 데이터의 길이와 데이터를 받았는지를 확인하는 값들로 이루어져 있다.
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
//========================================================================================================================= command
if(command.command_receive==1){
//printf("echo:%s\r\n",command.command_buf);
COMMAND_Decode(&command);
command.command_receive=0;
}
//=========================================================================================================================
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
|
IDLE interrupt가 발생하여 데이터를 전부 받았다면 commad_receive가 1이 되어 COMMAND_Decode함수를 실행하여 받은 데이터가 어떤 데이터인지 확인한다.
실제 확인하기 전 COMMAND_Decode함수는 주석 처리하고 데이더가 제대로 들어왔는지 echo로 확인해보는 것이 좋다.
코드 - stm32f4xx_it.c (2)
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
|
void USART1_IRQHandler(void)
{
/* USER CODE BEGIN USART1_IRQn 0 */
if(LL_USART_IsActiveFlag_RXNE(command.UART)){
LL_USART_ClearFlag_RXNE(command.UART);
command.command_buf[command.command_length]=LL_USART_ReceiveData8(command.UART);
command.command_length++;
}
if(LL_USART_IsActiveFlag_IDLE(command.UART)){
LL_USART_ClearFlag_IDLE(command.UART);
command.command_buf[command.command_length]=0x00;
command.command_length=0;
command.command_receive=1;
}
/* USER CODE END USART1_IRQn 0 */
/* USER CODE BEGIN USART1_IRQn 1 */
/* USER CODE END USART1_IRQn 1 */
}
|
RXNE interrupt가 발생했을 경우 버퍼에 데이터를 저장하고 버퍼의 길이를 나타내는 변수의 크기를 1씩 늘린다.
IDLE interrupt가 발생하면 버퍼의 마지막 위치에 0x00 즉 NULL문자를 넣어 문자열의 끝을 알리고 길이의 값을 0으로 초기화한 후 command_receive의 값을 1로 변경해 데이터가 들어온 것을 나타낸다.
코드 - util.c (3)
util.h와 연관된 헤더 파일들은 PID.h, init.h이 있고 PID.h파일은 PID 제어와 관련이 있는 파일이다. 이 글에서는 PID gain 구조체에 대해서만 나올 것이며, init.h파일은 공통적으로 사용되는 값들을 정의해 놓을 것이다. 현재 글에서 사용하지 않는다.
util.c의 파일은 어렵지는 않지만 양이 많아 코드의 직접적인 분석은 하지 않고 필요한 부분만 분석할 것이며 연관되어있는 PID.h파일에 관해 먼저 설명 후 util.c 분석을 할 것이다.
|
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
|
#define pid_para_size 13
#define bound_pitch_stabilize_p 70
#define bound_pitch_stabilize_i 30
#define bound_pitch_rate_p 70
#define bound_pitch_rate_i 20
#define bound_pitch_rate_d 100
#define bound_roll_stabilize_p 70
#define bound_roll_stabilize_i 30
#define bound_roll_rate_p 70
#define bound_roll_rate_i 20
#define bound_roll_rate_d 100
#define bound_yaw_p 70
#define bound_yaw_i 20
#define bound_yaw_d 1500
#define stabilize_p stabilize_pid_para[0]
#define stabilize_i stabilize_pid_para[1]
#define rate_p rate_pid.rate_pid_para[0]
#define rate_i rate_pid.rate_pid_para[1]
#define rate_d rate_pid.rate_pid_para[2]
#define para_p rate_pid_para[0]
#define para_i rate_pid_para[1]
#define para_d rate_pid_para[2]
typedef struct{
float rate_pid_para[3];
float rate_i_mem;
float pre_rate;
float pid_result;
}RATE_PID;
typedef struct{
RATE_PID rate_pid;
float stabilize_pid_para[2];
float stabilize_i_mem;
}STABILIZE_PID;
STABILIZE_PID pitch_para,roll_para;
RATE_PID yaw_para;
|
쿼드콥터를 제작할 때 일반적인 PID 제어를 적용해 봤지만 안정적인 비행이 불가능했다.
따라서 다른 제어 방식을 사용했고 그때 사용되는 파라미터 값들이 4개였다.
하지만 이번 헥사콥터를 만들 때는 1개의 값을 추가로 사용해서 5개의 파라미터를 사용하는 PID 코드를 만들 생각이다.
PID 제어에 관한 자세한 내용은 다른 글에서 다룰 생각이다.
Pitch, Roll에 관한 제어에 각각 5개의 변수가 필요하며, Yaw의 경우 일반적인 PID 제어로 3개의 파라미터 값이 필요하다. 따라서 필요한 파라미터 개수는 13개이므로 사이즈를 정의했다.
PID 값 데이터를 전송할 때 값을 실수로 크게 넣는 실수를 할 수 있으므로 바운드 설정을 했다. 실제로 쿼드콥터를 만들 때 값 설정을 잘못하여 손을 다친 적이 있다.
파라미터들은 배열에 저장된다. 하지만 배열의 위치에 따른 값이 어떤 값인지 헷갈릴 수 있으므로 매크로를 사용하여 쉽게 이해할 수 있도록 정의했다.
RATE_PID는 일반 PID 제어에 사용할 파라미터를 저장하는 구조체이다. 이외에 PID 제어의 결과값을 저장하며 I-제어를 위한 저장 변수와 D-제어를 위한 이전 에러 저장을 위한 변수 또한
STABILIZE_PID는 Pitch, Roll 제어에 사용할 파라미터를 저장하는 구조체이며, 내부에 RATE_PID 구조체가 선언되어 있다.
실제로 사용될 때에는 변수 이름들은 언제든지 바뀔 수 있다.
|
1
2
3
4
5
6
7
8
9
|
void save_pid_para();
void load_pid_para();
void show_pid_para();
void print_pid_para();
void COMMAND_Decode(UART_COMMAND* command);
|
save_pid_para()는 PID gain을 flash에 저장하는 함수이다. 저장하기 위해서 4byte float을 4개의 uint8_t 변수로 나눠주고 flash에 저장한다.
load_pid_para() 함수는 PID gain을 flash에서 읽어 float 변수들에 저장한다.
show_pid_para()를 통해 flash에 저장되어 있는 데이터를 확인할 수 있다.
print_pid_para()의 경우 Pitch, Roll, Yaw 구조체에 저장되어있는 데이터를 출력한다.
COMMAND_Decode()를 통해 PID, GPS, barometer 데이터를 받고 데이터를 변경할 수 있다.
PID 데이터를 변경하고자 할 때 처음 1byte는 h로 시작하며, 2번째 byte는 pitch일 경우 p, roll일 경우 r, yaw일 경우 y를 전송한다.
3번째 byte부터 Pitch, Roll 일 경우와 yaw의 경우 가 달라지는데, Pitch, Roll의 경우 stabilize, rate 파라미터에 따라 s, r의 데이터를 보내고 s의 경우 4번째 byte는 p, i이며 r의 경우 4번째 byte는 p, i, d의 값을 가진다.
앞의 데이터 뒤에는 실수 값을 문자열로 보내 데이터를 해당 파라미터 값을 바꾼다.
기타 명령에는 hA, hL, hS, hC로 순서대로 파라미터 값을 출력, 파라미터를 flash에서 가져옴, 현재 파라미터를 저장, flash 파라미터를 체크하는 명령이 있다.
실제로 hA를 보내면 PID 파라미터를 출력한다.
-
쿼드콥터를 만들 때 중요했던 기능인 컴퓨터에서 데이터를 전송하여 PID gain을 바꾸는 기능을 만들었다.
데이터 변경을 위한 전용 프로그램을 만들지는 못하지만 터미널 프로그램을 사용하여 데이터를 변경할 수 있다.
flash 같은 메모리가 있어야 데이터를 저장할 수 있어 flash가 없을 경우 사용할 수 없다.
PID와 지자기센서를 사용하여 Yaw제어에 대한 코드만 작성하면 기본적인 비행을 할 수 있을 것이다.
'드론' 카테고리의 다른 글
| [헥사콥터] F550 헥사콥터 제작 - HMC5883 지자기 센서 (5) (2) | 2020.07.26 |
|---|---|
| [헥사콥터] F550 헥사콥터 제작 - PID (4) (0) | 2020.07.25 |
| [헥사콥터] F550 헥사콥터 제작 - PWM (2) (2) | 2020.07.20 |
| [헥사콥터] F550 헥사콥터 제작 - Receiver (1) (0) | 2020.07.20 |
| [헥사콥터] F550 헥사콥터 제작 - 부품 (0) (2) | 2020.07.13 |
