Membaca Data Protokol MAVLink v1 di Arduino (TANPA LIBRARY)

 

Halo semua! Apa kabar? Semoga kalian baik-baik saja.

Tepat pada saat artikel ini saya publikasi, keadaan Indonesia perlahan mulai membaik. Menunjukkan perkembangan yang luar biasa. Dapat dilihat kegiatan mudik tahun ini, kegiatan wisata, car free day, dan kegiatan-kegiatan serupa sudah mulai diperbolehkan. Yep, Indonesia sudah terbebas dari musibah wabah penyakit menular (saya tidak akan menyebutkan nama penyakit atau sejenisnya, Google cukup sensitif).

Oke, tidak berlama-lama, saya ada sedikit informasi yang ingin saya sampaikan. Pernahkah kalian khususnya yang suka utak-atik drone kemudian iseng ingin membaca isi data dari flightcontrol kalian? Bisa?

Ada banyak tutorial bertebaran di internet sebenarnya. Namun hingga saat artikel ini saya tulis, saya masih belum menemukan program yang sederhana dan tidak memberatkan Arduino. Saya yakin yang kalian temukan di internet pasti menggunakan kode program serupa seperti di bawah ini, iya kan? Wkwk

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34

//function called by arduino to read any MAVlink messages sent by serial communication from flight controller to arduino uint8_t received_sysid; uint8_t received_compid; uint8_t received_msgid; void px_to_mp() { if(Serial2.available()) { mavlink_message_t msg; mavlink_status_t status; uint8_t c = Serial2.read(); if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { switch(msg.msgid) { case 0: { mavlink_heartbeat_t packet; mavlink_msg_heartbeat_decode(&msg, &packet); received_sysid = msg.sysid; // sysid compid msgid terpaksa dibuat - received_compid = msg.compid; // variabel tersendiri untuk menghindari - received_msgid = msg.msgid; // kerusakan data. belum diketahui penyebab - // 3 byte ini riskan rusak. struct combine { header head_data; hb payload_data; }__attribute__((packed)); struct combine merge; // paket header dan payload digabung 1 frame // paket checksum digabungkan dengan payload merge.head_data = {msg.magic, msg.len, msg.seq, received_sysid, received_compid, received_msgid}; merge.payload_data = {packet.custom_mode, packet.type, packet.autopilot, packet.base_mode, packet.system_status, packet.mavlink_version, msg.checksum}; //payloadSize digunakan untuk pembentukan keystream dan tag //sizeMerge adalah total data dari head-cksum yang dikirim ke MP //sizeMerge = sizeof(merge); payloadSize = sizeof(merge.payload_data); char _data[sizeof(merge)]; memcpy(_data, &merge, sizeof(merge)); encryption_start(_data, sizeof(merge), sizeof(merge.payload_data)); }break;

Saya baru menemukan satu kode program yang dituliskan tanpa library, baris program yang relatif pendek, namun sayangnya kode program tersebut ditulis menggunakan bahasa pemrograman Python. Yang mana program yang saya temukan itu tidak mungkin untuk diimplementasikan di Arduino atau ESP32 sekalipun menggunakan MicroPython.

Saya yang buta dengan bahasa ular sanca ini dengan terpaksa menerjemakannya perlahan. Saking pelannya tidak terasa 2 tahun lewat, (sejak 2020), saya baru bisa menerjemahkan 67%-nya saja. Bahkan sampai lupa siapa pemilik kode programnya. Jika kalian mengetahuinya, silahkan kirim komentar.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193

