CHƯƠNG 3: NỘI DUNG NGHIÊN CỨU
3.4 Nghiên cứu giải thuật và xây dựng phần mềm
3.4.1 Sơ đồ khối của hệ thống trên máy bay
Hình 3.48: Sơ đồ khối tổng quát của hệ thống trên máy bay
Vcc 3.3 V Vcc 5 V Vi điều khiển trung tâm GND
MSP430F5418A RF
module
GPS
module MPU6050
MSP430G2553 (2)
Receiver
ESC
MultiplexersTS3A24157 Rudder servo
Ailerons servo Elevator servo
Motor MSP430G2553 (1)
Pin Li-po
Nguồn Step-down
74LVC4245 OPTO 4N35
ESC
Board Raspbery Pi
Cầu phân điện áp
**Khối vi điều khiển trung tâm MSP430F5418A
Khi được cấp nguồn, vi điều khiển sẽ tiến hành kiểm tra việc kết nối với cảm biến MPU-6050 để đảm bảo sẽ đọc được giá trị từ cảm biến, nếu không nhận diện được cảm biến trên đườg truyền I2C, vi điều khiển sẽ liên tục gửi thông báo về trạm mặt đất thông qua module RF để cảnh báo người dùng.
Khi quá trình kiểm tra kết nối với cảm biến hoàn tất, vi điều khiển tiếp tục thiết lập cấu hình hoạt động cho cảm biến và tiến hành đọc giá trị gia tốc và vận tốc góc và chạy thuật toán lọc Kalman để xác định chính xác góc nghiêng của máy bay. Việc sử dụng bộ lọc Kalman lọc dữ liệu góc nghiêng từ cảm biến sẽ được tiến hành liên tục nhằm tránh trường hợp đáp ứng ban đầu của bộ lọc chưa ổn định, sinh ra sai lệch đang kể.
Khi có tín hiệu xác định trạng thái hoạt động của hệ thống là chế độ tự động thì dữ liệu góc nghiêng sẽ được xử lí và truyền xuống vi điều khiển MSP430G2553 thứ hai thông qua giao thức UART với tốc độ baud là 9600 bps.
Khi có bản dữ liệu GPS truyền về theo chuẩn truyền UART tốc độ baud 9600bps, vi điều khiển trung tâm sẽ lọc lấy thông tin kinh độ, vĩ độ và vận tốc hiện tại của máy bay. Sau đó sẽ truyền về trạm mặt đất thông qua mode RF HM-TRP.
**Vi điều khiển MSP430G2533 thứ nhất:
Do hệ thống sử dụng một kênh trong bộ truyền (Transmiter) JR Propo làm tín hiệu điều khiển xác định trạng thái máy bay hoạt động là tự động hay điều khiển tay.
Nhóm dùng một vi điều khiển hoạt động độc lập để xác định trạng thái hệ thống, nhằm tránh trường hợp vi điều khiển bị lỗi dẫn đến treo hệ thống và không lấy lại quyền điều khiển từ mặt đất.
Kênh điều khiển là kênh Gear trên bộ thu, khi hoạt động luôn tạo ra xung vuông có tần số 50 Hz và sẽ thay đổi độ rộng xung tùy theo nấc điều khiển trên bộ phát. Dựa vào kết quả đo đạc bằng Ocsiloscope tại phòng thí nghiệm Viễn thông, độ rộng xung của chân điều khiển tại nấc ON là 1,9 ms và nấc OFF là 0,8 ms.
Vi điều khiển MSP430G2533 này sẽ tiến hành xác định độ rộng xung ở kênh điều khiển, từ đó so sánh với khoảng độ rộng xung đo đạc được và xác định chế độ hoạt động của máy bay. Sau đó xuất thông báo cho khối điều khiển trung tâm và khối đa hợp bằng mức logic.
**Vi điều khiển MSP430G2553 thứ 2:
Máy bay mô hình được điều khiển thông qua các servo ở hai cánh trước Ailerons, cánh đuôi ngang Elevator, cánh đuôi đứng Rudder và bộ ESC. Vì vậy khi hệ thống chuyển sang chế độ tự động, phải có một vi điều khiển nhận chuỗi dữ liệu góc quay của các servo từ vi điều khiển trung tâm thông qua giao thức UART.
Trong chuỗi dữ liệu truyền xuống để điều chế xung PWM phải có một dạng khung truyền nhất định để giảm bớt các trường hợp nhiễu ảnh hưởng khi truyền.
Bảng 3.7: Cấu trúc khung truyền dữ liệu giữa MSP430F5418A và MSP430G2553(2)
Start Data
0x0D 0x0A Data[1] Data[2] Data[3] Data[4]
Cấu trúc khung truyền dữ liệu từ vi điều khiển trung tâm gồm có tín hiệu Start là chuỗi 2 byte 0x0A và 0x0D, tiếp theo đó là 4 bytes dữ liệu theo thứ tự sau:
Byte 1: phần trăm tốc độ kênh ga (throttle), điều khiển tốc độ quay động cơ Byte 2 : góc quay servo cánh đuôi đứng Rudder
Byte 3: góc quay servo cánh đuôi ngang Aileron Byte 4 : góc quay servo cánh trước Elevator
Sau khi nhận được chuỗi dữ liệu từ vi điều khiển trung tâm, vi điều khiển MSP430G2553 thứ hai tiến hành kiểm tra chuỗi thông tin nhận được với cấu trúc đã định sẵn, cập nhật độ rộng xung và điều chế xung PWM 50Hz.
**Khối truyền nhận FR HM-TRP 433
Khi vi điều khiển trung tâm có được dữ liệu kinh độ vĩ độ từ module GPS, chuỗi này sẽ được truyền từ module HM-TRP 433 đặt trên máy bay về module HM-TRP 433 đặt tại mặt đất để làm nguồn dữ liệu hiển thị bản đồ số.
Hai module HM-TRP 433 truyền nhận tín hiệu qua kênh truyền RF, để đảm bảo tính ổn định của tín hiệu truyền nhận và dễ dàng trong việc xác định chuỗi thông tin của trạm mặt đất, ta phải định dạng khung truyền nhận tín hiệu theo một khung truyền đã thống nhất giữa nơi phát và nơi nhận.
Bảng 3.8: Cấu trúc khung truyền dữ liệu thông tin GPS từ máy bay
Start Chuỗi dữ liệu
0x0D 0x0A <UTC>,<status>,<vĩ độ>,<hướng>,<kinh độ>,<hướng>,<vận tốc>
Tín hiệu Start gồm hai Byte là 0x0D và 0x0A và chuỗi dữ liệu là một đoạn được sao chép trong đoạn tin GPRMC.
**Module GPS
Module GPS Ublox NEO 6M có nhiệm vụ thu nhận tín hiệu GPS từ vệ tinh, xử lí và xuất ra chuỗi dữ liệu theo chuẩn bản tin NMEA. Bản tin này sẽ được truyền về vi điều khiển trung tâm qua giao thức UART, tốc độ baud 9600 bps để lọc lấy dữ liệu cần thiết và truyền về trạm mặt đất.
Vi điều khiển trung tâm liên tục nhận bản tin này từ module GPS với tần số 1Hz và dựa vào cấu trúc bản tin để xác định bản tin GPRMC, lọc thông tin kinh độ, vĩ độ và vận tốc.
**Khối cảm biến MPU-6050
Cảm biến MPU-6050 tích hợp bao gồm: 1 cảm biến gia tốc 3 trục và 1 con quay hồi chuyển 3 trục. Khi hệ thống hoạt động, cảm biến sẽ đo đạc gia tốc trọng trường chiếu lên 3 trục của cảm biến gia tốc và tốc độ góc của 3 trục thông qua con quay hồi chuyển. Dữ liệu này sẽ được vi điều khiển trung tâm lấy về thông qua chuẩn kết nối I2C với tốc độ khoảng 100 Kbps.
Theo thiết lập ban đầu của hệ thống, cảm biến hoạt động có cấu hình như sau:
-Khoảng đo lường gia tốc: ± 2 g
-Khoảng đo lường vận tốc góc: ± 500 độ/s -Tần số lấy mẫu: 100 Hz
-Tần số cắt bộ lọc hạ thông số: 42 Hz
**Khối đa hợp TS3A24157
Đa hợp TS3A24157 có chức năng lựa chọn nguồn xung PWM đưa vào các servo để điều khiển hướng cho máy bay.
Khối đa hợp bao gồm 2 IC đa hợp TS3A24157, có ngõ điều khiển nối với MSP430G2553 thứ nhất, xung PWM từ bộ thu và MSP430G2553 thứ hai là các kênh ngõ vào. Khi trạng thái hệ thống được xác định là điều khiển bằng tay, khối đa hợp chỉ cho phép xung PWM của 4 kênh ngõ ra bộ điều khiển đi qua và ngược lại, 4 kênh xung PWM từ vi điều khiển MSP430G2553 thứ 2 chỉ được đi qua khi hệ thống trong trạng thái tự động.
**Khối Pin Ly-po và nguồn Step-down
Nguồn cung cấp cho hệ thống đóng vai trò rất quan trọng đối với hoạt động của hệ thống. Các linh kiện của hệ thống chủ yếu là sản phẩm của Texas Instrument, do đó ngưỡng hoạt động đều trong khoảng 3V – 3.6V. Đồng thời, nguồn cung cấp phải có khả năng cấp dòng điện tương đố lớn vì hệ thống có cơ cấu chấp hành là 4 servo với dòng tiêu thụ trung bình mỗi servo là 200 mA và phải có hiệu suất chuyển đổi điện áp cao để nâng cao thời gian hoạt động của hệ thống.
Do đó nhóm sử dụng loại nguồn Step-dowm dùng IC LM2576 để có chuẩn điện áp ngõ ra phù hợp và hiệu suất chuyển đổi điện áp cao (xấp xỉ 80%).
Ngoài ra, pin Ly-po còn cung cấp nguồn cho ESC để điều khiển tốc độ động cơ.
**Khối chuyển đổi điện áp 74LVC4245
Khối có chức năng là chuyển đổi điện áp ngõ ra bộ thu từ 5.0V xuống 3.3V để có thể hoạt động với đa hợp.
**Khối Opto 4N35
Khối có chức năng cách điện giữa các servo và ngõ ra đa hợp. Khối sẽ giúp bảo vệ mạch điều khiển khi có servo gặp vấn đề trong cơ cấu chuyển động, có thể sinh ra dòng tiêu thụ lớn.
**Board Raspbery Pi
Khối dùng để chụp ảnh và lưu vào bộ nhớ USB với chu kỳ định trước.
3.4.2 Sơ đồ khối của hệ thống ở trạm thu
Hình 3.49: Sơ đồ khối của hệ thống ở trạm thu
-Module RF HM-TRP tại mặt đất có nhiệm vụ nhận dữ liệu từ module RF đăt trên máy bay và truyền thông tin điều khiển động cơ khi máy bay ở trạng thái tự động.
-Khối USB to UART sẽ cho phép truyền nhận dữ liệu giữa module RF-TRP và phần mềm hiển thị bản đồ số trên PC.
-Phần mềm hiển thị bản đồ số được viết trên nền tảng C#, có chức năng nhận chuỗi thông tin vị trí và vận tốc của máy bay truyền về bằng đường truyền RF. Sau khi nhận được chuỗi dữ liệu, phần mềm tiến hành lọc lấy kinh độ, vĩ độ, vận tốc và hiển thị tọa độ này lên bản đồ Google Map. Ngoài ra, phần mềm cho phép gửi dữ liệu điều chỉnh tốc độ động cơ máy bay.
3.4.3 Lưu đồ giải thuật
**Lưu đồ giải thuật của vi điều khiển trung tâm MSP430F5418A
MODULE RF UART - USB PC
Hiện thị bản đồ số
Hình 3.50: Lưu đồ giải thuật của board xử lý trung tâm MSP430F5418A Ngắt
Timer A0
Dem_timer ++
End Thiết lập dao động DCO
Thiết lập UART A0 và UART A1 Thiết lập Timer
Thiết lập ngõ vào ra Begin
Kiểm tra kết nối với cảm biến MPU6050
Cấu hình cảm biến MPU 6050
Đọc cảm biến
Gửi dữ liệu dùng kênh truyền UART A0
Lọc Kalman
Chế độ tự động?
Đ Có dữ liệu truyền lên?
Thay đổi kênh ga và rudder Đ
S
S
Giải thích lưu đồ:
Khi khởi động, vi điều khiển sẽ thiết lập xung clock DCO hoạt động là 16777216 Hz, cấu hình 2 kênh UART A0 và A1 có tốc độ baud 9600 bps, thiết lập bộ định thời Timer với chu kì 0.1 ms dùng cho bộ lọc Kalman và thiết lập ngõ vào xác định trạng thái hệ thống (điều khiển tay/tự động).
Sau khi hoàn tất các thiết lập, vi điều khiển trung tâm tiến hành kiểm tra việc kết nối I2C với cảm biến MPU-6050. Quá trình kiểm tra sẽ được tiến hành theo sơ đồ sau:
Hình 3.51: Lưu đồ giải thuật kiểm tra kết nối MPU6050
Toàn bộ quá trình kiểm tra được đặt trong vòng lặp while vô tận. Biến check được gán giá trị của thanh ghi WHO_AM_I có địa chỉ là 0x75 trong cảm biến MPU- 6050. Nếu biến check có giá trị là địa chỉ I2C của cảm biến (0x68) thì việc kết nối với giữa vi điều khiển và cảm biến thành công và thoát khỏi vòng lặp while thực hiện công việc kế tiếp.
Sau khi thông qua phần kiểm tra kết nối với cảm biến, vi điều khiển trung tâm sẽ đọc giá trị giá tốc và vận tốc góc từ các thanh ghi của cảm biến và thực hiện quá trình lọc Kalman để xác định chính xác góc Pitch và Roll của máy bay. Dữ liệu sau khi lọc sẽ được dùng để điều khiển các servo điều chính hướng của máy bay.
Trong quá trình hoạt động, nếu có dữ liệu về phần trăm công suất động cơ và góc quay của Rudder truyền lên từ trạm mặt đất, vi điều khiển sẽ thực hiện thay đổi giá trị truyền đi của kênh ga và rudder.
Thông qua xác định mức logic tại Port 1.4, vi điều khiển trung tâm sẽ xác định trạng thái của hệ thống (tự động/điều khiển). Nếu mức logic là 1 thì hệ thống ở trạng thái điều khiển bằng tay, vi điều khiển trung tâm sẽ không gửi dữ liệu điều khiển máy bay đến MSP430G2553(2), và ngược lại, khi mức logic là mức 0 thì vi điều khiển trung tâm sẽ gửi 4 byte dữ liệu về góc quay của các servo và phần trăm công suất của động cơ thông qua chuẩn giao tiếp UART A1.
Toàn bộ quá trình đọc dữ liệu, lọc Kalman, xác định trạng thái hệ thống và thay đổi kênh ga, rudder được đặt trong vòng lặp vô hạn, đảm bảo hoạt động liên tục của máy bay.
Check = read_slave(0x75)
Check==0x6 8
Break S
Gửi cảnh báo
**Giải thuật lọc thông tin kinh độ, vĩ độ từ bản tin GPS
Hình 3.52: Lưu đồ giải thuật lọc thông tin từ bản tin NMEA Ngắt UART A0
Gps_flag = 1 Count_nhan = 0 UCA0RXBUF==R
Gps_flag ==1 UCA0RXBUF==’*
’
Gps_flag = 0 Count_nhan = 0 Xác định vị trí dấy phẩy (,)
trong chuỗi nhận Lấy trạng thái bản tin
Trạng thái == ‘V’
Gửi “No data” qua UART A0
END
temp[count_nhan] = UCA0RXBUF;
count_nhan++;
Đ
S Đ S
Đ
S
Copy chuỗi dữ liệu Truyền qua UART A0
Giải thích lưu đồ:
Việc nhận bản tin NMEA từ module GPS và lọc lấy thông tin về kinh độ, vĩ độ và vận tốc của máy bay được thực hiện trong chương trình ngắt nhận UART A0.
Theo cấu hình được thiết lập cho module GPS, mỗi giây module sẽ xuất ra một bản tin định dạng NMEA chứa các thông tin mà module thu được từ tín hiệu GPS thông qua kênh truyền UART có tốc độ baud 9600 bps. Ta chỉ cần lọc đoạn tin GPRMC là có đủ thông tin cơ bản về kinh độ, vĩ độ và vận tốc của máy bay.
Trong chuỗi kí tự mà module GPS truyền về, nếu vi điều khiển trung tâm nhận được kí tự ‘R’ thì sẽ bắt đầu cho phép nhận chuỗi bằng cách cho biến gps_flag nhận giá trị là 1. Quá trình nhận chuỗi sẽ kết thúc khi kí tự nhận được là dấu “*”.
Lúc này, đoạn tin RMC đã được lưu trữ trong biến tạm, sẵn sàng để trích xuất dữ liệu và các biến nhận (gps_flag và count_nhan) sẽ được reset về 0.
Trong bản tin theo chuẩn NMEA, các phần thông tin được cách nhau bởi dấu phẩy (,), do đó bằng cách xác định được vị trí các dấu phẩy nằm trong chuỗi nhận được, so sánh với cấu trúc bản tin chuẩn, ta có thể lấy được dữ liệu mong muốn trong đoạn tin.
Sau khi nhận được đoạn tin RMC, ta tiến hành kiểm tra Byte dữ liệu giữa dấu phẩy thứ hai và dấu phẩy thứ ba, nếu là chữ ‘V’ có nghĩa là trong chuỗi nhận được vẫn chưa có thông tin do module GPS chưa thu đủ số lượng tín hiệu vệ tinh cần thiết.
Vi điều khiển trung tâm sẽ truyền chuỗi “No data” thông qua kênh truyền UART A0
Khi Byte dữ liệu này là ‘A’ thì trong chuỗi nhận được đã chứa thông tin kinh độ, vĩ độ và vận tốc. Ta sẽ copy chuỗi dữ liệu từ dấy phẩy thứ nhất đến dấu phẩy thứ 8, đoạn này sẽ chứa thông tin về giờ UTC, kinh độ, vĩ độ và vận tốc theo đơn vị knot/s. Chuỗi này sẽ được truyền đến module RF thông qua kênh truyền UART A0.
**Lưu đồ giải thuật nhận thông tin điều khiển từ trạm mặt đất
Hình 3.53: Lưu đồ giải thuật nhận thông tin điều khiển từ trạm mặt đất Quá trình nhận chuỗi dữ liệu từ trạm mặt đất được vi điều khiển trung tâm thực hiện trong chương trình ngắt nhận UART A1. Dữ liệu nhận được sẽ điều khiển tốc độ động cơ và góc quay của rudder
Khi có chuỗi nhận từ module RF, vi điều khiển tâm sẽ kiểm tra từng byte trong chuỗi nhận được. Nếu gặp byte 0x0D, quá trình nhận được bắt đầu. Các byte tiếp theo sẽ lần lượt được lưu vào mảng.
Khi đã lưu được 4 byte vào mảng, quá trình nhận sẽ kết thúc, các biến nhận (flag2 và dem) sẽ được reset về 0.
**Lưu đô giải thuật xác định trạng thái hệ thống Ngắt UART A1
UCA1RXBUF==0x0D
Flag2 = 1
Flag2 == 1
Dem ==4
flag2=0;
dem2=0;
rt_array[dem2] = UCA1RXBUF dem2++
End Đ S
Đ S
Đ
S
Hình 3.54: Lưu đồ giải thuật xác định trạng thái hoạt động của hệ thống Ngắt Port
2
Dem_timer = 0 Ngắt cạnh lên
Dem ==10 Ngắt cạnh xuống
Data=dem_timer Dem_timer = 0
15<Data<22
Pulse_ok ++
Trạng thái điều khiển tay Dem++
Pulse_ok>7 Trạng thái điều khiển tự động
End Đ S
Đ
Đ
S
Đ
S Đ
Ngắt Timer A0
Dem_timer
>11000 Dem_timer
++
Trạng thái điều khiển tự động
End Đ S S
S
Pule_ok=0
Giải thích lưu đồ:
Quá trình xác định trạng thái hoạt động của hệ thống được thực hiện dựa ở chương trình ngắt vi điều khiển MSP430G2553 thứ nhất.
Do ngõ ra kênh điều khiển là tín hiệu xung vuông tần số 50 Hz nên ta thiết lập các ngắt I/O cho Port 2.1 và Port.2 để xác định cạnh lên và xuống của xung vuông.
Khi có ngắt cạnh lên, biến đếm thời gian dem_timer được reset về 0. Khi ngắt cạnh xuống, ta sẽ thu được thời gian tồn tại mức dương của xung. Nếu độ rộng của xung thu được nằm trong khoảng 1.5ms đến 2.2ms thì đây là xung hợp lệ, biến đếm xung hợpvà biến đếm xung đã xét lệ tăng lên. Khi biến đếm xung đã xét đạt 10, tức là đã có 10 chu kỳ được kiểm tra, vi điều khiển tiến hành kiểm tra lại số lượng xung hợp lệ. Nếu số xung hợp lệ từ 7 trở lên thì máy bay ở trạng thái điều khiển bằng tay và ngược lại. Quá trình kiểm tra bằng ngắt I/O cần thời gian khoảng 0.2s.
Ngoài ra, trong chương trình ngắt Timer, ta cũng liên tục kiểm tra giá trị của biến đếm timer. Do biến đếm timer luôn bị reset khi có ngắt cạnh lên nên giá trị luôn dao động trong khoảng cố định từ 0 đến 22. Khi không có xung từ ngõ vào nữa do người điều khiển tắt bộ điều khiển thì phần kiểm tra trạng thái hệ thống bằng ngắt I/O không phát huy tác dụng. Tuy nhiên, đây là điều kiện để biến đếm timer liên tục tăng lên và khi biến đếm timer tăng đến 11000 thì trạng thái máy bay sẽ là điều khiển tự động. Việc kiểm tra trạng thái bay bằng biến timer cần khoảng 1,1 giây.