Chương 3. PHAN TÍCH VÀ THIET KE HE THÓNG
3.4.1.1. Thiết kế chương trình truyền nhận dữ liệu với client
Chương trình được chạy trên Raspberry Pi 3B+ nhận dữ liệu chỉ khi có kết nối cố
định giữa client và chương trình nhận đỡ liệu thông qua stream socket. Hình 3.18 là lưu đô giải thuật của truyên nhận dữ liệu với client.
Waiting for
client
Connected
Lost connect Remove client
Set value to landing
drone
Connected
Receive
message
Check data received
Hình 3.18: Lưu đồ giải thuật truyền nhận dữ liệu với client
Câu lệnh điều khién được truyền từ client gồm 2 phan chính: tên các giá trị điều khiển
và giá tri tương ứng. Các lệnh và giá trị tuong ứng như trong bang 3.1.
46
Bảng 3.1: Bảng các giá trị điều khiển Tên câu lệnh điều khiển Giá trị
power on/off throttle 1000 - 2000
roll 1000 - 2000
pitch 1000 - 2000
yaw 1000 - 2000
Câu lệnh được truyền đi dưới dạng chuỗi có định dạng như sau:
!<độ dài tên giá trị điều khién>/<tén giá trị điều khién>/<d6 dài chuỗi giá trị>/< giá trị điều khién>
Ví dụ: !4/roll/4/1600
Các điều hướng của chương trình nhận dữ liệu:
- Sau khi kết nối chương trình sẽ chờ nhận dữ liệu từ client, khi package có đầu
vào là power on chương trình sẽ khởi tạo luồng gửi dữ liệu cho hệ thống điều khiến bay và luồng stream video.
- Khi nhận các gói dữ liệu throttle, yaw, pitch, roll, power chương trình sẽ cập
nhật các biến này trong server.
- Khi mat kết nối với client, chương trình sẽ ngừng gửi gói dữ liệu cho hệ thống
điều khiển bay.
3.4.1.2. Thiết lập truyền video qua mjpg-streamer Mijpg-streamer được thiết lập qua lệnh có hỗ trợ đầu vào và ra như sau:
- Pau vào input_ raspicam trong mỗi giây là 15 khung hình .
- Pau ra output_http được thiết lập stream trực tiếp trên port 5555.
Hình 3.19 là lưu đỗ giải thuật stream video.
47
Init Stream
Get camera data
Put data to HTTP
Wait 1/15s (15fps)
Hình 3.19: Lưu đồ giải thuật stream video
3.4.1.3. Thiết lập truyền dữ liệu cho bộ điều khiến bay
Server thực hiện thiết lập truyền dữ liệu điều khién tới bộ điều khiển bay đề điều
khiển máy bay. Các thiết lập của UART được cấu hình trùng với cấu hình trên bộ điều khiển bay. Dữ liệu điều khiển sẽ được gửi liên tục sau mỗi 10ms. Hình 3.20 là lưu đồ giải thuật truyền nhận dữ liệu cho bộ điều khiến bay.
Start
ART
L
2. c
Prepare package
Send package
Wait 10ms
HHI
Hình 3.20: Lưu đồ giải thuật truyền dữ liệu cho bộ điều khién bay
48
3.4.2. Client (Smartphone)
Ung dụng điêu khién trên client (smartphone) sẽ gửi các các thao tác điêu khiên của
người dùng đên server băng các gói tin và hiên thi video stream từ server trên man hình chính của ứng dụng.
3.4.2.1. Lưu đồ thực thi
e = Màn hình nhập địa chi IP
Két nôi
vào socket
Người dùng Giao diện màn hình nhập
s IP
1.1 Người dùng nhập vào IP
! port socket ,port video stream
và nhân xác nhận 2.1 Kiểm tra IP
3.1.2 Thông báo lỗi “A. Vee
` vn TA A 4 3.1.1 Gửi yêu cau kết nôi |
.... và cho người dùng nhập lại ___. : h
< >
h z 4.1 Server accept '
h 5.1.2 Thông báo lôi
mi..." c1w >
' 1.2 Người dùng nhắn cancel 5.1.1 Chuyên người dùng dén
màn hình chính 2.2 Điêu hướng người dùng
: thoát khỏi ứng dụng
< errr. 1ể.. .ẽố.. .
Hình 3.21: Lưu đồ thực thi trên màn hình nhập địa chi IP
Các thành phan của hệ thống trong màn hình nhập địa chỉ IP: giao điện màn hình
nhập IP, giao diện màn hình chính, server.
Khái quát các bước trong chức năng dựa vào sơ đồ hình 3.21:
- Nguoi dùng: Người dùng nhập vào các giá tri như dia chỉ IP, port của video
và socket dé yêu cầu kết nói. Người dùng nhắn xác nhận (OK) dé kết nối đến server hoặc hủy bỏ (Cancel) dé thoát khỏi ứng dụng.
- Giao diện màn hình nhập IP: Sau khi nhận các giá trị nhập vào của người dùng
ứng dụng sẽ kiểm tra dữ liệu nhập vào. Giao diện sẽ báo lỗi nếu người dùng nhập sai và cho nhập lại nếu kết quả kiểm tra nhập sai và không nhận được
49
phản hồi từ server. Ứng dụng sẽ điều hướng người dùng đến màn hình chính khi có sự xác nhận kết nối từ server.
- Server: Nhận kết nối từ ứng dụng và chấp nhận nếu như không vượt quá số
lượng kết nối. Server sẽ không nhận kết nói nếu vượt quá số lượng kết nối cho
phép.
e Màn hình chính:
Giao điện màn hình Giao điện man hình cải Server
chính đặt
1.1 Người ding nhân vào power on
2.1 Gửi giả tri power on
3.1 Nhan video stream
gid Thụng bao nộu cụ lỗi stream ô
' 4.1. Hiễn thị
_5.1 Người đùng sử đụng bộ | video stream
điều khiên chuyên động của drone MA :
> 6.1 Gửi các giá trị điêu khiến
7.1 Người ding nhân vào power off
8.1 Gửi gia tri power off
9.1. Dừng hién thi
1.2 Người ding sử dung bộ video stream
điều khiên chuyên động của drone
>
1.3 Người ding nhẫn vào cai đặt =
Hình 3.22: Lưu đồ thực thi trên màn hình chính
Các thành phần của hệ thống trong màn hình chính: giao diện màn hình chính, giao
diện màn hình cài đặt, server.
Khái quát các bước trong chức năng dựa vao sơ đô hình 3.22:
- _ Người dùng: Người dùng có thé tương tác với thiết bị bay qua giao diện màn
hình chính bằng cách bật/tắt nguồn, các button điều khiển giá tri throttle, yaw, pitch, roll. Người dùng có thể chuyên qua giao diện cài đặt bằng cách nhắn
vào button Setting.
50
- Giao diện màn hình chính: Khi người dùng chưa bật công tắc (switch) nguồn
thì máy bay sẽ chưa được bật và người dùng cũng chưa thê tương tác với máy bay. Người dùng chỉ thao tác được với button setting dé chuyên đến giao diện màn hình cài đặt. Sau khi người dùng nhắn vào công tắc nguồn giao diện màn hình chính sẽ gửi lệnh “power on” đến server và nhận video stream từ server. Sau đó màn hình hiển thi video lên nền của giao diện màn hình chính và nếu
có lỗi không thé phát video ứng dụng sẽ thông báo lỗi đến người dùng. Sau đó nếu người dùng tương tác với các button điều hướng cho thiết bị bay, các giá trị này sẽ được gửi đi đến server thông qua giao diện màn hình chính.
- Server: Nhận các giá trị điều khiển từ giao diện màn hình chính và gửi video
stream đên giao diện màn hình chính.
Giao điện man hinh cai Giao dién man hinh
dat chinh
1..1 Người dùng nhập các gia trị
| điều khiễn và nhấn xác nhận 2.1 Kiểm tra các giá
trị nhập : 3.1.2 Thông bao giá trị nhậ :
| lo } 3.1.1 Chuyễn người ding đến
man hinh chinh
2.2 Chuyén người ding đến
man hinh chính
Các thành phan trong giao diện màn hình cai đặt: giao diện màn hình cài đặt, giao
diện màn hình chính.
Khái quát các bước trong chức năng dựa vào sơ đồ hình 3.23:
51
- Nguoi dùng: Người dùng nhập vào các giá trị điều khiển phù hợp (lớn hơn 0
và nhỏ hơn 500). Sau khi nhập người dùng có thé nhấn xác nhận (OK) dé lưu lại thay đối. Nếu giá trị người dùng nhập ngoài khoảng phù hợp sẽ bắt người dùng nhập lại. Người dùng có thể nhắn hủy bỏ (Cancel) néu người dùng không muốn thay đôi giá trị hiện tại.
- _ Giao diện màn hình cài đặt: Sau khi kiểm tra các giá trị nhập vào của người
dùng, giao diện màn hình cài đặt sẽ thông báo lỗi nếu giá trị không phù hợp. Nếu không xảy ra lỗi sẽ điều hướng người dùng về lại màn hình chính.
3.4.2.2. Hiện thực ứng dụng
e Man hình nhập địa chỉ kết nối như hình 3.24:
JFPf S(PINEN.AF'FI.
Address: 192.168.12.1
Port Video: 5555
\ CANCEL OK
Hình 3.24: Giao diện màn hình nhập dia chỉ
Mục đích: Người dùng nhập địa chỉ IP va port dé kết nối với server.
Điều hướng và tương tác người dùng:
(1): Người dùng nhập vào giá tri IP dé kết nối đến server.
(2): Người dùng nhập vào Port của socket server.
(3): Người dùng nhập vào Port của video stream.
(4): Khi nhắn vào button ứng dụng sẽ điều hướng người dùng thoát khỏi ứng dụng,
52
(5): Khi nhắn vào button ứng dụng sẽ điều hướng người dùng đến màn hình chính nếu kết nối với socket được thành lập nếu xảy ra lỗi màn hình nhập sẽ xuất hiện
lại.
e = Man hình chính như hình 3.25 và hình 3.26:
On/Off ằ G) Z5 xt
( )
_
Hình 3.25: Giao diện màn hình chính 1
Mục đích: Người dùng có thê tương tác với drone sau khi được kết nối thông qua các button điều khién ở hai góc của màn hình. Người dùng có thé quan sát video stream
thông qua nên của màn hình chính.
33
Điều hướng và tương tác người dùng:
(1): Khi người dùng nhấn vào switch On/Off ứng dung sẽ bắt đầu nhận video stream và gửi tín hiệu bật thiết bị bay đến server.
(2): Khi người dùng nhắn vào button ứng dụng sẽ điều hướng người dùng đến giao
diện cài đặt.
(3): Joystick bên trái dùng dé điều khiến throttle và yaw bao gồm 4 button lên, xuống, trái, phải tương ứng với việc tăng throttle, giảm throttle, giảm yaw, tăng yaw. Mỗi button có 2 chế độ nhắn là nhắn thường và nhắn giữ dùng dé gửi giá trị điều khiển theo 2 mức thường và lớn.
(4): Joystick phải có thiết kế tương tự với cấu tao joystick trái, joystick phải dùng
dé điều khiển pitch theo chiều đọc và roll theo chiều ngang.
(5): Màn hình hiền thị video stream nhận được từ server.
Màn hình cai đặt như hình 3.27 và hình 3.28:
Setting
Yaw normal : 100
Yaw max : 200
Pitch normal : 100
(3 \ CANCEL OK „
\ H |
Hình 3.27: Giao diện màn hình cài đặt 2
54
Yaw normal :
Roll normal :
Roll max :
CANCEL OK
Hinh 3.28: Giao dién man hinh cai dat 1
Mục dich: Người dùng có thé thay đổi các giá trị điều khién thông qua màn hình cài đặt, các giá trị này được lưu lại nên sẽ không phải điều chỉnh mỗi lần chạy lại ứng
dụng.
Điều hướng và tương tác người dùng:
(1): Người dùng nhập vào giá trị điều khiển mong muốn trong khoảng từ 0 — 500
(2): Sau khi nhấn button xác nhận (OK) thì ứng dụng sẽ lưu lại các giá tri điều khiến nên không xảy ra lỗi, nêu có lỗi người dùng sẽ được yêu cầu nhập lại.
(3): Người dùng nhấn button dé không thay đổi các giá trị điều khiển và trở lại
màn hình chính.
3.4.3. Bộ điều khiển bay
3.4.3.1. Tống quan về bộ điều khiến bay
Bộ điều khiển bay là một bộ xử lý có nhiệm vụ chính là điều khiển bay cho hệ thống.
Bộ điều khiển sẽ có các nhiệm vụ như đọc giá trị từ cảm biến (MPU-9255, MS5611), nhận tín hiệu điều khiển từ server, xử lý các tín hiệu đã lay được từ cam biến bang thuat toan Mahony Filter dé tinh ra trang thái hiện tai của may bay. Bộ diéu khién PID sử dung các giá trị trạng thái hiện tai của máy bay và các giá trị điều khién đến
55
từ server dé tính toán giá trị điều khiển. Các giá trị đầu ra từ các bộ điều khiển PID
sẽ được trộn lại cho từng động cơ và tín hiệu điều khiển được xuất ra thông qua tín hiệu PWM. Hình 3.29 thé hiện lưu đồ giải thuật tong quát của bộ điều khiến bay.
Initial System
RX Data Available
IsWritePWM Timeout Write PWM Read RX data
Calculate PID IsPIDTimeout |sBaroTimeout Read Barometer
Read Accelerometer
—T—
IsAttitudeTimeout IsAccelTimeout
Calculate Attitude
Read Magnetometer IsMagTimeout lsGyroTimeout Read Gyroscope
Hình 3.29: Lưu đồ giải thuật tổng quát nhiệm vu của bộ điều khiển bay
3.4.3.2. Đọc giá trị cảm biến MPU-9255
Cảm biến MPU-9255 bao gồm dit liệu cảm của biến gia tốc kế, con quay hồi chuyền,
từ kế. Các đữ liệu này sẽ được cập nhật theo một thời gian nhất định tùy vào từng loại sensor hỗ trợ. Ở đây nhóm thực hiện sẽ thực hiện đọc dữ liệu gia tốc kế và con quay
z
hồi chuyền với tan số 1000Hz và từ kế với tan số 100Hz như trong hình 3.30. Tan sốA
56
lây mau càng cao sẽ giúp cho chúng ta có thê loại bỏ các giá tri nhiêu ở tân sô càng
cao. Hình 3.30 thé hiện lưu đồ giải thuật doc dit liệu từ MPU-9255.
Set Timeout
Start (1000Hz)
rt
Set Timeout Read Gyroscope Set Timeout
(1000Hz) Data (100Hz)
Read Filter Data with Read Accelerometer Data Biquad Filter 1 Magnetometer Data
Filter Data with
Biquad Filter 2 Filter Data with LPF
Filter Data with LPF
A
End
Hình 3.30: Lưu đồ giải thuật của đọc dữ liệu từ MPU-9255 3.4.3.3. Đọc giá trị cam biến MS5611
Cảm biến MS5611 trả về áp suất hiện tại của không khí. Dữ liệu sẽ được cập nhật mới sau một khoảng thời gian nhất định. Tần số cập nhật dữ liệu của MS5611 hỗ trợ lên tới 40Hz. Nhung cảm biến MS5611 cần một khoảng thời gian dé lấy dữ liệu sau khi nhận được request. Vì thế nhóm thực hiện quyết định chia nhỏ việc đọc dữ liệu
từ MS5611 ra thành nhiều case nhỏ với tần số lặp lại là 400Hz. Việc chia thành nhiều
case nhỏ giúp tránh tình trạng thời gian cho task đọc dữ liệu từ MS5611 quá lâu và
gây ra hoãn quá lâu cho các tác vụ khác sẽ ảnh hưởng tới chương trình. Quy trình đọc
cảm biến MS5611 như hình 3.31 dưới đây.
57
No
Yes
Request D1 register Set D1 waiting time Set case = Task 2
Yes No
ORO) No
Yes
Set new Data wating time
Read D2 data Calculate Baro value Set case = Task 1
End
Hình 3.31: Luu đồ giải thuật đọc dt liệu từ MS5611
58
3.4.3.4. Đọc tín hiệu điều khiến từ server
Tín hiệu điều khiển máy bay sẽ được tự động gửi từ server tới bộ điều khiển bay với tan số là 60 lần/s nên việc đọc sẽ được lặp với tần số 60Hz. Tín hiệu điều khiến sẽ
được truyền thông qua giao tiếp UART. Các giá trị được truyền thông qua UART sẽ
được tự động truyền vào RX_buffer. Task đọc giá tri điều khiển sẽ kiểm tra và đọc
giá trị trong RX_buffer do server gửi. Các giá trị này sau khi được kiểm tra và nêu hợp lệ thì sẽ được lưu vào giá trị điều khiển. Nếu server không gửi tín hiệu điều khiển mới tới bộ điều khiển bay trong một khoảng thời gian được thiết lập thì sẽ xác nhận mat kết nối tới server và thực hiện thiết lập các giá trị điều khién về giá trị khi mat kết nối dé thực hiện quá trình hạ cánh. Hình 3.32 thé hiện lưu đồ giải thuật nhận tín
hiệu từ server.
Set Timeout (60Hz)
Lost Connect Value
Read Data Check Data Save to Control Data Set new Data Timeout (1s) Data Available
59
3.4.3.5. Ước tính các giá trị trạng thái của máy bay
Các giá trị trang thái của máy bay được thực hiện tính toán bang các giá tri đã lay được từ các cảm biến. Những giá trị được tính toán bao gồm trạng thái xoay hiện tại của máy bay so với trái đất (góc nghiêng của máy bay so với bề mặt trái đất, hướng đầu của máy bay) và độ cao hiện tại của máy bay so với mực nước biển. Việc ước tính các giá trị trên được thực hiện bằng cách sử dụng nhiều hơn một loại cảm biến giúp cho độ sai lệch theo thời gian giảm xuống và các giá trị ước tính có độ chính xác
cao hơn.
Trạng thái nghiêng của máy bay được ước tính bằng cách sử dụng các cảm biến như con quay hồi chuyền, gia tốc kế, từ kế. Thuật toán Mahony Filter sẽ giúp thực hiện việc tính toán các dữ liệu trên và trả về trạng thái nghiêng hiện tại của máy bay như
trong hình 3.34.
Độ cao của máy bay được ước tinh băng cách sử dụng các giá tri áp suât của không
khí, gia tốc của bản thân máy bay và độ cao trước đó như trong hình 3.33.
Attitude | | Accelerometer
Accelerometer Velocity from
of Body Frame Accelerometeree |
Estimated Estimated
Velocity
Velocity from Pressure
PreAltitude
|
Hình 3.33: Lưu đồ giải thuật ước tính độ cao máy tính
Pressure
60
Set Timeout (1000Hz)
Mahony Filter
Calculate Body Frame
Accelerometer
Estimate Altitude