#----------------------------------------------------------------------- # python mavlink testing # # this program demonstrates how to connect to a mavlink device, # receive, decode, and verify mavlink messages. #----------------------------------------------------------------------- import sys import array import struct import serial # install from https://pypi.python.org/pypi/pyserial #----------------------------------------------------------------------- # set your port and baud rates here #----------------------------------------------------------------------- PORT='COM10' # typical windows port PORT='/dev/ttyACM0' # typical mac port #PORT='/dev/tty.usbmodem621' # typical mac port BAUD=57600 # baud rate for radio connection BAUD=115200 # baud rate for direct usb connection #----------------------------------------------------------------------- # manifest constants #----------------------------------------------------------------------- MAV_STARTB=0xfe # this byte indicates start of mavlink packet #----------------------------------------------------------------------- # utility stuff #----------------------------------------------------------------------- xdumpf=''.join([(len(repr(chr(x)))==3) and chr(x) or '.' for x in range(256)]) def xdump(src, length=16): """dump a string in classic hexdump format adapted from http://code.activestate.com/recipes/142812-hex-dumper """ N=0; while src: s,src = src[:length],src[length:] hexa = ' '.join(["%02X"%ord(x) for x in s]) s = s.translate(xdumpf) print "%04X %-*s %s" % (N, length*3, hexa, s) N+=length #----------------------------------------------------------------------- # checksum special note: # # each message type (0,1,2,...) has an extra one-byte magic number. # heartbeat (message 0) has magic number of 50, so you can index this # table with the message number to get the magic number. After # accumulating the bytes of the message, accumulate the magic number. # this table is copied from the mavlink source. #----------------------------------------------------------------------- MAVLINK_MESSAGE_CRCS=[ 50,124,137,0,237,217,104,119,0,0,0,89,0,0,0,0,0,0,0,0,214,159,220,168, 24,23,170,144,67,115,39,246,185,104,237,244,222,212,9,254,230,28,28, 132,221,232,11,153,41,39,214,223,141,33,15,3,100,24,239,238,30,240,183, 130,130,0,148,21,0,243,124,0,0,0,20,0,152,143,0,0,127,106,0,0,0,0,0,0, 0,231,183,63,54,0,0,0,0,0,0,0,175,102,158,208,56,93,0,0,0,0,235,93,124, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,42, 241,15,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,204,49, 170,44,83,46,0] #----------------------------------------------------------------------- class x25crc(object): """x25 CRC - based on checksum.h from mavlink library""" def __init__(self, buf=''): self.crc = 0xffff self.accumulate(buf) def accumulate(self, buf): '''add in some more bytes''' bytes = array.array('B') if isinstance(buf, array.array): bytes.extend(buf) else: bytes.fromstring(buf) accum = self.crc for b in bytes: tmp = b ^ (accum & 0xff) tmp = (tmp ^ (tmp<<4)) & 0xFF accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4) accum = accum & 0xFFFF self.crc = accum #----------------------------------------------------------------------- def decode2(buf): """decode and process a command""" # we don't do anything here pass #----------------------------------------------------------------------- lastseq=255 def decode(buf): """decode a mavlink message""" global lastseq print '-----------' xdump(buf) magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('<6B', buf[:6]) b0,b1=struct.unpack('2B',buf[6+mlen:]) givenCk=b0+b1*256 crc=x25crc() crc.accumulate(buf[1:len(buf)-2]) # skip magic and cksum crc.accumulate(chr(MAVLINK_MESSAGE_CRCS[msgId])) print 'mlen=%d, seq=%d, sys=(%d,%d), msgId=%d, sums=(%04x,%04x)'%\ (mlen,seq,srcSystem,srcComponent,msgId,givenCk,crc.crc) if crc.crc == givenCk: # good message, process it if seq != (lastseq+1)%256: print 'WARNING, lost message? seq=%d,lastseq=%d'%(seq,lastseq) lastseq=seq decode2(buf) else: # complain! print 'BAD CRC on message' #----------------------------------------------------------------------- def timedread(ser,n): """read octets, complain upon timeout""" while True: x = ser.read(n) if len(x) == 0: print "TIMEOUT" else: return x #----------------------------------------------------------------------- def process(ser): """process the mavlink input stream""" while True: # scan stream until we see sync byte x = timedread(ser,1) if ord(x) == MAV_STARTB: # read the length and rest of message, and process len=timedread(ser,1) rest=timedread(ser,4+ord(len)+2) buf=x buf+=len buf+=rest decode(buf) #----------------------------------------------------------------------- def connect(): """connect to the mavlink device""" autoselect=1 # here's some autoselect logic for mac if autoselect and sys.platform == 'darwin': import glob candidates=glob.glob('/dev/tty*usb*') print 'candidate ports (%d):'%(len(candidates)) for c in candidates: print ' ',c myport=candidates[0] mybaud=57600 mybaud=115200 else: myport=PORT mybaud=BAUD print 'connecting to:', myport ser = serial.Serial() ser.port=myport ser.baudrate=mybaud ser.parity=serial.PARITY_NONE ser.stopbits=serial.STOPBITS_ONE ser.bytesize=serial.EIGHTBITS ser.timeout=2 ser.open() return ser #----------------------------------------------------------------------- def main(): """the main thing!""" ser=connect() print 'STARTING' process(ser) #----------------------------------------------------------------------- if __name__=="__main__": main()

Jika diperhatikan kode program Python di atas terdapat 3 bagian utama. Pertama pembacaan header MAVLink. Ini penting untuk mengidentifikasi frame data yang lewat dan untuk proses parsing. Bagian kedua adalah pengidentifikasian panjang data. Ini juga menjadi satu bagian dengan proses pengumpulan byte data berdasarkan informasi panjang data yang diidentifikasi. Terakhir yaitu penghitungan checksum. Proses ini berguna untuk pencocokan apakah data yang sudah dikumpulkan adalah benar, atau terjadi perubahan atau kerusakan? Jika data benar, maka proses dapat dilanjutkan untuk frame selanjutnya.

