├── .vscode └── settings.json ├── README.md ├── src ├── ioss │ ├── barometer_sensor.cpp │ ├── barometer_sensor.h │ ├── gps_sensor.cpp │ ├── gps_sensor.h │ ├── imu_sensor.cpp │ ├── imu_sensor.h │ ├── rc_input.cpp │ ├── rc_input.h │ └── sbus_reader.cpp ├── oss │ ├── os_api.cpp │ ├── thread_manager.cpp │ └── timer.cpp ├── psss │ ├── attitude_controller.cpp │ ├── attitude_controller.h │ ├── ekf.cpp │ ├── ekf.h │ ├── flight_control.cpp │ ├── flight_control.h │ ├── flight_mode.cpp │ ├── flight_mode.h │ ├── main.cpp │ ├── motor_control.cpp │ ├── motor_control.h │ ├── pose_estimator.cpp │ └── pose_estimator.h └── tss │ ├── udp_communication.cpp │ └── udp_communication.h ├── test_code ├── amc.cpp ├── attitude_controller.cpp ├── attitude_controller.h ├── barometer_sensor.cpp ├── barometer_sensor.h ├── ekf.cpp ├── ekf.h ├── flight_control.cpp ├── flight_control.h ├── flight_mode.cpp ├── flight_mode.h ├── gps_sensor.cpp ├── gps_sensor.h ├── imu_calibration.cpp ├── imu_calibration.h ├── imu_sensor.cpp ├── imu_sensor.h ├── main.cpp ├── os_api.cpp ├── pid_controller.cpp ├── pid_controller.h ├── pose_estimator.cpp ├── pose_estimator.h ├── rc_input.cpp ├── rc_input.h ├── sbus_reader.cpp ├── test.cpp ├── test2.cpp ├── test4.cpp ├── test5.cpp ├── test7.cpp ├── thread_manager.cpp ├── timer.cpp ├── udp_communication.cpp └── udp_communication.h └── 폴더 설명.txt /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "array": "cpp", 4 | "atomic": "cpp", 5 | "bit": "cpp", 6 | "*.tcc": "cpp", 7 | "cctype": "cpp", 8 | "charconv": "cpp", 9 | "chrono": "cpp", 10 | "clocale": "cpp", 11 | "cmath": "cpp", 12 | "compare": "cpp", 13 | "complex": "cpp", 14 | "concepts": "cpp", 15 | "condition_variable": "cpp", 16 | "csignal": "cpp", 17 | "cstdarg": "cpp", 18 | "cstddef": "cpp", 19 | "cstdint": "cpp", 20 | "cstdio": "cpp", 21 | "cstdlib": "cpp", 22 | "cstring": "cpp", 23 | "ctime": "cpp", 24 | "cwchar": "cpp", 25 | "cwctype": "cpp", 26 | "deque": "cpp", 27 | "map": "cpp", 28 | "set": "cpp", 29 | "string": "cpp", 30 | "unordered_map": "cpp", 31 | "vector": "cpp", 32 | "exception": "cpp", 33 | "algorithm": "cpp", 34 | "functional": "cpp", 35 | "iterator": "cpp", 36 | "memory": "cpp", 37 | "memory_resource": "cpp", 38 | "numeric": "cpp", 39 | "optional": "cpp", 40 | "random": "cpp", 41 | "ratio": "cpp", 42 | "string_view": "cpp", 43 | "system_error": "cpp", 44 | "tuple": "cpp", 45 | "type_traits": "cpp", 46 | "utility": "cpp", 47 | "format": "cpp", 48 | "fstream": "cpp", 49 | "initializer_list": "cpp", 50 | "iomanip": "cpp", 51 | "iosfwd": "cpp", 52 | "iostream": "cpp", 53 | "istream": "cpp", 54 | "limits": "cpp", 55 | "mutex": "cpp", 56 | "new": "cpp", 57 | "numbers": "cpp", 58 | "ostream": "cpp", 59 | "semaphore": "cpp", 60 | "span": "cpp", 61 | "sstream": "cpp", 62 | "stdexcept": "cpp", 63 | "stop_token": "cpp", 64 | "streambuf": "cpp", 65 | "thread": "cpp", 66 | "typeinfo": "cpp", 67 | "variant": "cpp", 68 | "core": "cpp" 69 | } 70 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dvaincilabs 다빈치랩스 2 | # onboard-flight-control-software 3 | Onboard Computer based flight control software for drone and UAM 4 | -------------------------------------------------------------------------------- /src/ioss/barometer_sensor.cpp: -------------------------------------------------------------------------------- 1 | // UART 방식 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | int main() { 10 | int serial_fd = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NDELAY); 11 | if (serial_fd == -1) { 12 | std::cerr << "UART 포트를 열 수 없습니다." << std::endl; 13 | return -1; 14 | } 15 | 16 | struct termios options; 17 | tcgetattr(serial_fd, &options); 18 | cfsetispeed(&options, B115200); 19 | cfsetospeed(&options, B115200); 20 | options.c_cflag |= (CLOCAL | CREAD); 21 | options.c_cflag &= ~PARENB; 22 | options.c_cflag &= ~CSTOPB; 23 | options.c_cflag &= ~CSIZE; 24 | options.c_cflag |= CS8; 25 | tcsetattr(serial_fd, TCSANOW, &options); 26 | 27 | unsigned char buffer[256]; 28 | while (true) { 29 | int bytes_read = read(serial_fd, &buffer, sizeof(buffer)); 30 | if (bytes_read > 0) { 31 | std::cout << "Raw data (hex): "; 32 | for (int i = 0; i < bytes_read; i++) { 33 | // 받은 데이터를 16진수로 출력 34 | std::cout << std::hex << static_cast(buffer[i]) << " "; 35 | } 36 | std::cout << std::endl; 37 | 38 | // 압력 값이 4바이트로 들어온다고 가정하고 이를 처리 39 | if (bytes_read >= 4) { 40 | // 첫 4바이트를 하나의 압력 값으로 해석 (float 또는 int 형식에 따라) 41 | uint32_t pressure_raw = (buffer[0] << 24) | (buffer[1] << 16) | (buffer[2] << 8) | buffer[3]; 42 | // 이 값을 실제 압력 값으로 변환 (센서에 따라 변환 공식이 다를 수 있음) 43 | float pressure = static_cast(pressure_raw) / 100.0; // 예: 압력 값을 헥토파스칼 단위로 변환 44 | std::cout << "Pressure = " << pressure << " hPa" << std::endl; 45 | } 46 | } 47 | usleep(100000); // 1초 대기 48 | } 49 | 50 | close(serial_fd); 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /src/ioss/barometer_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef BAROMETER_SENSOR_H 2 | #define BAROMETER_SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | // 기압 데이터를 저장하는 구조체 8 | struct BarometerData { 9 | float pressure; 10 | float temperature; 11 | }; 12 | 13 | // 기압 센서를 초기화하는 함수 14 | void initBarometer(const std::string& port, int baudRate); 15 | 16 | // 기압 데이터를 읽는 함수 17 | BarometerData readBarometer(); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /src/ioss/gps_sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "gps_sensor.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | int serialPort = -1; // 전역 변수로 시리얼 포트 관리 11 | 12 | // 시리얼 포트 설정 함수 13 | void initGPS(const char* port, int baudRate) { 14 | serialPort = open(port, O_RDWR | O_NOCTTY | O_NDELAY); 15 | if (serialPort == -1) { 16 | perror("Unable to open serial port"); 17 | return; 18 | } 19 | 20 | struct termios options; 21 | tcgetattr(serialPort, &options); 22 | cfsetispeed(&options, baudRate); // Baud rate 설정 23 | cfsetospeed(&options, baudRate); 24 | options.c_cflag |= (CLOCAL | CREAD); 25 | options.c_cflag &= ~PARENB; // 패리티 비트 없음 26 | options.c_cflag &= ~CSTOPB; // 1 스톱 비트 27 | options.c_cflag &= ~CSIZE; 28 | options.c_cflag |= CS8; // 8 비트 데이터 비트 29 | options.c_cflag &= ~CRTSCTS; // RTS/CTS 흐름 제어 없음 30 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 캐노닉 모드 해제 31 | options.c_iflag &= ~(IXON | IXOFF | IXANY); // 소프트웨어 흐름 제어 없음 32 | options.c_oflag &= ~OPOST; // 원시 모드 33 | 34 | tcsetattr(serialPort, TCSANOW, &options); 35 | } 36 | 37 | // GPS 데이터 파싱 함수 38 | GPSData parseGpsData(const vector& data) { 39 | GPSData gpsData = {}; 40 | 41 | if (data.size() < 100) { 42 | return gpsData; // 데이터가 부족하면 빈 값 반환 43 | } 44 | 45 | uint8_t msgId = data[3]; 46 | uint16_t length = (data[5] << 8) | data[4]; // Length 필드 (2바이트) 47 | 48 | if (msgId == 0x07) { 49 | gpsData.numSV = data[29]; 50 | gpsData.longitude = (data[33] << 24) | (data[32] << 16) | (data[31] << 8) | data[30]; 51 | gpsData.latitude = (data[37] << 24) | (data[36] << 16) | (data[35] << 8) | data[34]; 52 | gpsData.altitude = (data[41] << 24) | (data[40] << 16) | (data[39] << 8) | data[38]; 53 | gpsData.gSpeed = (data[69] << 24) | (data[68] << 16) | (data[67] << 8) | data[66]; 54 | gpsData.velocityX = (data[57] << 24) | (data[56] << 16) | (data[55] << 8) | data[54]; 55 | gpsData.velocityY = (data[61] << 24) | (data[60] << 16) | (data[59] << 8) | data[58]; 56 | gpsData.velocityZ = (data[65] << 24) | (data[64] << 16) | (data[63] << 8) | data[62]; 57 | } 58 | 59 | return gpsData; 60 | } 61 | 62 | // GPS 데이터를 읽는 함수 63 | GPSData readGPS() { 64 | vector receivedData; 65 | uint8_t buffer[1024]; 66 | GPSData gpsData = {}; 67 | bool flag = false; // 파싱이 성공했는지 확인하는 플래그 68 | 69 | while (!flag) { 70 | int bytesRead = read(serialPort, buffer, sizeof(buffer)); 71 | if (bytesRead > 0) { 72 | receivedData.insert(receivedData.end(), buffer, buffer + bytesRead); 73 | 74 | // 메시지 파싱을 위한 루프 75 | while (true) { 76 | if (receivedData.size() < 5) { 77 | break; // 수신된 데이터가 충분하지 않으면 대기 78 | } 79 | 80 | // 헤더 찾기 81 | if (receivedData[0] == 0xB5 && receivedData[1] == 0x62 && receivedData[3] == 0x07) { 82 | uint16_t length = (receivedData[5] << 8) | receivedData[4]; // Length 필드 (2바이트) 83 | uint16_t totalMessageLength = length + 6 + 2; // Length + Header + Checksum 84 | 85 | if (receivedData.size() >= totalMessageLength) { 86 | gpsData = parseGpsData(vector(receivedData.begin(), receivedData.begin() + totalMessageLength)); 87 | flag = true; // 올바른 값이 파싱되면 플래그를 true로 설정 88 | 89 | // 처리 후 남은 데이터 관리 90 | receivedData.erase(receivedData.begin(), receivedData.begin() + totalMessageLength); 91 | } else { 92 | break; // 전체 메시지가 아직 수신되지 않음 93 | } 94 | } else { 95 | // 헤더가 잘못된 경우 첫 번째 바이트를 삭제 96 | receivedData.erase(receivedData.begin()); 97 | } 98 | } 99 | } else { 100 | // 수신된 데이터가 없을 때 usleep으로 대기 (예: 100ms 대기) 101 | usleep(100000); // 100,000 마이크로초 = 100ms 102 | } 103 | } 104 | 105 | // flag가 true일 때 gpsData를 반환 106 | return gpsData; 107 | } -------------------------------------------------------------------------------- /src/ioss/gps_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef GPS_SENSOR_H 2 | #define GPS_SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | // GPSData 구조체 정의 8 | struct GPSData { 9 | int64_t longitude; // 경도 (1e-7 도 단위로 표현 가능) 10 | int64_t latitude; // 위도 (1e-7 도 단위로 표현 가능) 11 | int64_t altitude; // 고도 (더 큰 범위 지원) 12 | int64_t gSpeed; // 지상 속도 (더 큰 범위 지원) 13 | uint8_t numSV; // 위성 수 14 | int64_t velocityX; // NED 북 방향 속도 (mm/s, 더 큰 범위 지원) 15 | int64_t velocityY; // NED 동 방향 속도 (mm/s, 더 큰 범위 지원) 16 | int64_t velocityZ; // NED 하강 방향 속도 (mm/s, 더 큰 범위 지원) 17 | }; 18 | 19 | // GPS 초기화 함수 20 | void initGPS(const char* port, int baudRate); 21 | 22 | // GPS 데이터를 읽는 함수 23 | GPSData readGPS(); 24 | 25 | #endif -------------------------------------------------------------------------------- /src/ioss/imu_sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "imu_sensor.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define BUFFER_SIZE 128 16 | #define COMMAND_SIZE 100 // 명령어 크기 정의 17 | #define CRC_SIZE 4 // CRC 크기 정의 18 | 19 | // 시그널 플래그 20 | static int serial_port; 21 | static long previous_timestamp = 0; // 이전 타임스탬프 저장 변수 22 | 23 | // CRC 계산 함수 (데이터 유효성 검증에 사용) 24 | static unsigned short calculateCRC(const unsigned char* data, unsigned int length) { 25 | unsigned short crc = 0; 26 | for (unsigned int i = 0; i < length; i++) { 27 | crc = (unsigned char)(crc >> 8) | (crc << 8); 28 | crc ^= data[i]; 29 | crc ^= (unsigned char)(crc & 0xff) >> 4; 30 | crc ^= crc << 12; 31 | crc ^= (crc & 0x00ff) << 5; 32 | } 33 | return crc; 34 | } 35 | 36 | // 시리얼 포트 설정 함수 37 | static int configureSerial(const std::string& port, int baudrate) { 38 | serial_port = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 39 | if (serial_port == -1) { 40 | perror("Failed to open serial port"); 41 | return -1; 42 | } 43 | 44 | struct termios options; 45 | tcgetattr(serial_port, &options); 46 | cfsetispeed(&options, baudrate); 47 | cfsetospeed(&options, baudrate); 48 | options.c_cflag |= (CLOCAL | CREAD); 49 | options.c_cflag &= ~PARENB; 50 | options.c_cflag &= ~CSTOPB; 51 | options.c_cflag &= ~CSIZE; 52 | options.c_cflag |= CS8; 53 | options.c_iflag &= ~(IXON | IXOFF | IXANY); 54 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 55 | options.c_oflag &= ~OPOST; 56 | tcsetattr(serial_port, TCSANOW, &options); 57 | 58 | return serial_port; 59 | } 60 | 61 | // IMU 초기화 함수 62 | void initIMU(const std::string& port, int baudRate) { 63 | if (configureSerial(port, baudRate) == -1) { 64 | throw std::runtime_error("Unable to configure serial port"); 65 | } 66 | } 67 | 68 | // IMU 데이터 요청 함수 69 | void sendIMURequest() { 70 | char command[COMMAND_SIZE]; 71 | snprintf(command, sizeof(command), "$VNRRG,20"); 72 | unsigned short crc = calculateCRC((unsigned char *)command + 1, strlen(command) - 1); 73 | snprintf(command, sizeof(command), "$VNRRG,20*%04X\r\n", crc); 74 | write(serial_port, command, strlen(command)); 75 | } 76 | 77 | // IMU 데이터 읽기 및 처리 함수 78 | IMUData readIMU() { 79 | char buffer[BUFFER_SIZE]; // IMU 데이터 저장 버퍼 (128로 설정) 80 | int buffer_index = 0; 81 | IMUData imuData = {}; 82 | 83 | while (true) { 84 | sendIMURequest(); 85 | usleep(1000); // 요청 간격 설정 86 | 87 | int bytes_read = read(serial_port, buffer + buffer_index, sizeof(buffer) - buffer_index - 1); 88 | if (bytes_read > 0) { 89 | buffer_index += bytes_read; 90 | buffer[buffer_index] = '\0'; 91 | 92 | char* line_start = buffer; 93 | char* line_end; 94 | 95 | while ((line_end = strchr(line_start, '\n')) != NULL) { 96 | *line_end = '\0'; 97 | 98 | if (strncmp(line_start, "$VNRRG", 6) == 0) { 99 | char* end_of_data = strchr(line_start, '*'); 100 | if (end_of_data) { 101 | *end_of_data = '\0'; 102 | 103 | std::vector parts; 104 | std::istringstream ss(line_start); 105 | std::string token; 106 | 107 | while (std::getline(ss, token, ',')) { 108 | parts.push_back(token); 109 | } 110 | 111 | if (parts.size() >= 11) { // 자기장 데이터 포함 112 | unsigned short received_crc = std::stoi(end_of_data + 1, nullptr, 16); 113 | unsigned short calculated_crc = calculateCRC((unsigned char *)line_start + 1, strlen(line_start) - 1); 114 | 115 | if (received_crc == calculated_crc) { 116 | imuData.accelX = std::stof(parts[5]); 117 | imuData.accelY = std::stof(parts[6]); 118 | imuData.accelZ = std::stof(parts[7]); 119 | imuData.gyroX = std::stof(parts[8]); 120 | imuData.gyroY = std::stof(parts[9]); 121 | imuData.gyroZ = std::stof(parts[10]); 122 | imuData.magX = std::stof(parts[2]); 123 | imuData.magY = std::stof(parts[3]); 124 | imuData.magZ = std::stof(parts[4]); 125 | 126 | struct timeval current_time; 127 | gettimeofday(¤t_time, NULL); 128 | imuData.timestamp = (current_time.tv_sec * 1000.0) + (current_time.tv_usec / 1000.0); 129 | imuData.elapsed_time = imuData.timestamp - previous_timestamp; 130 | previous_timestamp = imuData.timestamp; 131 | 132 | return imuData; 133 | } else { 134 | fprintf(stderr, "CRC mismatch: Received: %04X, Calculated: %04X\n", received_crc, calculated_crc); 135 | } 136 | } else { 137 | fprintf(stderr, "Invalid data format\n"); 138 | } 139 | } 140 | } 141 | 142 | // 다음 줄로 이동 143 | line_start = line_end + 1; 144 | } 145 | 146 | // 남은 데이터가 있으면 버퍼의 시작으로 이동 147 | if (line_start < buffer + buffer_index) { 148 | int remaining_bytes = buffer + buffer_index - line_start; 149 | memmove(buffer, line_start, remaining_bytes); 150 | buffer_index = remaining_bytes; 151 | } else { 152 | buffer_index = 0; 153 | } 154 | } 155 | } 156 | } -------------------------------------------------------------------------------- /src/ioss/imu_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_SENSOR_H 2 | #define IMU_SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | // IMU 데이터를 저장하는 구조체 8 | struct IMUData { 9 | float accelX; // X축 가속도 10 | float accelY; // Y축 가속도 11 | float accelZ; // Z축 가속도 12 | float gyroX; // X축 자이로스코프 13 | float gyroY; // Y축 자이로스코프 14 | float gyroZ; // Z축 자이로스코프 15 | float magX; // X축 자기장 16 | float magY; // Y축 자기장 17 | float magZ; // Z축 자기장 18 | double timestamp; // 타임스탬프 19 | double elapsed_time; // 경과 시간 20 | }; 21 | 22 | void initIMU(const std::string& port, int baudRate); 23 | IMUData readIMU(); 24 | 25 | #endif -------------------------------------------------------------------------------- /src/ioss/rc_input.cpp: -------------------------------------------------------------------------------- 1 | #include "rc_input.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define SBUS_FRAME_SIZE 35 15 | #define START_BYTE 0x0F 16 | 17 | static int serial_port; 18 | static uint16_t channels[16]; // 16채널 값을 저장할 배열 19 | static std::deque data_buffer; // 최신 데이터를 저장할 버퍼 20 | 21 | // 시리얼 포트 설정 함수 22 | static int configureSerial(const std::string& port, int baudrate) { 23 | serial_port = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 24 | if (serial_port == -1) { 25 | perror("Failed to open serial port"); 26 | return -1; 27 | } 28 | 29 | struct termios options; 30 | tcgetattr(serial_port, &options); 31 | cfsetispeed(&options, baudrate); 32 | cfsetospeed(&options, baudrate); 33 | options.c_cflag |= (CLOCAL | CREAD); 34 | options.c_cflag &= ~PARENB; 35 | options.c_cflag &= ~CSTOPB; 36 | options.c_cflag &= ~CSIZE; 37 | options.c_cflag |= CS8; 38 | options.c_iflag &= ~(IXON | IXOFF | IXANY); 39 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 40 | options.c_oflag &= ~OPOST; 41 | tcsetattr(serial_port, TCSANOW, &options); 42 | 43 | return serial_port; 44 | } 45 | 46 | // RC 입력 초기화 함수 47 | void initRC(const std::string& port, int baudRate) { 48 | // 올바르게 초기화되지 않았을 경우 반복적으로 시도 49 | while (true) { 50 | if (configureSerial(port, baudRate) == -1) { 51 | std::cerr << "Failed to initialize RC input. Retrying..." << std::endl; 52 | std::this_thread::sleep_for(std::chrono::seconds(1)); // 1초 대기 후 재시도 53 | continue; 54 | } 55 | break; 56 | } 57 | } 58 | 59 | // 최신 RC 채널 값을 읽고 업데이트하는 함수 60 | int readRCChannel(int channel) { 61 | if (channel < 1 || channel > 16) { 62 | std::cerr << "Invalid channel number: " << channel << std::endl; 63 | return -1; 64 | } 65 | 66 | // 시리얼 포트에서 데이터 읽어 버퍼에 추가 67 | uint8_t byte; 68 | while (read(serial_port, &byte, 1) > 0) { 69 | data_buffer.push_back(byte); 70 | 71 | // 오래된 데이터를 삭제하여 버퍼 크기를 제한 72 | if (data_buffer.size() > SBUS_FRAME_SIZE * 10) { 73 | data_buffer.pop_front(); 74 | } 75 | } 76 | 77 | // 버퍼에서 최신 프레임을 찾아 데이터 갱신 78 | while (data_buffer.size() >= SBUS_FRAME_SIZE) { 79 | // 버퍼에서 프레임 추출 80 | std::vector frame(data_buffer.begin(), data_buffer.begin() + SBUS_FRAME_SIZE); 81 | 82 | // 시작 바이트 확인 83 | if (frame[0] != START_BYTE) { 84 | data_buffer.pop_front(); 85 | continue; 86 | } 87 | 88 | // 체크섬 검증 89 | uint8_t xor_checksum = 0; 90 | for (int i = 1; i < SBUS_FRAME_SIZE - 1; ++i) { 91 | xor_checksum ^= frame[i]; 92 | } 93 | 94 | if (xor_checksum != frame[SBUS_FRAME_SIZE - 1]) { 95 | data_buffer.pop_front(); // 잘못된 프레임을 버림 96 | continue; 97 | } 98 | 99 | // 유효한 프레임이면 채널 데이터 업데이트 100 | for (int i = 0; i < 16; ++i) { 101 | channels[i] = (frame[1 + i * 2] << 8) | frame[2 + i * 2]; 102 | } 103 | 104 | // 프레임을 버퍼에서 제거 105 | data_buffer.erase(data_buffer.begin(), data_buffer.begin() + SBUS_FRAME_SIZE); 106 | break; 107 | } 108 | 109 | // 요청된 채널 값을 반환 110 | return channels[channel - 1]; 111 | } -------------------------------------------------------------------------------- /src/ioss/rc_input.h: -------------------------------------------------------------------------------- 1 | #ifndef RC_INPUT_H 2 | #define RC_INPUT_H 3 | 4 | #include 5 | 6 | // RC 입력 초기화 함수 7 | void initRC(const std::string& port, int baudRate); 8 | 9 | // RC 데이터를 읽는 함수 10 | int readRCChannel(int channel); 11 | 12 | #endif -------------------------------------------------------------------------------- /src/ioss/sbus_reader.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define SERIAL_PORT "/dev/ttyAMA0" 10 | #define BAUDRATE B115200 11 | #define SBUS_FRAME_SIZE 35 12 | #define START_BYTE 0x0F 13 | 14 | int main() { 15 | int serial_port = open(SERIAL_PORT, O_RDWR | O_NOCTTY | O_NDELAY); 16 | if (serial_port == -1) { 17 | std::cerr << "Error opening serial port: " << SERIAL_PORT << std::endl; 18 | return 1; 19 | } 20 | 21 | termios tty; 22 | memset(&tty, 0, sizeof(tty)); 23 | 24 | if (tcgetattr(serial_port, &tty) != 0) { 25 | std::cerr << "Error getting terminal attributes." << std::endl; 26 | close(serial_port); 27 | return 1; 28 | } 29 | 30 | cfsetispeed(&tty, BAUDRATE); 31 | cfsetospeed(&tty, BAUDRATE); 32 | 33 | tty.c_cflag &= ~PARENB; 34 | tty.c_cflag &= ~CSTOPB; 35 | tty.c_cflag &= ~CSIZE; 36 | tty.c_cflag |= CS8; 37 | 38 | tty.c_cflag &= ~CRTSCTS; 39 | tty.c_cflag |= CREAD | CLOCAL; 40 | tty.c_lflag &= ~ICANON; 41 | tty.c_lflag &= ~(ECHO | ECHOE | ISIG); 42 | 43 | tty.c_iflag &= ~(IXON | IXOFF | IXANY); 44 | tty.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP); 45 | tty.c_oflag &= ~OPOST; 46 | 47 | tty.c_cc[VMIN] = SBUS_FRAME_SIZE; 48 | tty.c_cc[VTIME] = 1; 49 | 50 | if (tcsetattr(serial_port, TCSANOW, &tty) != 0) { 51 | std::cerr << "Error setting terminal attributes." << std::endl; 52 | close(serial_port); 53 | return 1; 54 | } 55 | 56 | uint8_t sbus_data[SBUS_FRAME_SIZE]; 57 | 58 | while (true) { 59 | int num_bytes = read(serial_port, &sbus_data, SBUS_FRAME_SIZE); 60 | if (num_bytes == SBUS_FRAME_SIZE && sbus_data[0] == START_BYTE) { 61 | uint8_t xor_checksum = 0; 62 | for (int i = 1; i < SBUS_FRAME_SIZE - 1; ++i) { 63 | xor_checksum ^= sbus_data[i]; 64 | } 65 | 66 | if (xor_checksum != sbus_data[SBUS_FRAME_SIZE - 1]) { 67 | std::cerr << "Checksum error. Discarding frame." << std::endl; 68 | continue; 69 | } 70 | 71 | uint16_t channels[16]; 72 | for (int i = 0; i < 16; ++i) { 73 | channels[i] = (sbus_data[1 + i * 2] << 8) | sbus_data[2 + i * 2]; 74 | } 75 | 76 | // 채널 1~5를 같은 줄에 print하여 제자리에서 업데이트 77 | std::cout << "\r"; 78 | for (int i = 0; i < 5; i++) { 79 | std::cout << "Channel " << (i + 1) << ": " << std::setw(4) << channels[i] << " "; 80 | } 81 | std::cout << std::flush; 82 | 83 | // 플래그 처리(다른 용도로 필요한 경우) 84 | uint8_t flags = sbus_data[33]; 85 | bool ch17 = flags & 0x80; 86 | bool ch18 = flags & 0x40; 87 | bool frame_lost = flags & 0x20; 88 | bool failsafe = flags & 0x10; 89 | } 90 | } 91 | 92 | close(serial_port); 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /src/oss/os_api.cpp: -------------------------------------------------------------------------------- 1 | // 운영 체제에 맞춘 API 호출 코드 (예: POSIX 또는 RTOS용 API) -------------------------------------------------------------------------------- /src/oss/thread_manager.cpp: -------------------------------------------------------------------------------- 1 | // 비행 제어에서 사용될 여러 스레드와 태스크 관리 2 | // 비행 제어 시스템에서 여러 태스크나 스레드를 관리하는 역할을 합니다. 이를 통해 비행 제어 시스템은 여러 개의 센서 데이터를 병렬로 처리하거나, 모터 제어와 같은 중요한 작업을 실시간으로 처리할 수 있습니다. -------------------------------------------------------------------------------- /src/oss/timer.cpp: -------------------------------------------------------------------------------- 1 | // 주기적인 타이머를 설정하고 관리 -------------------------------------------------------------------------------- /src/psss/attitude_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "attitude_controller.h" 2 | #include "motor_control.h" // 모터 제어 헤더 포함 3 | #include "pose_estimator.h" 4 | #include "../ioss/rc_input.h" 5 | 6 | // 각 축에 대한 PID 제어기 설정 7 | PIDController roll_pid(0.1f, 0.01f, 0.05f, 10.0f, 100.0f); // integral_limit과 output_limit 추가 8 | PIDController pitch_pid(0.1f, 0.01f, 0.05f, 10.0f, 100.0f); 9 | PIDController yaw_pid(0.2f, 0.01f, 0.1f, 10.0f, 100.0f); 10 | 11 | void controlAttitude(const Eigen::VectorXf& currentState, const RCInput& rcInput, float dt) { 12 | if (dt < 0.001f) { 13 | return; // 너무 작은 dt 값 방어 14 | } 15 | 16 | // 목표 각도 설정 (RC 입력에 따라) 17 | float targetRoll = rcInput.roll * 45.0f; 18 | float targetPitch = rcInput.pitch * 45.0f; 19 | float targetYaw = rcInput.yaw * 180.0f; 20 | 21 | // 현재 각도 (PoseEstimator에서 상태 값으로 받아옴) 22 | float currentRoll = currentState(6); 23 | float currentPitch = currentState(7); 24 | float currentYaw = currentState(8); 25 | 26 | // Yaw 각도 wrap-around 처리 27 | float yawError = targetYaw - currentYaw; 28 | if (yawError > 180.0f) yawError -= 360.0f; 29 | if (yawError < -180.0f) yawError += 360.0f; 30 | 31 | // PID 제어기로 각 축에 대한 제어 신호 계산 32 | float roll_output = roll_pid.update(targetRoll, currentRoll, dt); 33 | float pitch_output = pitch_pid.update(targetPitch, currentPitch, dt); 34 | float yaw_output = yaw_pid.update(yawError, 0.0f, dt); // Yaw는 각도 에러만 사용 35 | 36 | // 각 모터에 전달할 PWM 신호 계산 37 | float pwm_motor1 = roll_output - pitch_output + yaw_output; 38 | float pwm_motor2 = -roll_output + pitch_output + yaw_output; 39 | float pwm_motor3 = -roll_output - pitch_output - yaw_output; 40 | float pwm_motor4 = roll_output + pitch_output - yaw_output; 41 | 42 | // 모터 PWM 값 제한 43 | pwm_motor1 = fmaxf(fminf(pwm_motor1, PWM_MAX), PWM_MIN); 44 | pwm_motor2 = fmaxf(fminf(pwm_motor2, PWM_MAX), PWM_MIN); 45 | pwm_motor3 = fmaxf(fminf(pwm_motor3, PWM_MAX), PWM_MIN); 46 | pwm_motor4 = fmaxf(fminf(pwm_motor4, PWM_MAX), PWM_MIN); 47 | 48 | // 3 1 모터 넘버 49 | // 2 4 50 | 51 | // 모터 제어 함수 호출 (PWM 신호 전달) 52 | setMotorPWM(1, pwm_motor1); 53 | setMotorPWM(2, pwm_motor2); 54 | setMotorPWM(3, pwm_motor3); 55 | setMotorPWM(4, pwm_motor4); 56 | } -------------------------------------------------------------------------------- /src/psss/attitude_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef ATTITUDE_CONTROLLER_H 2 | #define ATTITUDE_CONTROLLER_H 3 | 4 | #include 5 | 6 | class PIDController { 7 | public: 8 | PIDController(float kp, float ki, float kd) 9 | : _kp(kp), _ki(ki), _kd(kd), _prev_error(0.0f), _integral(0.0f) {} 10 | 11 | float update(float target, float current, float dt) { 12 | float error = target - current; 13 | _integral += error * dt; 14 | float derivative = (error - _prev_error) / dt; 15 | _prev_error = error; 16 | return (_kp * error) + (_ki * _integral) + (_kd * derivative); 17 | } 18 | 19 | private: 20 | float _kp, _ki, _kd; 21 | float _prev_error, _integral; 22 | }; 23 | 24 | void controlAttitude(const Eigen::VectorXf& currentState, const RCInput& rcInput, float dt); 25 | 26 | #endif -------------------------------------------------------------------------------- /src/psss/ekf.cpp: -------------------------------------------------------------------------------- 1 | //쿼터니언 사용 2 | #include "ekf.h" 3 | #include 4 | #include 5 | #include 6 | 7 | // 상수 정의 8 | const float GRAVITY = 9.80665f; // 중력 상수 (m/s^2) 9 | const float GYRO_THRESHOLD = 0.01f; // 자이로 변화 임계값 (너무 작은 자이로 변화는 무시) 10 | const float ALPHA = 0.9f; // 저주파 필터 계수 (노이즈 필터링에 사용) 11 | 12 | // 유틸리티 함수 13 | float radToDeg(float rad) { return rad * (180.0f / M_PI); } 14 | float degToRad(float deg) { return deg * (M_PI / 180.0f); } 15 | bool isValidValue(float value) { return !std::isnan(value) && !std::isinf(value); } 16 | 17 | // EKF 생성자 18 | EKF::EKF() { 19 | state = Eigen::VectorXf::Zero(16); // 상태 벡터 확장 20 | state(6) = 1.0f; 21 | 22 | covariance = Eigen::MatrixXf::Identity(16, 16) * 0.05f; 23 | 24 | processNoise = Eigen::MatrixXf::Zero(16, 16); 25 | processNoise.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity() * 0.05f; 26 | processNoise.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity() * 0.05f; 27 | processNoise.block<4, 4>(6, 6) = Eigen::Matrix4f::Identity() * 0.001f; 28 | 29 | measurementNoise = Eigen::MatrixXf::Zero(6, 6); 30 | measurementNoise.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity() * 0.1f; // 노이즈 증가 31 | measurementNoise.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity() * 0.1f; // 노이즈 증가 32 | 33 | accelLast = Eigen::Vector3f::Zero(); 34 | gyroLast = Eigen::Vector3f::Zero(); 35 | } 36 | 37 | // EKF 소멸자 38 | EKF::~EKF() {} 39 | 40 | // 쿼터니언을 회전 행렬로 변환하는 함수 41 | Eigen::Matrix3f EKF::quaternionToRotationMatrix(const Eigen::Quaternionf& q) const { 42 | return q.toRotationMatrix(); // Eigen 라이브러리의 내장 함수 사용 43 | } 44 | 45 | // 저주파 필터 함수 (가속도와 자이로의 노이즈를 줄이기 위해 사용) 46 | Eigen::Vector3f EKF::lowPassFilter(const Eigen::Vector3f& input, const Eigen::Vector3f& last, float alpha) { 47 | return alpha * last + (1.0f - alpha) * input; // 필터링된 새로운 값 반환 48 | } 49 | 50 | // 예측 함수 51 | void EKF::predict(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 52 | if (!isValidValue(accel.norm()) || !isValidValue(gyro.norm())) { 53 | std::cerr << "유효하지 않은 IMU 데이터 감지됨" << std::endl; 54 | return; 55 | } 56 | 57 | Eigen::Vector3f filteredAccel = lowPassFilter(accel, accelLast, ALPHA); 58 | Eigen::Vector3f filteredGyro = lowPassFilter(gyro, gyroLast, ALPHA); 59 | 60 | if (filteredGyro.norm() < GYRO_THRESHOLD) { 61 | return; 62 | } 63 | 64 | predictState(filteredAccel, filteredGyro, dt); 65 | computeJacobian(filteredAccel, filteredGyro, dt); 66 | covariance = jacobian * covariance * jacobian.transpose() + processNoise; 67 | 68 | accelLast = filteredAccel; 69 | gyroLast = filteredGyro; 70 | } 71 | 72 | // 상태 예측 함수 73 | void EKF::predictState(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 74 | Eigen::Vector3f position = state.segment<3>(0); 75 | Eigen::Vector3f velocity = state.segment<3>(3); 76 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 77 | attitude.normalize(); 78 | 79 | Eigen::Vector3f angleAxis = gyro * dt; 80 | float angle = angleAxis.norm(); 81 | Eigen::Quaternionf deltaQ; 82 | 83 | if (angle > 1e-6f) { 84 | deltaQ = Eigen::AngleAxisf(angle, angleAxis.normalized()); 85 | } else { 86 | deltaQ = Eigen::Quaternionf(1.0f, angleAxis.x()/2.0f, angleAxis.y()/2.0f, angleAxis.z()/2.0f); 87 | } 88 | deltaQ.normalize(); 89 | attitude = (attitude * deltaQ).normalized(); 90 | 91 | Eigen::Matrix3f rotationMatrix = quaternionToRotationMatrix(attitude); 92 | Eigen::Vector3f accelWorld = rotationMatrix * accel; 93 | accelWorld(2) -= GRAVITY; 94 | 95 | velocity += accelWorld * dt; 96 | position += velocity * dt + 0.5f * accelWorld * dt * dt; 97 | 98 | state.segment<3>(0) = position; 99 | state.segment<3>(3) = velocity; 100 | state(6) = attitude.w(); 101 | state.segment<3>(7) = attitude.vec(); 102 | } 103 | 104 | // 자코비안 계산 함수 105 | void EKF::computeJacobian(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 106 | jacobian = Eigen::MatrixXf::Identity(16, 16); 107 | jacobian.block<3, 3>(0, 3) = Eigen::Matrix3f::Identity() * dt; 108 | } 109 | 110 | // 상태 업데이트 함수 (GPS 기반) 111 | void EKF::updateWithGPS(const Eigen::Vector3f& gpsPos, const Eigen::Vector3f& gpsVel) { 112 | Eigen::Vector3f gpsPos_latlon = gpsPos; 113 | Eigen::Vector3f gpsVel_m = gpsVel / 1000.0f; 114 | 115 | Eigen::VectorXf z(6); 116 | z.segment<3>(0) = gpsPos_latlon; 117 | z.segment<3>(3) = gpsVel_m; 118 | 119 | Eigen::VectorXf z_pred(6); 120 | z_pred.segment<3>(0) = state.segment<3>(0); 121 | z_pred.segment<3>(3) = state.segment<3>(3); 122 | 123 | Eigen::VectorXf y = z - z_pred; 124 | Eigen::MatrixXf H = Eigen::MatrixXf::Zero(6, 16); 125 | H.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity(); 126 | H.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity(); 127 | 128 | Eigen::MatrixXf S = H * covariance * H.transpose() + measurementNoise; 129 | Eigen::MatrixXf K = covariance * H.transpose() * S.inverse(); 130 | 131 | state += K * y; 132 | covariance = (Eigen::MatrixXf::Identity(16, 16) - K * H) * covariance; 133 | 134 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 135 | attitude.normalize(); 136 | state(6) = attitude.w(); 137 | state.segment<3>(7) = attitude.vec(); 138 | } 139 | 140 | // 자기장 업데이트 (yaw 보정) 141 | void EKF::updateWithMag(const Eigen::Vector3f& mag) { 142 | static int updateCounter = 0; // 보정 주기 설정을 위한 카운터 143 | 144 | // 보정 주기 설정 (예: 매 10번째 호출마다 보정) 145 | if (updateCounter++ % 10 != 0) { 146 | return; 147 | } 148 | 149 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 150 | attitude.normalize(); 151 | 152 | Eigen::Matrix3f rotationMatrix = quaternionToRotationMatrix(attitude); 153 | Eigen::Vector3f expectedMag = rotationMatrix.transpose() * Eigen::Vector3f(1, 0, 0); 154 | 155 | Eigen::Vector3f normalizedMag = mag.normalized(); 156 | Eigen::Vector3f normalizedExpectedMag = expectedMag.normalized(); 157 | 158 | float yawError = atan2(normalizedMag.y(), normalizedMag.x()) - atan2(normalizedExpectedMag.y(), normalizedExpectedMag.x()); 159 | 160 | const float MIN_YAW_ERROR = degToRad(0.01f); // 최소 보정 오차 (예: 0.01도) 161 | const float MAX_YAW_ERROR = degToRad(1.0f); // 최대 보정 한계 162 | const float CORRECTION_SCALE = 0.05f; // 점진적 보정 비율 163 | 164 | // Yaw 보정 적용 조건 165 | if (fabs(yawError) > MIN_YAW_ERROR) { 166 | yawError = std::clamp(yawError, -MAX_YAW_ERROR, MAX_YAW_ERROR); 167 | float correctionFactor = CORRECTION_SCALE * (fabs(yawError) / MAX_YAW_ERROR); // 동적 보정 비율 168 | yawError *= correctionFactor; 169 | 170 | Eigen::AngleAxisf yawCorrection(yawError, Eigen::Vector3f::UnitZ()); 171 | attitude = (attitude * Eigen::Quaternionf(yawCorrection)).normalized(); 172 | } 173 | 174 | state(6) = attitude.w(); 175 | state.segment<3>(7) = attitude.vec(); 176 | } 177 | 178 | // 현재 상태 반환 함수 179 | Eigen::VectorXf EKF::getState() const { 180 | Eigen::VectorXf stateOut(10); 181 | 182 | stateOut.segment<3>(0) = state.segment<3>(0); 183 | stateOut.segment<3>(3) = state.segment<3>(3); 184 | stateOut(6) = state(6); 185 | stateOut.segment<3>(7) = state.segment<3>(7); 186 | 187 | return stateOut; 188 | } -------------------------------------------------------------------------------- /src/psss/ekf.h: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #ifndef EKF_H 3 | #define EKF_H 4 | 5 | #include 6 | 7 | class EKF { 8 | public: 9 | EKF(); 10 | ~EKF(); 11 | 12 | void predict(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 13 | void updateWithGPS(const Eigen::Vector3f& gpsPos, const Eigen::Vector3f& gpsVel); 14 | void updateWithMag(const Eigen::Vector3f& mag); // 자기장 업데이트 함수 15 | Eigen::VectorXf getState() const; 16 | private: 17 | Eigen::VectorXf state; // 상태 벡터 (위치 3, 속도 3, 자세 4) 18 | Eigen::MatrixXf covariance; // 오차 공분산 행렬 19 | Eigen::MatrixXf processNoise; // 프로세스 노이즈 행렬 20 | Eigen::MatrixXf measurementNoise; // 측정 노이즈 행렬 21 | Eigen::MatrixXf jacobian; // Jacobian 행렬 22 | 23 | Eigen::Vector3f accelLast; // 마지막 가속도 값 24 | Eigen::Vector3f gyroLast; // 마지막 자이로 값 (저주파 필터에 사용) 25 | 26 | Eigen::Matrix3f quaternionToRotationMatrix(const Eigen::Quaternionf& q) const; 27 | void computeJacobian(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 28 | void predictState(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 29 | Eigen::Matrix3f skewSymmetric(const Eigen::Vector3f& v); 30 | 31 | Eigen::Vector3f lowPassFilter(const Eigen::Vector3f& input, const Eigen::Vector3f& last, float alpha); 32 | }; 33 | 34 | #endif -------------------------------------------------------------------------------- /src/psss/flight_control.cpp: -------------------------------------------------------------------------------- 1 | #include "flight_control.h" 2 | #include "../ioss/rc_input.h" 3 | #include "../ioss/gps_sensor.h" 4 | #include "../ioss/imu_sensor.h" 5 | #include 6 | #include 7 | 8 | // 모든 장치를 초기화하는 함수 9 | void flight_control_init() { 10 | // RC 초기화 11 | std::cout << "Initializing RC input..." << std::endl; 12 | initRC("/dev/ttyAMA0", B115200); 13 | 14 | // GPS 초기화 15 | std::cout << "Initializing GPS..." << std::endl; 16 | initGPS("/dev/ttyUSB1", B115200); // GPS 포트 및 보드레이트 설정 17 | 18 | // IMU 초기화 19 | std::cout << "Initializing IMU..." << std::endl; 20 | initIMU("/dev/ttyUSB0", B115200); // IMU 포트 및 보드레이트 설정 21 | 22 | std::cout << "Flight control system initialized." << std::endl; 23 | } 24 | 25 | -------------------------------------------------------------------------------- /src/psss/flight_control.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_CONTROL_H 2 | #define FLIGHT_CONTROL_H 3 | 4 | // 모든 장치를 초기화하는 함수 5 | void flight_control_init(); 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /src/psss/flight_mode.cpp: -------------------------------------------------------------------------------- 1 | #include "flight_mode.h" 2 | #include "rc_input.h" 3 | #include "motor_control.h" 4 | #include "gps_sensor.h" 5 | #include "udp_communication.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // 수동 비행 모드 12 | void manualFlight(Eigen::VectorXf& state) { 13 | while (true) { 14 | // RC 입력을 읽어옵니다. 15 | RCInput rcInput = readRCInput(); 16 | 17 | // 각 목표 각도 및 스로틀 계산 18 | float targetRoll = rcInput.roll * 45.0; // 롤의 목표 각도 (±45도) 19 | float targetPitch = rcInput.pitch * 45.0; // 피치의 목표 각도 (±45도) 20 | float targetYaw = rcInput.yaw * 180.0; // 요의 목표 각도 (±180도) 21 | float targetThrottle = rcInput.throttle * 1000 + 1000; // 스로틀 값 계산 (1000 ~ 2000) 22 | 23 | // 모터 출력 계산 24 | float motorOutputs[4]; 25 | calculateMotorOutputs(state(6), state(7), state(8), targetRoll, targetPitch, targetYaw, targetThrottle, motorOutputs); 26 | 27 | // 모터 제어 신호 전달 28 | controlMotors(motorOutputs); 29 | 30 | // 일정 주기로 반복 31 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 (20ms) 32 | } 33 | } 34 | 35 | // 자동 비행 모드 36 | void autoFlight(Eigen::VectorXf& state) { 37 | while (true) { 38 | // GPS 센서 데이터를 읽어옵니다. 39 | GPSData gpsData = readGPSSensor(); 40 | 41 | // 목표 위치 계산 (목표는 미리 설정된 경도, 위도, 고도 등) 42 | Eigen::Vector3f targetPosition(37.7749, -122.4194, 100.0); // 예시로 특정 좌표와 고도 설정 43 | 44 | // 위치 오류 계산 45 | Eigen::Vector3f positionError = targetPosition - gpsData.position; 46 | 47 | // 위치 오류를 기반으로 모터 출력 계산 48 | float motorOutputs[4]; 49 | calculateMotorOutputs(state(6), state(7), state(8), positionError(0), positionError(1), positionError(2), 1500, motorOutputs); 50 | 51 | // 모터 제어 신호 전달 52 | controlMotors(motorOutputs); 53 | 54 | // 일정 주기로 반복 55 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 56 | } 57 | } 58 | 59 | // 고투(Goto) 비행 모드 60 | void gotoFlight(Eigen::VectorXf& state) { 61 | // GPS 목표 좌표를 받아서 설정 62 | Eigen::Vector3f targetPosition = getGotoTargetPosition(); 63 | 64 | while (true) { 65 | // GPS 센서 데이터를 읽어옵니다. 66 | GPSData gpsData = readGPSSensor(); 67 | 68 | // 목표 위치까지의 거리 계산 69 | Eigen::Vector3f positionError = targetPosition - gpsData.position; 70 | 71 | // 위치 오류를 기반으로 모터 출력 계산 72 | float motorOutputs[4]; 73 | calculateMotorOutputs(state(6), state(7), state(8), positionError(0), positionError(1), positionError(2), 1500, motorOutputs); 74 | 75 | // 모터 제어 신호 전달 76 | controlMotors(motorOutputs); 77 | 78 | // 목표 위치에 도달했는지 확인 79 | if (positionError.norm() < 0.5) { 80 | std::cout << "목표 지점에 도달했습니다!" << std::endl; 81 | break; 82 | } 83 | 84 | // 일정 주기로 반복 85 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /src/psss/flight_mode.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_MODE_H 2 | #define FLIGHT_MODE_H 3 | 4 | #include 5 | 6 | // 비행 모드 열거형 정의 7 | enum FlightMode { 8 | MANUAL, 9 | AUTO, 10 | GOTO 11 | }; 12 | 13 | // 수동 비행 모드 함수 14 | void manualFlight(Eigen::VectorXf& state); 15 | 16 | // 자동 비행 모드 함수 17 | void autoFlight(Eigen::VectorXf& state); 18 | 19 | // 고투(Goto) 비행 모드 함수 20 | void gotoFlight(Eigen::VectorXf& state); 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /src/psss/main.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_estimator.h" 2 | #include "flight_mode.h" 3 | #include "flight_control.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | int main() { 10 | // 루프 실행 주기 설정 (100ms) 11 | const std::chrono::milliseconds loopDuration(100); 12 | 13 | // 비행 제어 시스템 초기화 (RC, GPS, IMU 등) 14 | flight_control_init(); 15 | 16 | // EKF 기반 자세 추정 클래스 생성 17 | PoseEstimator poseEstimator; 18 | std::this_thread::sleep_for(loopDuration); 19 | 20 | 21 | // CSV 파일 열기 22 | std::ofstream csvFile("current_pose.csv"); 23 | if (!csvFile.is_open()) { 24 | std::cerr << "CSV 파일을 열 수 없습니다." << std::endl; 25 | return 1; // 파일 열기 실패 시 프로그램 종료 26 | } 27 | 28 | // CSV 파일 헤더 작성 29 | csvFile << "X,Y,Z,Roll,Pitch,Yaw" << std::endl; 30 | 31 | // 메인 루프 32 | while (true) { 33 | // 100ms 주기로 상태 값을 가져옴 34 | Eigen::VectorXf state = poseEstimator.getPose(); 35 | 36 | // 자세 추정값 출력 (x, y, z 위치와 roll, pitch, yaw만 출력) 37 | std::cout << std::fixed << std::setprecision(7); // 소수점 7자리까지 표시 38 | std::cout << "Current Pose: " 39 | << state(0) << " " 40 | << state(1) << " " 41 | << state(2) << " " 42 | << state(6) << " " 43 | << state(7) << " " 44 | << state(8) << std::endl; 45 | 46 | // 100ms 동안 대기 47 | std::this_thread::sleep_for(loopDuration); 48 | } 49 | 50 | // CSV 파일 닫기 51 | csvFile.close(); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /src/psss/motor_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "../ioss/rc_input.h" 8 | #include "motor_control.h" 9 | #include 10 | 11 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 12 | #define MODE1 0x00 // 모드1 레지스터 13 | #define PRESCALE 0xFE // 프리스케일 레지스터 14 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 15 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 16 | 17 | const int RC_MIN = 172; 18 | const int RC_MAX = 1811; 19 | const int RC_MID = 991; 20 | const int PWM_MIN = 210; 21 | const int PWM_MAX = 405; 22 | const int MAX_ADJUSTMENT = 25; // 각 제어 입력의 최대 PWM 조정 값 23 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 24 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 25 | const int LOOP_DELAY_US = 10000; // 주기적인 대기 시간 (10ms) 26 | 27 | class PCA9685 { 28 | public: 29 | PCA9685(int address = PCA9685_ADDR) { 30 | char filename[20]; 31 | snprintf(filename, 19, "/dev/i2c-1"); 32 | fd = open(filename, O_RDWR); 33 | if (fd < 0) { 34 | std::cerr << "Failed to open the i2c bus" << std::endl; 35 | exit(1); 36 | } 37 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 38 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 39 | exit(1); 40 | } 41 | reset(); 42 | setPWMFreq(50); // Set frequency to 50Hz for motor control 43 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 44 | } 45 | 46 | ~PCA9685() { 47 | stopAllMotors(); // 종료 시 모든 모터를 정지 48 | if (fd >= 0) { 49 | close(fd); 50 | } 51 | } 52 | 53 | void setPWM(int channel, int on, int off) { 54 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 55 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 56 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 57 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 58 | } 59 | 60 | void setMotorSpeed(int channel, int pwm_value) { 61 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 62 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 63 | return; 64 | } 65 | setPWM(channel, 0, pwm_value); 66 | } 67 | 68 | private: 69 | int fd; 70 | 71 | void reset() { 72 | writeRegister(MODE1, 0x00); 73 | } 74 | 75 | void setPWMFreq(int freq) { 76 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 77 | uint8_t oldmode = readRegister(MODE1); 78 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 79 | writeRegister(MODE1, newmode); 80 | writeRegister(PRESCALE, prescale); 81 | writeRegister(MODE1, oldmode); 82 | usleep(5000); 83 | writeRegister(MODE1, oldmode | 0xA1); 84 | } 85 | 86 | void writeRegister(uint8_t reg, uint8_t value) { 87 | uint8_t buffer[2] = {reg, value}; 88 | int retries = 0; 89 | while (write(fd, buffer, 2) != 2) { 90 | if (++retries >= I2C_RETRY_LIMIT) { 91 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 92 | exit(1); 93 | } 94 | usleep(1000); // 1ms 대기 후 재시도 95 | } 96 | } 97 | 98 | uint8_t readRegister(uint8_t reg) { 99 | int retries = 0; 100 | while (write(fd, ®, 1) != 1) { 101 | if (++retries >= I2C_RETRY_LIMIT) { 102 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 103 | exit(1); 104 | } 105 | usleep(1000); 106 | } 107 | uint8_t value; 108 | if (read(fd, &value, 1) != 1) { 109 | std::cerr << "Failed to read from the i2c bus" << std::endl; 110 | exit(1); 111 | } 112 | return value; 113 | } 114 | 115 | void initializeMotors() { 116 | for (int i = 0; i < 4; ++i) { 117 | setMotorSpeed(i, SAFE_PWM); 118 | } 119 | } 120 | 121 | void stopAllMotors() { 122 | for (int i = 0; i < 4; ++i) { 123 | setMotorSpeed(i, SAFE_PWM); 124 | } 125 | std::cout << "All motors stopped safely." << std::endl; 126 | } 127 | }; 128 | 129 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 130 | double mapThrottle(int value) { 131 | if (value <= RC_MIN) return 0.0; 132 | if (value >= RC_MAX) return 1.0; 133 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 134 | } 135 | 136 | // 제어 입력(에일러론, 엘리베이터, 러더)을 -1.0 ~ 1.0 범위로 매핑하는 함수 137 | double mapControlInput(int value) { 138 | if (value < RC_MIN || value > RC_MAX) { 139 | std::cerr << "Control input out of range: " << value << std::endl; 140 | return 0.0; 141 | } 142 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 143 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 144 | return 0.0; 145 | } 146 | 147 | // 스로틀 PWM 계산 함수 148 | int computeThrottlePWM(double throttle_normalized) { 149 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 150 | } 151 | 152 | // 에일러론, 엘리베이터, 러더 조정 값 계산 함수 153 | int computeAdjustment(double control_normalized) { 154 | return static_cast(control_normalized * MAX_ADJUSTMENT); 155 | } 156 | 157 | // 값이 특정 범위 내에 있도록 제한하는 함수 158 | int clamp(int value, int min_value, int max_value) { 159 | return value < min_value ? min_value : (value > max_value ? max_value : value); 160 | } 161 | 162 | int main() { 163 | PCA9685 pca9685; 164 | initRC("/dev/ttyAMA0", B115200); // RC 입력 초기화 165 | 166 | while (true) { 167 | int throttle_value = readRCChannel(3); // 채널 3에서 스로틀 값 읽기 168 | int aileron_value = readRCChannel(1); // 채널 1에서 에일러론 값 읽기 169 | int elevator_value = readRCChannel(2); // 채널 2에서 엘리베이터 값 읽기 170 | int rudder_value = readRCChannel(4); // 채널 4에서 러더 값 읽기 171 | 172 | double throttle_normalized = mapThrottle(throttle_value); 173 | double aileron_normalized = mapControlInput(aileron_value); 174 | double elevator_normalized = mapControlInput(elevator_value); 175 | double rudder_normalized = mapControlInput(rudder_value); 176 | 177 | // 새 범위에 맞춘 스로틀 PWM 계산 178 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 179 | int aileron_adj = computeAdjustment(aileron_normalized); 180 | int elevator_adj = computeAdjustment(elevator_normalized); 181 | int rudder_adj = computeAdjustment(rudder_normalized); 182 | 183 | // 각 모터별로 스로틀과 조정 값을 계산하여 PWM 설정 184 | int motor1_PWM = throttle_PWM - aileron_adj - elevator_adj - rudder_adj; 185 | int motor2_PWM = throttle_PWM + aileron_adj - elevator_adj + rudder_adj; 186 | int motor3_PWM = throttle_PWM - aileron_adj + elevator_adj + rudder_adj; 187 | int motor4_PWM = throttle_PWM + aileron_adj + elevator_adj - rudder_adj; 188 | 189 | // PWM 값이 최소 값을 유지하도록 조정 190 | int min_motor_PWM = std::min(std::min(motor1_PWM, motor2_PWM), std::min(motor3_PWM, motor4_PWM)); 191 | if (min_motor_PWM < PWM_MIN) { 192 | int adjustment = PWM_MIN - min_motor_PWM; 193 | motor1_PWM += adjustment; 194 | motor2_PWM += adjustment; 195 | motor3_PWM += adjustment; 196 | motor4_PWM += adjustment; 197 | } 198 | 199 | // PWM 값이 범위 내에 있도록 제한 200 | motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 201 | motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 202 | motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 203 | motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 204 | 205 | // 각 모터에 계산된 PWM 값 적용 206 | pca9685.setMotorSpeed(0, motor1_PWM); 207 | pca9685.setMotorSpeed(1, motor2_PWM); 208 | pca9685.setMotorSpeed(2, motor3_PWM); 209 | pca9685.setMotorSpeed(3, motor4_PWM); 210 | 211 | std::cout << "\rThrottle PWM: " << throttle_PWM 212 | << " Motor1: " << motor1_PWM 213 | << " Motor2: " << motor2_PWM 214 | << " Motor3: " << motor3_PWM 215 | << " Motor4: " << motor4_PWM << std::flush; 216 | 217 | usleep(10000); // 10ms 대기 218 | } 219 | 220 | return 0; 221 | } -------------------------------------------------------------------------------- /src/psss/motor_control.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTERCONTROL_H 2 | #define MOTERCONTROL_H 3 | 4 | #include 5 | 6 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 7 | #define MODE1 0x00 // 모드1 레지스터 8 | #define PRESCALE 0xFE // 프리스케일 레지스터 9 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 10 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 11 | 12 | // PCA9685 초기화 함수: 주파수를 설정하여 PCA9685를 초기화합니다. 13 | void initPCA9685(int pwm_freq); 14 | 15 | // 특정 채널에 PWM 신호를 설정하는 함수 16 | void setPWM(int channel, int on, int off); 17 | 18 | // RC 채널 값을 PWM 값으로 변환하는 함수 19 | int mapChannelValueToPWM(int value); 20 | 21 | // 0~3번 채널의 모터를 제어하는 함수 22 | void controlMotors(); 23 | 24 | #endif -------------------------------------------------------------------------------- /src/psss/pose_estimator.cpp: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #include "pose_estimator.h" 3 | #include "ekf.h" 4 | #include "imu_sensor.h" 5 | #include "gps_sensor.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | Eigen::Vector3f quaternionToEuler(const Eigen::Quaternionf& q) { 13 | float ysqr = q.y() * q.y(); 14 | 15 | float t0 = +2.0f * (q.w() * q.x() + q.y() * q.z()); 16 | float t1 = +1.0f - 2.0f * (q.x() * q.x() + ysqr); 17 | float roll = std::atan2(t0, t1); 18 | 19 | float t2 = +2.0f * (q.w() * q.y() - q.z() * q.x()); 20 | t2 = std::clamp(t2, -1.0f, 1.0f); 21 | float pitch = std::asin(t2); 22 | 23 | float t3 = +2.0f * (q.w() * q.z() + q.x() * q.y()); 24 | float t4 = +1.0f - 2.0f * (ysqr + q.z() * q.z()); 25 | float yaw = std::atan2(t3, t4); 26 | 27 | return Eigen::Vector3f(roll, pitch, yaw) * (180.0f / M_PI); 28 | } 29 | 30 | // PoseEstimator 생성자 31 | PoseEstimator::PoseEstimator() : ekf(), running(true) { 32 | currentState = Eigen::VectorXf::Zero(16); 33 | imuAccel = Eigen::Vector3f::Zero(); 34 | imuGyro = Eigen::Vector3f::Zero(); 35 | imuMag = Eigen::Vector3f::Zero(); 36 | gpsPos = Eigen::Vector3f::Zero(); 37 | gpsVel = Eigen::Vector3f::Zero(); 38 | gyroOffset = Eigen::Vector3f::Zero(); 39 | 40 | imuThread = std::thread(&PoseEstimator::processIMU, this); 41 | gpsThread = std::thread(&PoseEstimator::processGPS, this); 42 | estimationThread = std::thread(&PoseEstimator::calculatePose, this); 43 | } 44 | 45 | // PoseEstimator 소멸자 46 | PoseEstimator::~PoseEstimator() { 47 | running = false; 48 | if (estimationThread.joinable()) { 49 | estimationThread.join(); 50 | } 51 | if (imuThread.joinable()) { 52 | imuThread.join(); 53 | } 54 | if (gpsThread.joinable()) { 55 | gpsThread.join(); 56 | } 57 | } 58 | 59 | // 자이로 캘리브레이션 함수 60 | void PoseEstimator::calibrateGyro() { 61 | int sampleCount = 100; 62 | Eigen::Vector3f gyroSum = Eigen::Vector3f::Zero(); 63 | for (int i = 0; i < sampleCount; ++i) { 64 | IMUData imuData = readIMU(); 65 | gyroSum += Eigen::Vector3f(imuData.gyroX, imuData.gyroY, imuData.gyroZ); 66 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 67 | } 68 | gyroOffset = gyroSum / sampleCount; 69 | isGyroCalibrated = true; 70 | } 71 | 72 | // 포즈 계산 함수 73 | void PoseEstimator::calculatePose() { 74 | while (running) { 75 | float dt = 0.1f; 76 | 77 | // 유효하지 않은 IMU 데이터가 감지되면 대체 값으로 초기화 78 | if (imuAccel.hasNaN() || imuGyro.hasNaN() || imuMag.hasNaN()) { 79 | std::cerr << "Invalid IMU data detected, using last valid data" << std::endl; 80 | imuAccel.setZero(); 81 | imuGyro.setZero(); 82 | imuMag.setZero(); 83 | } 84 | 85 | // EKF 예측 및 자기장 업데이트 단계 실행 86 | ekf.predict(imuAccel, imuGyro, dt); 87 | // ekf.updateWithMag(imuMag); 88 | ekf.updateWithGPS(gpsPos ,gpsVel); 89 | 90 | // 현재 상태를 보호된 상태로 업데이트 91 | { 92 | std::lock_guard lock(poseMutex); 93 | currentState = ekf.getState(); 94 | } 95 | 96 | // 계산 주기 설정 (100ms) 97 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 98 | } 99 | } 100 | 101 | // IMU 데이터 처리 함수 102 | void PoseEstimator::processIMU() { 103 | 104 | while (running) { 105 | IMUData imuData = readIMU(); // IMU 센서에서 데이터 읽기 106 | Eigen::Vector3f newAccel = Eigen::Vector3f(imuData.accelX, imuData.accelY, imuData.accelZ); 107 | Eigen::Vector3f newGyro = Eigen::Vector3f(imuData.gyroX, imuData.gyroY, imuData.gyroZ) - gyroOffset; 108 | Eigen::Vector3f newMag = Eigen::Vector3f(imuData.magX, imuData.magY, imuData.magZ); 109 | 110 | // 유효한 IMU 데이터인 경우에만 업데이트 111 | if (!newAccel.hasNaN() && !newGyro.hasNaN() && !newMag.hasNaN()) { 112 | { 113 | std::lock_guard lock(poseMutex); // 동기화 보호 114 | imuAccel = newAccel; 115 | imuGyro = newGyro; 116 | imuMag = newMag; 117 | } 118 | } else { 119 | std::cerr << "Invalid IMU data, keeping last valid data" << std::endl; 120 | } 121 | 122 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 주기 설정 123 | } 124 | } 125 | 126 | // GPS 데이터 처리 함수 127 | void PoseEstimator::processGPS() { 128 | while (running) { 129 | // 기존의 GPS 데이터 읽기 부분을 주석 처리합니다. 130 | /* 131 | GPSData gpsData = readGPS(); 132 | Eigen::Vector3f newPos = Eigen::Vector3f(gpsData.latitude / 1e7, gpsData.longitude / 1e7, gpsData.altitude / 1000.0f); 133 | Eigen::Vector3f newVel = Eigen::Vector3f(gpsData.velocityX, gpsData.velocityY, gpsData.velocityZ); 134 | 135 | // GPS 데이터가 유효하지 않으면 마지막 유효 데이터를 그대로 사용 136 | if (newPos.hasNaN() || newVel.hasNaN()) { 137 | std::cerr << "Invalid GPS data, keeping last valid data" << std::endl; 138 | } else { 139 | gpsPos = newPos; // 새로운 유효한 GPS 데이터로 업데이트 140 | gpsVel = newVel; 141 | } 142 | */ 143 | gpsPos = Eigen::Vector3f::Zero(); 144 | gpsVel = Eigen::Vector3f::Zero(); 145 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 146 | } 147 | } 148 | 149 | // 현재 포즈를 얻는 함수 150 | Eigen::VectorXf PoseEstimator::getPose() { 151 | std::lock_guard lock(poseMutex); 152 | 153 | Eigen::VectorXf pose(9); // 위치, 속도, 오일러 각을 포함한 9차원 벡터 154 | pose.segment<3>(0) = currentState.segment<3>(0); // 위치 (x, y, z) 155 | pose.segment<3>(3) = currentState.segment<3>(3); // 속도 (vx, vy, vz) 156 | 157 | // 쿼터니언에서 Roll, Pitch, Yaw 계산 158 | Eigen::Quaternionf attitude(currentState(6), currentState(7), currentState(8), currentState(9)); 159 | attitude.normalize(); // 쿼터니언 정규화 160 | pose.segment<3>(6) = quaternionToEuler(attitude); // 오일러 각 (Roll, Pitch, Yaw) 추가 161 | 162 | return pose; 163 | } 164 | -------------------------------------------------------------------------------- /src/psss/pose_estimator.h: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "ekf.h" 8 | #include "imu_sensor.h" 9 | #include "gps_sensor.h" 10 | 11 | class PoseEstimator { 12 | public: 13 | PoseEstimator(); 14 | ~PoseEstimator(); 15 | 16 | Eigen::VectorXf getPose(); 17 | 18 | private: 19 | EKF ekf; 20 | 21 | std::thread estimationThread; 22 | std::thread imuThread; 23 | std::thread gpsThread; 24 | std::atomic running; 25 | 26 | Eigen::VectorXf currentState; 27 | Eigen::Vector3f imuAccel; 28 | Eigen::Vector3f imuGyro; 29 | Eigen::Vector3f imuMag; 30 | Eigen::Vector3f gpsPos; 31 | Eigen::Vector3f gpsVel; 32 | std::mutex poseMutex; 33 | 34 | Eigen::Vector3f gyroOffset; 35 | bool isGyroCalibrated = false; 36 | 37 | void calibrateGyro(); 38 | void calculatePose(); 39 | void processIMU(); 40 | void processGPS(); 41 | 42 | const std::chrono::milliseconds loopDuration = std::chrono::milliseconds(20); 43 | }; -------------------------------------------------------------------------------- /src/tss/udp_communication.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davincilabs-code/onboard-flight-control-software/9b69f1c26a44698479f6b1ae1dce22dfb7ea724b/src/tss/udp_communication.cpp -------------------------------------------------------------------------------- /src/tss/udp_communication.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davincilabs-code/onboard-flight-control-software/9b69f1c26a44698479f6b1ae1dce22dfb7ea724b/src/tss/udp_communication.h -------------------------------------------------------------------------------- /test_code/amc.cpp: -------------------------------------------------------------------------------- 1 | #include "attitude_controller.h" 2 | #include "motor_control.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // Delta time 계산 함수 9 | double getDeltaTime() { 10 | static auto last_time = std::chrono::steady_clock::now(); 11 | auto current_time = std::chrono::steady_clock::now(); 12 | double dt = std::chrono::duration(current_time - last_time).count(); 13 | last_time = current_time; 14 | return dt; 15 | } 16 | 17 | int controlMotors(PCA9685 &pca9685) { 18 | while (true) { 19 | // Delta time 계산 20 | double dt = getDeltaTime(); 21 | 22 | // Attitude controller를 통해 PWM 값 계산 23 | std::array motor_pwms = controlAttitude(dt); 24 | 25 | // PCA9685 모듈을 사용해 각 모터에 PWM 값 전달 26 | pca9685.setMotorSpeed(0, static_cast(motor_pwms[0])); 27 | pca9685.setMotorSpeed(1, static_cast(motor_pwms[1])); 28 | pca9685.setMotorSpeed(2, static_cast(motor_pwms[2])); 29 | pca9685.setMotorSpeed(3, static_cast(motor_pwms[3])); 30 | 31 | // PWM 값 출력 32 | std::cout << "\rMotor1: " << motor_pwms[0] 33 | << " Motor2: " << motor_pwms[1] 34 | << " Motor3: " << motor_pwms[2] 35 | << " Motor4: " << motor_pwms[3] << " " << std::flush; 36 | 37 | // 10ms 대기 38 | usleep(10000); 39 | } 40 | 41 | return 0; 42 | } 43 | 44 | int main() { 45 | // PCA9685 모터 드라이버 초기화 46 | PCA9685 pca9685; 47 | controlMotors(pca9685); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /test_code/attitude_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "attitude_controller.h" 2 | #include "pose_estimator.h" 3 | #include "rc_input.h" 4 | #include "motor_control.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // 각 축에 대한 PID 제어기 설정 변수 15 | PIDController roll_pid(0.1f, 0.01f, 0.05f); 16 | PIDController pitch_pid(0.1f, 0.01f, 0.05f); 17 | PIDController yaw_pid(0.2f, 0.01f, 0.1f); 18 | 19 | // PoseEstimator 객체 생성 20 | PoseEstimator poseEstimator; // pose_estimator 초기화 21 | 22 | // IMU 캘리브레이션 오프셋 값 23 | const float offsetX = 0.12407f; 24 | const float offsetY = -0.13802f; 25 | const float offsetZ = -9.73542f; 26 | 27 | // 각도 제한 상수 28 | const float MAX_TARGET_ANGLE = 30.0f; // 최대 목표 각도 (롤, 피치) 29 | 30 | std::array controlAttitude(float dt) { 31 | if (dt < 0.0001f) { 32 | std::cout << "false" << std::endl; 33 | return {PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}; // dt가 너무 작을 경우 안전한 최소값 반환 34 | } 35 | 36 | // RC 입력 값을 직접 읽어 목표 각도를 설정 37 | int rollInput = readRCChannel(1) / 1; // 채널 1: 롤 입력 38 | int pitchInput = readRCChannel(2) / 1; // 채널 2: 피치 입력 39 | int yawInput = readRCChannel(4) / 1; // 채널 4: 요 입력 40 | int throttleInput = readRCChannel(3) / 1; // 채널 3: 스로틀 입력 41 | 42 | // RC 입력이 없을 경우 목표 각도를 0으로 설정하여 수평 상태 유지 43 | float targetRoll = (fabs(rollInput) < 0.05f) ? offsetX : rollInput * MAX_TARGET_ANGLE; 44 | float targetPitch = (fabs(pitchInput) < 0.05f) ? offsetY : pitchInput * MAX_TARGET_ANGLE; 45 | float targetYaw = yawInput * 180.0f; 46 | 47 | // 현재 자세 추정값 가져오기 (x, y, z 위치와 roll, pitch, yaw만 사용) 48 | Eigen::VectorXf currentPose = poseEstimator.getPose(); 49 | float currentRoll = currentPose(3); // roll 값 (상태 벡터의 4번째 요소) 50 | float currentPitch = currentPose(4); // pitch 값 (상태 벡터의 5번째 요소) 51 | float currentYaw = currentPose(5); // yaw 값 (상태 벡터의 6번째 요소) 52 | 53 | // 각도 오차 계산 54 | float yawError = targetYaw - currentYaw; 55 | if (yawError > 180.0f) yawError -= 360.0f; 56 | if (yawError < -180.0f) yawError += 360.0f; 57 | 58 | // PID 제어기 업데이트 59 | float roll_output = roll_pid.update(targetRoll, currentRoll, dt); 60 | float pitch_output = pitch_pid.update(targetPitch, currentPitch, dt); 61 | float yaw_output = yaw_pid.update(yawError, 0.0f, dt); 62 | float throttle_out = throttleInput; 63 | 64 | // 모터에 대한 PWM 값 계산 65 | float pwm_motor1 = throttle_out + roll_output + pitch_output + yaw_output; // 모터 1 (CW) 66 | float pwm_motor2 = throttle_out - roll_output + pitch_output - yaw_output; // 모터 2 (CCW) 67 | float pwm_motor3 = throttle_out - roll_output - pitch_output + yaw_output; // 모터 3 (CCW) 68 | float pwm_motor4 = throttle_out + roll_output - pitch_output - yaw_output; // 모터 4 (CW) 69 | 70 | // PWM 값 제한 71 | pwm_motor1 = fmaxf(fminf(pwm_motor1, PWM_MAX), PWM_MIN); 72 | pwm_motor2 = fmaxf(fminf(pwm_motor2, PWM_MAX), PWM_MIN); 73 | pwm_motor3 = fmaxf(fminf(pwm_motor3, PWM_MAX), PWM_MIN); 74 | pwm_motor4 = fmaxf(fminf(pwm_motor4, PWM_MAX), PWM_MIN); 75 | 76 | // RC 입력이 중립에 있을 경우 PID 제어기 초기화 77 | if (fabs(rollInput) < 0.05f && fabs(pitchInput) < 0.05f && fabs(yawInput) < 0.05f) { 78 | roll_pid.reset(); 79 | pitch_pid.reset(); 80 | yaw_pid.reset(); 81 | } 82 | 83 | return {pwm_motor1, pwm_motor2, pwm_motor3, pwm_motor4}; 84 | } 85 | 86 | void setMotorPWM(int motor_index, float pwm_value); 87 | 88 | void setMotorPWMImpl(int motor_index, float pwm_value) { 89 | // 모터 PWM 설정 함수 구현 (실제 환경에 맞게 수정 필요) 90 | std::cout << "Setting motor " << motor_index << " to PWM value: " << pwm_value << std::endl; 91 | } 92 | 93 | int main() { 94 | // RC 입력 초기화 95 | const std::string port = "/dev/ttyAMA0"; 96 | const int baudRate = 115200; // B115200 대신 일반 정수 값 사용 97 | initRC(port, baudRate); 98 | 99 | // 루프를 돌면서 주기적으로 제어 수행 100 | auto last_time = std::chrono::steady_clock::now(); 101 | while (true) { // 무한 반복 102 | // 현재 시간과 이전 시간의 차이를 계산하여 dt 계산 103 | auto current_time = std::chrono::steady_clock::now(); 104 | float dt = std::chrono::duration(current_time - last_time).count(); 105 | last_time = current_time; 106 | // dt가 0인 경우 방지 107 | if (dt <= 0.0f) { 108 | dt = 0.001f; // 최소한의 값으로 설정하여 계산이 진행되도록 함 109 | } 110 | // 자세 제어 및 PWM 값 계산 111 | std::array pwm_values = controlAttitude(dt); 112 | // 모터 제어 (모터에 PWM 신호를 전송하는 부분은 실제 환경에 맞게 구현해야 함) 113 | setMotorPWMImpl(0, pwm_values[0]); 114 | setMotorPWMImpl(1, pwm_values[1]); 115 | setMotorPWMImpl(2, pwm_values[2]); 116 | setMotorPWMImpl(3, pwm_values[3]); 117 | 118 | // 20ms 대기 (50Hz 주기) 119 | usleep(20000); 120 | } 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /test_code/attitude_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef ATTITUDE_CONTROLLER_H 2 | #define ATTITUDE_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | const int PWM_MIN = 215; 9 | const int PWM_MAX = 410; 10 | 11 | // PID 제어기 클래스 정의함 12 | class PIDController { 13 | public: 14 | PIDController(float kp, float ki, float kd) 15 | : _kp(kp), _ki(ki), _kd(kd), _prev_error(0.0f), _integral(0.0f) {} 16 | 17 | float update(float target, float current, float dt) { 18 | float error = target - current; 19 | _integral += error * dt; 20 | float derivative = (error - _prev_error) / dt; 21 | _prev_error = error; 22 | return (_kp * error) + (_ki * _integral) + (_kd * derivative); 23 | } 24 | 25 | // 리셋 함수 추가 26 | void reset() { 27 | _prev_error = 0.0f; // 이전 오차 초기화 28 | _integral = 0.0f; // 적분 값 초기화 29 | } 30 | 31 | private: 32 | float _kp, _ki, _kd; 33 | float _prev_error, _integral; 34 | }; 35 | 36 | // controlAttitude 함수 선언 37 | std::array controlAttitude(float dt); 38 | 39 | #endif // ATTITUDE_CONTROLLER_H 40 | -------------------------------------------------------------------------------- /test_code/barometer_sensor.cpp: -------------------------------------------------------------------------------- 1 | // UART 방식으로 시도 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | int main() { 10 | int serial_fd = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NDELAY); 11 | if (serial_fd == -1) { 12 | std::cerr << "UART 포트를 열 수 없습니다." << std::endl; 13 | return -1; 14 | } 15 | 16 | struct termios options; 17 | tcgetattr(serial_fd, &options); 18 | cfsetispeed(&options, B115200); 19 | cfsetospeed(&options, B115200); 20 | options.c_cflag |= (CLOCAL | CREAD); 21 | options.c_cflag &= ~PARENB; 22 | options.c_cflag &= ~CSTOPB; 23 | options.c_cflag &= ~CSIZE; 24 | options.c_cflag |= CS8; 25 | tcsetattr(serial_fd, TCSANOW, &options); 26 | 27 | unsigned char buffer[256]; 28 | while (true) { 29 | int bytes_read = read(serial_fd, &buffer, sizeof(buffer)); 30 | if (bytes_read > 0) { 31 | std::cout << "Raw data (hex): "; 32 | for (int i = 0; i < bytes_read; i++) { 33 | // 받은 데이터를 16진수로 출력 34 | std::cout << std::hex << static_cast(buffer[i]) << " "; 35 | } 36 | std::cout << std::endl; 37 | 38 | // 압력 값이 4바이트로 들어온다고 가정하고 이를 처리 39 | if (bytes_read >= 4) { 40 | // 첫 4바이트를 하나의 압력 값으로 해석 (float 또는 int 형식에 따라) 41 | uint32_t pressure_raw = (buffer[0] << 24) | (buffer[1] << 16) | (buffer[2] << 8) | buffer[3]; 42 | // 이 값을 실제 압력 값으로 변환 (센서에 따라 변환 공식이 다를 수 있음) 43 | float pressure = static_cast(pressure_raw) / 100.0; // 예: 압력 값을 헥토파스칼 단위로 변환 44 | std::cout << "Pressure = " << pressure << " hPa" << std::endl; 45 | } 46 | } 47 | usleep(100000); // 1초 대기 48 | } 49 | 50 | close(serial_fd); 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /test_code/barometer_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef BAROMETER_SENSOR_H 2 | #define BAROMETER_SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | // 기압 데이터를 저장하는 구조체 8 | struct BarometerData { 9 | float pressure; 10 | float temperature; 11 | }; 12 | 13 | // 기압 센서를 초기화하는 함수 14 | void initBarometer(const std::string& port, int baudRate); 15 | 16 | // 기압 데이터를 읽는 함수 17 | BarometerData readBarometer(); 18 | 19 | #endif // BAROMETER_SENSOR_H 20 | -------------------------------------------------------------------------------- /test_code/ekf.cpp: -------------------------------------------------------------------------------- 1 | // 11월 12일 코드 통합 2 | #include "ekf.h" 3 | #include 4 | #include 5 | #include 6 | 7 | // 상수 정의 8 | const float GRAVITY = 9.80665f; // 중력 상수 (m/s^2) 9 | const float GYRO_THRESHOLD = 0.01f; // 자이로 변화 임계값 (너무 작은 자이로 변화는 무시) 10 | const float ALPHA = 0.9f; // 저주파 필터 계수 (노이즈 필터링에 사용) 11 | 12 | // 유틸리티 함수 13 | float radToDeg(float rad) { return rad * (180.0f / M_PI); } 14 | float degToRad(float deg) { return deg * (M_PI / 180.0f); } 15 | bool isValidValue(float value) { return !std::isnan(value) && !std::isinf(value); } 16 | 17 | // EKF 생성자 18 | EKF::EKF() { 19 | state = Eigen::VectorXf::Zero(16); // 상태 벡터 확장 20 | state(6) = 1.0f; 21 | 22 | covariance = Eigen::MatrixXf::Identity(16, 16) * 0.05f; 23 | 24 | processNoise = Eigen::MatrixXf::Zero(16, 16); 25 | processNoise.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity() * 0.05f; 26 | processNoise.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity() * 0.05f; 27 | processNoise.block<4, 4>(6, 6) = Eigen::Matrix4f::Identity() * 0.001f; 28 | 29 | measurementNoise = Eigen::MatrixXf::Zero(6, 6); 30 | measurementNoise.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity() * 0.1f; // 노이즈 증가 31 | measurementNoise.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity() * 0.1f; // 노이즈 증가 32 | 33 | accelLast = Eigen::Vector3f::Zero(); 34 | gyroLast = Eigen::Vector3f::Zero(); 35 | } 36 | 37 | // EKF 소멸자 38 | EKF::~EKF() {} 39 | 40 | // 쿼터니언을 회전 행렬로 변환하는 함수 41 | Eigen::Matrix3f EKF::quaternionToRotationMatrix(const Eigen::Quaternionf& q) const { 42 | return q.toRotationMatrix(); // Eigen 라이브러리의 내장 함수 사용 43 | } 44 | 45 | // 저주파 필터 함수 (가속도와 자이로의 노이즈를 줄이기 위해 사용) 46 | Eigen::Vector3f EKF::lowPassFilter(const Eigen::Vector3f& input, const Eigen::Vector3f& last, float alpha) { 47 | return alpha * last + (1.0f - alpha) * input; // 필터링된 새로운 값 반환 48 | } 49 | 50 | // 예측 함수 51 | void EKF::predict(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 52 | if (!isValidValue(accel.norm()) || !isValidValue(gyro.norm())) { 53 | std::cerr << "유효하지 않은 IMU 데이터 감지됨" << std::endl; 54 | return; 55 | } 56 | 57 | Eigen::Vector3f filteredAccel = lowPassFilter(accel, accelLast, ALPHA); 58 | Eigen::Vector3f filteredGyro = lowPassFilter(gyro, gyroLast, ALPHA); 59 | 60 | if (filteredGyro.norm() < GYRO_THRESHOLD) { 61 | return; 62 | } 63 | 64 | predictState(filteredAccel, filteredGyro, dt); 65 | computeJacobian(filteredAccel, filteredGyro, dt); 66 | covariance = jacobian * covariance * jacobian.transpose() + processNoise; 67 | 68 | accelLast = filteredAccel; 69 | gyroLast = filteredGyro; 70 | } 71 | 72 | // 상태 예측 함수 73 | void EKF::predictState(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 74 | Eigen::Vector3f position = state.segment<3>(0); 75 | Eigen::Vector3f velocity = state.segment<3>(3); 76 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 77 | attitude.normalize(); 78 | 79 | Eigen::Vector3f angleAxis = gyro * dt; 80 | float angle = angleAxis.norm(); 81 | Eigen::Quaternionf deltaQ; 82 | 83 | if (angle > 1e-6f) { 84 | deltaQ = Eigen::AngleAxisf(angle, angleAxis.normalized()); 85 | } else { 86 | deltaQ = Eigen::Quaternionf(1.0f, angleAxis.x()/2.0f, angleAxis.y()/2.0f, angleAxis.z()/2.0f); 87 | } 88 | deltaQ.normalize(); 89 | attitude = (attitude * deltaQ).normalized(); 90 | 91 | Eigen::Matrix3f rotationMatrix = quaternionToRotationMatrix(attitude); 92 | Eigen::Vector3f accelWorld = rotationMatrix * accel; 93 | accelWorld(2) -= GRAVITY; 94 | 95 | velocity += accelWorld * dt; 96 | position += velocity * dt + 0.5f * accelWorld * dt * dt; 97 | 98 | state.segment<3>(0) = position; 99 | state.segment<3>(3) = velocity; 100 | state(6) = attitude.w(); 101 | state.segment<3>(7) = attitude.vec(); 102 | } 103 | 104 | // 자코비안 계산 함수 105 | void EKF::computeJacobian(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt) { 106 | jacobian = Eigen::MatrixXf::Identity(16, 16); 107 | jacobian.block<3, 3>(0, 3) = Eigen::Matrix3f::Identity() * dt; 108 | } 109 | 110 | // 상태 업데이트 함수 (GPS 기반) 111 | void EKF::updateWithGPS(const Eigen::Vector3f& gpsPos, const Eigen::Vector3f& gpsVel) { 112 | Eigen::Vector3f gpsPos_latlon = gpsPos; 113 | Eigen::Vector3f gpsVel_m = gpsVel / 1000.0f; 114 | 115 | Eigen::VectorXf z(6); 116 | z.segment<3>(0) = gpsPos_latlon; 117 | z.segment<3>(3) = gpsVel_m; 118 | 119 | Eigen::VectorXf z_pred(6); 120 | z_pred.segment<3>(0) = state.segment<3>(0); 121 | z_pred.segment<3>(3) = state.segment<3>(3); 122 | 123 | Eigen::VectorXf y = z - z_pred; 124 | Eigen::MatrixXf H = Eigen::MatrixXf::Zero(6, 16); 125 | H.block<3, 3>(0, 0) = Eigen::Matrix3f::Identity(); 126 | H.block<3, 3>(3, 3) = Eigen::Matrix3f::Identity(); 127 | 128 | Eigen::MatrixXf S = H * covariance * H.transpose() + measurementNoise; 129 | Eigen::MatrixXf K = covariance * H.transpose() * S.inverse(); 130 | 131 | state += K * y; 132 | covariance = (Eigen::MatrixXf::Identity(16, 16) - K * H) * covariance; 133 | 134 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 135 | attitude.normalize(); 136 | state(6) = attitude.w(); 137 | state.segment<3>(7) = attitude.vec(); 138 | } 139 | 140 | // 자기장 업데이트 (yaw 보정) 141 | void EKF::updateWithMag(const Eigen::Vector3f& mag) { 142 | static int updateCounter = 0; // 보정 주기 설정을 위한 카운터 143 | 144 | if (updateCounter++ % 10 != 0) { 145 | return; 146 | } 147 | 148 | Eigen::Quaternionf attitude(state(6), state(7), state(8), state(9)); 149 | attitude.normalize(); 150 | 151 | Eigen::Matrix3f rotationMatrix = quaternionToRotationMatrix(attitude); 152 | Eigen::Vector3f expectedMag = rotationMatrix.transpose() * Eigen::Vector3f(1, 0, 0); 153 | 154 | Eigen::Vector3f normalizedMag = mag.normalized(); 155 | Eigen::Vector3f normalizedExpectedMag = expectedMag.normalized(); 156 | 157 | float yawError = atan2(normalizedMag.y(), normalizedMag.x()) - atan2(normalizedExpectedMag.y(), normalizedExpectedMag.x()); 158 | 159 | const float MIN_YAW_ERROR = degToRad(0.01f); 160 | const float MAX_YAW_ERROR = degToRad(1.0f); 161 | const float CORRECTION_SCALE = 0.05f; 162 | 163 | if (fabs(yawError) > MIN_YAW_ERROR) { 164 | yawError = std::clamp(yawError, -MAX_YAW_ERROR, MAX_YAW_ERROR); 165 | float correctionFactor = CORRECTION_SCALE * (fabs(yawError) / MAX_YAW_ERROR); 166 | yawError *= correctionFactor; 167 | 168 | Eigen::AngleAxisf yawCorrection(yawError, Eigen::Vector3f::UnitZ()); 169 | attitude = (attitude * Eigen::Quaternionf(yawCorrection)).normalized(); 170 | } 171 | 172 | state(6) = attitude.w(); 173 | state.segment<3>(7) = attitude.vec(); 174 | } 175 | 176 | // 현재 상태 반환 함수 177 | Eigen::VectorXf EKF::getState() const { 178 | Eigen::VectorXf stateOut(10); 179 | 180 | stateOut.segment<3>(0) = state.segment<3>(0); 181 | stateOut.segment<3>(3) = state.segment<3>(3); 182 | stateOut(6) = state(6); 183 | stateOut.segment<3>(7) = state.segment<3>(7); 184 | 185 | return stateOut; 186 | } 187 | -------------------------------------------------------------------------------- /test_code/ekf.h: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #ifndef EKF_H 3 | #define EKF_H 4 | 5 | #include 6 | 7 | class EKF { 8 | public: 9 | EKF(); 10 | ~EKF(); 11 | 12 | void predict(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 13 | void updateWithGPS(const Eigen::Vector3f& gpsPos, const Eigen::Vector3f& gpsVel); 14 | void updateWithMag(const Eigen::Vector3f& mag); // 자기장 업데이트 함수 15 | Eigen::VectorXf getState() const; 16 | private: 17 | Eigen::VectorXf state; // 상태 벡터 (위치 3, 속도 3, 자세 4) 18 | Eigen::MatrixXf covariance; // 오차 공분산 행렬 19 | Eigen::MatrixXf processNoise; // 프로세스 노이즈 행렬 20 | Eigen::MatrixXf measurementNoise; // 측정 노이즈 행렬 21 | Eigen::MatrixXf jacobian; // Jacobian 행렬 22 | 23 | Eigen::Vector3f accelLast; // 마지막 가속도 값 24 | Eigen::Vector3f gyroLast; // 마지막 자이로 값 (저주파 필터에 사용) 25 | 26 | Eigen::Matrix3f quaternionToRotationMatrix(const Eigen::Quaternionf& q) const; 27 | void computeJacobian(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 28 | void predictState(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro, float dt); 29 | Eigen::Matrix3f skewSymmetric(const Eigen::Vector3f& v); 30 | 31 | Eigen::Vector3f lowPassFilter(const Eigen::Vector3f& input, const Eigen::Vector3f& last, float alpha); 32 | }; 33 | 34 | #endif // EKF_H -------------------------------------------------------------------------------- /test_code/flight_control.cpp: -------------------------------------------------------------------------------- 1 | #include "flight_control.h" 2 | #include "rc_input.h" 3 | #include "gps_sensor.h" 4 | #include "imu_sensor.h" 5 | #include 6 | #include // B115200 보드레이트 상수 정의를 위해 필요. 7 | 8 | // 모든 장치를 초기화하는 함수 9 | void flight_control_init() { 10 | // RC 초기화 11 | std::cout << "Initializing RC input..." << std::endl; 12 | initRC("/dev/ttyAMA0", B115200); 13 | 14 | // GPS 초기화 15 | std::cout << "Initializing GPS..." << std::endl; 16 | initGPS("/dev/ttyUSB1", B115200); 17 | 18 | // IMU 초기화 19 | std::cout << "Initializing IMU..." << std::endl; 20 | initIMU("/dev/ttyUSB0", B115200); 21 | 22 | std::cout << "Flight control system initialized." << std::endl; 23 | } 24 | 25 | -------------------------------------------------------------------------------- /test_code/flight_control.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_CONTROL_H 2 | #define FLIGHT_CONTROL_H 3 | 4 | // 모든 장치를 초기화하는 함수 5 | void flight_control_init(); 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /test_code/flight_mode.cpp: -------------------------------------------------------------------------------- 1 | #include "flight_mode.h" 2 | #include "rc_input.h" 3 | #include "motor_control.h" 4 | #include "gps_sensor.h" 5 | #include "udp_communication.h" // UDP 통신 관련 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // 수동 비행 모드 12 | void manualFlight(Eigen::VectorXf& state) { 13 | while (true) { 14 | // RC 입력을 읽어옵니다. 15 | RCInput rcInput = readRCInput(); 16 | 17 | // 각 목표 각도 및 스로틀 계산 18 | float targetRoll = rcInput.roll * 45.0; // 롤의 목표 각도 (±45도) 19 | float targetPitch = rcInput.pitch * 45.0; // 피치의 목표 각도 (±45도) 20 | float targetYaw = rcInput.yaw * 180.0; // 요의 목표 각도 (±180도) 21 | float targetThrottle = rcInput.throttle * 1000 + 1000; // 스로틀 값 계산 (1000 ~ 2000) 22 | 23 | // 모터 출력 계산 24 | float motorOutputs[4]; 25 | calculateMotorOutputs(state(6), state(7), state(8), targetRoll, targetPitch, targetYaw, targetThrottle, motorOutputs); 26 | 27 | // 모터 제어 신호 전달 28 | controlMotors(motorOutputs); 29 | 30 | // 일정 주기로 반복 31 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 (20ms) 32 | } 33 | } 34 | 35 | // 자동 비행 모드 36 | void autoFlight(Eigen::VectorXf& state) { 37 | while (true) { 38 | // GPS 센서 데이터를 읽어옵니다. 39 | GPSData gpsData = readGPSSensor(); 40 | 41 | // 목표 위치 계산 (목표는 미리 설정된 경도, 위도, 고도 등) 42 | Eigen::Vector3f targetPosition(37.7749, -122.4194, 100.0); // 예시로 특정 좌표와 고도 설정 43 | 44 | // 위치 오류 계산 45 | Eigen::Vector3f positionError = targetPosition - gpsData.position; 46 | 47 | // 위치 오류를 기반으로 모터 출력 계산 48 | float motorOutputs[4]; 49 | calculateMotorOutputs(state(6), state(7), state(8), positionError(0), positionError(1), positionError(2), 1500, motorOutputs); 50 | 51 | // 모터 제어 신호 전달 52 | controlMotors(motorOutputs); 53 | 54 | // 일정 주기로 반복 55 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 56 | } 57 | } 58 | 59 | // 고투(Goto) 비행 모드 60 | void gotoFlight(Eigen::VectorXf& state) { 61 | // GPS 목표 좌표를 받아서 설정 62 | Eigen::Vector3f targetPosition = getGotoTargetPosition(); 63 | 64 | while (true) { 65 | // GPS 센서 데이터를 읽어옵니다. 66 | GPSData gpsData = readGPSSensor(); 67 | 68 | // 목표 위치까지의 거리 계산 69 | Eigen::Vector3f positionError = targetPosition - gpsData.position; 70 | 71 | // 위치 오류를 기반으로 모터 출력 계산 72 | float motorOutputs[4]; 73 | calculateMotorOutputs(state(6), state(7), state(8), positionError(0), positionError(1), positionError(2), 1500, motorOutputs); 74 | 75 | // 모터 제어 신호 전달 76 | controlMotors(motorOutputs); 77 | 78 | // 목표 위치에 도달했는지 확인 79 | if (positionError.norm() < 0.5) { 80 | std::cout << "목표 지점에 도달했습니다!" << std::endl; 81 | break; 82 | } 83 | 84 | // 일정 주기로 반복 85 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz 주기로 실행 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /test_code/flight_mode.h: -------------------------------------------------------------------------------- 1 | #ifndef FLIGHT_MODE_H 2 | #define FLIGHT_MODE_H 3 | 4 | #include 5 | 6 | // 비행 모드 열거형 정의 7 | enum FlightMode { 8 | MANUAL, 9 | AUTO, 10 | GOTO 11 | }; 12 | 13 | // 수동 비행 모드 함수 14 | void manualFlight(Eigen::VectorXf& state); 15 | 16 | // 자동 비행 모드 함수 17 | void autoFlight(Eigen::VectorXf& state); 18 | 19 | // 고투(Goto) 비행 모드 함수 20 | void gotoFlight(Eigen::VectorXf& state); 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /test_code/gps_sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "gps_sensor.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | int serialPort = -1; // 전역 변수로 시리얼 포트 관리 11 | 12 | // 시리얼 포트 설정 함수 13 | void initGPS(const char* port, int baudRate) { 14 | serialPort = open(port, O_RDWR | O_NOCTTY | O_NDELAY); 15 | if (serialPort == -1) { 16 | perror("Unable to open serial port"); 17 | return; 18 | } 19 | 20 | struct termios options; 21 | tcgetattr(serialPort, &options); 22 | cfsetispeed(&options, baudRate); // Baud rate 설정 23 | cfsetospeed(&options, baudRate); 24 | options.c_cflag |= (CLOCAL | CREAD); 25 | options.c_cflag &= ~PARENB; // 패리티 비트 없음 26 | options.c_cflag &= ~CSTOPB; // 1 스톱 비트 27 | options.c_cflag &= ~CSIZE; 28 | options.c_cflag |= CS8; // 8 비트 데이터 비트 29 | options.c_cflag &= ~CRTSCTS; // RTS/CTS 흐름 제어 없음 30 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 캐노닉 모드 해제 31 | options.c_iflag &= ~(IXON | IXOFF | IXANY); // 소프트웨어 흐름 제어 없음 32 | options.c_oflag &= ~OPOST; // 원시 모드 33 | 34 | tcsetattr(serialPort, TCSANOW, &options); 35 | } 36 | 37 | // GPS 데이터 파싱 함수 38 | GPSData parseGpsData(const vector& data) { 39 | GPSData gpsData = {}; 40 | 41 | if (data.size() < 100) { 42 | return gpsData; // 데이터가 부족하면 빈 값 반환 43 | } 44 | 45 | uint8_t msgId = data[3]; 46 | uint16_t length = (data[5] << 8) | data[4]; // Length 필드 (2바이트) 47 | 48 | if (msgId == 0x07) { 49 | gpsData.numSV = data[29]; 50 | gpsData.longitude = (data[33] << 24) | (data[32] << 16) | (data[31] << 8) | data[30]; 51 | gpsData.latitude = (data[37] << 24) | (data[36] << 16) | (data[35] << 8) | data[34]; 52 | gpsData.altitude = (data[41] << 24) | (data[40] << 16) | (data[39] << 8) | data[38]; 53 | gpsData.gSpeed = (data[69] << 24) | (data[68] << 16) | (data[67] << 8) | data[66]; 54 | gpsData.velocityX = (data[57] << 24) | (data[56] << 16) | (data[55] << 8) | data[54]; 55 | gpsData.velocityY = (data[61] << 24) | (data[60] << 16) | (data[59] << 8) | data[58]; 56 | gpsData.velocityZ = (data[65] << 24) | (data[64] << 16) | (data[63] << 8) | data[62]; 57 | } 58 | 59 | return gpsData; 60 | } 61 | 62 | // GPS 데이터를 읽는 함수 63 | GPSData readGPS() { 64 | vector receivedData; 65 | uint8_t buffer[1024]; 66 | GPSData gpsData = {}; 67 | bool flag = false; // 파싱이 성공했는지 확인하는 플래그 68 | 69 | while (!flag) { 70 | int bytesRead = read(serialPort, buffer, sizeof(buffer)); 71 | if (bytesRead > 0) { 72 | receivedData.insert(receivedData.end(), buffer, buffer + bytesRead); 73 | 74 | // 메시지 파싱을 위한 루프 75 | while (true) { 76 | if (receivedData.size() < 5) { 77 | break; // 수신된 데이터가 충분하지 않으면 대기 78 | } 79 | 80 | // 헤더 찾기 81 | if (receivedData[0] == 0xB5 && receivedData[1] == 0x62 && receivedData[3] == 0x07) { 82 | uint16_t length = (receivedData[5] << 8) | receivedData[4]; // Length 필드 (2바이트) 83 | uint16_t totalMessageLength = length + 6 + 2; // Length + Header + Checksum 84 | 85 | if (receivedData.size() >= totalMessageLength) { 86 | gpsData = parseGpsData(vector(receivedData.begin(), receivedData.begin() + totalMessageLength)); 87 | flag = true; // 올바른 값이 파싱되면 플래그를 true로 설정 88 | 89 | // 처리 후 남은 데이터 관리 90 | receivedData.erase(receivedData.begin(), receivedData.begin() + totalMessageLength); 91 | } else { 92 | break; // 전체 메시지가 아직 수신되지 않음 93 | } 94 | } else { 95 | // 헤더가 잘못된 경우 첫 번째 바이트를 삭제 96 | receivedData.erase(receivedData.begin()); 97 | } 98 | } 99 | } else { 100 | // 수신된 데이터가 없을 때 usleep으로 대기 (예: 100ms 대기) 101 | usleep(100000); // 100,000 마이크로초 = 100ms 102 | } 103 | } 104 | 105 | // flag가 true일 때 gpsData를 반환 106 | return gpsData; 107 | } -------------------------------------------------------------------------------- /test_code/gps_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef GPS_SENSOR_H 2 | #define GPS_SENSOR_H 3 | 4 | #include 5 | #include // int64_t를 사용하기 위한 헤더 6 | 7 | // GPSData 구조체 정의 8 | struct GPSData { 9 | int64_t longitude; // 경도 (1e-7 도 단위로 표현 가능) 10 | int64_t latitude; // 위도 (1e-7 도 단위로 표현 가능) 11 | int64_t altitude; // 고도 (더 큰 범위 지원) 12 | int64_t gSpeed; // 지상 속도 (더 큰 범위 지원) 13 | uint8_t numSV; // 위성 수 14 | int64_t velocityX; // NED 북 방향 속도 (mm/s, 더 큰 범위 지원) 15 | int64_t velocityY; // NED 동 방향 속도 (mm/s, 더 큰 범위 지원) 16 | int64_t velocityZ; // NED 하강 방향 속도 (mm/s, 더 큰 범위 지원) 17 | }; 18 | 19 | // GPS 초기화 함수 20 | void initGPS(const char* port, int baudRate); 21 | 22 | // GPS 데이터를 읽는 함수 23 | GPSData readGPS(); 24 | 25 | #endif -------------------------------------------------------------------------------- /test_code/imu_calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "imu_calibration.h" 2 | #include "imu_sensor.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #define _USE_MATH_DEFINES 12 | 13 | IMUCalibrationData calibrateIMU() { 14 | initIMU("/dev/ttyUSB0", B115200); 15 | IMUCalibrationData calibrationData; 16 | IMUData imuData; 17 | std::vector accelX_samples, accelY_samples, accelZ_samples; 18 | std::vector gyroX_samples, gyroY_samples, gyroZ_samples; 19 | std::vector roll_samples, pitch_samples, yaw_samples; 20 | 21 | // 보완 필터 상수 22 | const float alpha = 0.98f; 23 | 24 | // 초기 각도 (static으로 선언하여 이전 값을 유지) 25 | float yaw = 0.0f; 26 | 27 | // 시작 시간 설정 28 | auto start_time = std::chrono::high_resolution_clock::now(); 29 | int sample_count = 0; 30 | 31 | // 100개의 IMU 데이터 샘플 수집 32 | while (sample_count < 100) { 33 | IMUData imuData = readIMU(); // IMU 데이터 읽기 34 | 35 | // 각 축의 가속도 데이터를 샘플로 수집 36 | roll_samples.push_back(imuData.roll_angle); 37 | pitch_samples.push_back(imuData.pitch_angle); 38 | yaw_samples.push_back(imuData.yaw_angle); 39 | 40 | // 각 축의 자이로스코프 데이터를 샘플로 수집 41 | gyroX_samples.push_back(imuData.gyroX); 42 | gyroY_samples.push_back(imuData.gyroY); 43 | gyroZ_samples.push_back(imuData.gyroZ); 44 | 45 | usleep(25000); // (100000)샘플 간 시간 간격 설정 (100ms) 46 | sample_count++; 47 | } 48 | 49 | // 평균값 계산하여 오프셋 설정 50 | if (!roll_samples.empty()) { 51 | calibrationData.offsetroll = std::accumulate(roll_samples.begin(), roll_samples.end(), 0.0f) / roll_samples.size(); 52 | } 53 | 54 | if (!pitch_samples.empty()) { 55 | calibrationData.offsetpitch = std::accumulate(pitch_samples.begin(), pitch_samples.end(), 0.0f) / pitch_samples.size(); 56 | } 57 | 58 | if (!yaw_samples.empty()) { 59 | calibrationData.offsetyaw = std::accumulate(yaw_samples.begin(), yaw_samples.end(), 0.0f) / yaw_samples.size(); 60 | } 61 | 62 | // 자이로스코프 평균값도 필요하다면 아래와 같이 추가할 수 있습니다. 63 | if (!gyroX_samples.empty()) { 64 | calibrationData.offsetGyroX = std::accumulate(gyroX_samples.begin(), gyroX_samples.end(), 0.0f) / gyroX_samples.size(); 65 | } 66 | 67 | if (!gyroY_samples.empty()) { 68 | calibrationData.offsetGyroY = std::accumulate(gyroY_samples.begin(), gyroY_samples.end(), 0.0f) / gyroY_samples.size(); 69 | } 70 | 71 | if (!gyroZ_samples.empty()) { 72 | calibrationData.offsetGyroZ = std::accumulate(gyroZ_samples.begin(), gyroZ_samples.end(), 0.0f) / gyroZ_samples.size(); 73 | } 74 | 75 | std::cout << "Calibration Complete:\n"; 76 | 77 | std::cout << "Accelerometer Offsets:\n"; 78 | std::cout << "roll: " << calibrationData.offsetroll << "\n"; 79 | std::cout << "pitch: " << calibrationData.offsetpitch << "\n"; 80 | std::cout << "yaw: " << calibrationData.offsetyaw << "\n"; 81 | 82 | std::cout << "Gyroscope Offsets:\n"; 83 | std::cout << "Offset Gyro X: " << calibrationData.offsetGyroX << "\n"; 84 | std::cout << "Offset Gyro Y: " << calibrationData.offsetGyroY << "\n"; 85 | std::cout << "Offset Gyro Z: " << calibrationData.offsetGyroZ << "\n"; 86 | 87 | // 보정된 자이로스코프 데이터 88 | float gyroX_calibrated = imuData.gyroX - calibrationData.offsetGyroX; 89 | float gyroY_calibrated = imuData.gyroY - calibrationData.offsetGyroY; 90 | float gyroZ_calibrated = imuData.gyroZ - calibrationData.offsetGyroZ; 91 | 92 | std::cout << "target angle:\n"; 93 | std::cout << "roll: " << calibrationData.offsetroll << "\n"; 94 | std::cout << "pitch: " << calibrationData.offsetpitch << "\n"; 95 | std::cout << "yaw: " << calibrationData.offsetyaw << "\n"; 96 | 97 | return calibrationData; 98 | } -------------------------------------------------------------------------------- /test_code/imu_calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_CALIBRATION_H 2 | #define IMU_CALIBRATION_H 3 | 4 | #include "imu_sensor.h" 5 | 6 | struct IMUCalibrationData { 7 | float offsetX; 8 | float offsetY; 9 | float offsetZ; 10 | float offsetroll; 11 | float offsetpitch; 12 | float offsetyaw; 13 | float offsetGyroX; 14 | float offsetGyroY; 15 | float offsetGyroZ; 16 | float roll_acc; 17 | float pitch_acc; 18 | float yaw; 19 | float accelX_calibrated; 20 | float accelY_calibrated; 21 | float accelZ_calibrated; 22 | float gyroX_calibrated; 23 | float gyroY_calibrated; 24 | float gyroZ_calibrated; 25 | }; 26 | 27 | IMUCalibrationData calibrateIMU(); 28 | 29 | #endif -------------------------------------------------------------------------------- /test_code/imu_sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "imu_sensor.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define BUFFER_SIZE 1024 // 버퍼 크기 정의 16 | #define COMMAND_SIZE 100 // 명령어 크기 정의 17 | #define CRC_SIZE 4 // CRC 크기 정의 18 | 19 | // 시그널 플래그 20 | static int serial_port; 21 | static long previous_timestamp = 0; // 이전 타임스탬프 저장 변수 22 | 23 | // CRC 계산 함수 (데이터 유효성 검증에 사용) 24 | static unsigned short calculateCRC(const unsigned char* data, unsigned int length) { 25 | unsigned short crc = 0; 26 | for (unsigned int i = 0; i < length; i++) { 27 | crc = (unsigned char)(crc >> 8) | (crc << 8); 28 | crc ^= data[i]; 29 | crc ^= (unsigned char)(crc & 0xff) >> 4; 30 | crc ^= crc << 12; 31 | crc ^= (crc & 0x00ff) << 5; 32 | } 33 | return crc; 34 | } 35 | 36 | // 시리얼 포트 설정 함수 37 | static int configureSerial(const std::string& port, int baudrate) { 38 | serial_port = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); // 포트 열기 39 | if (serial_port == -1) { // 실패 시 에러 메시지 출력 40 | perror("Failed to open serial port"); 41 | return -1; 42 | } 43 | 44 | struct termios options; 45 | tcgetattr(serial_port, &options); // 현재 포트 설정 가져오기 46 | cfsetispeed(&options, baudrate); // 입력 보드레이트 설정 47 | cfsetospeed(&options, baudrate); // 출력 보드레이트 설정 48 | options.c_cflag |= (CLOCAL | CREAD); // 포트 활성화 49 | options.c_cflag &= ~PARENB; // 패리티 비트 비활성화 50 | options.c_cflag &= ~CSTOPB; // 스톱 비트 비활성화 51 | options.c_cflag &= ~CSIZE; // 데이터 크기 설정 52 | options.c_cflag |= CS8; // 8비트 데이터 설정 53 | options.c_iflag &= ~(IXON | IXOFF | IXANY); // 소프트웨어 흐름 제어 비활성화 54 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 표준 모드 비활성화 55 | options.c_oflag &= ~OPOST; // 출력 처리 비활성화 56 | tcsetattr(serial_port, TCSANOW, &options); // 설정 즉시 적용 57 | 58 | return serial_port; 59 | } 60 | 61 | // IMU 초기화 함수 62 | void initIMU(const std::string& port, int baudRate) { 63 | // 시리얼 포트 설정 실패 시 예외 발생 64 | if (configureSerial(port, baudRate) == -1) { 65 | throw std::runtime_error("Unable to configure serial port"); 66 | } 67 | } 68 | 69 | // IMU 데이터 요청 함수 70 | void sendIMURequest() { 71 | char command[COMMAND_SIZE]; 72 | snprintf(command, sizeof(command), "$VNRRG,20"); // IMU 데이터 요청 명령어 73 | unsigned short crc = calculateCRC((unsigned char *)command + 1, strlen(command) - 1); // CRC 계산 74 | snprintf(command, sizeof(command), "$VNRRG,20*%04X\r\n", crc); // 명령어 포맷팅 75 | write(serial_port, command, strlen(command)); // 시리얼 포트에 쓰기 76 | } 77 | 78 | IMUData readIMU() { 79 | char response[256]; // 응답 데이터를 저장할 배열 80 | char buffer[BUFFER_SIZE]; // IMU 데이터 저장 버퍼 81 | int buffer_index = 0; // 버퍼 인덱스 초기화 82 | IMUData imuData = {}; // IMU 데이터 구조체 초기화 83 | 84 | // 원하는 데이터가 올 때까지 대기 85 | bool data_received = false; // 데이터 수신 여부 플래그 86 | while (!data_received) { 87 | 88 | sendIMURequest(); // IMU 데이터 요청 89 | usleep(25000); // (100)100ms 대기 (데이터 반환 주기 설정) 90 | 91 | int bytes_read = read(serial_port, response, sizeof(response) - 1); 92 | if (bytes_read > 0) { 93 | response[bytes_read] = '\0'; // 응답 데이터 문자열 종료 94 | 95 | // 버퍼 오버플로우 방지 96 | if (buffer_index + bytes_read < BUFFER_SIZE) { 97 | memcpy(buffer + buffer_index, response, bytes_read); // 버퍼에 데이터 복사 98 | buffer_index += bytes_read; // 인덱스 증가 99 | } else { 100 | //fprintf(stderr, "Buffer overflow!\n"); // 버퍼 오버플로우 에러 메시지 출력 101 | buffer_index = 0; // 인덱스 초기화 102 | } 103 | 104 | // 한 줄씩 데이터 처리 105 | buffer[buffer_index] = '\0'; // 버퍼 종료 문자 추가 106 | char* line_start = buffer; 107 | char* line_end; 108 | char* last_line = NULL; 109 | 110 | // 줄 단위로 데이터를 분리하고 마지막 줄 찾기 111 | while ((line_end = strchr(line_start, '\n')) != NULL) { 112 | *line_end = '\0'; // 줄 종료 문자 삽입 113 | if (strncmp(line_start, "$VNRRG", 6) == 0) { // $VNRRG로 시작하는 줄 찾기 114 | last_line = line_start; // 마지막 줄로 저장 115 | data_received = true; // 데이터 수신 플래그 설정 116 | } 117 | line_start = line_end + 1; 118 | } 119 | 120 | // 유효한 데이터가 있으면 파싱 시작 121 | if (last_line) { 122 | char* end_of_data = strchr(last_line, '*'); // CRC 확인을 위해 '*' 문자 찾기 123 | if (end_of_data) { 124 | *end_of_data = '\0'; // CRC 제외한 데이터로 자르기 125 | 126 | // ','를 기준으로 데이터를 분리 127 | std::vector parts; 128 | std::istringstream ss(last_line); 129 | std::string token; 130 | 131 | while (std::getline(ss, token, ',')) { 132 | parts.push_back(token); 133 | } 134 | 135 | // 10개의 데이터가 존재할 때만 파싱 진행 136 | if (parts.size() >= 11) { 137 | // CRC 계산 138 | unsigned short received_crc = std::stoi(end_of_data + 1, nullptr, 16); // 수신한 CRC 139 | unsigned short calculated_crc = calculateCRC((unsigned char *)last_line + 1, strlen(last_line) - 1); // 계산된 CRC 140 | 141 | if (received_crc == calculated_crc) { // CRC가 맞으면 142 | imuData.roll_angle = std::stof(parts[3]); // roll 각도 143 | imuData.pitch_angle = std::stof(parts[2]); // pitch 각도 144 | imuData.yaw_angle = std::stof(parts[1]); // yaw 각도 145 | imuData.gyroX = std::stof(parts[6]); // X축 자이로스코프 146 | imuData.gyroY = std::stof(parts[7]); // Y축 자이로스코프 147 | imuData.gyroZ = std::stof(parts[8]); // Z축 자이로스코프 148 | 149 | // 타임스탬프 계산 150 | struct timeval current_time; 151 | gettimeofday(¤t_time, NULL); // 현재 시간 가져오기 152 | imuData.timestamp = (current_time.tv_sec * 1000.0) + (current_time.tv_usec / 1000.0); // 밀리초 단위 타임스탬프 계산 153 | imuData.elapsed_time = imuData.timestamp - previous_timestamp; // 경과 시간 계산 154 | previous_timestamp = imuData.timestamp; // 이전 타임스탬프 갱신 155 | } else { 156 | fprintf(stderr, "CRC mismatch: Received: %04X, Calculated: %04X\n", received_crc, calculated_crc); // CRC 불일치 에러 메시지 출력 157 | } 158 | } else { 159 | fprintf(stderr, "Invalid data format\n"); // 잘못된 데이터 형식 에러 메시지 출력 160 | } 161 | } 162 | } 163 | } 164 | } 165 | 166 | return imuData; // IMU 데이터 반환 167 | } -------------------------------------------------------------------------------- /test_code/imu_sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_SENSOR_H 2 | #define IMU_SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | // IMU 데이터를 저장하는 구조체 8 | struct IMUData { 9 | float accelX; // X축 가속도 10 | float accelY; // Y축 가속도 11 | float accelZ; // Z축 가속도 12 | float gyroX; // X축 자이로스코프 13 | float gyroY; // Y축 자이로스코프 14 | float gyroZ; // Z축 자이로스코프 15 | float magX; // X축 자기장 16 | float magY; // Y축 자기장 17 | float magZ; // Z축 자기장 18 | double timestamp; // 타임스탬프 19 | double elapsed_time; // 경과 시간 20 | float roll_angle; 21 | float pitch_angle; 22 | float yaw_angle; 23 | }; 24 | 25 | void initIMU(const std::string& port, int baudRate); 26 | IMUData readIMU(); 27 | #endif // IMU_SENSOR_H -------------------------------------------------------------------------------- /test_code/main.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_estimator.h" 2 | #include "flight_mode.h" 3 | #include "flight_control.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | int main() { 10 | // 루프 실행 주기 설정 (100ms) 11 | const std::chrono::milliseconds loopDuration(100); 12 | 13 | // 비행 제어 시스템 초기화 (RC, GPS, IMU 등) 14 | flight_control_init(); 15 | 16 | // EKF 기반 자세 추정 클래스 생성 17 | PoseEstimator poseEstimator; 18 | std::this_thread::sleep_for(loopDuration); 19 | 20 | 21 | // CSV 파일 열기 22 | std::ofstream csvFile("current_pose.csv"); 23 | if (!csvFile.is_open()) { 24 | std::cerr << "CSV 파일을 열 수 없습니다." << std::endl; 25 | return 1; // 파일 열기 실패 시 프로그램 종료 26 | } 27 | 28 | // CSV 파일 헤더 작성 29 | csvFile << "X,Y,Z,Roll,Pitch,Yaw" << std::endl; 30 | 31 | // 메인 루프 32 | while (true) { 33 | // 100ms 주기로 상태 값을 가져옴 34 | Eigen::VectorXf state = poseEstimator.getPose(); 35 | 36 | // 자세 추정값 출력 (x, y, z 위치와 roll, pitch, yaw만 출력) 37 | std::cout << std::fixed << std::setprecision(7); // 소수점 7자리까지 표시 38 | std::cout << "Current Pose: " 39 | << state(0) << " " 40 | << state(1) << " " 41 | << state(2) << " " 42 | << state(6) << " " 43 | << state(7) << " " 44 | << state(8) << std::endl; 45 | 46 | // 100ms 동안 대기 47 | std::this_thread::sleep_for(loopDuration); 48 | } 49 | 50 | // CSV 파일 닫기 51 | csvFile.close(); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /test_code/os_api.cpp: -------------------------------------------------------------------------------- 1 | // 운영 체제에 맞춘 API 호출 코드 (예: POSIX 또는 RTOS용 API) -------------------------------------------------------------------------------- /test_code/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "rc_input.h" 10 | #include "imu_sensor.h" 11 | #include "imu_calibration.h" 12 | #include 13 | #include 14 | #include 15 | 16 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 17 | #define MODE1 0x00 // 모드1 레지스터 18 | #define PRESCALE 0xFE // 프리스케일 레지스터 19 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 20 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 21 | 22 | const int RC_MIN = 172; 23 | const int RC_MAX = 1811; 24 | const int RC_MID = 991; 25 | const int PWM_MIN = 210; 26 | const int PWM_MAX = 405; 27 | const int MAX_ADJUSTMENT = 15; // 각 제어 입력의 최대 PWM 조정 값 28 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 29 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 30 | const int LOOP_DELAY_US = 26000; // 주기적인 대기 시간 (10ms) 31 | const float MAX_ANGLE = 90.0f; // 최대 각도 (예시) 32 | const float TOLERANCE_ROLL = 0.05f * MAX_ANGLE; // 롤 허용 오차 (0.45도) 33 | const float TOLERANCE_PITCH = 0.01f * MAX_ANGLE; // 피치 허용 오차 (0.45도) 34 | 35 | // IMUData 36 | IMUData imuData; 37 | 38 | class PCA9685 { 39 | public: 40 | PCA9685(int address = PCA9685_ADDR) { 41 | char filename[20]; 42 | snprintf(filename, 19, "/dev/i2c-1"); 43 | fd = open(filename, O_RDWR); 44 | if (fd < 0) { 45 | std::cerr << "Failed to open the i2c bus" << std::endl; 46 | exit(1); 47 | } 48 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 49 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 50 | exit(1); 51 | } 52 | reset(); 53 | setPWMFreq(50); // 모터 제어를 위해 주파수를 50Hz로 설정e 54 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 55 | } 56 | 57 | ~PCA9685() { 58 | stopAllMotors(); // 종료 시 모든 모터를 정지 59 | if (fd >= 0) { 60 | close(fd); 61 | } 62 | } 63 | 64 | void setPWM(int channel, int on, int off) { 65 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 66 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 67 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 68 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 69 | } 70 | 71 | void setMotorSpeed(int channel, int pwm_value) { 72 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 73 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 74 | return; 75 | } 76 | setPWM(channel, 0, pwm_value); 77 | } 78 | 79 | private: 80 | int fd; 81 | 82 | void reset() { 83 | writeRegister(MODE1, 0x00); 84 | } 85 | 86 | void setPWMFreq(int freq) { 87 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 88 | uint8_t oldmode = readRegister(MODE1); 89 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 90 | writeRegister(MODE1, newmode); 91 | writeRegister(PRESCALE, prescale); 92 | writeRegister(MODE1, oldmode); 93 | usleep(5000); 94 | writeRegister(MODE1, oldmode | 0xA1); 95 | } 96 | 97 | void writeRegister(uint8_t reg, uint8_t value) { 98 | uint8_t buffer[2] = {reg, value}; 99 | int retries = 0; 100 | while (write(fd, buffer, 2) != 2) { 101 | if (++retries >= I2C_RETRY_LIMIT) { 102 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 103 | exit(1); 104 | } 105 | usleep(1000); // 1ms 대기 후 재시도 106 | } 107 | } 108 | 109 | uint8_t readRegister(uint8_t reg) { 110 | int retries = 0; 111 | while (write(fd, ®, 1) != 1) { 112 | if (++retries >= I2C_RETRY_LIMIT) { 113 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 114 | exit(1); 115 | } 116 | usleep(1000); 117 | } 118 | uint8_t value; 119 | if (read(fd, &value, 1) != 1) { 120 | std::cerr << "Failed to read from the i2c bus" << std::endl; 121 | exit(1); 122 | } 123 | return value; 124 | } 125 | 126 | void initializeMotors() { 127 | for (int i = 0; i < 4; ++i) { 128 | setMotorSpeed(i, SAFE_PWM); 129 | } 130 | } 131 | 132 | void stopAllMotors() { 133 | for (int i = 0; i < 4; ++i) { 134 | setMotorSpeed(i, SAFE_PWM); 135 | } 136 | std::cout << "All motors stopped safely." << std::endl; 137 | } 138 | }; 139 | 140 | struct PIDController { 141 | float kp, ki, kd; // PID 게인 142 | float prev_error; // 이전 오차값 (미분 항 계산용) 143 | float integral; // 적분값 144 | float integral_limit; // 적분값 제한 145 | float output_limit; // 출력값 제한 146 | float prev_derivative; // 이전 미분값 (저역 필터 계산용) 147 | float alpha; // 저역 필터 계수 (0.0 ~ 1.0) 148 | 149 | PIDController(float p, float i, float d, float i_limit = 10.0f, 150 | float out_limit = 400.0f, float filter_alpha = 0.1f) 151 | : kp(p), ki(i), kd(d), prev_error(0.0f), integral(0.0f), 152 | integral_limit(i_limit), output_limit(out_limit), prev_derivative(0.0f), alpha(filter_alpha) {} 153 | 154 | void reset() { 155 | prev_error = 0.0f; 156 | integral = 0.0f; 157 | prev_derivative = 0.0f; 158 | } 159 | 160 | float calculate(float setpoint, float measurement, float dt) { 161 | // 오차 계산 162 | float error = setpoint - measurement; 163 | 164 | // 적분 항 계산 및 제한 165 | integral += error * dt; 166 | integral = std::clamp(integral, -integral_limit, integral_limit); 167 | 168 | // 미분 항 계산 (저역 필터 적용) 169 | float raw_derivative = (error - prev_error) / dt; 170 | float filtered_derivative = alpha * raw_derivative + (1 - alpha) * prev_derivative; 171 | prev_derivative = filtered_derivative; 172 | 173 | prev_error = error; 174 | 175 | // PID 출력 계산 176 | float p_term = kp * error; 177 | float i_term = ki * integral; 178 | float d_term = kd * filtered_derivative; 179 | 180 | float output = p_term + i_term + d_term; 181 | 182 | // 출력 제한 183 | return std::clamp(output, -output_limit, output_limit); 184 | } 185 | }; 186 | 187 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 188 | double mapThrottle(int value) { 189 | if (value <= RC_MIN) return 0.0; 190 | if (value >= RC_MAX) return 1.0; 191 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 192 | } 193 | 194 | // 제어 입력(에일러론, 엘리베이터, 러더)을 -1.0 ~ 1.0 범위로 매핑하는 함수 195 | double mapControlInput(int value) { 196 | if (value < RC_MIN || value > RC_MAX) { 197 | return 0.0; 198 | } 199 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 200 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 201 | return 0.0; 202 | } 203 | 204 | int computeThrottlePWM(double throttle_normalized) { 205 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 206 | } 207 | 208 | int computeAdjustment(double control_normalized) { 209 | return static_cast(control_normalized * MAX_ADJUSTMENT); 210 | } 211 | 212 | int clamp(int value, int min_value, int max_value) { 213 | return value < min_value ? min_value : (value > max_value ? max_value : value); 214 | } 215 | 216 | int main() { 217 | PCA9685 pca9685; 218 | initRC("/dev/ttyAMA0", B115200); // RC 입력 초기화 219 | initIMU("/dev/ttyUSB0", B115200); // IMU 초기화 220 | 221 | // 캘리브레이션 오프셋 변수 선언 222 | float offsetX = 0.0f, offsetY = 0.0f, offsetZ = 0.0f; 223 | float offsetGyroX = 0.0f, offsetGyroY = 0.0f, offsetGyroZ = 0.0f; 224 | float stlPitch = 0.0f, stlRoll = 0.0f; 225 | 226 | try { 227 | // 캘리브레이션 수행 228 | IMUCalibrationData calibrationData = calibrateIMU(); 229 | 230 | // 캘리브레이션 완료 후 데이터 출력 231 | std::cout << "IMU Calibration offsets are set." << std::endl; 232 | 233 | // 캘리브레이션 데이터로 오프셋 값 설정 234 | offsetX = calibrationData.offsetX; 235 | offsetY = calibrationData.offsetY; 236 | offsetZ = calibrationData.offsetZ; 237 | 238 | offsetGyroX = calibrationData.offsetGyroX; 239 | offsetGyroY = calibrationData.offsetGyroY; 240 | offsetGyroZ = calibrationData.offsetGyroZ; 241 | 242 | // // 목표 각도는 0도로 설정하여 드론이 수평을 유지하도록 합니다. 243 | stlRoll = calibrationData.stlRoll; 244 | stlPitch = calibrationData.stlPitch; 245 | 246 | } catch (const std::exception &e) { 247 | std::cerr << "Error during calibration: " << e.what() << std::endl; 248 | return -1; 249 | } 250 | 251 | float targetRoll = stlRoll; 252 | float targetPitch = stlPitch; 253 | 254 | // PIDController 초기화 (output_limit을 MAX_ADJUSTMENT로 설정) 255 | PIDController rollPID(3.5, 0.5, 0.2, 0.0, 10.0f, MAX_ADJUSTMENT); // Roll PID 256 | PIDController pitchPID(0.5, 0.1, 0.1, 0.0, 10.0f, MAX_ADJUSTMENT); // Pitch PID 257 | PIDController yawPID(0.5, 0.0, 0.05, 0.0, 10.0f, MAX_ADJUSTMENT); // Yaw PID 258 | 259 | float currentRoll = 0.0f; 260 | float currentPitch = 0.0f; 261 | 262 | // 루프 주기 시간 측정을 위한 변수 263 | auto previousTime = std::chrono::steady_clock::now(); 264 | 265 | while (true) { 266 | float dt = 0.025f; 267 | 268 | int throttle_value = readRCChannel(3); // 채널 3에서 스로틀 값 읽기 269 | int aileron_value = readRCChannel(1); // 채널 1에서 에일러론 값 읽기 270 | int elevator_value = readRCChannel(2); // 채널 2에서 엘리베이터 값 읽기 271 | int rudder_value = readRCChannel(4); // 채널 4에서 러더 값 읽기 272 | 273 | double throttle_normalized = mapThrottle(throttle_value); 274 | double aileron_normalized = mapControlInput(aileron_value); 275 | double elevator_normalized = mapControlInput(elevator_value); 276 | double rudder_normalized = mapControlInput(rudder_value); 277 | 278 | // IMU 데이터 읽기 279 | IMUData imuData = readIMU(); 280 | 281 | // 가속도계 데이터 보정 282 | float correctedAccelX = imuData.accelX - offsetX; 283 | float correctedAccelY = imuData.accelY - offsetY; 284 | float correctedAccelZ = imuData.accelZ - offsetZ; 285 | 286 | float correctedGyroZ = imuData.gyroZ - offsetGyroZ; 287 | 288 | float accelRoll = atan2(imuData.accelY, imuData.accelZ); 289 | float accelPitch = atan2(-correctedAccelX, sqrt(correctedAccelY * correctedAccelY + correctedAccelZ * correctedAccelZ)); 290 | 291 | // Yaw PID 컨트롤러에 보정된 자이로스코프 데이터 사용 292 | int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 293 | 294 | 295 | // 수평 상태 목표 각도 적용 296 | int roll_adj = rollPID.calculate(targetRoll, accelRoll, dt); 297 | int pitch_adj = pitchPID.calculate(targetPitch, accelPitch, dt); 298 | 299 | // 총 조정값 계산 300 | int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 301 | int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 302 | 303 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 304 | 305 | int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 306 | 307 | if (throttle_PWM <= PWM_MIN) { 308 | // 스로틀 값이 최소값 이하일 경우 모든 모터 정지 309 | motor1_PWM = PWM_MIN; 310 | motor2_PWM = PWM_MIN; 311 | motor3_PWM = PWM_MIN; 312 | motor4_PWM = PWM_MIN; 313 | } else { 314 | 315 | // 모터 조정값 계산 316 | int motor1_adj = + aileron_adj_total - elevator_adj_total + yaw_adj; 317 | int motor2_adj = - aileron_adj_total + elevator_adj_total + yaw_adj; 318 | int motor3_adj = - aileron_adj_total - elevator_adj_total - yaw_adj; 319 | int motor4_adj = + aileron_adj_total + elevator_adj_total - yaw_adj; 320 | 321 | // 조정값을 모터 PWM에 적용 322 | motor1_PWM = throttle_PWM + motor1_adj; 323 | motor2_PWM = throttle_PWM + motor2_adj; 324 | motor3_PWM = throttle_PWM + motor3_adj; 325 | motor4_PWM = throttle_PWM + motor4_adj; 326 | 327 | // 모터 PWM이 유효한 범위 내에 있는지 확인 328 | motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 329 | motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 330 | motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 331 | motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 332 | } 333 | 334 | // 모터에 PWM 값 설정 335 | pca9685.setMotorSpeed(0, motor1_PWM); 336 | pca9685.setMotorSpeed(1, motor2_PWM); 337 | pca9685.setMotorSpeed(2, motor3_PWM); 338 | pca9685.setMotorSpeed(3, motor4_PWM); 339 | std::cout << "\rThrottle PWM: " << throttle_PWM 340 | << " Motor1: " << motor1_PWM 341 | << " Motor2: " << motor2_PWM 342 | << " Motor3: " << motor3_PWM 343 | << " Motor4: " << motor4_PWM < 5 | 6 | #define PCA9685_ADDR 0x40 7 | #define MODE1 0x00 8 | #define PRESCALE 0xFE 9 | #define LED0_ON_L 0x06 10 | #define LED0_OFF_L 0x08 11 | 12 | class PCA9685 { 13 | public: 14 | PCA9685(int address = PCA9685_ADDR); 15 | ~PCA9685(); 16 | void setMotorSpeed(int channel, int pwm_value); 17 | 18 | private: 19 | int fd; 20 | void reset(); 21 | void setPWMFreq(int freq); 22 | void setPWM(int channel, int on, int off); 23 | void writeRegister(uint8_t reg, uint8_t value); 24 | uint8_t readRegister(uint8_t reg); 25 | void initializeMotors(); 26 | void stopAllMotors(); 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /test_code/pose_estimator.cpp: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #include "pose_estimator.h" 3 | #include "ekf.h" 4 | #include "imu_sensor.h" 5 | #include "gps_sensor.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | Eigen::Vector3f quaternionToEuler(const Eigen::Quaternionf& q) { 13 | float ysqr = q.y() * q.y(); 14 | 15 | float t0 = +2.0f * (q.w() * q.x() + q.y() * q.z()); 16 | float t1 = +1.0f - 2.0f * (q.x() * q.x() + ysqr); 17 | float roll = std::atan2(t0, t1); 18 | 19 | float t2 = +2.0f * (q.w() * q.y() - q.z() * q.x()); 20 | t2 = std::clamp(t2, -1.0f, 1.0f); 21 | float pitch = std::asin(t2); 22 | 23 | float t3 = +2.0f * (q.w() * q.z() + q.x() * q.y()); 24 | float t4 = +1.0f - 2.0f * (ysqr + q.z() * q.z()); 25 | float yaw = std::atan2(t3, t4); 26 | 27 | return Eigen::Vector3f(roll, pitch, yaw) * (180.0f / M_PI); 28 | } 29 | 30 | // PoseEstimator 생성자 31 | PoseEstimator::PoseEstimator() : ekf(), running(true) { 32 | currentState = Eigen::VectorXf::Zero(16); 33 | imuAccel = Eigen::Vector3f::Zero(); 34 | imuGyro = Eigen::Vector3f::Zero(); 35 | imuMag = Eigen::Vector3f::Zero(); 36 | gpsPos = Eigen::Vector3f::Zero(); 37 | gpsVel = Eigen::Vector3f::Zero(); 38 | gyroOffset = Eigen::Vector3f::Zero(); 39 | 40 | imuThread = std::thread(&PoseEstimator::processIMU, this); 41 | gpsThread = std::thread(&PoseEstimator::processGPS, this); 42 | estimationThread = std::thread(&PoseEstimator::calculatePose, this); 43 | } 44 | 45 | // PoseEstimator 소멸자 46 | PoseEstimator::~PoseEstimator() { 47 | running = false; 48 | if (estimationThread.joinable()) { 49 | estimationThread.join(); 50 | } 51 | if (imuThread.joinable()) { 52 | imuThread.join(); 53 | } 54 | if (gpsThread.joinable()) { 55 | gpsThread.join(); 56 | } 57 | } 58 | 59 | // 자이로 캘리브레이션 함수 60 | void PoseEstimator::calibrateGyro() { 61 | int sampleCount = 50; 62 | Eigen::Vector3f gyroSum = Eigen::Vector3f::Zero(); 63 | for (int i = 0; i < sampleCount; ++i) { 64 | IMUData imuData = readIMU(); 65 | gyroSum += Eigen::Vector3f(imuData.gyroX, imuData.gyroY, imuData.gyroZ); 66 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 67 | } 68 | gyroOffset = gyroSum / sampleCount; 69 | isGyroCalibrated = true; 70 | } 71 | 72 | // 포즈 계산 함수 73 | void PoseEstimator::calculatePose() { 74 | while (running) { 75 | float dt = 0.01f; 76 | 77 | // 유효하지 않은 IMU 데이터가 감지되면 대체 값으로 초기화 78 | if (imuAccel.hasNaN() || imuGyro.hasNaN() || imuMag.hasNaN()) { 79 | std::cerr << "Invalid IMU data detected, using last valid data" << std::endl; 80 | imuAccel.setZero(); 81 | imuGyro.setZero(); 82 | imuMag.setZero(); 83 | } 84 | 85 | // EKF 예측 및 자기장 업데이트 단계 실행 86 | ekf.predict(imuAccel, imuGyro, dt); 87 | // ekf.updateWithMag(imuMag); 88 | ekf.updateWithGPS(gpsPos ,gpsVel); 89 | 90 | // 현재 상태를 보호된 상태로 업데이트 91 | { 92 | std::lock_guard lock(poseMutex); 93 | currentState = ekf.getState(); 94 | } 95 | 96 | // 계산 주기 설정 (100ms) 97 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 98 | } 99 | } 100 | 101 | // IMU 데이터 처리 함수 102 | void PoseEstimator::processIMU() { 103 | // if (!isGyroCalibrated) { 104 | // calibrateGyro(); // 자이로 캘리브레이션 수행 105 | // } 106 | 107 | while (running) { 108 | IMUData imuData = readIMU(); // IMU 센서에서 데이터 읽기 109 | Eigen::Vector3f newAccel = Eigen::Vector3f(imuData.accelX, imuData.accelY, imuData.accelZ); 110 | Eigen::Vector3f newGyro = Eigen::Vector3f(imuData.gyroX, imuData.gyroY, imuData.gyroZ) - gyroOffset; 111 | Eigen::Vector3f newMag = Eigen::Vector3f(imuData.magX, imuData.magY, imuData.magZ); 112 | 113 | // 유효한 IMU 데이터인 경우에만 업데이트 114 | if (!newAccel.hasNaN() && !newGyro.hasNaN() && !newMag.hasNaN()) { 115 | { 116 | std::lock_guard lock(poseMutex); // 동기화 보호 117 | imuAccel = newAccel; 118 | imuGyro = newGyro; 119 | imuMag = newMag; 120 | } 121 | } else { 122 | std::cerr << "Invalid IMU data, keeping last valid data" << std::endl; 123 | } 124 | 125 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 주기 설정 126 | } 127 | } 128 | 129 | // GPS 데이터 처리 함수 130 | void PoseEstimator::processGPS() { 131 | while (running) { 132 | GPSData gpsData = readGPS(); 133 | Eigen::Vector3f newPos = Eigen::Vector3f(gpsData.latitude / 1e7, gpsData.longitude / 1e7, gpsData.altitude / 1000.0f); 134 | Eigen::Vector3f newVel = Eigen::Vector3f(gpsData.velocityX, gpsData.velocityY, gpsData.velocityZ); 135 | 136 | // GPS 데이터가 유효하지 않으면 마지막 유효 데이터를 그대로 사용 137 | if (newPos.hasNaN() || newVel.hasNaN()) { 138 | std::cerr << "Invalid GPS data, keeping last valid data" << std::endl; 139 | } else { 140 | gpsPos = newPos; // 새로운 유효한 GPS 데이터로 업데이트 141 | gpsVel = newVel; 142 | } 143 | // 테스트를 위한 GPS 데이터 생성 144 | // gpsPos = Eigen::Vector3f::Zero(); 145 | // gpsVel = Eigen::Vector3f::Zero(); 146 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 147 | } 148 | } 149 | 150 | // 현재 포즈를 얻는 함수 151 | Eigen::VectorXf PoseEstimator::getPose() { 152 | std::lock_guard lock(poseMutex); 153 | 154 | Eigen::VectorXf pose(9); // 위치, 속도, 오일러 각을 포함한 9차원 벡터 155 | pose.segment<3>(0) = currentState.segment<3>(0); // 위치 (x, y, z) 156 | pose.segment<3>(3) = currentState.segment<3>(3); // 속도 (vx, vy, vz) 157 | 158 | // 쿼터니언에서 Roll, Pitch, Yaw 계산 159 | Eigen::Quaternionf attitude(currentState(6), currentState(7), currentState(8), currentState(9)); 160 | attitude.normalize(); // 쿼터니언 정규화 161 | pose.segment<3>(6) = quaternionToEuler(attitude); // 오일러 각 (Roll, Pitch, Yaw) 추가 162 | 163 | return pose; 164 | } 165 | -------------------------------------------------------------------------------- /test_code/pose_estimator.h: -------------------------------------------------------------------------------- 1 | // 쿼터니언 사용 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "ekf.h" 8 | #include "imu_sensor.h" 9 | #include "gps_sensor.h" 10 | 11 | class PoseEstimator { 12 | public: 13 | PoseEstimator(); 14 | ~PoseEstimator(); 15 | 16 | Eigen::VectorXf getPose(); 17 | 18 | private: 19 | EKF ekf; 20 | 21 | std::thread estimationThread; 22 | std::thread imuThread; 23 | std::thread gpsThread; 24 | std::atomic running; 25 | 26 | Eigen::VectorXf currentState; 27 | Eigen::Vector3f imuAccel; 28 | Eigen::Vector3f imuGyro; 29 | Eigen::Vector3f imuMag; // 자기장 데이터 저장을 위한 변수 추가 30 | Eigen::Vector3f gpsPos; 31 | Eigen::Vector3f gpsVel; 32 | std::mutex poseMutex; 33 | 34 | Eigen::Vector3f gyroOffset; 35 | bool isGyroCalibrated = false; 36 | 37 | void calibrateGyro(); 38 | void calculatePose(); 39 | void processIMU(); 40 | void processGPS(); 41 | 42 | const std::chrono::milliseconds loopDuration = std::chrono::milliseconds(20); 43 | }; -------------------------------------------------------------------------------- /test_code/rc_input.cpp: -------------------------------------------------------------------------------- 1 | #include "rc_input.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define SBUS_FRAME_SIZE 35 15 | #define START_BYTE 0x0F 16 | 17 | static int serial_port; 18 | static uint16_t channels[16]; // 16채널 값을 저장할 배열 19 | static std::deque data_buffer; // 최신 데이터를 저장할 버퍼 20 | 21 | // 시리얼 포트 설정 함수 22 | static int configureSerial(const std::string& port, int baudrate) { 23 | serial_port = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 24 | if (serial_port == -1) { 25 | perror("Failed to open serial port"); 26 | return -1; 27 | } 28 | 29 | struct termios options; 30 | tcgetattr(serial_port, &options); 31 | cfsetispeed(&options, baudrate); 32 | cfsetospeed(&options, baudrate); 33 | options.c_cflag |= (CLOCAL | CREAD); 34 | options.c_cflag &= ~PARENB; 35 | options.c_cflag &= ~CSTOPB; 36 | options.c_cflag &= ~CSIZE; 37 | options.c_cflag |= CS8; 38 | options.c_iflag &= ~(IXON | IXOFF | IXANY); 39 | options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 40 | options.c_oflag &= ~OPOST; 41 | tcsetattr(serial_port, TCSANOW, &options); 42 | 43 | return serial_port; 44 | } 45 | 46 | // RC 입력 초기화 함수 47 | void initRC(const std::string& port, int baudRate) { 48 | // 올바르게 초기화되지 않았을 경우 반복적으로 시도 49 | while (true) { 50 | if (configureSerial(port, baudRate) == -1) { 51 | std::cerr << "Failed to initialize RC input. Retrying..." << std::endl; 52 | std::this_thread::sleep_for(std::chrono::seconds(1)); // 1초 대기 후 재시도 53 | continue; 54 | } 55 | break; 56 | } 57 | } 58 | 59 | // 최신 RC 채널 값을 읽고 업데이트하는 함수 60 | int readRCChannel(int channel) { 61 | if (channel < 1 || channel > 16) { 62 | std::cerr << "Invalid channel number: " << channel << std::endl; 63 | return -1; 64 | } 65 | 66 | // 시리얼 포트에서 데이터 읽어 버퍼에 추가 67 | uint8_t byte; 68 | while (true) { // 데이터 처리가 완료될 때까지 계속 실행 69 | while (read(serial_port, &byte, 1) > 0) { 70 | data_buffer.push_back(byte); 71 | 72 | // 오래된 데이터를 삭제하여 버퍼 크기를 제한 73 | if (data_buffer.size() > SBUS_FRAME_SIZE * 10) { 74 | data_buffer.pop_front(); 75 | } 76 | } 77 | 78 | // 버퍼에서 최신 프레임을 찾아 데이터 갱신 79 | while (data_buffer.size() >= SBUS_FRAME_SIZE) { 80 | // 버퍼에서 프레임 추출 81 | std::vector frame(data_buffer.begin(), data_buffer.begin() + SBUS_FRAME_SIZE); 82 | 83 | // 시작 바이트 확인 84 | if (frame[0] != START_BYTE) { 85 | data_buffer.pop_front(); 86 | continue; 87 | } 88 | 89 | // 체크섬 검증 90 | uint8_t xor_checksum = 0; 91 | for (int i = 1; i < SBUS_FRAME_SIZE - 1; ++i) { 92 | xor_checksum ^= frame[i]; 93 | } 94 | 95 | if (xor_checksum != frame[SBUS_FRAME_SIZE - 1]) { 96 | data_buffer.pop_front(); // 잘못된 프레임을 버림 97 | continue; 98 | } 99 | 100 | // 유효한 프레임이면 채널 데이터 업데이트 101 | for (int i = 0; i < 16; ++i) { 102 | channels[i] = (frame[1 + i * 2] << 8) | frame[2 + i * 2]; 103 | } 104 | 105 | // 프레임을 버퍼에서 제거 106 | data_buffer.erase(data_buffer.begin(), data_buffer.begin() + SBUS_FRAME_SIZE); 107 | break; 108 | } 109 | 110 | // 요청된 채널 값을 반환 111 | // usleep(100000); // 0.1초(100ms) 대기 112 | usleep(100); // 0.0001(0.1ms)초 대기 113 | return channels[channel - 1]; 114 | } 115 | } 116 | -------------------------------------------------------------------------------- /test_code/rc_input.h: -------------------------------------------------------------------------------- 1 | #ifndef RC_INPUT_H 2 | #define RC_INPUT_H 3 | 4 | #include 5 | 6 | // RC 입력 초기화 함수 7 | void initRC(const std::string& port, int baudRate); 8 | 9 | // RC 데이터를 읽는 함수 10 | int readRCChannel(int channel); 11 | double mapThrottle(int value); 12 | double mapControlInput(int value); 13 | 14 | #endif -------------------------------------------------------------------------------- /test_code/sbus_reader.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define SERIAL_PORT "/dev/ttyAMA0" 10 | #define BAUDRATE B115200 11 | #define SBUS_FRAME_SIZE 35 12 | #define START_BYTE 0x0F 13 | 14 | int main() { 15 | int serial_port = open(SERIAL_PORT, O_RDWR | O_NOCTTY | O_NDELAY); 16 | if (serial_port == -1) { 17 | std::cerr << "Error opening serial port: " << SERIAL_PORT << std::endl; 18 | return 1; 19 | } 20 | 21 | termios tty; 22 | memset(&tty, 0, sizeof(tty)); 23 | 24 | if (tcgetattr(serial_port, &tty) != 0) { 25 | std::cerr << "Error getting terminal attributes." << std::endl; 26 | close(serial_port); 27 | return 1; 28 | } 29 | 30 | cfsetispeed(&tty, BAUDRATE); 31 | cfsetospeed(&tty, BAUDRATE); 32 | 33 | tty.c_cflag &= ~PARENB; 34 | tty.c_cflag &= ~CSTOPB; 35 | tty.c_cflag &= ~CSIZE; 36 | tty.c_cflag |= CS8; 37 | 38 | tty.c_cflag &= ~CRTSCTS; 39 | tty.c_cflag |= CREAD | CLOCAL; 40 | tty.c_lflag &= ~ICANON; 41 | tty.c_lflag &= ~(ECHO | ECHOE | ISIG); 42 | 43 | tty.c_iflag &= ~(IXON | IXOFF | IXANY); 44 | tty.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP); 45 | tty.c_oflag &= ~OPOST; 46 | 47 | tty.c_cc[VMIN] = SBUS_FRAME_SIZE; 48 | tty.c_cc[VTIME] = 1; 49 | 50 | if (tcsetattr(serial_port, TCSANOW, &tty) != 0) { 51 | std::cerr << "Error setting terminal attributes." << std::endl; 52 | close(serial_port); 53 | return 1; 54 | } 55 | 56 | uint8_t sbus_data[SBUS_FRAME_SIZE]; 57 | 58 | while (true) { 59 | int num_bytes = read(serial_port, &sbus_data, SBUS_FRAME_SIZE); 60 | if (num_bytes == SBUS_FRAME_SIZE && sbus_data[0] == START_BYTE) { 61 | uint8_t xor_checksum = 0; 62 | for (int i = 1; i < SBUS_FRAME_SIZE - 1; ++i) { 63 | xor_checksum ^= sbus_data[i]; 64 | } 65 | 66 | if (xor_checksum != sbus_data[SBUS_FRAME_SIZE - 1]) { 67 | std::cerr << "Checksum error. Discarding frame." << std::endl; 68 | continue; 69 | } 70 | 71 | uint16_t channels[16]; 72 | for (int i = 0; i < 16; ++i) { 73 | channels[i] = (sbus_data[1 + i * 2] << 8) | sbus_data[2 + i * 2]; 74 | } 75 | 76 | // 채널 1~5를 같은 줄에 print하여 제자리에서 업데이트 77 | std::cout << "\r"; 78 | for (int i = 0; i < 5; i++) { 79 | std::cout << "Channel " << (i + 1) << ": " << std::setw(4) << channels[i] << " "; 80 | } 81 | std::cout << std::flush; 82 | 83 | // 플래그 처리(다른 용도로 필요한 경우) 84 | uint8_t flags = sbus_data[33]; 85 | bool ch17 = flags & 0x80; 86 | bool ch18 = flags & 0x40; 87 | bool frame_lost = flags & 0x20; 88 | bool failsafe = flags & 0x10; 89 | } 90 | } 91 | 92 | close(serial_port); 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /test_code/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // 메모리 잠금 6 | #include 7 | #include 8 | #include // 수학 함수 사용 (atan2, sqrt, M_PI) 9 | #include // Eigen 라이브러리 10 | #include // POSIX 스레드 11 | #include // 고해상도 타이머 12 | #include "rc_input.h" // RC 입력 헤더 파일 13 | #include "imu_sensor.h" // IMU 센서 헤더 포함 14 | #include "imu_calibration.h" // IMU 캘리브레이션 헤더 파일 15 | #include // B115200 설정 16 | #include // std::clamp 함수 사용 17 | #include // 시간 측정을 위한 라이브러리 18 | #include 19 | 20 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 21 | #define MODE1 0x00 // 모드1 레지스터 22 | #define PRESCALE 0xFE // 프리스케일 레지스터 23 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 24 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 25 | 26 | const int RC_MIN = 172; 27 | const int RC_MAX = 1811; 28 | const int RC_MID = 991; 29 | const int PWM_MIN = 210; 30 | const int PWM_MAX = 405; 31 | const int MAX_ADJUSTMENT = 25; // 각 제어 입력의 최대 PWM 조정 값 32 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 33 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 34 | const int LOOP_DELAY_US = 26000; // 주기적인 대기 시간 (10ms) 35 | const float MAX_ANGLE = 90.0f; // 최대 각도 (예시) 36 | const float TOLERANCE_ROLL = 0.05f * MAX_ANGLE; // 롤 허용 오차 (0.45도) 37 | const float TOLERANCE_PITCH = 0.01f * MAX_ANGLE; // 피치 허용 오차 (0.45도) 38 | 39 | // IMUData 40 | IMUData imuData; 41 | 42 | class PCA9685 { 43 | public: 44 | PCA9685(int address = PCA9685_ADDR) { 45 | char filename[20]; 46 | snprintf(filename, 19, "/dev/i2c-1"); 47 | fd = open(filename, O_RDWR); 48 | if (fd < 0) { 49 | std::cerr << "Failed to open the i2c bus" << std::endl; 50 | exit(1); 51 | } 52 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 53 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 54 | exit(1); 55 | } 56 | reset(); 57 | setPWMFreq(50); // Set frequency to 50Hz for motor control 58 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 59 | } 60 | 61 | ~PCA9685() { 62 | stopAllMotors(); // 종료 시 모든 모터를 정지 63 | if (fd >= 0) { 64 | close(fd); 65 | } 66 | } 67 | 68 | void setPWM(int channel, int on, int off) { 69 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 70 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 71 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 72 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 73 | } 74 | 75 | void setMotorSpeed(int channel, int pwm_value) { 76 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 77 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 78 | return; 79 | } 80 | setPWM(channel, 0, pwm_value); 81 | } 82 | 83 | private: 84 | int fd; 85 | 86 | void reset() { 87 | writeRegister(MODE1, 0x00); 88 | } 89 | 90 | void setPWMFreq(int freq) { 91 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 92 | uint8_t oldmode = readRegister(MODE1); 93 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 94 | writeRegister(MODE1, newmode); 95 | writeRegister(PRESCALE, prescale); 96 | writeRegister(MODE1, oldmode); 97 | usleep(5000); 98 | writeRegister(MODE1, oldmode | 0xA1); 99 | } 100 | 101 | void writeRegister(uint8_t reg, uint8_t value) { 102 | uint8_t buffer[2] = {reg, value}; 103 | int retries = 0; 104 | while (write(fd, buffer, 2) != 2) { 105 | if (++retries >= I2C_RETRY_LIMIT) { 106 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 107 | exit(1); 108 | } 109 | usleep(1000); // 1ms 대기 후 재시도 110 | } 111 | } 112 | 113 | uint8_t readRegister(uint8_t reg) { 114 | int retries = 0; 115 | while (write(fd, ®, 1) != 1) { 116 | if (++retries >= I2C_RETRY_LIMIT) { 117 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 118 | exit(1); 119 | } 120 | usleep(1000); 121 | } 122 | uint8_t value; 123 | if (read(fd, &value, 1) != 1) { 124 | std::cerr << "Failed to read from the i2c bus" << std::endl; 125 | exit(1); 126 | } 127 | return value; 128 | } 129 | 130 | void initializeMotors() { 131 | for (int i = 0; i < 4; ++i) { 132 | setMotorSpeed(i, SAFE_PWM); 133 | } 134 | } 135 | 136 | void stopAllMotors() { 137 | for (int i = 0; i < 4; ++i) { 138 | setMotorSpeed(i, SAFE_PWM); 139 | } 140 | std::cout << "All motors stopped safely." << std::endl; 141 | } 142 | }; 143 | 144 | struct PIDController { 145 | float kp, ki, kd; 146 | float prev_error; 147 | float integral; 148 | float integral_limit; 149 | float output_limit; 150 | float feedforward; 151 | float filtered_derivative; // 필터링된 미분 항 152 | 153 | PIDController(float p, float i, float d, float ff = 0.0f, float i_limit = 10.0f, float out_limit = 400.0f) 154 | : kp(p), ki(i), kd(d), feedforward(ff), prev_error(0.0f), integral(0.0f), 155 | integral_limit(i_limit), output_limit(out_limit), filtered_derivative(0.0f) {} 156 | 157 | void reset() { 158 | prev_error = 0.0f; 159 | integral = 0.0f; 160 | filtered_derivative = 0.0f; 161 | } 162 | 163 | float calculate(float setpoint, float measurement, float dt) { 164 | if (dt <= 0.0f) { 165 | // dt가 0 이하일 경우, 계산을 중단하거나 기본 값을 반환 166 | return 0.0f; 167 | } 168 | 169 | // 오차 계산 170 | float error = setpoint - measurement; 171 | 172 | // 비례 항 계산 173 | float pTerm = kp * error; 174 | 175 | // 적분 항 계산 및 적분 제한 적용 176 | integral += error * dt; 177 | integral = std::clamp(integral, -integral_limit, integral_limit); 178 | float iTerm = ki * integral; 179 | 180 | // 미분 항 계산 (필터링 적용) 181 | float derivative = (error - prev_error) / dt; 182 | float alpha = 0.1f; // 필터 계수 183 | filtered_derivative = alpha * derivative + (1.0f - alpha) * filtered_derivative; 184 | float dTerm = kd * filtered_derivative; 185 | 186 | // 이전 오차 업데이트 187 | prev_error = error; 188 | 189 | // PID 출력 계산 (피드포워드 포함) 190 | float output = feedforward * setpoint + pTerm + iTerm + dTerm; 191 | 192 | // 출력 제한 193 | output = std::clamp(output, -output_limit, output_limit); 194 | 195 | return output; 196 | } 197 | }; 198 | 199 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 200 | double mapThrottle(int value) { 201 | if (value <= RC_MIN) return 0.0; 202 | if (value >= RC_MAX) return 1.0; 203 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 204 | } 205 | 206 | // 제어 입력(에일러론, 엘리베이터, 러더)을 -1.0 ~ 1.0 범위로 매핑하는 함수 207 | double mapControlInput(int value) { 208 | if (value < RC_MIN || value > RC_MAX) { 209 | return 0.0; 210 | } 211 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 212 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 213 | return 0.0; 214 | } 215 | 216 | int computeThrottlePWM(double throttle_normalized) { 217 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 218 | } 219 | 220 | int computeAdjustment(double control_normalized) { 221 | return static_cast(control_normalized * MAX_ADJUSTMENT); 222 | } 223 | 224 | int clamp(int value, int min_value, int max_value) { 225 | return value < min_value ? min_value : (value > max_value ? max_value : value); 226 | } 227 | 228 | // POSIX 스레드 함수 정의 229 | void *controlLoop(void *arg) { 230 | PCA9685 pca9685; 231 | // RC 입력 초기화 및 IMU 초기화 (필요 시 수정) 232 | initRC("/dev/ttyAMA0", B115200); 233 | initIMU("/dev/ttyUSB0", B115200); 234 | 235 | // 캘리브레이션 및 초기화 (기존 코드와 동일) 236 | IMUCalibrationData calibrationData = calibrateIMU(); 237 | float roll_com = calibrationData.offsetroll; 238 | float pitch_com = calibrationData.offsetpitch; 239 | float yaw_com = calibrationData.offsetyaw; 240 | 241 | float offsetGyroZ = calibrationData.offsetGyroZ; 242 | 243 | PIDController rollPID(1.0f, 0.1f, 0.05f); 244 | PIDController pitchPID(1.0f, 0.1f, 0.05f); 245 | PIDController yawPID(1.2f, 0.5f, 0.5f); 246 | 247 | // PIDController rollPID(1.5f, 0.05f, 0.03f); // Roll PID 게인 조정 248 | // PIDController pitchPID(1.5f, 0.05f, 0.03f); // Pitch PID 게인 조정 249 | // PIDController yawPID(0.8f, 0.1f, 0.05f); // Yaw PID 게인 조정 250 | 251 | // 루프 타이밍을 위한 변수 초기화 252 | auto previousTime = std::chrono::steady_clock::now(); 253 | 254 | // // 기존 코드입니다. 255 | // while (true) { 256 | // // // 현재 시간 계산 257 | // // auto currentTime = std::chrono::steady_clock::now(); 258 | // // std::chrono::duration elapsed = currentTime - previousTime; 259 | // // previousTime = currentTime; 260 | // // float dt = elapsed.count(); // 초 단위의 경과 시간 261 | 262 | // // float dt = 0.025f; 263 | 264 | // // IMUData imuData = readIMU(); 265 | 266 | // // 현재 시간 계산 267 | // auto imuStartTime = std::chrono::steady_clock::now(); 268 | // // IMU 및 RC 입력 데이터 읽기 269 | // IMUData imuData = readIMU(); // IMU 데이터를 읽어서 imuData에 저장 270 | // // 주기 10Hz (100ms) 271 | // std::chrono::duration loopDuration = std::chrono::milliseconds(100); // 100ms 272 | 273 | // // 현재 시간과 IMU 시작 시간의 경과 시간 측정 274 | // auto imuEndTime = std::chrono::steady_clock::now(); 275 | // std::chrono::duration imuElapsed = imuEndTime - imuStartTime; 276 | // float dt = imuElapsed.count(); // dt는 초 단위 277 | // // 실시간으로 dt 값을 출력 278 | // // std::cout << "\rCurrent dt: " << dt << " seconds" << std::flush; 279 | 280 | 281 | // int throttle_value = readRCChannel(3); // 스로틀 값 282 | // int aileron_value = readRCChannel(1); // 에일러론 값 283 | // int elevator_value = readRCChannel(2); // 엘리베이터 값 284 | // int rudder_value = readRCChannel(4); // 러더 값 285 | 286 | // // 조종기 입력 값 매핑 287 | // double throttle_normalized = mapThrottle(throttle_value); 288 | // double aileron_normalized = mapControlInput(aileron_value); 289 | // double elevator_normalized = mapControlInput(elevator_value); 290 | // double rudder_normalized = mapControlInput(rudder_value); 291 | 292 | // // IMU 데이터 보정 293 | // float correctedGyroZ = imuData.gyroZ - offsetGyroZ; // 보정된 자이로 Z값 294 | 295 | // // PID 계산 296 | // int roll_adj = rollPID.calculate(roll_com, imuData.roll_angle, dt); 297 | // int pitch_adj = pitchPID.calculate(pitch_com, imuData.pitch_angle, dt); 298 | // int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 299 | 300 | // // 추가 조정값 계산 301 | // int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 302 | // int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 303 | 304 | // // 스로틀 PWM 계산 305 | // int throttle_PWM = computeThrottlePWM(throttle_normalized); 306 | 307 | // // 모터 PWM 계산 308 | // int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 309 | // // 각 모터에 대한 보정값 적용 310 | // int motor1_adj = aileron_adj_total - elevator_adj_total + yaw_adj; 311 | // int motor2_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 312 | // int motor3_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 313 | // int motor4_adj = aileron_adj_total + elevator_adj_total - yaw_adj; 314 | 315 | // // 최종 모터 PWM 값 계산 316 | // motor1_PWM = throttle_PWM + motor1_adj; 317 | // motor2_PWM = throttle_PWM + motor2_adj; 318 | // motor3_PWM = throttle_PWM + motor3_adj; 319 | // motor4_PWM = throttle_PWM + motor4_adj; 320 | 321 | // if (throttle_PWM <= PWM_MIN) { 322 | // // 스로틀 값이 최소값 이하일 경우 모든 모터 정지 323 | // motor1_PWM = PWM_MIN; 324 | // motor2_PWM = PWM_MIN; 325 | // motor3_PWM = PWM_MIN; 326 | // motor4_PWM = PWM_MIN; 327 | // } else { 328 | // // 각 모터에 대한 보정값 적용 329 | // int motor1_adj = aileron_adj_total - elevator_adj_total + yaw_adj; 330 | // int motor2_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 331 | // int motor3_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 332 | // int motor4_adj = aileron_adj_total + elevator_adj_total - yaw_adj; 333 | 334 | // // 최종 모터 PWM 값 계산 335 | // motor1_PWM = throttle_PWM + motor1_adj; 336 | // motor2_PWM = throttle_PWM + motor2_adj; 337 | // motor3_PWM = throttle_PWM + motor3_adj; 338 | // motor4_PWM = throttle_PWM + motor4_adj; 339 | 340 | // // 모터 PWM이 유효한 범위 내에 있는지 확인 341 | // motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 342 | // motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 343 | // motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 344 | // motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 345 | // } 346 | 347 | // // 모터에 PWM 값 설정 348 | // pca9685.setMotorSpeed(0, motor1_PWM); 349 | // pca9685.setMotorSpeed(1, motor2_PWM); 350 | // pca9685.setMotorSpeed(2, motor3_PWM); 351 | // pca9685.setMotorSpeed(3, motor4_PWM); 352 | 353 | // // // 디버깅 출력 (필요 시 주석 해제) 354 | // // std::cout << "\nAHRS Roll: " << imuData.roll_angle 355 | // // << " AHRS Pitch: " << imuData.pitch_angle 356 | // // << " AHRS Yaw: " << imuData.yaw_angle 357 | // // << " Motor1: " << motor1_PWM 358 | // // << " Motor2: " << motor2_PWM 359 | // // << " Motor3: " << motor3_PWM 360 | // // << " Motor4: " << motor4_PWM 361 | // // << std::flush; 362 | 363 | // // 디버깅 출력 364 | // std::cout << "\rAHRS Roll: " << imuData.roll_angle 365 | // << " AHRS Pitch: " << imuData.pitch_angle 366 | // << " AHRS Yaw: " << imuData.yaw_angle 367 | // << " Motor1: " << motor1_PWM 368 | // << " Motor2: " << motor2_PWM 369 | // << " Motor3: " << motor3_PWM 370 | // << " Motor4: " << motor4_PWM 371 | // << std::flush; 372 | 373 | // // std::cout << "\r Roll: " << roll_adj << " Pitch: " << pitch_adj << std::flush; 374 | 375 | // // std::this_thread::sleep_for(std::chrono::milliseconds(25)); 376 | // } 377 | while (true) { 378 | // 현재 시간 계산 379 | auto imuStartTime = std::chrono::steady_clock::now(); 380 | 381 | // IMU 데이터 읽기 382 | IMUData imuData = readIMU(); 383 | 384 | // 현재 시간과 IMU 시작 시간의 경과 시간 측정 385 | auto imuEndTime = std::chrono::steady_clock::now(); 386 | std::chrono::duration imuElapsed = imuEndTime - imuStartTime; 387 | float dt = imuElapsed.count(); // dt는 초 단위 388 | 389 | // IMU 데이터 보정 390 | float correctedGyroZ = imuData.gyroZ - offsetGyroZ; // 보정된 자이로 Z값 391 | 392 | // PID 계산 (RC 입력 대신 IMU 자세값 기반으로) 393 | int roll_adj = rollPID.calculate(roll_com, imuData.roll_angle, dt); 394 | int pitch_adj = pitchPID.calculate(pitch_com, imuData.pitch_angle, dt); 395 | int yaw_adj = yawPID.calculate(yaw_com, correctedGyroZ, dt); 396 | 397 | // 각 모터에 대한 보정값 적용 398 | int motor1_adj = roll_adj - pitch_adj + yaw_adj; 399 | int motor2_adj = -roll_adj - pitch_adj - yaw_adj; 400 | int motor3_adj = -roll_adj + pitch_adj + yaw_adj; 401 | int motor4_adj = roll_adj + pitch_adj - yaw_adj; 402 | 403 | // PWM 값 계산 (자세 유지용) 404 | int motor1_PWM = PWM_MIN + motor1_adj; 405 | int motor2_PWM = PWM_MIN + motor2_adj; 406 | int motor3_PWM = PWM_MIN + motor3_adj; 407 | int motor4_PWM = PWM_MIN + motor4_adj; 408 | 409 | // PWM 값 범위 제한 410 | motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 411 | motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 412 | motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 413 | motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 414 | 415 | // 모터에 PWM 값 설정 416 | pca9685.setMotorSpeed(0, motor1_PWM); 417 | pca9685.setMotorSpeed(1, motor2_PWM); 418 | pca9685.setMotorSpeed(2, motor3_PWM); 419 | pca9685.setMotorSpeed(3, motor4_PWM); 420 | 421 | // 디버깅 출력 422 | std::cout << "\rRoll Adj: " << roll_adj 423 | << " Pitch Adj: " << pitch_adj 424 | << " Yaw Adj: " << yaw_adj 425 | << "AHRS Roll: " << imuData.roll_angle 426 | << " AHRS Pitch: " << imuData.pitch_angle 427 | << " AHRS Yaw: " << imuData.yaw_angle 428 | << " Motor1: " << motor1_PWM 429 | << " Motor2: " << motor2_PWM 430 | << " Motor3: " << motor3_PWM 431 | << " Motor4: " << motor4_PWM 432 | << std::flush; 433 | 434 | // // 루프 주기 유지 (10Hz, 100ms) 435 | // std::this_thread::sleep_for(std::chrono::milliseconds(100)); 436 | } 437 | return nullptr; 438 | } 439 | 440 | auto previousTime = std::chrono::steady_clock::now(); 441 | 442 | int main() { 443 | // 메모리 잠금 444 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 445 | std::cerr << "Failed to lock memory!" << std::endl; 446 | return -1; 447 | } 448 | 449 | // POSIX 스레드 생성 450 | pthread_t thread; 451 | pthread_attr_t attr; 452 | pthread_attr_init(&attr); 453 | 454 | // 스케줄링 정책 설정 (SCHED_FIFO, 우선순위 99) 455 | sched_param param; 456 | param.sched_priority = 99; 457 | pthread_attr_setschedpolicy(&attr, SCHED_FIFO); 458 | pthread_attr_setschedparam(&attr, ¶m); 459 | 460 | // 스레드 생성 461 | if (pthread_create(&thread, &attr, controlLoop, nullptr) != 0) { 462 | std::cerr << "Failed to create thread!" << std::endl; 463 | return -1; 464 | } 465 | 466 | // 메인 스레드 대기 467 | pthread_join(thread, nullptr); 468 | 469 | return 0; 470 | } -------------------------------------------------------------------------------- /test_code/test2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // 메모리 잠금 6 | #include 7 | #include 8 | #include // 수학 함수 사용 (atan2, sqrt, M_PI) 9 | #include // Eigen 라이브러리 10 | #include // POSIX 스레드 11 | #include // 고해상도 타이머 12 | #include "rc_input.h" // RC 입력 헤더 파일 13 | #include "imu_sensor.h" // IMU 센서 헤더 포함 14 | #include "imu_calibration.h" // IMU 캘리브레이션 헤더 파일 15 | #include // B115200 설정 16 | #include // std::clamp 함수 사용 17 | #include // 시간 측정을 위한 라이브러리 18 | #include 19 | 20 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 21 | #define MODE1 0x00 // 모드1 레지스터 22 | #define PRESCALE 0xFE // 프리스케일 레지스터 23 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 24 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 25 | 26 | const int RC_MIN = 172; 27 | const int RC_MAX = 1811; 28 | const int RC_MID = 991; 29 | const int PWM_MIN = 210; 30 | const int PWM_MAX = 405; 31 | const int MAX_ADJUSTMENT = 25; // 각 제어 입력의 최대 PWM 조정 값 32 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 33 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 34 | const int LOOP_DELAY_US = 26000; // 주기적인 대기 시간 (10ms) 35 | const float MAX_ANGLE = 90.0f; // 최대 각도 (예시) 36 | const float TOLERANCE_ROLL = 0.05f * MAX_ANGLE; // 롤 허용 오차 (0.45도) 37 | const float TOLERANCE_PITCH = 0.01f * MAX_ANGLE; // 피치 허용 오차 (0.45도) 38 | 39 | // IMUData 40 | IMUData imuData; 41 | 42 | class PCA9685 { 43 | public: 44 | PCA9685(int address = PCA9685_ADDR) { 45 | char filename[20]; 46 | snprintf(filename, 19, "/dev/i2c-1"); 47 | fd = open(filename, O_RDWR); 48 | if (fd < 0) { 49 | std::cerr << "Failed to open the i2c bus" << std::endl; 50 | exit(1); 51 | } 52 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 53 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 54 | exit(1); 55 | } 56 | reset(); 57 | setPWMFreq(50); // Set frequency to 50Hz for motor control 58 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 59 | } 60 | 61 | ~PCA9685() { 62 | stopAllMotors(); // 종료 시 모든 모터를 정지 63 | if (fd >= 0) { 64 | close(fd); 65 | } 66 | } 67 | 68 | void setPWM(int channel, int on, int off) { 69 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 70 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 71 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 72 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 73 | } 74 | 75 | void setMotorSpeed(int channel, int pwm_value) { 76 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 77 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 78 | return; 79 | } 80 | setPWM(channel, 0, pwm_value); 81 | } 82 | 83 | private: 84 | int fd; 85 | 86 | void reset() { 87 | writeRegister(MODE1, 0x00); 88 | } 89 | 90 | void setPWMFreq(int freq) { 91 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 92 | uint8_t oldmode = readRegister(MODE1); 93 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 94 | writeRegister(MODE1, newmode); 95 | writeRegister(PRESCALE, prescale); 96 | writeRegister(MODE1, oldmode); 97 | usleep(5000); 98 | writeRegister(MODE1, oldmode | 0xA1); 99 | } 100 | 101 | void writeRegister(uint8_t reg, uint8_t value) { 102 | uint8_t buffer[2] = {reg, value}; 103 | int retries = 0; 104 | while (write(fd, buffer, 2) != 2) { 105 | if (++retries >= I2C_RETRY_LIMIT) { 106 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 107 | exit(1); 108 | } 109 | usleep(1000); // 1ms 대기 후 재시도 110 | } 111 | } 112 | 113 | uint8_t readRegister(uint8_t reg) { 114 | int retries = 0; 115 | while (write(fd, ®, 1) != 1) { 116 | if (++retries >= I2C_RETRY_LIMIT) { 117 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 118 | exit(1); 119 | } 120 | usleep(1000); 121 | } 122 | uint8_t value; 123 | if (read(fd, &value, 1) != 1) { 124 | std::cerr << "Failed to read from the i2c bus" << std::endl; 125 | exit(1); 126 | } 127 | return value; 128 | } 129 | 130 | void initializeMotors() { 131 | for (int i = 0; i < 4; ++i) { 132 | setMotorSpeed(i, SAFE_PWM); 133 | } 134 | } 135 | 136 | void stopAllMotors() { 137 | for (int i = 0; i < 4; ++i) { 138 | setMotorSpeed(i, SAFE_PWM); 139 | } 140 | std::cout << "All motors stopped safely." << std::endl; 141 | } 142 | }; 143 | 144 | struct PIDController { 145 | float kp, ki, kd; 146 | float prev_error; 147 | float integral; 148 | float integral_limit; 149 | float output_limit; 150 | float feedforward; 151 | float filtered_derivative; // 필터링된 미분 항 152 | 153 | PIDController(float p, float i, float d, float ff = 0.0f, float i_limit = 10.0f, float out_limit = 400.0f) 154 | : kp(p), ki(i), kd(d), feedforward(ff), prev_error(0.0f), integral(0.0f), 155 | integral_limit(i_limit), output_limit(out_limit), filtered_derivative(0.0f) {} 156 | 157 | void reset() { 158 | prev_error = 0.0f; 159 | integral = 0.0f; 160 | filtered_derivative = 0.0f; 161 | } 162 | 163 | float calculate(float setpoint, float measurement, float dt) { 164 | if (dt <= 0.0f) { 165 | // dt가 0 이하일 경우, 계산을 중단하거나 기본 값을 반환 166 | return 0.0f; 167 | } 168 | 169 | // 오차 계산 170 | float error = setpoint - measurement; 171 | 172 | // 비례 항 계산 173 | float pTerm = kp * error; 174 | 175 | // 적분 항 계산 및 적분 제한 적용 176 | integral += error * dt; 177 | integral = std::clamp(integral, -integral_limit, integral_limit); 178 | float iTerm = ki * integral; 179 | 180 | // 미분 항 계산 (필터링 적용) 181 | float derivative = (error - prev_error) / dt; 182 | float alpha = 0.1f; // 필터 계수 183 | filtered_derivative = alpha * derivative + (1.0f - alpha) * filtered_derivative; 184 | float dTerm = kd * filtered_derivative; 185 | 186 | // 이전 오차 업데이트 187 | prev_error = error; 188 | 189 | // PID 출력 계산 (피드포워드 포함) 190 | float output = feedforward * setpoint + pTerm + iTerm + dTerm; 191 | 192 | // 출력 제한 193 | output = std::clamp(output, -output_limit, output_limit); 194 | 195 | return output; 196 | } 197 | }; 198 | 199 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 200 | double mapThrottle(int value) { 201 | if (value <= RC_MIN) return 0.0; 202 | if (value >= RC_MAX) return 1.0; 203 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 204 | } 205 | 206 | // 제어 입력(에일러론, 엘리베이터, 러더)을 -1.0 ~ 1.0 범위로 매핑하는 함수 207 | double mapControlInput(int value) { 208 | if (value < RC_MIN || value > RC_MAX) { 209 | return 0.0; 210 | } 211 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 212 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 213 | return 0.0; 214 | } 215 | 216 | int computeThrottlePWM(double throttle_normalized) { 217 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 218 | } 219 | 220 | int computeAdjustment(double control_normalized) { 221 | return static_cast(control_normalized * MAX_ADJUSTMENT); 222 | } 223 | 224 | int clamp(int value, int min_value, int max_value) { 225 | return value < min_value ? min_value : (value > max_value ? max_value : value); 226 | } 227 | 228 | // POSIX 스레드 함수 정의 229 | void *controlLoop(void *arg) { 230 | PCA9685 pca9685; 231 | // RC 입력 초기화 및 IMU 초기화 (필요 시 수정) 232 | initRC("/dev/ttyAMA0", B115200); 233 | initIMU("/dev/ttyUSB0", B115200); 234 | 235 | // 캘리브레이션 및 초기화 (기존 코드와 동일) 236 | IMUCalibrationData calibrationData = calibrateIMU(); 237 | float roll_com = calibrationData.offsetroll; 238 | float pitch_com = calibrationData.offsetpitch; 239 | float yaw_com = calibrationData.offsetyaw; 240 | 241 | float offsetGyroZ = calibrationData.offsetGyroZ; 242 | 243 | PIDController rollPID(2.0f, 0.1f, 0.05f); 244 | PIDController pitchPID(2.0f, 0.5f, 0.2f); 245 | PIDController yawPID(1.2f, 0.5f, 0.5f); 246 | 247 | // PIDController rollPID(1.5f, 0.05f, 0.03f); // Roll PID 게인 조정 248 | // PIDController pitchPID(1.5f, 0.05f, 0.03f); // Pitch PID 게인 조정 249 | // PIDController yawPID(0.8f, 0.1f, 0.05f); // Yaw PID 게인 조정 250 | 251 | // 루프 타이밍을 위한 변수 초기화 252 | auto previousTime = std::chrono::steady_clock::now(); 253 | 254 | // // 기존 코드입니다. 255 | while (true) { 256 | // // 현재 시간 계산 257 | // auto currentTime = std::chrono::steady_clock::now(); 258 | // std::chrono::duration elapsed = currentTime - previousTime; 259 | // previousTime = currentTime; 260 | // float dt = elapsed.count(); // 초 단위의 경과 시간 261 | 262 | // float dt = 0.025f; 263 | 264 | // IMUData imuData = readIMU(); 265 | 266 | // 현재 시간 계산 267 | auto imuStartTime = std::chrono::steady_clock::now(); 268 | // IMU 및 RC 입력 데이터 읽기 269 | IMUData imuData = readIMU(); // IMU 데이터를 읽어서 imuData에 저장 270 | // 주기 10Hz (100ms) 271 | std::chrono::duration loopDuration = std::chrono::milliseconds(100); // 100ms 272 | 273 | // 현재 시간과 IMU 시작 시간의 경과 시간 측정 274 | auto imuEndTime = std::chrono::steady_clock::now(); 275 | std::chrono::duration imuElapsed = imuEndTime - imuStartTime; 276 | float dt = imuElapsed.count(); // dt는 초 단위 277 | // 실시간으로 dt 값을 출력 278 | // std::cout << "\rCurrent dt: " << dt << " seconds" << std::flush; 279 | 280 | 281 | int throttle_value = readRCChannel(3); // 스로틀 값 282 | int aileron_value = readRCChannel(1); // 에일러론 값 283 | int elevator_value = readRCChannel(2); // 엘리베이터 값 284 | int rudder_value = readRCChannel(4); // 러더 값 285 | 286 | // 조종기 입력 값 매핑 287 | double throttle_normalized = mapThrottle(throttle_value); 288 | double aileron_normalized = mapControlInput(aileron_value); 289 | double elevator_normalized = mapControlInput(elevator_value); 290 | double rudder_normalized = mapControlInput(rudder_value); 291 | 292 | // IMU 데이터 보정 293 | float correctedGyroZ = imuData.gyroZ - offsetGyroZ; // 보정된 자이로 Z값 294 | 295 | // PID 계산 296 | // int roll_adj = rollPID.calculate(roll_com, imuData.roll_angle, dt); 297 | // int pitch_adj = pitchPID.calculate(pitch_com, imuData.pitch_angle, dt); 298 | 299 | int roll_adj = 0; 300 | int pitch_adj = 0; 301 | int yaw_adj = 0; 302 | 303 | // PID 계산 (허용 오차 적용) 304 | if (std::abs(roll_com - imuData.roll_angle) > TOLERANCE_ROLL) { 305 | roll_adj = rollPID.calculate(roll_com, imuData.roll_angle, dt); 306 | } 307 | 308 | if (std::abs(pitch_com - imuData.pitch_angle) > TOLERANCE_PITCH) { 309 | pitch_adj = pitchPID.calculate(pitch_com, imuData.pitch_angle, dt); 310 | } 311 | // int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 312 | 313 | // 추가 조정값 계산 314 | int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 315 | int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 316 | 317 | // 스로틀 PWM 계산 318 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 319 | 320 | // 모터 PWM 계산 321 | int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 322 | 323 | if (throttle_PWM <= PWM_MIN) { 324 | // 스로틀 값이 최소값 이하일 경우 모든 모터 정지 325 | motor1_PWM = PWM_MIN; 326 | motor2_PWM = PWM_MIN; 327 | motor3_PWM = PWM_MIN; 328 | motor4_PWM = PWM_MIN; 329 | } else { 330 | // 각 모터에 대한 보정값 적용 331 | int motor1_adj = aileron_adj_total + elevator_adj_total + yaw_adj; 332 | int motor2_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 333 | int motor3_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 334 | int motor4_adj = aileron_adj_total - elevator_adj_total - yaw_adj; 335 | // 각 모터에 대한 보정값 적용 336 | // int motor1_adj = + elevator_adj_total; 337 | // int motor2_adj = - elevator_adj_total; 338 | // int motor3_adj = + elevator_adj_total; 339 | // int motor4_adj = - elevator_adj_total; 340 | 341 | // 최종 모터 PWM 값 계산 342 | motor1_PWM = throttle_PWM + motor1_adj; 343 | motor2_PWM = throttle_PWM + motor2_adj; 344 | motor3_PWM = throttle_PWM + motor3_adj; 345 | motor4_PWM = throttle_PWM + motor4_adj; 346 | 347 | // 모터 PWM이 유효한 범위 내에 있는지 확인 348 | motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 349 | motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 350 | motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 351 | motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 352 | } 353 | 354 | // 모터에 PWM 값 설정 355 | pca9685.setMotorSpeed(0, motor1_PWM); 356 | pca9685.setMotorSpeed(1, motor2_PWM); 357 | pca9685.setMotorSpeed(2, motor3_PWM); 358 | pca9685.setMotorSpeed(3, motor4_PWM); 359 | 360 | // // 디버깅 출력 (필요 시 주석 해제) 361 | // std::cout << "\nAHRS Roll: " << imuData.roll_angle 362 | // << " AHRS Pitch: " << imuData.pitch_angle 363 | // << " AHRS Yaw: " << imuData.yaw_angle 364 | // << " Motor1: " << motor1_PWM 365 | // << " Motor2: " << motor2_PWM 366 | // << " Motor3: " << motor3_PWM 367 | // << " Motor4: " << motor4_PWM 368 | // << std::flush; 369 | 370 | // 디버깅 출력 371 | // 디버깅 출력 372 | std::cout << "\rRoll Adj: " << roll_adj 373 | << " Pitch Adj: " << pitch_adj 374 | << " Yaw Adj: " << yaw_adj 375 | << "AHRS Roll: " << imuData.roll_angle 376 | << " AHRS Pitch: " << imuData.pitch_angle 377 | << " AHRS Yaw: " << imuData.yaw_angle 378 | << " Motor1: " << motor1_PWM 379 | << " Motor2: " << motor2_PWM 380 | << " Motor3: " << motor3_PWM 381 | << " Motor4: " << motor4_PWM 382 | << std::flush; 383 | 384 | // std::cout << "\r Roll: " << roll_adj << " Pitch: " << pitch_adj << std::flush; 385 | 386 | // std::this_thread::sleep_for(std::chrono::milliseconds(25)); 387 | } 388 | // while (true) { 389 | // // 현재 시간 계산 390 | // auto imuStartTime = std::chrono::steady_clock::now(); 391 | 392 | // // IMU 데이터 읽기 393 | // IMUData imuData = readIMU(); 394 | 395 | // // 현재 시간과 IMU 시작 시간의 경과 시간 측정 396 | // auto imuEndTime = std::chrono::steady_clock::now(); 397 | // std::chrono::duration imuElapsed = imuEndTime - imuStartTime; 398 | // float dt = imuElapsed.count(); // dt는 초 단위 399 | 400 | // // IMU 데이터 보정 401 | // float correctedGyroZ = imuData.gyroZ - offsetGyroZ; // 보정된 자이로 Z값 402 | 403 | // // PID 계산 (RC 입력 대신 IMU 자세값 기반으로) 404 | // int roll_adj = rollPID.calculate(roll_com, imuData.roll_angle, dt); 405 | // int pitch_adj = pitchPID.calculate(pitch_com, imuData.pitch_angle, dt); 406 | // int yaw_adj = yawPID.calculate(yaw_com, correctedGyroZ, dt); 407 | 408 | // // 각 모터에 대한 보정값 적용 409 | // int motor1_adj = roll_adj - pitch_adj + yaw_adj; 410 | // int motor2_adj = -roll_adj - pitch_adj - yaw_adj; 411 | // int motor3_adj = -roll_adj + pitch_adj + yaw_adj; 412 | // int motor4_adj = roll_adj + pitch_adj - yaw_adj; 413 | 414 | // // PWM 값 계산 (자세 유지용) 415 | // int motor1_PWM = PWM_MIN + motor1_adj; 416 | // int motor2_PWM = PWM_MIN + motor2_adj; 417 | // int motor3_PWM = PWM_MIN + motor3_adj; 418 | // int motor4_PWM = PWM_MIN + motor4_adj; 419 | 420 | // // PWM 값 범위 제한 421 | // motor1_PWM = clamp(motor1_PWM, PWM_MIN, PWM_MAX); 422 | // motor2_PWM = clamp(motor2_PWM, PWM_MIN, PWM_MAX); 423 | // motor3_PWM = clamp(motor3_PWM, PWM_MIN, PWM_MAX); 424 | // motor4_PWM = clamp(motor4_PWM, PWM_MIN, PWM_MAX); 425 | 426 | // // 모터에 PWM 값 설정 427 | // pca9685.setMotorSpeed(0, motor1_PWM); 428 | // pca9685.setMotorSpeed(1, motor2_PWM); 429 | // pca9685.setMotorSpeed(2, motor3_PWM); 430 | // pca9685.setMotorSpeed(3, motor4_PWM); 431 | 432 | // // 디버깅 출력 433 | // std::cout << "\rRoll Adj: " << roll_adj 434 | // << " Pitch Adj: " << pitch_adj 435 | // << " Yaw Adj: " << yaw_adj 436 | // << "AHRS Roll: " << imuData.roll_angle 437 | // << " AHRS Pitch: " << imuData.pitch_angle 438 | // << " AHRS Yaw: " << imuData.yaw_angle 439 | // << " Motor1: " << motor1_PWM 440 | // << " Motor2: " << motor2_PWM 441 | // << " Motor3: " << motor3_PWM 442 | // << " Motor4: " << motor4_PWM 443 | // << std::flush; 444 | 445 | // // // 루프 주기 유지 (10Hz, 100ms) 446 | // // std::this_thread::sleep_for(std::chrono::milliseconds(100)); 447 | // } 448 | return nullptr; 449 | } 450 | 451 | auto previousTime = std::chrono::steady_clock::now(); 452 | 453 | int main() { 454 | // 메모리 잠금 455 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 456 | std::cerr << "Failed to lock memory!" << std::endl; 457 | return -1; 458 | } 459 | 460 | // POSIX 스레드 생성 461 | pthread_t thread; 462 | pthread_attr_t attr; 463 | pthread_attr_init(&attr); 464 | 465 | // 스케줄링 정책 설정 (SCHED_FIFO, 우선순위 99) 466 | sched_param param; 467 | param.sched_priority = 99; 468 | pthread_attr_setschedpolicy(&attr, SCHED_FIFO); 469 | pthread_attr_setschedparam(&attr, ¶m); 470 | 471 | // 스레드 생성 472 | if (pthread_create(&thread, &attr, controlLoop, nullptr) != 0) { 473 | std::cerr << "Failed to create thread!" << std::endl; 474 | return -1; 475 | } 476 | 477 | // 메인 스레드 대기 478 | pthread_join(thread, nullptr); 479 | 480 | return 0; 481 | } -------------------------------------------------------------------------------- /test_code/test4.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // 메모리 잠금 6 | #include 7 | #include 8 | #include // 수학 함수 사용 (atan2, sqrt, M_PI) 9 | #include // Eigen 라이브러리 10 | #include // POSIX 스레드 11 | #include // 고해상도 타이머 12 | #include "rc_input.h" // RC 입력 헤더 파일 13 | #include "imu_sensor.h" // IMU 센서 헤더 포함 14 | #include "imu_calibration.h" // IMU 캘리브레이션 헤더 파일 15 | #include // B115200 설정 16 | #include // std::clamp 함수 사용 17 | #include // 시간 측정을 위한 라이브러리 18 | #include 19 | #include 20 | 21 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 22 | #define MODE1 0x00 // 모드1 레지스터 23 | #define PRESCALE 0xFE // 프리스케일 레지스터 24 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 25 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 26 | 27 | const int RC_MIN = 172; 28 | const int RC_MAX = 1811; 29 | const int RC_MID = 991; 30 | const int PWM_MIN = 210; 31 | const int PWM_MAX = 405; 32 | const int MAX_ADJUSTMENT = 10; // 각 제어 입력의 최대 PWM 조정 값 33 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 34 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 35 | const int LOOP_DELAY_US = 26000; // 주기적인 대기 시간 (10ms) 36 | const float MAX_ANGLE = 90.0f; // 최대 각도 (예시) 37 | const float TOLERANCE_ROLL = 0.05f * MAX_ANGLE; // 롤 허용 오차 (0.45도) 38 | const float TOLERANCE_PITCH = 0.01f * MAX_ANGLE; // 피치 허용 오차 (0.45도) 39 | 40 | // IMU 데이터와 mutex 정의 41 | IMUData imuData; 42 | std::mutex imuMutex; 43 | 44 | class PCA9685 { 45 | public: 46 | PCA9685(int address = PCA9685_ADDR) { 47 | char filename[20]; 48 | snprintf(filename, 19, "/dev/i2c-1"); 49 | fd = open(filename, O_RDWR); 50 | if (fd < 0) { 51 | std::cerr << "Failed to open the i2c bus" << std::endl; 52 | exit(1); 53 | } 54 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 55 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 56 | exit(1); 57 | } 58 | reset(); 59 | setPWMFreq(50); // Set frequency to 50Hz for motor control 60 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 61 | } 62 | 63 | ~PCA9685() { 64 | stopAllMotors(); // 종료 시 모든 모터를 정지 65 | if (fd >= 0) { 66 | close(fd); 67 | } 68 | } 69 | 70 | void setPWM(int channel, int on, int off) { 71 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 72 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 73 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 74 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 75 | } 76 | 77 | void setMotorSpeed(int channel, int pwm_value) { 78 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 79 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 80 | return; 81 | } 82 | setPWM(channel, 0, pwm_value); 83 | } 84 | 85 | private: 86 | int fd; 87 | 88 | void reset() { 89 | writeRegister(MODE1, 0x00); 90 | } 91 | 92 | void setPWMFreq(int freq) { 93 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 94 | uint8_t oldmode = readRegister(MODE1); 95 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 96 | writeRegister(MODE1, newmode); 97 | writeRegister(PRESCALE, prescale); 98 | writeRegister(MODE1, oldmode); 99 | usleep(5000); 100 | writeRegister(MODE1, oldmode | 0xA1); 101 | } 102 | 103 | void writeRegister(uint8_t reg, uint8_t value) { 104 | uint8_t buffer[2] = {reg, value}; 105 | int retries = 0; 106 | while (write(fd, buffer, 2) != 2) { 107 | if (++retries >= I2C_RETRY_LIMIT) { 108 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 109 | exit(1); 110 | } 111 | usleep(1000); // 1ms 대기 후 재시도 112 | } 113 | } 114 | 115 | uint8_t readRegister(uint8_t reg) { 116 | int retries = 0; 117 | while (write(fd, ®, 1) != 1) { 118 | if (++retries >= I2C_RETRY_LIMIT) { 119 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 120 | exit(1); 121 | } 122 | usleep(1000); 123 | } 124 | uint8_t value; 125 | if (read(fd, &value, 1) != 1) { 126 | std::cerr << "Failed to read from the i2c bus" << std::endl; 127 | exit(1); 128 | } 129 | return value; 130 | } 131 | 132 | void initializeMotors() { 133 | for (int i = 0; i < 4; ++i) { 134 | setMotorSpeed(i, SAFE_PWM); 135 | } 136 | } 137 | 138 | void stopAllMotors() { 139 | for (int i = 0; i < 4; ++i) { 140 | setMotorSpeed(i, SAFE_PWM); 141 | } 142 | std::cout << "All motors stopped safely." << std::endl; 143 | } 144 | }; 145 | 146 | struct PIDController { 147 | float kp, ki, kd; 148 | float prev_error; 149 | float integral; 150 | float integral_limit; 151 | float output_limit; 152 | float feedforward; 153 | float filtered_derivative; // 필터링된 미분 항 154 | float alpha; // 필터 계수 155 | 156 | // outlimit 설정 400->10로 기본 세팅 157 | PIDController(float p, float i, float d, float ff = 0.0f, float i_limit = 10.0f, float out_limit = 10.0f, float filter_alpha = 0.01f) 158 | : kp(p), ki(i), kd(d), feedforward(ff), prev_error(0.0f), integral(0.0f), 159 | integral_limit(i_limit), output_limit(out_limit), filtered_derivative(0.0f), alpha(filter_alpha) {} 160 | 161 | void reset() { 162 | prev_error = 0.0f; 163 | integral = 0.0f; 164 | filtered_derivative = 0.0f; 165 | } 166 | 167 | float calculate(float setpoint, float measurement, float dt) { 168 | if (dt <= 0.0f) { 169 | // dt가 0 이하일 경우, 계산을 중단 170 | return 0.0f; 171 | } 172 | 173 | // 오차 계산 174 | float error = setpoint - measurement; 175 | 176 | // 비례 항 177 | float pTerm = kp * error; 178 | 179 | // 적분 항 (적분 제한 적용) 180 | integral += error * dt; 181 | integral = std::clamp(integral, -integral_limit, integral_limit); 182 | float iTerm = ki * integral; 183 | 184 | // 미분 항 (필터링 적용) 185 | float derivative = (error - prev_error) / dt; 186 | filtered_derivative = alpha * derivative + (1.0f - alpha) * filtered_derivative; 187 | float dTerm = kd * filtered_derivative; 188 | 189 | // 이전 오차 업데이트 190 | prev_error = error; 191 | 192 | // PID 출력 (피드포워드 포함) 193 | float output = feedforward * setpoint + pTerm + iTerm + dTerm; 194 | 195 | // 출력 제한 196 | output = std::clamp(output, -output_limit, output_limit); 197 | 198 | return output; 199 | } 200 | }; 201 | 202 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 203 | double mapThrottle(int value) { 204 | if (value <= RC_MIN) return 0.0; 205 | if (value >= RC_MAX) return 1.0; 206 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 207 | } 208 | 209 | double mapControlInput(int value) { 210 | if (value < RC_MIN || value > RC_MAX) { 211 | return 0.0; 212 | } 213 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 214 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 215 | return 0.0; 216 | } 217 | 218 | int computeThrottlePWM(double throttle_normalized) { 219 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 220 | } 221 | 222 | int computeAdjustment(double control_normalized) { 223 | return static_cast(control_normalized * MAX_ADJUSTMENT); 224 | } 225 | 226 | int clamp(int value, int min_value, int max_value) { 227 | return value < min_value ? min_value : (value > max_value ? max_value : value); 228 | } 229 | 230 | // IMU 스레드 함수 231 | void *imuThread(void *arg) { 232 | initIMU("/dev/ttyUSB0", B115200); 233 | IMUCalibrationData calibrationData = calibrateIMU(); 234 | float offsetGyroZ = calibrationData.offsetGyroZ; 235 | 236 | while (true) { 237 | IMUData localIMUData = readIMU(); 238 | localIMUData.gyroZ -= offsetGyroZ; // 보정된 자이로 Z값 239 | 240 | // Mutex로 IMU 데이터 보호 241 | { 242 | std::lock_guard lock(imuMutex); 243 | imuData = localIMUData; 244 | } 245 | 246 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 247 | } 248 | return nullptr; 249 | } 250 | 251 | // PID 계산 스레드 함수 252 | void *controlLoop(void *arg) { 253 | PCA9685 pca9685; 254 | initRC("/dev/ttyAMA0", B115200); 255 | 256 | PIDController rollPID(20.0f, 0.0f, 15.0f); 257 | PIDController pitchPID(2.0f, 0.5f, 0.2f); 258 | PIDController yawPID(1.2f, 0.5f, 0.5f); 259 | 260 | float roll_com = 0; 261 | float pitch_com = 0; 262 | auto previousTime = std::chrono::steady_clock::now(); 263 | 264 | while (true) { 265 | auto currentTime = std::chrono::steady_clock::now(); 266 | std::chrono::duration elapsed = currentTime - previousTime; 267 | float dt = elapsed.count(); 268 | previousTime = currentTime; 269 | 270 | IMUData localIMUData; 271 | { 272 | std::lock_guard lock(imuMutex); 273 | localIMUData = imuData; 274 | } 275 | 276 | int throttle_value = readRCChannel(3); 277 | int aileron_value = readRCChannel(1); 278 | int elevator_value = readRCChannel(2); 279 | int rudder_value = readRCChannel(4); 280 | 281 | double throttle_normalized = mapThrottle(throttle_value); 282 | double aileron_normalized = mapControlInput(aileron_value); 283 | double elevator_normalized = mapControlInput(elevator_value); 284 | double rudder_normalized = mapControlInput(rudder_value); 285 | 286 | int roll_adj = 0; 287 | int pitch_adj = 0; 288 | int yaw_adj = 0; 289 | if (std::abs(roll_com - localIMUData.roll_angle) > TOLERANCE_ROLL) { 290 | roll_adj = rollPID.calculate(roll_com, localIMUData.roll_angle, dt); 291 | } 292 | if (std::abs(pitch_com - localIMUData.pitch_angle) > TOLERANCE_PITCH) { 293 | pitch_adj = pitchPID.calculate(pitch_com, localIMUData.pitch_angle, dt); 294 | } 295 | // int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 296 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 297 | int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 298 | 299 | if (throttle_PWM <= PWM_MIN) { 300 | pca9685.setMotorSpeed(0, PWM_MIN); 301 | pca9685.setMotorSpeed(1, PWM_MIN); 302 | pca9685.setMotorSpeed(2, PWM_MIN); 303 | pca9685.setMotorSpeed(3, PWM_MIN); 304 | continue; 305 | } else { 306 | int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 307 | int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 308 | 309 | int motor1_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 310 | int motor2_adj = aileron_adj_total - elevator_adj_total - yaw_adj; 311 | int motor3_adj = aileron_adj_total + elevator_adj_total + yaw_adj; 312 | int motor4_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 313 | 314 | motor1_PWM = clamp(throttle_PWM + motor1_adj, PWM_MIN, PWM_MAX); 315 | motor2_PWM = clamp(throttle_PWM + motor2_adj, PWM_MIN, PWM_MAX); 316 | motor3_PWM = clamp(throttle_PWM + motor3_adj, PWM_MIN, PWM_MAX); 317 | motor4_PWM = clamp(throttle_PWM + motor4_adj, PWM_MIN, PWM_MAX); 318 | 319 | // 최소값 보장 (PWM_MIN 이상의 값 유지) 320 | motor1_PWM = std::max(motor1_PWM, PWM_MIN + 5); // 5은 임의의 최소 여유값 321 | motor2_PWM = std::max(motor2_PWM, PWM_MIN + 5); 322 | motor3_PWM = std::max(motor3_PWM, PWM_MIN + 5); 323 | motor4_PWM = std::max(motor4_PWM, PWM_MIN + 5); 324 | } 325 | 326 | pca9685.setMotorSpeed(0, motor1_PWM); 327 | pca9685.setMotorSpeed(1, motor2_PWM); 328 | pca9685.setMotorSpeed(2, motor3_PWM); 329 | pca9685.setMotorSpeed(3, motor4_PWM); 330 | 331 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 332 | std::cout << " roll: " << imuData.roll_angle << "\n"; 333 | } 334 | return nullptr; 335 | } 336 | 337 | int main() { 338 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 339 | std::cerr << "Failed to lock memory!" << std::endl; 340 | return -1; 341 | } 342 | 343 | pthread_t imuThreadHandle, controlThreadHandle; 344 | 345 | if (pthread_create(&imuThreadHandle, nullptr, imuThread, nullptr) != 0) { 346 | std::cerr << "Failed to create IMU thread!" << std::endl; 347 | return -1; 348 | } 349 | 350 | if (pthread_create(&controlThreadHandle, nullptr, controlLoop, nullptr) != 0) { 351 | std::cerr << "Failed to create Control thread!" << std::endl; 352 | return -1; 353 | } 354 | 355 | pthread_join(imuThreadHandle, nullptr); 356 | pthread_join(controlThreadHandle, nullptr); 357 | 358 | return 0; 359 | } 360 | -------------------------------------------------------------------------------- /test_code/test5.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // 메모리 잠금 6 | #include 7 | #include 8 | #include // 수학 함수 사용 (atan2, sqrt, M_PI) 9 | #include // Eigen 라이브러리 10 | #include // POSIX 스레드 11 | #include // 고해상도 타이머 12 | #include "rc_input.h" // RC 입력 헤더 파일 13 | #include "imu_sensor.h" // IMU 센서 헤더 포함 14 | #include "imu_calibration.h" // IMU 캘리브레이션 헤더 파일 15 | #include // B115200 설정 16 | #include // std::clamp 함수 사용 17 | #include // 시간 측정을 위한 라이브러리 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include // 리얼타임 우선순위 확인 및 설정 23 | 24 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 25 | #define MODE1 0x00 // 모드1 레지스터 26 | #define PRESCALE 0xFE // 프리스케일 레지스터 27 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 28 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 29 | 30 | const int RC_MIN = 172; 31 | const int RC_MAX = 1811; 32 | const int RC_MID = 991; 33 | const int PWM_MIN = 210; 34 | const int PWM_MAX = 405; 35 | const int MAX_ADJUSTMENT = 10; // 각 제어 입력의 최대 PWM 조정 값 36 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 37 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 38 | const float TOLERANCE_ROLL = 1; // 롤 허용 오차 (0.45도) 39 | const float TOLERANCE_PITCH = 1; // 피치 허용 오차 (0.45도) 40 | 41 | void setRealtimePriority(pthread_t thread, int priority) { 42 | struct sched_param param; 43 | param.sched_priority = priority; // 우선순위 1~99 44 | if (pthread_setschedparam(thread, SCHED_FIFO, ¶m) != 0) { 45 | perror("Failed to set real-time priority"); 46 | } 47 | } 48 | class PCA9685 { 49 | public: 50 | PCA9685(int address = PCA9685_ADDR) { 51 | char filename[20]; 52 | snprintf(filename, 19, "/dev/i2c-1"); 53 | fd = open(filename, O_RDWR); 54 | if (fd < 0) { 55 | std::cerr << "Failed to open the i2c bus" << std::endl; 56 | exit(1); 57 | } 58 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 59 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 60 | exit(1); 61 | } 62 | reset(); 63 | setPWMFreq(50); // Set frequency to 50Hz for motor control 64 | // setPWMFreq(100); 65 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 66 | } 67 | 68 | ~PCA9685() { 69 | stopAllMotors(); // 종료 시 모든 모터를 정지 70 | if (fd >= 0) { 71 | close(fd); 72 | } 73 | } 74 | 75 | void setPWM(int channel, int on, int off) { 76 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 77 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 78 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 79 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 80 | } 81 | 82 | void setMotorSpeed(int channel, int pwm_value) { 83 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 84 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 85 | return; 86 | } 87 | setPWM(channel, 0, pwm_value); 88 | } 89 | 90 | void setAllMotorsSpeeds(const int pwm_values[4]) { 91 | uint8_t buffer[17]; 92 | buffer[0] = LED0_ON_L; 93 | 94 | for (int i = 0; i < 4; i++) { 95 | int pwm = std::clamp(pwm_values[i], PWM_MIN, PWM_MAX); 96 | buffer[1 + i * 4] = 0; 97 | buffer[2 + i * 4] = 0; 98 | buffer[3 + i * 4] = pwm & 0xFF; 99 | buffer[4 + i * 4] = pwm >> 8; 100 | } 101 | 102 | asyncWrite(buffer, sizeof(buffer)); 103 | } 104 | 105 | private: 106 | int fd; 107 | 108 | void reset() { 109 | writeRegister(MODE1, 0x00); 110 | } 111 | 112 | void setPWMFreq(int freq) { 113 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 114 | uint8_t oldmode = readRegister(MODE1); 115 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 116 | writeRegister(MODE1, newmode); 117 | writeRegister(PRESCALE, prescale); 118 | writeRegister(MODE1, oldmode); 119 | usleep(5000); 120 | writeRegister(MODE1, oldmode | 0xA1); 121 | } 122 | 123 | void writeRegister(uint8_t reg, uint8_t value) { 124 | uint8_t buffer[2] = {reg, value}; 125 | int retries = 0; 126 | while (write(fd, buffer, 2) != 2) { 127 | if (++retries >= I2C_RETRY_LIMIT) { 128 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 129 | exit(1); 130 | } 131 | usleep(1000); // 1ms 대기 후 재시도 132 | } 133 | } 134 | 135 | uint8_t readRegister(uint8_t reg) { 136 | int retries = 0; 137 | while (write(fd, ®, 1) != 1) { 138 | if (++retries >= I2C_RETRY_LIMIT) { 139 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 140 | exit(1); 141 | } 142 | usleep(1000); 143 | } 144 | uint8_t value; 145 | if (read(fd, &value, 1) != 1) { 146 | std::cerr << "Failed to read from the i2c bus" << std::endl; 147 | exit(1); 148 | } 149 | return value; 150 | } 151 | 152 | void initializeMotors() { 153 | for (int i = 0; i < 4; ++i) { 154 | setMotorSpeed(i, SAFE_PWM); 155 | } 156 | } 157 | 158 | void stopAllMotors() { 159 | for (int i = 0; i < 4; ++i) { 160 | setMotorSpeed(i, SAFE_PWM); 161 | } 162 | std::cout << "All motors stopped safely." << std::endl; 163 | } 164 | }; 165 | 166 | struct PIDController { 167 | float kp, ki, kd; 168 | float prev_error; 169 | float integral; 170 | float integral_limit; 171 | float output_limit; 172 | float feedforward; 173 | float filtered_derivative; 174 | float alpha; 175 | 176 | // outlimit 설정 400->10로 기본 세팅 177 | PIDController(float p, float i, float d, float ff = 0.0f, float i_limit = 10.0f, float out_limit = 10.0f, float filter_alpha = 0.1f) 178 | : kp(p), ki(i), kd(d), feedforward(ff), prev_error(0.0f), integral(0.0f), 179 | integral_limit(i_limit), output_limit(out_limit), filtered_derivative(0.0f), alpha(filter_alpha) {} 180 | 181 | void reset() { 182 | prev_error = 0.0f; 183 | integral = 0.0f; 184 | filtered_derivative = 0.0f; 185 | } 186 | 187 | float calculate(float setpoint, float measurement, float dt) { 188 | if (dt <= 0.0f) { 189 | // dt가 0 이하일 경우, 계산을 중단 190 | return 0.0f; 191 | } 192 | 193 | // 오차 계산 194 | float error = setpoint - measurement; 195 | 196 | // 비례 항 197 | float pTerm = kp * error; 198 | 199 | // 적분 항 (적분 제한 적용) 200 | integral += error * dt; 201 | integral = std::clamp(integral, -integral_limit, integral_limit); 202 | float iTerm = ki * integral; 203 | 204 | // 미분 항 (필터링 적용) 205 | float derivative = (error - prev_error) / dt; 206 | filtered_derivative = alpha * derivative + (1.0f - alpha) * filtered_derivative; 207 | float dTerm = kd * filtered_derivative; 208 | 209 | // 이전 오차 업데이트 210 | prev_error = error; 211 | 212 | // PID 출력 (피드포워드 포함) 213 | float output = feedforward * setpoint + pTerm + iTerm + dTerm; 214 | 215 | // 출력 제한 216 | output = std::clamp(output, -output_limit, output_limit); 217 | 218 | return output; 219 | } 220 | }; 221 | 222 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 223 | double mapThrottle(int value) { 224 | if (value <= RC_MIN) return 0.0; 225 | if (value >= RC_MAX) return 1.0; 226 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 227 | } 228 | 229 | double mapControlInput(int value) { 230 | if (value < RC_MIN || value > RC_MAX) { 231 | return 0.0; 232 | } 233 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 234 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 235 | return 0.0; 236 | } 237 | 238 | int computeThrottlePWM(double throttle_normalized) { 239 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 240 | } 241 | 242 | int computeAdjustment(double control_normalized) { 243 | return static_cast(control_normalized * MAX_ADJUSTMENT); 244 | } 245 | 246 | int clamp(int value, int min_value, int max_value) { 247 | return value < min_value ? min_value : (value > max_value ? max_value : value); 248 | } 249 | 250 | // IMU 데이터와 mutex 정의 251 | IMUData imuData; 252 | std::mutex imuQueueMutex; 253 | std::queue imuDataQueue; 254 | std::condition_variable imuDataCv; 255 | 256 | void *sendIMURequestThread(void *arg) { 257 | while (true) { 258 | sendIMURequest(); // IMU 데이터 요청 전송 259 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 요청 주기 조정 260 | } 261 | return nullptr; 262 | } 263 | 264 | // IMU 스레드 함수 265 | void *imuThread(void *arg) { 266 | initIMU("/dev/ttyUSB0", B115200); 267 | IMUCalibrationData calibrationData = calibrateIMU(); 268 | float offsetGyroZ = calibrationData.offsetGyroZ; 269 | 270 | while (true) { 271 | IMUData localIMUData = readIMU(); 272 | localIMUData.gyroZ -= offsetGyroZ; // 보정된 자이로 Z값 273 | 274 | // IMU 데이터를 큐에 추가 275 | { 276 | std::lock_guard lock(imuQueueMutex); 277 | imuDataQueue.push(localIMUData); 278 | } 279 | imuDataCv.notify_one(); // 데이터 추가 알림 280 | } 281 | return nullptr; 282 | } 283 | 284 | // PID 계산 스레드 함수 285 | void *controlLoop(void *arg) { 286 | PCA9685 pca9685; 287 | initRC("/dev/ttyAMA0", B115200); 288 | 289 | PIDController rollPID(1.5f, 0.0f, 1.0f); 290 | PIDController pitchPID(2.0f, 0.5f, 0.2f); 291 | PIDController yawPID(1.2f, 0.5f, 0.5f); 292 | 293 | float roll_com = 0; 294 | float pitch_com = 0; 295 | auto previousTime = std::chrono::steady_clock::now(); 296 | 297 | while (true) { 298 | auto currentTime = std::chrono::steady_clock::now(); 299 | std::chrono::duration elapsed = currentTime - previousTime; 300 | float dt = elapsed.count(); 301 | previousTime = currentTime; 302 | 303 | IMUData localIMUData; 304 | { 305 | std::lock_guard lock(imuMutex); 306 | localIMUData = imuData; 307 | } 308 | 309 | int throttle_value = readRCChannel(3); 310 | int aileron_value = readRCChannel(1); 311 | int elevator_value = readRCChannel(2); 312 | int rudder_value = readRCChannel(4); 313 | 314 | double throttle_normalized = mapThrottle(throttle_value); 315 | double aileron_normalized = mapControlInput(aileron_value); 316 | double elevator_normalized = mapControlInput(elevator_value); 317 | double rudder_normalized = mapControlInput(rudder_value); 318 | 319 | int roll_adj = 0; 320 | int pitch_adj = 0; 321 | int yaw_adj = 0; 322 | if (std::abs(roll_com - localIMUData.roll_angle) > TOLERANCE_ROLL) { 323 | roll_adj = rollPID.calculate(roll_com, localIMUData.roll_angle, dt); 324 | } 325 | if (std::abs(pitch_com - localIMUData.pitch_angle) > TOLERANCE_PITCH) { 326 | pitch_adj = pitchPID.calculate(pitch_com, localIMUData.pitch_angle, dt); 327 | } 328 | // int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 329 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 330 | int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 331 | 332 | 333 | if (throttle_PWM <= PWM_MIN) { 334 | pca9685.setMotorSpeed(0, PWM_MIN); 335 | pca9685.setMotorSpeed(1, PWM_MIN); 336 | pca9685.setMotorSpeed(2, PWM_MIN); 337 | pca9685.setMotorSpeed(3, PWM_MIN); 338 | continue; 339 | } else { 340 | int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 341 | int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 342 | 343 | int motor1_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 344 | int motor2_adj = aileron_adj_total - elevator_adj_total - yaw_adj; 345 | int motor3_adj = aileron_adj_total + elevator_adj_total + yaw_adj; 346 | int motor4_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 347 | 348 | motor1_PWM = clamp(throttle_PWM + motor1_adj, PWM_MIN, PWM_MAX); 349 | motor2_PWM = clamp(throttle_PWM + motor2_adj, PWM_MIN, PWM_MAX); 350 | motor3_PWM = clamp(throttle_PWM + motor3_adj, PWM_MIN, PWM_MAX); 351 | motor4_PWM = clamp(throttle_PWM + motor4_adj, PWM_MIN, PWM_MAX); 352 | } 353 | 354 | int motor_pwm[4] = {motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM}; 355 | 356 | pca9685.setMotorSpeed(0, motor1_PWM); 357 | pca9685.setMotorSpeed(1, motor2_PWM); 358 | pca9685.setMotorSpeed(2, motor3_PWM); 359 | pca9685.setMotorSpeed(3, motor4_PWM); 360 | } 361 | return nullptr; 362 | } 363 | 364 | int main() { 365 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 366 | std::cerr << "Failed to lock memory!" << std::endl; 367 | return -1; 368 | } 369 | 370 | pthread_t imuThreadHandle, controlThreadHandle; 371 | 372 | if (pthread_create(&imuThreadHandle, nullptr, imuThread, nullptr) != 0) { 373 | std::cerr << "Failed to create IMU thread!" << std::endl; 374 | return -1; 375 | } 376 | 377 | if (pthread_create(&controlThreadHandle, nullptr, controlLoop, nullptr) != 0) { 378 | std::cerr << "Failed to create Control thread!" << std::endl; 379 | return -1; 380 | } 381 | 382 | pthread_join(imuThreadHandle, nullptr); 383 | pthread_join(controlThreadHandle, nullptr); 384 | 385 | return 0; 386 | } 387 | -------------------------------------------------------------------------------- /test_code/test7.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // 메모리 잠금 6 | #include 7 | #include 8 | #include // 수학 함수 사용 (atan2, sqrt, M_PI) 9 | #include // Eigen 라이브러리 10 | #include // POSIX 스레드 11 | #include // 고해상도 타이머 12 | #include "rc_input.h" // RC 입력 헤더 파일 13 | #include "imu_sensor.h" // IMU 센서 헤더 포함 14 | #include "imu_calibration.h" // IMU 캘리브레이션 헤더 파일 15 | #include // B115200 설정 16 | #include // std::clamp 함수 사용 17 | #include // 시간 측정을 위한 라이브러리 18 | #include 19 | #include 20 | 21 | #define PCA9685_ADDR 0x40 // PCA9685 I2C 주소 22 | #define MODE1 0x00 // 모드1 레지스터 23 | #define PRESCALE 0xFE // 프리스케일 레지스터 24 | #define LED0_ON_L 0x06 // 첫 번째 채널 ON 낮은 바이트 레지스터 25 | #define LED0_OFF_L 0x08 // 첫 번째 채널 OFF 낮은 바이트 레지스터 26 | 27 | const int RC_MIN = 172; 28 | const int RC_MAX = 1811; 29 | const int RC_MID = 991; 30 | // const int PWM_MIN = 210; // 50Hz 기준 31 | // const int PWM_MAX = 405; // 50Hz 기준 32 | const int PWM_MIN = 1680; // 400Hz 기준 33 | const int PWM_MAX = 3240; // 400Hz 기준 34 | const int MAX_ADJUSTMENT = 10; // 각 제어 입력의 최대 PWM 조정 값 35 | const int I2C_RETRY_LIMIT = 3; // I2C 오류 시 재시도 횟수 36 | const int SAFE_PWM = PWM_MIN; // 초기화 및 안전한 PWM 값 37 | const int LOOP_DELAY_US = 26000; // 주기적인 대기 시간 (10ms) 38 | const float MAX_ANGLE = 90.0f; // 최대 각도 (예시) 39 | const float TOLERANCE_ROLL = 0.05f * MAX_ANGLE; // 롤 허용 오차 (0.45도) 40 | const float TOLERANCE_PITCH = 0.01f * MAX_ANGLE; // 피치 허용 오차 (0.45도) 41 | 42 | class PCA9685 { 43 | public: 44 | PCA9685(int address = PCA9685_ADDR) { 45 | char filename[20]; 46 | snprintf(filename, 19, "/dev/i2c-1"); 47 | fd = open(filename, O_RDWR); 48 | if (fd < 0) { 49 | std::cerr << "Failed to open the i2c bus" << std::endl; 50 | exit(1); 51 | } 52 | if (ioctl(fd, I2C_SLAVE, address) < 0) { 53 | std::cerr << "Failed to acquire bus access and/or talk to slave" << std::endl; 54 | exit(1); 55 | } 56 | reset(); 57 | // setPWMFreq(100); // Set frequency to 100Hz for motor control 58 | setPWMFreq(400); // Set frequency to 400Hz for motor control 59 | initializeMotors(); // 모든 모터를 초기 안전 PWM 값으로 설정 60 | } 61 | 62 | ~PCA9685() { 63 | stopAllMotors(); // 종료 시 모든 모터를 정지 64 | if (fd >= 0) { 65 | close(fd); 66 | } 67 | } 68 | 69 | void setPWM(int channel, int on, int off) { 70 | writeRegister(LED0_ON_L + 4 * channel, on & 0xFF); 71 | writeRegister(LED0_ON_L + 4 * channel + 1, on >> 8); 72 | writeRegister(LED0_OFF_L + 4 * channel, off & 0xFF); 73 | writeRegister(LED0_OFF_L + 4 * channel + 1, off >> 8); 74 | } 75 | 76 | void setMotorSpeed(int channel, int pwm_value) { 77 | if (pwm_value < PWM_MIN || pwm_value > PWM_MAX) { 78 | std::cerr << "PWM value out of range (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 79 | return; 80 | } 81 | setPWM(channel, 0, pwm_value); 82 | } 83 | 84 | void setAllMotorsSpeed(const std::vector& pwm_values) { 85 | if (pwm_values.size() > 16) { // PCA9685는 최대 16 채널까지 지원 86 | std::cerr << "Too many channels. PCA9685 supports up to 16 channels." << std::endl; 87 | return; 88 | } 89 | 90 | uint8_t buffer[69]; // 최대 16 채널 (4 bytes * 16 = 64) + 1 byte (start register address) 91 | buffer[0] = LED0_ON_L; // 시작 레지스터 (LED0_ON_L) 92 | 93 | // 각 모터 채널에 대해 PWM 데이터를 작성 94 | for (size_t i = 0; i < pwm_values.size(); ++i) { 95 | if (pwm_values[i] < PWM_MIN || pwm_values[i] > PWM_MAX) { 96 | std::cerr << "PWM value out of range for channel " << i 97 | << " (" << PWM_MIN << "-" << PWM_MAX << ")" << std::endl; 98 | return; 99 | } 100 | 101 | int on = 0; // ON 시간은 항상 0으로 설정 (PWM 신호 시작 시점 고정) 102 | int off = pwm_values[i]; // OFF 시간만 설정 (PWM 듀티 비율 조절) 103 | 104 | buffer[1 + i * 4] = on & 0xFF; // LEDn_ON_L 105 | buffer[2 + i * 4] = (on >> 8) & 0xFF; // LEDn_ON_H 106 | buffer[3 + i * 4] = off & 0xFF; // LEDn_OFF_L 107 | buffer[4 + i * 4] = (off >> 8) & 0xFF; // LEDn_OFF_H 108 | } 109 | 110 | // 블록 전송으로 모든 채널 업데이트 111 | int retries = 0; 112 | while (write(fd, buffer, 1 + pwm_values.size() * 4) != 1 + pwm_values.size() * 4) { 113 | if (++retries >= I2C_RETRY_LIMIT) { 114 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 115 | exit(1); 116 | } 117 | usleep(1000); // 1ms 대기 후 재시도 118 | } 119 | } 120 | private: 121 | int fd; 122 | 123 | void reset() { 124 | writeRegister(MODE1, 0x00); 125 | } 126 | 127 | void setPWMFreq(int freq) { 128 | uint8_t prescale = static_cast(25000000.0 / (4096.0 * freq) - 1.0); 129 | uint8_t oldmode = readRegister(MODE1); 130 | uint8_t newmode = (oldmode & 0x7F) | 0x10; 131 | writeRegister(MODE1, newmode); 132 | writeRegister(PRESCALE, prescale); 133 | writeRegister(MODE1, oldmode); 134 | usleep(5000); 135 | writeRegister(MODE1, oldmode | 0xA1); 136 | } 137 | 138 | void writeRegister(uint8_t reg, uint8_t value) { 139 | uint8_t buffer[2] = {reg, value}; 140 | int retries = 0; 141 | while (write(fd, buffer, 2) != 2) { 142 | if (++retries >= I2C_RETRY_LIMIT) { 143 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 144 | exit(1); 145 | } 146 | // usleep(1000); // 1ms 대기 후 재시도 147 | } 148 | } 149 | 150 | uint8_t readRegister(uint8_t reg) { 151 | int retries = 0; 152 | while (write(fd, ®, 1) != 1) { 153 | if (++retries >= I2C_RETRY_LIMIT) { 154 | std::cerr << "Failed to write to the i2c bus after retries" << std::endl; 155 | exit(1); 156 | } 157 | usleep(1000); 158 | } 159 | uint8_t value; 160 | if (read(fd, &value, 1) != 1) { 161 | std::cerr << "Failed to read from the i2c bus" << std::endl; 162 | exit(1); 163 | } 164 | return value; 165 | } 166 | 167 | void initializeMotors() { 168 | for (int i = 0; i < 4; ++i) { 169 | setMotorSpeed(i, SAFE_PWM); 170 | } 171 | } 172 | 173 | void stopAllMotors() { 174 | for (int i = 0; i < 4; ++i) { 175 | setMotorSpeed(i, SAFE_PWM); 176 | } 177 | std::cout << "All motors stopped safely." << std::endl; 178 | } 179 | }; 180 | 181 | struct PIDController { 182 | float kp, ki, kd; 183 | float prev_error; 184 | float integral; 185 | float integral_limit; 186 | float output_limit; 187 | float feedforward; 188 | float filtered_derivative; // 필터링된 미분 항 189 | float alpha; // 필터 계수 190 | 191 | PIDController(float p, float i, float d, float ff = 0.0f, float i_limit = 10.0f, float out_limit = (PWM_MAX - PWM_MIN)*0.1f, float filter_alpha = 0.1f) 192 | : kp(p), ki(i), kd(d), feedforward(ff), prev_error(0.0f), integral(0.0f), 193 | integral_limit(i_limit), output_limit(out_limit), filtered_derivative(0.0f), alpha(filter_alpha) {} 194 | 195 | void reset() { 196 | prev_error = 0.0f; 197 | integral = 0.0f; 198 | filtered_derivative = 0.0f; 199 | } 200 | 201 | float calculate(float setpoint, float measurement, float dt) { 202 | if (dt <= 0.0f) { 203 | // dt가 0 이하일 경우, 계산을 중단 204 | return 0.0f; 205 | } 206 | 207 | // 오차 계산 208 | float error = setpoint - measurement; 209 | 210 | // 비례 항 211 | float pTerm = kp * error; 212 | 213 | // 적분 항 (적분 제한 적용) 214 | integral += error * dt; 215 | integral = std::clamp(integral, -integral_limit, integral_limit); 216 | float iTerm = ki * integral; 217 | 218 | // 미분 항 (필터링 적용) 219 | float derivative = (error - prev_error) / dt; 220 | filtered_derivative = alpha * derivative + (1.0f - alpha) * filtered_derivative; 221 | float dTerm = kd * filtered_derivative; 222 | 223 | // 이전 오차 업데이트 224 | prev_error = error; 225 | 226 | // PID 출력 (피드포워드 포함) 227 | float output = feedforward * setpoint + pTerm + iTerm + dTerm; 228 | 229 | // 출력 제한 230 | output = std::clamp(output, -output_limit, output_limit); 231 | 232 | return output; 233 | } 234 | }; 235 | 236 | // 스로틀 값을 0.0 ~ 1.0 범위로 매핑하는 함수 237 | double mapThrottle(int value) { 238 | if (value <= RC_MIN) return 0.0; 239 | if (value >= RC_MAX) return 1.0; 240 | return static_cast(value - RC_MIN) / (RC_MAX - RC_MIN); 241 | } 242 | 243 | double mapControlInput(int value) { 244 | if (value < RC_MIN || value > RC_MAX) { 245 | return 0.0; 246 | } 247 | if (value < RC_MID) return static_cast(value - RC_MID) / (RC_MID - RC_MIN); 248 | if (value > RC_MID) return static_cast(value - RC_MID) / (RC_MAX - RC_MID); 249 | return 0.0; 250 | } 251 | 252 | int computeThrottlePWM(double throttle_normalized) { 253 | return static_cast(PWM_MIN + throttle_normalized * (PWM_MAX - PWM_MIN)); 254 | } 255 | 256 | int computeAdjustment(double control_normalized) { 257 | return static_cast(control_normalized * MAX_ADJUSTMENT); 258 | } 259 | 260 | int clamp(int value, int min_value, int max_value) { 261 | return value < min_value ? min_value : (value > max_value ? max_value : value); 262 | } 263 | 264 | // IMU 데이터와 mutex 정의 265 | IMUData imuData = {}; 266 | std::mutex imuMutex; 267 | // IMU 스레드 함수 268 | void *imuThread(void *arg) { 269 | initIMU("/dev/ttyUSB0", B115200); 270 | IMUCalibrationData calibrationData = calibrateIMU(); 271 | float offsetGyroZ = calibrationData.offsetGyroZ; 272 | 273 | while (true) { 274 | IMUData localIMUData = readIMU(imuData); 275 | localIMUData.gyroZ -= offsetGyroZ; // 보정된 자이로 Z값 276 | 277 | // Mutex로 IMU 데이터 보호 278 | { 279 | std::lock_guard lock(imuMutex); 280 | imuData = localIMUData; 281 | } 282 | 283 | std::this_thread::sleep_for(std::chrono::milliseconds(10)); 284 | } 285 | return nullptr; 286 | } 287 | 288 | // PID 계산 스레드 함수 289 | void *controlLoop(void *arg) { 290 | PCA9685 pca9685; 291 | initRC("/dev/ttyAMA0", B115200); 292 | 293 | PIDController rollPID(1.0f, 0.01f, 0.5f); 294 | PIDController pitchPID(0.0f, 0.0f, 0.0f); 295 | PIDController yawPID(1.2f, 0.5f, 0.5f); 296 | 297 | float roll_com = 0; 298 | float pitch_com = 0; 299 | auto previousTime = std::chrono::high_resolution_clock::now(); 300 | 301 | int roll_adj = 0; 302 | int pitch_adj = 0; 303 | int yaw_adj = 0; 304 | 305 | while (true) { 306 | // std::this_thread::sleep_for(std::chrono::milliseconds(100)); 307 | IMUData localIMUData; 308 | { 309 | std::lock_guard lock(imuMutex); 310 | localIMUData = imuData; 311 | } 312 | auto currentTime = std::chrono::high_resolution_clock::now(); 313 | std::chrono::duration elapsed = currentTime - previousTime; // 경과 시간 계산 314 | float dt = elapsed.count(); // 밀리초로 변환 315 | previousTime = currentTime; 316 | // std::cout << "dt: " << dt << std::endl; 317 | 318 | dt = dt / 1000.0f; 319 | 320 | int throttle_value = readRCChannel(3); 321 | int aileron_value = readRCChannel(1); 322 | int elevator_value = readRCChannel(2); 323 | int rudder_value = readRCChannel(4); 324 | 325 | double throttle_normalized = mapThrottle(throttle_value); 326 | double aileron_normalized = mapControlInput(aileron_value); 327 | double elevator_normalized = mapControlInput(elevator_value); 328 | double rudder_normalized = mapControlInput(rudder_value); 329 | 330 | if (std::abs(roll_com - localIMUData.roll_angle) > TOLERANCE_ROLL) { 331 | roll_adj = rollPID.calculate(roll_com, localIMUData.roll_angle, dt); 332 | } 333 | if (std::abs(pitch_com - localIMUData.pitch_angle) > TOLERANCE_PITCH) { 334 | pitch_adj = pitchPID.calculate(pitch_com, localIMUData.pitch_angle, dt); 335 | } 336 | 337 | // int yaw_adj = yawPID.calculate(rudder_normalized, correctedGyroZ, dt); 338 | int throttle_PWM = computeThrottlePWM(throttle_normalized); 339 | int motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM; 340 | std::cout << "localIMUData.roll_angle: " << localIMUData.roll_angle << std::endl; 341 | std::cout << "roll_adj: " << roll_adj << std::endl; 342 | if (throttle_PWM <= PWM_MIN) { 343 | std::vector motor_min_pwm = {PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN}; 344 | pca9685.setAllMotorsSpeed(motor_min_pwm); 345 | continue; 346 | } 347 | else { 348 | int aileron_adj_total = computeAdjustment(aileron_normalized) + roll_adj; 349 | // int elevator_adj_total = computeAdjustment(elevator_normalized) + pitch_adj; 350 | int elevator_adj_total = 0; 351 | //std::cout << "adj: " << roll_adj << "total" << aileron_adj_total << std::endl; 352 | int motor1_adj = -aileron_adj_total + elevator_adj_total + yaw_adj; 353 | int motor2_adj = aileron_adj_total - elevator_adj_total - yaw_adj; 354 | int motor3_adj = aileron_adj_total + elevator_adj_total + yaw_adj; 355 | int motor4_adj = -aileron_adj_total - elevator_adj_total - yaw_adj; 356 | 357 | motor1_PWM = clamp(throttle_PWM + motor1_adj, PWM_MIN, PWM_MAX); 358 | motor2_PWM = clamp(throttle_PWM + motor2_adj, PWM_MIN, PWM_MAX); 359 | motor3_PWM = clamp(throttle_PWM + motor3_adj, PWM_MIN, PWM_MAX); 360 | motor4_PWM = clamp(throttle_PWM + motor4_adj, PWM_MIN, PWM_MAX); 361 | 362 | } 363 | std::vector motor_pwm = {motor1_PWM, motor2_PWM, motor3_PWM, motor4_PWM}; 364 | pca9685.setAllMotorsSpeed(motor_pwm); 365 | 366 | pca9685.setMotorSpeed(0, motor1_PWM); 367 | pca9685.setMotorSpeed(1, motor2_PWM); 368 | pca9685.setMotorSpeed(2, motor3_PWM); 369 | pca9685.setMotorSpeed(3, motor4_PWM); 370 | 371 | // std::this_thread::sleep_for(std::chrono::milliseconds(10)); 372 | 373 | } 374 | return nullptr; 375 | } 376 | 377 | int main() { 378 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 379 | std::cerr << "Failed to lock memory!" << std::endl; 380 | return -1; 381 | } 382 | 383 | pthread_t imuThreadHandle, controlThreadHandle; 384 | 385 | if (pthread_create(&imuThreadHandle, nullptr, imuThread, nullptr) != 0) { 386 | std::cerr << "Failed to create IMU thread!" << std::endl; 387 | return -1; 388 | } 389 | 390 | if (pthread_create(&controlThreadHandle, nullptr, controlLoop, nullptr) != 0) { 391 | std::cerr << "Failed to create Control thread!" << std::endl; 392 | return -1; 393 | } 394 | 395 | pthread_join(imuThreadHandle, nullptr); 396 | pthread_join(controlThreadHandle, nullptr); 397 | 398 | return 0; 399 | } 400 | -------------------------------------------------------------------------------- /test_code/thread_manager.cpp: -------------------------------------------------------------------------------- 1 | // 비행 제어에서 사용될 여러 스레드와 태스크 관리 2 | // 비행 제어 시스템에서 여러 태스크나 스레드를 관리하는 역할을 합니다. 이를 통해 비행 제어 시스템은 여러 개의 센서 데이터를 병렬로 처리하거나, 모터 제어와 같은 중요한 작업을 실시간으로 처리할 수 있습니다. -------------------------------------------------------------------------------- /test_code/timer.cpp: -------------------------------------------------------------------------------- 1 | // 주기적인 타이머를 설정하고 관리 -------------------------------------------------------------------------------- /test_code/udp_communication.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davincilabs-code/onboard-flight-control-software/9b69f1c26a44698479f6b1ae1dce22dfb7ea724b/test_code/udp_communication.cpp -------------------------------------------------------------------------------- /test_code/udp_communication.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davincilabs-code/onboard-flight-control-software/9b69f1c26a44698479f6b1ae1dce22dfb7ea724b/test_code/udp_communication.h -------------------------------------------------------------------------------- /폴더 설명.txt: -------------------------------------------------------------------------------- 1 | └── src 2 | ├── ioss 3 | │   ├── gps_sensor.cpp 4 | │   ├── gps_sensor.h 5 | │   ├── imu_sensor.cpp 6 | │   ├── imu_sensor.h 7 | │   ├── motor_input.cpp 8 | │   ├── motor_input.h 9 | │   ├── rc.input.cpp 10 | │   └── rc_input.h 11 | │   ├── barometer_sensor.cpp 12 | │   └── barometer_sensor.h 13 | ├── oss 14 | │   ├── os_api.cpp 15 | │   ├── thread_manager.cpp 16 | │   └── timer.cpp 17 | ├── pcs 18 | ├── psss 19 | │   ├── current_pose.csv 20 | │   ├── ekf.cpp 21 | │   ├── ekf.h 22 | │   ├── flight_control.cpp 23 | │   ├── flight_control.h 24 | │   ├── flight_mode.cpp 25 | │   ├── flight_mode.h 26 | │   ├── main.cpp 27 | │   ├── pose_estimator.cpp 28 | │   └── pose_estimator.h 29 | │   ├── motor_contorl.h 30 | │   └── motor_control.cpp 31 | └── tss 32 | ├── udp_communication.cpp 33 | └── udp_communication.h 34 | --------------------------------------------------------------------------------