crazyflie_client_8ino-example.html 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247
  1. <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
  2. <html xmlns="http://www.w3.org/1999/xhtml">
  3. <head>
  4. <meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
  5. <meta http-equiv="X-UA-Compatible" content="IE=9"/>
  6. <meta name="generator" content="Doxygen 1.8.5"/>
  7. <title>NRF24: crazyflie_client.ino</title>
  8. <link href="tabs.css" rel="stylesheet" type="text/css"/>
  9. <script type="text/javascript" src="jquery.js"></script>
  10. <script type="text/javascript" src="dynsections.js"></script>
  11. <link href="doxygen.css" rel="stylesheet" type="text/css" />
  12. </head>
  13. <body>
  14. <div id="top"><!-- do not remove this div, it is closed by doxygen! -->
  15. <div id="titlearea">
  16. <table cellspacing="0" cellpadding="0">
  17. <tbody>
  18. <tr style="height: 56px;">
  19. <td style="padding-left: 0.5em;">
  20. <div id="projectname">NRF24
  21. </div>
  22. </td>
  23. </tr>
  24. </tbody>
  25. </table>
  26. </div>
  27. <!-- end header part -->
  28. <!-- Generated by Doxygen 1.8.5 -->
  29. <div id="navrow1" class="tabs">
  30. <ul class="tablist">
  31. <li><a href="index.html"><span>Main&#160;Page</span></a></li>
  32. <li><a href="annotated.html"><span>Classes</span></a></li>
  33. <li><a href="files.html"><span>Files</span></a></li>
  34. <li><a href="examples.html"><span>Examples</span></a></li>
  35. </ul>
  36. </div>
  37. </div><!-- top -->
  38. <div class="header">
  39. <div class="headertitle">
  40. <div class="title">crazyflie_client.ino</div> </div>
  41. </div><!--header-->
  42. <div class="contents">
  43. <p>This sketch act like a Crazyflie client (ie the transmitter) using the CRTP radiolink protocol: <a href="http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol">http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol</a> to control a Crazyflie quadcopter <a href="http://www.bitcraze.se/">http://www.bitcraze.se/</a> using a RC transmitter in trainer mode, such as the Spektrum DX6i and others.</p>
  44. <div class="fragment"><div class="line"><span class="comment">// crazyflie_client.pde</span></div>
  45. <div class="line"><span class="comment">// -*- mode: C++ -*-</span></div>
  46. <div class="line"><span class="comment">//</span></div>
  47. <div class="line"><span class="comment">// This sketch act like a Crazyflie client (ie the transmitter) using the CRTP radiolink protocol:</span></div>
  48. <div class="line"><span class="comment">// http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol</span></div>
  49. <div class="line"><span class="comment">// to control a Crazyflie quadcopter http://www.bitcraze.se/</span></div>
  50. <div class="line"><span class="comment">//</span></div>
  51. <div class="line"><span class="comment">// Requires </span></div>
  52. <div class="line"><span class="comment">// - RC Transmitter that can act in trainer mode (eg Spektrum DX6i and others)</span></div>
  53. <div class="line"><span class="comment">// - NRF24 radio module such as the sparkfun WRL-00691 http://www.sparkfun.com/products/691</span></div>
  54. <div class="line"><span class="comment">// - Arduino such as Uno</span></div>
  55. <div class="line"><span class="comment">//</span></div>
  56. <div class="line"><span class="comment">// Uses NRF24 library to comunicate withthe Crazyflie, </span></div>
  57. <div class="line"><span class="comment">// http://www.airspayce.com/mikem/arduino/NRF24</span></div>
  58. <div class="line"><span class="comment">// and uses</span></div>
  59. <div class="line"><span class="comment">// the RcTrainer library to get servo positions from an RC transmitter running as a trainer</span></div>
  60. <div class="line"><span class="comment">// see http://www.airspayce.com/mikem/arduino/RcTrainer</span></div>
  61. <div class="line"><span class="comment">// Transmits the servo posiiotns to the Crazyflie, therefore allowing you to control the Crazyflie from </span></div>
  62. <div class="line"><span class="comment">// a conventional RC transmitter (ie no PC and no CrazyRadio module required).</span></div>
  63. <div class="line"><span class="comment">//</span></div>
  64. <div class="line"><span class="comment">// Only uses the Link ECHO and commander messages. No Log or Param messages are used </span></div>
  65. <div class="line"><span class="comment">// Author: Mike McCauley</span></div>
  66. <div class="line"><span class="comment">// Copyright (C) 2012 Mike McCauley</span></div>
  67. <div class="line"></div>
  68. <div class="line"><span class="preprocessor">#include &lt;NRF24.h&gt;</span></div>
  69. <div class="line"><span class="preprocessor">#include &lt;SPI.h&gt;</span></div>
  70. <div class="line"><span class="preprocessor">#include &lt;RcTrainer.h&gt;</span></div>
  71. <div class="line"></div>
  72. <div class="line"><span class="comment">// Structure of Crazyflie commander messages</span></div>
  73. <div class="line"><span class="preprocessor">#pragma pack(1);</span></div>
  74. <div class="line"><span class="preprocessor"></span><span class="keyword">typedef</span> <span class="keyword">struct </span></div>
  75. <div class="line">{</div>
  76. <div class="line"> <span class="keywordtype">float</span> roll;</div>
  77. <div class="line"> <span class="keywordtype">float</span> pitch;</div>
  78. <div class="line"> <span class="keywordtype">float</span> yaw;</div>
  79. <div class="line"> uint16_t thrust;</div>
  80. <div class="line">} CommanderCrtpValues;</div>
  81. <div class="line"><span class="preprocessor">#pragma pack()</span></div>
  82. <div class="line"><span class="preprocessor"></span></div>
  83. <div class="line"><span class="comment">// Useful macros for CRTP message contents and formatting</span></div>
  84. <div class="line"><span class="preprocessor">#define CRTP_HEADER(port, channel) (((port &amp; 0x0F) &lt;&lt; 4) | (channel &amp; 0x0F))</span></div>
  85. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CRTP_HEADER_PORT(h) ((h &gt;&gt; 4) &amp; 0xf)</span></div>
  86. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CRTP_HEADER_CHANNEL(h) (h &amp; 0x3)</span></div>
  87. <div class="line"><span class="preprocessor"></span></div>
  88. <div class="line"><span class="comment">// Param channels</span></div>
  89. <div class="line"><span class="preprocessor">#define PARAM_TOC_CH 0</span></div>
  90. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define PARAM_READ_CH 1</span></div>
  91. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define PARAM_WRITE_CH 2</span></div>
  92. <div class="line"><span class="preprocessor"></span></div>
  93. <div class="line"><span class="comment">// Log channels</span></div>
  94. <div class="line"><span class="preprocessor">#define LOG_TOC_CH 0</span></div>
  95. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_CONTROL_CH 1</span></div>
  96. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_LOG_CH 2</span></div>
  97. <div class="line"><span class="preprocessor"></span><span class="comment">// Log packet parameters storage</span></div>
  98. <div class="line"><span class="preprocessor">#define LOG_MAX_OPS 64</span></div>
  99. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_MAX_BLOCKS 8</span></div>
  100. <div class="line"><span class="preprocessor"></span></div>
  101. <div class="line"><span class="comment">// Port definitions</span></div>
  102. <div class="line"><span class="keyword">typedef</span> <span class="keyword">enum</span> {</div>
  103. <div class="line"> CRTP_PORT_CONSOLE = 0x00,</div>
  104. <div class="line"> CRTP_PORT_PARAM = 0x02,</div>
  105. <div class="line"> CRTP_PORT_COMMANDER = 0x03,</div>
  106. <div class="line"> CRTP_PORT_LOG = 0x05,</div>
  107. <div class="line"> CRTP_PORT_LINK = 0x0F,</div>
  108. <div class="line">} CRTPPort;</div>
  109. <div class="line"></div>
  110. <div class="line"><span class="comment">// Common command numbers</span></div>
  111. <div class="line"><span class="preprocessor">#define CMD_GET_ITEM 0</span></div>
  112. <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CMD_GET_INFO 1</span></div>
  113. <div class="line"><span class="preprocessor"></span></div>
  114. <div class="line"><span class="comment">// Singleton instance of the radio</span></div>
  115. <div class="line"><a name="_a0"></a><a class="code" href="classNRF24.html">NRF24</a> nrf24;</div>
  116. <div class="line"><span class="comment">// NRF24 nrf24(8, 7); // use this to be electrically compatible with Mirf</span></div>
  117. <div class="line"><span class="comment">// NRF24 nrf24(8, 10);// For Leonardo, need explicit SS pin</span></div>
  118. <div class="line"></div>
  119. <div class="line"><span class="comment">// The currently used radio channel</span></div>
  120. <div class="line">uint8_t channel;</div>
  121. <div class="line"></div>
  122. <div class="line"><span class="comment">// The expected address of the Crazyflie server (copter)</span></div>
  123. <div class="line"><span class="comment">// This is in fact also the default address of the NRF24</span></div>
  124. <div class="line">uint8_t address[] = { 0xe7, 0xe7, 0xe7, 0xe7, 0xe7 };</div>
  125. <div class="line">uint32_t failed_packet_count = 0;</div>
  126. <div class="line"></div>
  127. <div class="line"><span class="comment">// Object to get servo positions from RC trainer on pin D2</span></div>
  128. <div class="line">RcTrainer rc;</div>
  129. <div class="line"></div>
  130. <div class="line"><span class="keywordtype">void</span> setup() </div>
  131. <div class="line">{</div>
  132. <div class="line"> Serial.begin(115200);</div>
  133. <div class="line"> <span class="keywordflow">while</span> (!Serial)</div>
  134. <div class="line"> ; <span class="comment">// Wait for serial port, only required for Leonardo</span></div>
  135. <div class="line"></div>
  136. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a1"></a><a class="code" href="classNRF24.html#a6e98cf508939240c1805245170a995f6">init</a>())</div>
  137. <div class="line"> Serial.println(<span class="stringliteral">&quot;NRF24 init failed&quot;</span>);</div>
  138. <div class="line"> </div>
  139. <div class="line"> <span class="comment">// Be Crazyflie radiolink compatible</span></div>
  140. <div class="line"> <span class="comment">// We use the NRF24 library for convenience, but</span></div>
  141. <div class="line"> <span class="comment">// we use a different configuration to the default NRF24</span></div>
  142. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a2"></a><a class="code" href="classNRF24.html#a3fa90005f98eb2744a533899f9b7e023">setChannel</a>(13))</div>
  143. <div class="line"> Serial.println(<span class="stringliteral">&quot;setChannel failed&quot;</span>);</div>
  144. <div class="line"> <span class="comment">// Set data rate to 250k and low power</span></div>
  145. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a3"></a><a class="code" href="classNRF24.html#a3453db8390d5be4aa99d8c7fde3062a4">setRF</a>(<a name="a4"></a><a class="code" href="classNRF24.html#a52d7af9188397d4a6f5dab55d4f04742ae4523002495ee537f2731b61a220e3af">NRF24::NRF24DataRate250kbps</a>, <a name="a5"></a><a class="code" href="classNRF24.html#a8386e81a9a4b824ab26e785879684c7eacef1d6c037e276e6dadaff2b6056ec0c">NRF24::NRF24TransmitPower0dBm</a>))</div>
  146. <div class="line"> Serial.println(<span class="stringliteral">&quot;setRF failed&quot;</span>); </div>
  147. <div class="line"> <span class="comment">// Be compatible with Crazyflie: No interrupts, 2 bytes CRC</span></div>
  148. <div class="line"> nrf24.<a name="a6"></a><a class="code" href="classNRF24.html#a790d69e5cb1039fb2924f9405e30e250">setConfiguration</a>(NRF24_MASK_RX_DR | NRF24_MASK_TX_DS | NRF24_MASK_MAX_RT | NRF24_EN_CRC | NRF24_CRCO);</div>
  149. <div class="line"> nrf24.<a name="a7"></a><a class="code" href="classNRF24.html#a81931bc1679b213b1cb1ae65b8357985">spiWriteRegister</a>(NRF24_REG_1D_FEATURE, NRF24_EN_DPL | NRF24_EN_ACK_PAY); <span class="comment">// Dynamic size payload + ack</span></div>
  150. <div class="line"> nrf24.<a class="code" href="classNRF24.html#a81931bc1679b213b1cb1ae65b8357985">spiWriteRegister</a>(NRF24_REG_1C_DYNPD, NRF24_DPL_P0); <span class="comment">// Dynamic payload on pipe 0</span></div>
  151. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a8"></a><a class="code" href="classNRF24.html#a1ac9bfce3833725b8b66d9875a8bc00e">setRetry</a>(6, 3)) <span class="comment">// 1500us and 3 retries</span></div>
  152. <div class="line"> Serial.println(<span class="stringliteral">&quot;setRetry failed&quot;</span>); </div>
  153. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a9"></a><a class="code" href="classNRF24.html#a7b40c4c481bddf343acdac5179fddc75">setTransmitAddress</a>(address, <span class="keyword">sizeof</span>(address)))</div>
  154. <div class="line"> Serial.println(<span class="stringliteral">&quot;setTransmitAddress failed&quot;</span>); </div>
  155. <div class="line"></div>
  156. <div class="line"> Serial.println(<span class="stringliteral">&quot;initialised&quot;</span>);</div>
  157. <div class="line">}</div>
  158. <div class="line"></div>
  159. <div class="line"><span class="comment">// Debugging data dumper</span></div>
  160. <div class="line"><span class="keywordtype">void</span> dump(<span class="keywordtype">char</span>* prompt, uint8_t* data, uint8_t len)</div>
  161. <div class="line">{</div>
  162. <div class="line"> Serial.print(prompt);</div>
  163. <div class="line"> Serial.print(<span class="stringliteral">&quot;: &quot;</span>); </div>
  164. <div class="line"> <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; len; i++)</div>
  165. <div class="line"> {</div>
  166. <div class="line"> Serial.print(data[i], HEX);</div>
  167. <div class="line"> Serial.print(<span class="stringliteral">&quot; &quot;</span>);</div>
  168. <div class="line"> }</div>
  169. <div class="line"> Serial.println(<span class="stringliteral">&quot;&quot;</span>);</div>
  170. <div class="line">}</div>
  171. <div class="line"></div>
  172. <div class="line"><span class="comment">// Scan for a Crazyflie server (ie a copter) that responds</span></div>
  173. <div class="line"><span class="comment">// If one is found, set the channel and return true</span></div>
  174. <div class="line"><span class="keywordtype">boolean</span> scan()</div>
  175. <div class="line">{</div>
  176. <div class="line"> <span class="comment">// Standard LINK echo message</span></div>
  177. <div class="line"> <span class="comment">// If there is a Crazyflie present on a channel it will reply with an ACK and no payload</span></div>
  178. <div class="line"> uint8_t msg[] = { CRTP_HEADER(CRTP_PORT_LINK, 3) };</div>
  179. <div class="line"> <span class="keywordflow">for</span> (channel = 0; channel &lt; 125; channel++)</div>
  180. <div class="line"> {</div>
  181. <div class="line"> nrf24.<a class="code" href="classNRF24.html#a3fa90005f98eb2744a533899f9b7e023">setChannel</a>(channel);</div>
  182. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a10"></a><a class="code" href="classNRF24.html#aad280539b80b28e55ab3187714bc7f2f">send</a>(msg, <span class="keyword">sizeof</span>(msg)))</div>
  183. <div class="line"> Serial.println(<span class="stringliteral">&quot;scan send failed&quot;</span>);</div>
  184. <div class="line"> <span class="keywordflow">if</span> (nrf24.<a name="a11"></a><a class="code" href="classNRF24.html#a78a011cddedb14b8764ad5ea760c28ae">waitPacketSent</a>())</div>
  185. <div class="line"> <span class="keywordflow">return</span> <span class="keyword">true</span>; <span class="comment">// Got an ACK, expect no payload. This channel works, use it</span></div>
  186. <div class="line"> }</div>
  187. <div class="line"> <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
  188. <div class="line">}</div>
  189. <div class="line"></div>
  190. <div class="line"><span class="comment">// Send a message to the copter and detect failures</span></div>
  191. <div class="line"><span class="keywordtype">void</span> send(uint8_t* data, uint8_t len)</div>
  192. <div class="line">{</div>
  193. <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a class="code" href="classNRF24.html#aad280539b80b28e55ab3187714bc7f2f">send</a>(data, len))</div>
  194. <div class="line"> Serial.println(<span class="stringliteral">&quot;send failed&quot;</span>);</div>
  195. <div class="line"> </div>
  196. <div class="line"> <span class="keywordflow">if</span> (nrf24.<a class="code" href="classNRF24.html#a78a011cddedb14b8764ad5ea760c28ae">waitPacketSent</a>())</div>
  197. <div class="line"> {</div>
  198. <div class="line"> <span class="comment">// Analyze payload?</span></div>
  199. <div class="line"> }</div>
  200. <div class="line"> <span class="keywordflow">else</span></div>
  201. <div class="line"> {</div>
  202. <div class="line"> failed_packet_count++;</div>
  203. <div class="line"> } </div>
  204. <div class="line">}</div>
  205. <div class="line"></div>
  206. <div class="line"><span class="keywordtype">void</span> loop()</div>
  207. <div class="line">{</div>
  208. <div class="line"> <span class="comment">// Poll until we have a positive response from a Crazyflie</span></div>
  209. <div class="line"> <span class="comment">// and then use its channel</span></div>
  210. <div class="line"> <span class="keywordflow">while</span> (!scan())</div>
  211. <div class="line"> ;</div>
  212. <div class="line"> </div>
  213. <div class="line"> <span class="comment">// Now run a session with the one we found...</span></div>
  214. <div class="line"> failed_packet_count = 0;</div>
  215. <div class="line"> Serial.print(<span class="stringliteral">&quot;found a crazyflie on channel: &quot;</span>);</div>
  216. <div class="line"> Serial.println(channel);</div>
  217. <div class="line"> </div>
  218. <div class="line"> <span class="comment">// ...until we get too many comms failures</span></div>
  219. <div class="line"> <span class="keywordflow">while</span> (failed_packet_count &lt; 50)</div>
  220. <div class="line"> {</div>
  221. <div class="line"> <span class="comment">// Send a commander message</span></div>
  222. <div class="line"> uint8_t buf[32];</div>
  223. <div class="line"> buf[0] = CRTP_HEADER(CRTP_PORT_COMMANDER, 0);</div>
  224. <div class="line"> CommanderCrtpValues* msg = (CommanderCrtpValues*)(buf+1);</div>
  225. <div class="line"> <span class="comment">// Mode 2 stick layout, map standard stick values to suitable Crazyflie commander ranges</span></div>
  226. <div class="line"> msg-&gt;roll = map(rc.getChannel(1), 0, 1023, 30.0, -30.0);</div>
  227. <div class="line"> msg-&gt;pitch = map(rc.getChannel(2), 0, 1023, 30.0, -30.0);</div>
  228. <div class="line"> msg-&gt;yaw = map(rc.getChannel(3), 0, 1023, 200.0, -200.0);</div>
  229. <div class="line"> msg-&gt;thrust = map(rc.getChannel(0), 0, 1023, 0, 65000);</div>
  230. <div class="line"> send(buf, <span class="keyword">sizeof</span>(CommanderCrtpValues) + 1);</div>
  231. <div class="line"> delay(20); <span class="comment">// 50 per sec</span></div>
  232. <div class="line"> } </div>
  233. <div class="line"> Serial.println(<span class="stringliteral">&quot;connection failed&quot;</span>);</div>
  234. <div class="line"> </div>
  235. <div class="line"> <span class="comment">// Too many failed messages, fall out here and rescan for a new Crazyflie</span></div>
  236. <div class="line">}</div>
  237. <div class="line"></div>
  238. <div class="line"></div>
  239. </div><!-- fragment --> </div><!-- contents -->
  240. <!-- start footer part -->
  241. <hr class="footer"/><address class="footer"><small>
  242. Generated by &#160;<a href="http://www.doxygen.org/index.html">
  243. <img class="footer" src="doxygen.png" alt="doxygen"/>
  244. </a> 1.8.5
  245. </small></address>
  246. </body>
  247. </html>