Dari 3 bagian itu, saya hanya berhasil menerjemahkan bagian pertama dan kedua saja. Bagian terakhir atau proses penghitungan checksum masih belum mampu saya pahami. Mungkin diantara kalian yang membaca artikel ini bisa menyempurnakan, silahkan komentar.

Buah dari proses penerjemahan itu saya tuliskan berupa kode program Arduino seperti yang saya tuliskan di bawah ini:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41

void setup() { Serial.begin(57600); Serial2.begin(57600); // opens serial port, sets data rate to 9600 bps } void loop() { // check if data is available if (Serial2.available()) { // read the incoming byte: uint8_t stx = Serial2.read(); if ((Serial2.available()) && stx == 0xFE) { uint8_t len = Serial2.read(); /* +6 for stx, len, seq, sysid, compid, msgid; len for total payload; +2 for cksum */ uint8_t mav[6+len+2]; mav[0] = stx; mav[1] = len; for (int x = 2; x < sizeof(mav); x++) { if (Serial2.available()) { mav[x] = Serial2.read(); } } // Serial.write(mav, sizeof(mav)); for (int x = 0; x < sizeof(mav); x++) { Serial.print(mav[x], HEX); } Serial.print("\n"); } } delay(1); if (Serial.available() > 0) { uint8_t mp = Serial.read(); Serial2.write(mp); } }

Bandingkan kode di atas dengan kode program yang kalian temukan di internet. Tentu jauh lebih ringkas kode program dari saya, kan? Pastinya ini meringankan beban kerja dari si Arduino itu sendiri.

Penjelasan

Baik, kita langsung saja loncat ke baris 8. Perintah di baris itu berfungsi untuk mendeteksi apakah koneksi serial yang sudah terhubung terdapat aliran data atau tidak. Jika tidak ada aliran data, maka program akan menunggu hingga data melintas. Kemudian di baris 10, data yang melintas akan dimasukkan ke dalam variabel 'stx' yang kemudian di baris ke 11, variabel 'stx' akan dilakukan identifikasi apakah isi dari variabel tersebut bernilai 0xFE atau yang lain. Jika identifikasinya sama, maka lanjut di baris program berikutnya. Jika tidak, maka akan menunggu kembali sampai cocok.

Di baris 11 juga dilakukan hal yang sama yaitu mendeteksi kembali aliran data di koneksi serial. Kali ini yang ditunggu adalah data atau byte selanjutnya. Dimana byte ini di protokol MAVLink memuat informasi panjang payload dari satu frame MAVLink. Jika data berhasil didapat, maka akan dimasukkan ke dalam variabel 'len'. Apabila kedua informasi yaitu 'stx' dan 'len' berhasil didapatkan, maka selanjutnya adalah proses pengumpulan data yang tersisa dalam 1 frame untuk di "rakit" kembali pada variabel array 'mav' di baris program 17. Perhatikan, alokasi ruang untuk variabel array 'mav' sejumlah 6+len+2. Enam adalah jumlah byte yang terdapat pada bagian header MAVLink v1. Terdiri dari stx, len, sequence, compid, sysid, msgid. Kemudian 'len' sendiri mewakili panjang payload yang dimuat dalam 1 frame. Jika 'len' bernilai 9, maka alokasi 'len' untuk variabel array 'mav' adalah 9 byte. Kemudian +2 disediakan alokasi untuk menempatkan checksum.

Proses perakitan antara 'stx', 'len', dan sisa data selanjutnya terjadi di baris 18-19, dan 21-23. Di baris 21-23 data per-byte yang dibaca dari port serial dimasukkan satu persatu ke dalam variabel 'mav' sesuai dengan urutan elemen array-nya. Jika semua selesai di rakit, selanjutnya terserah kalian. Akan kalian apakan data itu? Kalian enkripsi kah, kalian analisis kah, kalian kirimkan lah, kalian implementasikan langsung di sistem Drone kalian? Terserah, bebas. Tapi disini saya mencoba untuk memunculkannya di serial monitor. Baris programnya di 28-31. Dan seperti ini hasilnya:

Pertanyaan, mengapa cukup banyak memanggil fungsi Serial.available? Alasannya karena jika fungsi Serial.available hanya diterapkan sekali, misal di baris 8 saja, maka data selanjutnya apabila terjadi kekosongan aliran data beberapa detik, maka data yang diterima pun menjadi data 'null'. Ini dibuktikan dengan munculnya nilai hexa FFFFFFF...FF di serial monitor.

Ada beberapa cara untuk menanggulanginya, salah satunya yang termudah adalah menggunakan fungsi Serial.available di setiap proses pembacaan data serialnya.

Okee, sekian dulu. Demikian yang bisa saya sampaikan, kurang lebihnya mohon maaf.

Terima kasih.