AR Drone 無人機是一臺小型的 Linux,當我們加入它提供的 WiFi 熱點的時候,我們就可以通過 192.168.1.1 來訪問無人機。
無人機的通訊采用了 UDP 協(xié)議,UDP 是至今沿用并占有主導(dǎo)地位的傳輸層協(xié)議之一,而另一個是 TCP 協(xié)議。
我們暫且先聊聊 TCP 協(xié)議,或者我們稱之為傳輸控制協(xié)議,基于它操作和使用起來極其方便,現(xiàn)在幾乎所有的網(wǎng)絡(luò)連接都是通過 TCP 來完成。 使用 TCP 協(xié)議的 API 非常直截了當,當你需要從一個設(shè)備傳輸數(shù)據(jù)到另一個硬件設(shè)備的時候,TCP 可以被所有硬件設(shè)備支持。使用 TCP 有多簡單?一旦建立連接,你把數(shù)據(jù)寫入 socket,另一臺設(shè)備將從 socket 讀取數(shù)據(jù),TCP 會確保數(shù)據(jù)正確的寫入并且傳輸給另一個設(shè)備。 許多復(fù)雜的細節(jié)隱匿其中。TCP 是基于 IP 層之上的,所有低級 IP 數(shù)據(jù)都不能按照其發(fā)送的順序到達,事實上,甚至有可能永遠都等不到它。但是 TCP 隱藏了這個玄機,它在 Unix 管道上建模,TCP 同時也管理著吞吐量;它不斷的適應(yīng)并達到最大的帶寬利用率。它似乎確實有著神奇的魔力可以變出三冊總頁數(shù)超過2556頁的書來闡述它的魅力。 TCP/IP Illustrated: The Protocols,The Implementation, TCP for Transactions。
UDP,是傳輸層的另一個重要組成部分,也是一個相對簡單的協(xié)議,但是使用 UDP 對開發(fā)者來說很痛苦,當你通過 UDP 發(fā)送數(shù)據(jù)的時候,無法得知數(shù)據(jù)是否成功被接收,也不知道數(shù)據(jù)到達的順序,同樣得不到(在不被帶寬變化影響而丟失數(shù)據(jù)的情況下)我們發(fā)送數(shù)據(jù)可達的最大速度。
就是說,UDP 是一個非常簡單的模型:UDP 允許你在設(shè)備之間發(fā)送所謂的數(shù)據(jù)包。這些數(shù)據(jù)包 (分組) 在另一端以同樣格式的數(shù)據(jù)包被接收(除非他們已經(jīng)在路上消失了)。
為了使用 UDP,一個應(yīng)用需要使用數(shù)據(jù)報 socket,它在通訊兩端綁定了一個 IP 地址和服務(wù)端口,并且因此建立了一個主機到主機的通訊,發(fā)送數(shù)據(jù)給一個指定的 socket 可以從匹配的另一端 socket 接收。
注意,UDP 是一個無連接協(xié)議,這里不需要設(shè)置連接,socket 對從哪里發(fā)送數(shù)據(jù)和數(shù)據(jù)何時到達進行簡單的跟蹤,當然,建立在數(shù)據(jù)能夠被 socket 捕捉的基礎(chǔ)上。
AR Drone 的接口建立在三個 UDP 端口上, 通過上面的討論我們知道 UDP 是一個還有待討論的設(shè)計方案,但是 Parrot 選擇了去實現(xiàn)它。
無人機的 IP 地址是 192.168.1.1, 并且這里有三個端口我們可以用來連接 UDP
導(dǎo)航控制數(shù)據(jù)端口 = 5554
機載視頻端口 = 5555
AT 指令端口 = 5556
我們需要利用 AT 指令集端口來發(fā)送命令到無人機,用導(dǎo)航數(shù)據(jù)端口來接收無人機返回的數(shù)據(jù)。其工作原理完全不同,因此只能分開討論兩者,即便如此,它們都依賴于 UDP socket 的。我們來看看這是如何實現(xiàn)的。
首先非常奇怪的是,Apple 沒有為 UDP 的運行提供 Objective-C helper 封裝。畢竟,這個協(xié)議甚至可以追溯到 1980 年,主因是幾乎沒有使用 UDP 的應(yīng)用,如果我們使用 UDP,至少訪問 UDP 的 Unix C API 將成為我們擔憂的一部分。因此大多數(shù)情況下我們會使用 TCP,而且對其來說,有很多 API 可供選擇。
C 語言的 API 我們使用了高級研究計劃署(發(fā)明互聯(lián)網(wǎng)的地方)定義在 sys/socket.h
,netinet/in.h
,arpa/inet.h
的方法。
首先,用下面的語句來創(chuàng)建 socket
int nativeSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
PF_INET
是 socket 的域名,在這個例子中是互聯(lián)網(wǎng),SOCK_DGRAM
定義了數(shù)據(jù)報的格式(相對于流式套接字)。最后,IPPROTO_UDP
定義了傳輸協(xié)議 UDP。socket 的工作方式類似于調(diào)用 open(2) 方法
接下來,我們創(chuàng)建了一個結(jié)構(gòu)體,包括我們的地址和無人機的地址,結(jié)構(gòu)體中的 sockaddr_in
是套接字的地址,我們使用 sin_me
來定義自己的地址,以及 sin_other
來定義另一端的地址
struct sockaddr_in sin_me = {};
sin_me.sin_len = (__uint8_t) sizeof(sin);
sin_me.sin_family = AF_INET;
sin_me.sin_port = htons(0);
sin_me.sin_addr.s_addr = htonl(INADDR_ANY);
struct sockaddr_in sin_other = {};
sin_other.sin_len = (__uint8_t) sizeof(sin_other);
sin_other.sin_family = AF_INET;
sin_other.sin_port = htons(self.port);
int r = inet_aton([self.address UTF8String], &sin_other.sin_addr)
用 ={}
來初始化結(jié)構(gòu)體總體來說是一個最佳實踐,可以不用考慮你使用什么結(jié)構(gòu),因為它確保一切開始時為零的。否則這些值無論在堆棧上的任何情況下都將是不確定的,我們會很容易碰到奇怪而又少見的 bug。
接下來,我們要給 sockaddr_in
賦值,并且指定 sin_len
來讓其可用,這樣允許多個地址,sin_family
就是地址類型的一種。有一種一長串的地址協(xié)議簇,當我們通過 internet 連接時候,總是用 IPv4 的 AF_INET
或者IPv6 的 AF_INET6
,然后我們設(shè)置端口和 IP
地址。
在我們這邊,我們指定端口為 0,并且地址是 INADDR_ANY
,0 端口意思是一個隨機的端口將會分配給我們的設(shè)備。 INADDR_ANY
則可以導(dǎo)入傳送路由數(shù)據(jù)包到另一端的地址(無人機)。
無人機的地址指定為 inet_aton(3)
, 它將轉(zhuǎn)換 C 字符串 192.168.1.1 成相應(yīng)的四字節(jié) 0xc0, 0xa2, 0x1, 0x1 - 作為無人機的IP地址。注意我們我們對地址和端口號調(diào)用了 htons(3)
和 htonl(3)
。htons 是 host-to-network-short 的縮寫,htonl 是 host-to-network-long 的縮寫。 大多數(shù)數(shù)據(jù)網(wǎng)絡(luò) (包括 IP) 是字節(jié)序是使用大端序 (big-endian)。為了確保數(shù)據(jù)按照正確的字節(jié)序發(fā)送我們需要調(diào)用這兩個功能。
現(xiàn)在我們綁定 socket 到我們的 socket 地址。
int r2 = bind(nativeSocket, (struct sockaddr *) &sin_me, sizeof(sin_me));
最后,我們通過下面的 socket 連到另一端 socket 地址:
int r3 = connect(nativeSocket, (struct sockaddr *) &sin_other, sizeof(sin_other));
最后一步是可選的,在每次發(fā)送數(shù)據(jù)包的時候我們也可以指定目的地址。
在我們示例代碼中,這是在 -[DatagramSocket configureIPv4WithError:]
方法中實現(xiàn)的,這個方法同時還進行了一些錯誤處理的操作。
當我們有一個可用的 socket 時,發(fā)送數(shù)據(jù)就很簡單了。比如我們要發(fā)送一個叫做 data
的 NSData
對象時,我們需要調(diào)用:
ssize_t const result = sendto(nativeSocket, [data bytes], data.length, 0, NULL, 0);
if (result < 0) {
NSLog(@"sendto() failed: %s (%d)", strerror(errno), errno);
} else if (result != data.length) {
NSLog(@"sendto() failed to send all bytes. Sent %ld of %lu bytes.", result, (unsigned long) data.length);
}
注意,UDP 從設(shè)計的上就是不可靠的,一旦調(diào)用 sendto(2)
,接下來網(wǎng)上數(shù)據(jù)傳輸過程就不是我們可以控制的了。
接收數(shù)據(jù)的核心非常簡單,這個方法叫做 recvfrom(2)
, 包括兩個參數(shù),第一個是 sin_other
指定了我們希望接受的數(shù)據(jù)的發(fā)送方,第二個參數(shù)是指向一個緩沖區(qū)的指針,的數(shù)據(jù)將被寫入其中。如果成功,這個方法返回讀取的字節(jié)數(shù):
NSMutableData *data = [NSMutableData dataWithLength:65535];
ssize_t count = recvfrom(nativeSocket, [data mutableBytes], [data length], 0, (struct sockaddr *) &sin_other, &length);
if (count < 0) {
NSLog(@"recvfrom() failed: %s (%d)", strerror(errno), errno);
data = nil;
} else {
data.length = count;
}
一個值得注意的事情, recvfrom(2)
是一個阻塞方法,線程一旦調(diào)用這個方法,則會等待直到數(shù)據(jù)全部讀完。正常情況下這都不是我們想要的。運用 GCD,我們可以設(shè)置一個事件源,每當 socket 有要讀取的數(shù)據(jù)它都能進行初始化。對于讀取來自 socket 的數(shù)據(jù)來說這是一個推薦的做法。
在我們的例子中,DatagramSocket
類運用了這個方法來設(shè)置事件源:
- (void)createReadSource
{
self.readEventSource = dispatch_source_create(DISPATCH_SOURCE_TYPE_READ, self.nativeSocket, 0, self.readEventQueue);
__weak DatagramSocket *weakSelf = self;
dispatch_source_set_event_handler(self.readEventSource, ^{
[weakSelf socketHasBytesAvailable];
});
dispatch_resume(self.readEventSource);
}
數(shù)據(jù)源開始時處于暫停狀態(tài),這就是為什么我們必須使用 dispatch_resume(3)
。 否則,將不會有事件傳到數(shù)據(jù)源,-socketHasBytesAvailable
之后會對 recvfrom(2)
進行調(diào)用。
為了避免一個小問題,我們要重寫 nativeSocket
的屬性方法。
@property (nonatomic) int nativeSocket;
這樣來實現(xiàn)
@synthesize nativeSocket = _nativeSocket;
- (void)setNativeSocket:(int)nativeSocket;
{
_nativeSocket = nativeSocket + 1;
}
- (int)nativeSocket
{
return _nativeSocket - 1;
}
我們從內(nèi)部的實例變量里減 1,首先因為 Objective-C 運行時保證在調(diào)用 -alloc
后所有實例變量初始值 0。其次,socket 只要為非負就被認為是有效的,比如大于 0 的均為有效的 socket 數(shù)字。
通過這樣的偏移,即使 -init
沒有被調(diào)用,我們?nèi)匀豢梢园踩貦z查 socket 值是否已經(jīng)被設(shè)定。
在 DatagramSocket 類 中我們封裝了所有低級的 UDP socket 的工作。DroneCommunicator 類用來和無人機的導(dǎo)航數(shù)據(jù)端口 5554 和 AT 指令集端口 5556 的通訊,就像這樣:
NSError *error = nil;
self.commandSocket = [DatagramSocket ipv4socketWithAddress:DroneAddress
port:ATCommandPort
receiveDelegate:self
receiveQueue:[NSOperationQueue mainQueue]
error:&error];
self.navigationDataSocket = [DatagramSocket ipv4socketWithAddress:DroneAddress
port:NavigationDataPort
receiveDelegate:self
receiveQueue:[NSOperationQueue mainQueue]
error:&error];
委托方法基于 socket 實現(xiàn)
- (void)datagramSocket:(DatagramSocket *)datagramSocket didReceiveData:(NSData *)data;
{
if (datagramSocket == self.navigationDataSocket) {
[self didReceiveNavigationData:data];
} else if (datagramSocket == self.commandSocket) {
[self didReceiveCommandResponseData:data];
}
}
在我們的示例 app 里需要處理的只有導(dǎo)航數(shù)據(jù),它被 DroneNavigationState
處理:
- (void)didReceiveNavigationData:(NSData *)data;
{
DroneNavigationState *state = [DroneNavigationState stateFromNavigationData:data];
if (state != nil) {
self.navigationState = state;
}
}
當 UDP socket 創(chuàng)建并運行后,發(fā)送的命令相對來說很很直接了。所謂的命令端口接受可以純 ASCII 命令, 看起來就像這樣:
AT*CONFIG=1,"general:navdata_demo","FALSE"
AT*CONFIG=2,"control:altitude_max","1600"
AT*CONFIG=3,"control:flying_mode","1000"
AT*COMWDG=4
AT*FTRIM=5
AR Drone SDK 包含了一個叫做 ARDrone Developer Guide 的 PDF 文檔,里面詳細介紹了所有的AT指令集。
我們在 DroneCommunicator
類中創(chuàng)造了一系列 helper 方法,使上述可以被發(fā)送:
[self setConfigurationKey:@"general:navdata_demo" toString:@"FALSE"];
[self setConfigurationKey:@"control:altitude_max" toString:@"1600"];
[self setConfigurationKey:@"control:flying_mode" toString:@"1000"];
[self sendCommand:@"COMWDG" arguments:nil];
[self sendCommand:@"FTRIM" arguments:nil];
所有的無人機指令以 AT* 開頭,跟著加上指令名以及 =,然后是被逗號隔開的參數(shù),第一個參數(shù)是命令的序列號。
為了方便使用,這里我們創(chuàng)建了一個叫做 -sendCommand:arguments:
的方法,它會在索引的開始 (index 0) 的地方插入命令序列號
- (int)sendCommand:(NSString *)command arguments:(NSArray *)arguments;
{
NSMutableArray *args2 = [NSMutableArray arrayWithArray:arguments];
self.commandSequence++;
NSString *seq = [NSString stringWithFormat:@"%d", self.commandSequence];
[args2 insertObject:seq atIndex:0];
[self sendCommandWithoutSequenceNumber:command arguments:args2];
return self.commandSequence;
}
這里調(diào)用了 -sendCommandWithoutSequenceNumber:arguments:
,這個方法加上了 AT* 前綴并且將命令和參數(shù)串接起來:
- (void)sendCommandWithoutSequenceNumber:(NSString *)command arguments:(NSArray *)arguments;
{
NSMutableString *atString = [NSMutableString stringWithString:@"AT*"];
[atString appendString:command];
NSArray* processedArgs = [arguments valueForKey:@"description"];
if (0 < arguments.count) {
[atString appendString:@"="];
[atString appendString:[processedArgs componentsJoinedByString:@","]];
}
[atString appendString:@"\r"];
[self sendString:atString];
}
最后,將完成的字符串轉(zhuǎn)換為 NSData 并且傳給 socket:
- (void)sendString:(NSString*)string
{
NSData *data = [string dataUsingEncoding:NSASCIIStringEncoding];
if (data != nil) {
[self.commandSocket asynchronouslySendData:data];
} else {
NSLog(@"Unable to convert string to ASCII: %@", string);
}
}
因為一些奇怪的原因,設(shè)計無人機協(xié)議的人規(guī)定了浮點值應(yīng)當作為具有相同位模式的整數(shù)來發(fā)送。這確實蠻奇怪的,但我們只能遵守協(xié)議。
比如說我們需要讓無人機的前進的相對速度是 0.5,浮點數(shù) 0.5 在二進制看起來是:
0011 1111 0000 0000 0000 0000 0000 0000
我們在 32 位整形中重新解釋這個數(shù)的話,它是 1056964608,所以我們發(fā)送到無人機的命令是:
AT*PCMD=6,1,0,1056964608,0,0
在我們的例子中,我們用一個 NSNumber
的封裝來完成,這個代碼最終看起來像:
NSNumber *number = (id) self.flightState[i];
union {
float f;
int i;
} u;
u.f = number.floatValue;
[result addObject:@(u.i)];
這里的技巧是使用 union - C 語言的一個鮮為人知的部分。union 允許多個不同的類型(在這種情況下,是整數(shù)和浮點型)駐留在同一存儲區(qū)域。然后,我們將浮點值存儲到 u.f 并從 u.i 讀取整數(shù)值。
注意:使用像 int i = *((int *) &f)
這樣的代碼是不合法的,這不是正確的 C 代碼,并且會導(dǎo)致未定義的行為。生成的代碼有時會工作,但有時候不會。所以不要做無謂的嘗試。你可以通過多閱讀 llvm 博客中 Violating Type Rules 下的文章來了解更多。悲劇的是 AR Drone Developer Guide 就是把這里弄錯了。