| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247 |
- <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
- <html xmlns="http://www.w3.org/1999/xhtml">
- <head>
- <meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
- <meta http-equiv="X-UA-Compatible" content="IE=9"/>
- <meta name="generator" content="Doxygen 1.8.5"/>
- <title>NRF24: crazyflie_client.ino</title>
- <link href="tabs.css" rel="stylesheet" type="text/css"/>
- <script type="text/javascript" src="jquery.js"></script>
- <script type="text/javascript" src="dynsections.js"></script>
- <link href="doxygen.css" rel="stylesheet" type="text/css" />
- </head>
- <body>
- <div id="top"><!-- do not remove this div, it is closed by doxygen! -->
- <div id="titlearea">
- <table cellspacing="0" cellpadding="0">
- <tbody>
- <tr style="height: 56px;">
- <td style="padding-left: 0.5em;">
- <div id="projectname">NRF24
- </div>
- </td>
- </tr>
- </tbody>
- </table>
- </div>
- <!-- end header part -->
- <!-- Generated by Doxygen 1.8.5 -->
- <div id="navrow1" class="tabs">
- <ul class="tablist">
- <li><a href="index.html"><span>Main Page</span></a></li>
- <li><a href="annotated.html"><span>Classes</span></a></li>
- <li><a href="files.html"><span>Files</span></a></li>
- <li><a href="examples.html"><span>Examples</span></a></li>
- </ul>
- </div>
- </div><!-- top -->
- <div class="header">
- <div class="headertitle">
- <div class="title">crazyflie_client.ino</div> </div>
- </div><!--header-->
- <div class="contents">
- <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>
- <div class="fragment"><div class="line"><span class="comment">// crazyflie_client.pde</span></div>
- <div class="line"><span class="comment">// -*- mode: C++ -*-</span></div>
- <div class="line"><span class="comment">//</span></div>
- <div class="line"><span class="comment">// This sketch act like a Crazyflie client (ie the transmitter) using the CRTP radiolink protocol:</span></div>
- <div class="line"><span class="comment">// http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol</span></div>
- <div class="line"><span class="comment">// to control a Crazyflie quadcopter http://www.bitcraze.se/</span></div>
- <div class="line"><span class="comment">//</span></div>
- <div class="line"><span class="comment">// Requires </span></div>
- <div class="line"><span class="comment">// - RC Transmitter that can act in trainer mode (eg Spektrum DX6i and others)</span></div>
- <div class="line"><span class="comment">// - NRF24 radio module such as the sparkfun WRL-00691 http://www.sparkfun.com/products/691</span></div>
- <div class="line"><span class="comment">// - Arduino such as Uno</span></div>
- <div class="line"><span class="comment">//</span></div>
- <div class="line"><span class="comment">// Uses NRF24 library to comunicate withthe Crazyflie, </span></div>
- <div class="line"><span class="comment">// http://www.airspayce.com/mikem/arduino/NRF24</span></div>
- <div class="line"><span class="comment">// and uses</span></div>
- <div class="line"><span class="comment">// the RcTrainer library to get servo positions from an RC transmitter running as a trainer</span></div>
- <div class="line"><span class="comment">// see http://www.airspayce.com/mikem/arduino/RcTrainer</span></div>
- <div class="line"><span class="comment">// Transmits the servo posiiotns to the Crazyflie, therefore allowing you to control the Crazyflie from </span></div>
- <div class="line"><span class="comment">// a conventional RC transmitter (ie no PC and no CrazyRadio module required).</span></div>
- <div class="line"><span class="comment">//</span></div>
- <div class="line"><span class="comment">// Only uses the Link ECHO and commander messages. No Log or Param messages are used </span></div>
- <div class="line"><span class="comment">// Author: Mike McCauley</span></div>
- <div class="line"><span class="comment">// Copyright (C) 2012 Mike McCauley</span></div>
- <div class="line"></div>
- <div class="line"><span class="preprocessor">#include <NRF24.h></span></div>
- <div class="line"><span class="preprocessor">#include <SPI.h></span></div>
- <div class="line"><span class="preprocessor">#include <RcTrainer.h></span></div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Structure of Crazyflie commander messages</span></div>
- <div class="line"><span class="preprocessor">#pragma pack(1);</span></div>
- <div class="line"><span class="preprocessor"></span><span class="keyword">typedef</span> <span class="keyword">struct </span></div>
- <div class="line">{</div>
- <div class="line"> <span class="keywordtype">float</span> roll;</div>
- <div class="line"> <span class="keywordtype">float</span> pitch;</div>
- <div class="line"> <span class="keywordtype">float</span> yaw;</div>
- <div class="line"> uint16_t thrust;</div>
- <div class="line">} CommanderCrtpValues;</div>
- <div class="line"><span class="preprocessor">#pragma pack()</span></div>
- <div class="line"><span class="preprocessor"></span></div>
- <div class="line"><span class="comment">// Useful macros for CRTP message contents and formatting</span></div>
- <div class="line"><span class="preprocessor">#define CRTP_HEADER(port, channel) (((port & 0x0F) << 4) | (channel & 0x0F))</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CRTP_HEADER_PORT(h) ((h >> 4) & 0xf)</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CRTP_HEADER_CHANNEL(h) (h & 0x3)</span></div>
- <div class="line"><span class="preprocessor"></span></div>
- <div class="line"><span class="comment">// Param channels</span></div>
- <div class="line"><span class="preprocessor">#define PARAM_TOC_CH 0</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define PARAM_READ_CH 1</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define PARAM_WRITE_CH 2</span></div>
- <div class="line"><span class="preprocessor"></span></div>
- <div class="line"><span class="comment">// Log channels</span></div>
- <div class="line"><span class="preprocessor">#define LOG_TOC_CH 0</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_CONTROL_CH 1</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_LOG_CH 2</span></div>
- <div class="line"><span class="preprocessor"></span><span class="comment">// Log packet parameters storage</span></div>
- <div class="line"><span class="preprocessor">#define LOG_MAX_OPS 64</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define LOG_MAX_BLOCKS 8</span></div>
- <div class="line"><span class="preprocessor"></span></div>
- <div class="line"><span class="comment">// Port definitions</span></div>
- <div class="line"><span class="keyword">typedef</span> <span class="keyword">enum</span> {</div>
- <div class="line"> CRTP_PORT_CONSOLE = 0x00,</div>
- <div class="line"> CRTP_PORT_PARAM = 0x02,</div>
- <div class="line"> CRTP_PORT_COMMANDER = 0x03,</div>
- <div class="line"> CRTP_PORT_LOG = 0x05,</div>
- <div class="line"> CRTP_PORT_LINK = 0x0F,</div>
- <div class="line">} CRTPPort;</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Common command numbers</span></div>
- <div class="line"><span class="preprocessor">#define CMD_GET_ITEM 0</span></div>
- <div class="line"><span class="preprocessor"></span><span class="preprocessor">#define CMD_GET_INFO 1</span></div>
- <div class="line"><span class="preprocessor"></span></div>
- <div class="line"><span class="comment">// Singleton instance of the radio</span></div>
- <div class="line"><a name="_a0"></a><a class="code" href="classNRF24.html">NRF24</a> nrf24;</div>
- <div class="line"><span class="comment">// NRF24 nrf24(8, 7); // use this to be electrically compatible with Mirf</span></div>
- <div class="line"><span class="comment">// NRF24 nrf24(8, 10);// For Leonardo, need explicit SS pin</span></div>
- <div class="line"></div>
- <div class="line"><span class="comment">// The currently used radio channel</span></div>
- <div class="line">uint8_t channel;</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// The expected address of the Crazyflie server (copter)</span></div>
- <div class="line"><span class="comment">// This is in fact also the default address of the NRF24</span></div>
- <div class="line">uint8_t address[] = { 0xe7, 0xe7, 0xe7, 0xe7, 0xe7 };</div>
- <div class="line">uint32_t failed_packet_count = 0;</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Object to get servo positions from RC trainer on pin D2</span></div>
- <div class="line">RcTrainer rc;</div>
- <div class="line"></div>
- <div class="line"><span class="keywordtype">void</span> setup() </div>
- <div class="line">{</div>
- <div class="line"> Serial.begin(115200);</div>
- <div class="line"> <span class="keywordflow">while</span> (!Serial)</div>
- <div class="line"> ; <span class="comment">// Wait for serial port, only required for Leonardo</span></div>
- <div class="line"></div>
- <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a1"></a><a class="code" href="classNRF24.html#a6e98cf508939240c1805245170a995f6">init</a>())</div>
- <div class="line"> Serial.println(<span class="stringliteral">"NRF24 init failed"</span>);</div>
- <div class="line"> </div>
- <div class="line"> <span class="comment">// Be Crazyflie radiolink compatible</span></div>
- <div class="line"> <span class="comment">// We use the NRF24 library for convenience, but</span></div>
- <div class="line"> <span class="comment">// we use a different configuration to the default NRF24</span></div>
- <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a name="a2"></a><a class="code" href="classNRF24.html#a3fa90005f98eb2744a533899f9b7e023">setChannel</a>(13))</div>
- <div class="line"> Serial.println(<span class="stringliteral">"setChannel failed"</span>);</div>
- <div class="line"> <span class="comment">// Set data rate to 250k and low power</span></div>
- <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>
- <div class="line"> Serial.println(<span class="stringliteral">"setRF failed"</span>); </div>
- <div class="line"> <span class="comment">// Be compatible with Crazyflie: No interrupts, 2 bytes CRC</span></div>
- <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>
- <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>
- <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>
- <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>
- <div class="line"> Serial.println(<span class="stringliteral">"setRetry failed"</span>); </div>
- <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>
- <div class="line"> Serial.println(<span class="stringliteral">"setTransmitAddress failed"</span>); </div>
- <div class="line"></div>
- <div class="line"> Serial.println(<span class="stringliteral">"initialised"</span>);</div>
- <div class="line">}</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Debugging data dumper</span></div>
- <div class="line"><span class="keywordtype">void</span> dump(<span class="keywordtype">char</span>* prompt, uint8_t* data, uint8_t len)</div>
- <div class="line">{</div>
- <div class="line"> Serial.print(prompt);</div>
- <div class="line"> Serial.print(<span class="stringliteral">": "</span>); </div>
- <div class="line"> <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i < len; i++)</div>
- <div class="line"> {</div>
- <div class="line"> Serial.print(data[i], HEX);</div>
- <div class="line"> Serial.print(<span class="stringliteral">" "</span>);</div>
- <div class="line"> }</div>
- <div class="line"> Serial.println(<span class="stringliteral">""</span>);</div>
- <div class="line">}</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Scan for a Crazyflie server (ie a copter) that responds</span></div>
- <div class="line"><span class="comment">// If one is found, set the channel and return true</span></div>
- <div class="line"><span class="keywordtype">boolean</span> scan()</div>
- <div class="line">{</div>
- <div class="line"> <span class="comment">// Standard LINK echo message</span></div>
- <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>
- <div class="line"> uint8_t msg[] = { CRTP_HEADER(CRTP_PORT_LINK, 3) };</div>
- <div class="line"> <span class="keywordflow">for</span> (channel = 0; channel < 125; channel++)</div>
- <div class="line"> {</div>
- <div class="line"> nrf24.<a class="code" href="classNRF24.html#a3fa90005f98eb2744a533899f9b7e023">setChannel</a>(channel);</div>
- <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>
- <div class="line"> Serial.println(<span class="stringliteral">"scan send failed"</span>);</div>
- <div class="line"> <span class="keywordflow">if</span> (nrf24.<a name="a11"></a><a class="code" href="classNRF24.html#a78a011cddedb14b8764ad5ea760c28ae">waitPacketSent</a>())</div>
- <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>
- <div class="line"> }</div>
- <div class="line"> <span class="keywordflow">return</span> <span class="keyword">false</span>;</div>
- <div class="line">}</div>
- <div class="line"></div>
- <div class="line"><span class="comment">// Send a message to the copter and detect failures</span></div>
- <div class="line"><span class="keywordtype">void</span> send(uint8_t* data, uint8_t len)</div>
- <div class="line">{</div>
- <div class="line"> <span class="keywordflow">if</span> (!nrf24.<a class="code" href="classNRF24.html#aad280539b80b28e55ab3187714bc7f2f">send</a>(data, len))</div>
- <div class="line"> Serial.println(<span class="stringliteral">"send failed"</span>);</div>
- <div class="line"> </div>
- <div class="line"> <span class="keywordflow">if</span> (nrf24.<a class="code" href="classNRF24.html#a78a011cddedb14b8764ad5ea760c28ae">waitPacketSent</a>())</div>
- <div class="line"> {</div>
- <div class="line"> <span class="comment">// Analyze payload?</span></div>
- <div class="line"> }</div>
- <div class="line"> <span class="keywordflow">else</span></div>
- <div class="line"> {</div>
- <div class="line"> failed_packet_count++;</div>
- <div class="line"> } </div>
- <div class="line">}</div>
- <div class="line"></div>
- <div class="line"><span class="keywordtype">void</span> loop()</div>
- <div class="line">{</div>
- <div class="line"> <span class="comment">// Poll until we have a positive response from a Crazyflie</span></div>
- <div class="line"> <span class="comment">// and then use its channel</span></div>
- <div class="line"> <span class="keywordflow">while</span> (!scan())</div>
- <div class="line"> ;</div>
- <div class="line"> </div>
- <div class="line"> <span class="comment">// Now run a session with the one we found...</span></div>
- <div class="line"> failed_packet_count = 0;</div>
- <div class="line"> Serial.print(<span class="stringliteral">"found a crazyflie on channel: "</span>);</div>
- <div class="line"> Serial.println(channel);</div>
- <div class="line"> </div>
- <div class="line"> <span class="comment">// ...until we get too many comms failures</span></div>
- <div class="line"> <span class="keywordflow">while</span> (failed_packet_count < 50)</div>
- <div class="line"> {</div>
- <div class="line"> <span class="comment">// Send a commander message</span></div>
- <div class="line"> uint8_t buf[32];</div>
- <div class="line"> buf[0] = CRTP_HEADER(CRTP_PORT_COMMANDER, 0);</div>
- <div class="line"> CommanderCrtpValues* msg = (CommanderCrtpValues*)(buf+1);</div>
- <div class="line"> <span class="comment">// Mode 2 stick layout, map standard stick values to suitable Crazyflie commander ranges</span></div>
- <div class="line"> msg->roll = map(rc.getChannel(1), 0, 1023, 30.0, -30.0);</div>
- <div class="line"> msg->pitch = map(rc.getChannel(2), 0, 1023, 30.0, -30.0);</div>
- <div class="line"> msg->yaw = map(rc.getChannel(3), 0, 1023, 200.0, -200.0);</div>
- <div class="line"> msg->thrust = map(rc.getChannel(0), 0, 1023, 0, 65000);</div>
- <div class="line"> send(buf, <span class="keyword">sizeof</span>(CommanderCrtpValues) + 1);</div>
- <div class="line"> delay(20); <span class="comment">// 50 per sec</span></div>
- <div class="line"> } </div>
- <div class="line"> Serial.println(<span class="stringliteral">"connection failed"</span>);</div>
- <div class="line"> </div>
- <div class="line"> <span class="comment">// Too many failed messages, fall out here and rescan for a new Crazyflie</span></div>
- <div class="line">}</div>
- <div class="line"></div>
- <div class="line"></div>
- </div><!-- fragment --> </div><!-- contents -->
- <!-- start footer part -->
- <hr class="footer"/><address class="footer"><small>
- Generated by  <a href="http://www.doxygen.org/index.html">
- <img class="footer" src="doxygen.png" alt="doxygen"/>
- </a> 1.8.5
- </small></address>
- </body>
- </html>
|