Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Sonar/RF12B/RF12B.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
24:7a3906c2f5d5 
Eurobot 2012 Primary;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 24:7a3906c2f5d5 1 #include "RF12B.h"
narshu 24:7a3906c2f5d5 2
narshu 24:7a3906c2f5d5 3 #include "RF_defs.h"
narshu 24:7a3906c2f5d5 4 #include <algorithm>
narshu 24:7a3906c2f5d5 5 #include "system.h"
narshu 24:7a3906c2f5d5 6 #include "globals.h"
narshu 24:7a3906c2f5d5 7
narshu 24:7a3906c2f5d5 8
narshu 24:7a3906c2f5d5 9 RF12B::RF12B(PinName _SDI,
narshu 24:7a3906c2f5d5 10 PinName _SDO,
narshu 24:7a3906c2f5d5 11 PinName _SCK,
narshu 24:7a3906c2f5d5 12 PinName _NCS,
narshu 24:7a3906c2f5d5 13 PinName _NIRQ):spi(_SDI, _SDO, _SCK),
narshu 24:7a3906c2f5d5 14 NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ) {// rfled(LED3) {
narshu 24:7a3906c2f5d5 15
narshu 24:7a3906c2f5d5 16 // SPI frequency, word lenght, polarity and phase */
narshu 24:7a3906c2f5d5 17 spi.format(16,0);
narshu 24:7a3906c2f5d5 18 spi.frequency(2000000);
narshu 24:7a3906c2f5d5 19
narshu 24:7a3906c2f5d5 20 // Set ~CS high
narshu 24:7a3906c2f5d5 21 NCS = 1;
narshu 24:7a3906c2f5d5 22
narshu 24:7a3906c2f5d5 23 // Initialise RF Module
narshu 24:7a3906c2f5d5 24 init();
narshu 24:7a3906c2f5d5 25
narshu 24:7a3906c2f5d5 26 // Setup interrupt to happen on falling edge of NIRQ
narshu 24:7a3906c2f5d5 27 NIRQ.fall(this, &RF12B::rxISR);
narshu 24:7a3906c2f5d5 28 }
narshu 24:7a3906c2f5d5 29
narshu 24:7a3906c2f5d5 30 // Returns the packet length if data is available in the receive buffer, 0 otherwise
narshu 24:7a3906c2f5d5 31 //unsigned int RF12B::available() {
narshu 24:7a3906c2f5d5 32 // return fifo.size();
narshu 24:7a3906c2f5d5 33 //}
narshu 24:7a3906c2f5d5 34
narshu 24:7a3906c2f5d5 35 // Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets
narshu 24:7a3906c2f5d5 36 /*bool RF12B::read(unsigned char* data, unsigned int size) {
narshu 24:7a3906c2f5d5 37 if (fifo.size() == 0) {
narshu 24:7a3906c2f5d5 38 return false;
narshu 24:7a3906c2f5d5 39 } else {
narshu 24:7a3906c2f5d5 40 unsigned int i = 0;
narshu 24:7a3906c2f5d5 41 while (fifo.size() > 0 && i < size) {
narshu 24:7a3906c2f5d5 42 data[i++] = fifo.front();
narshu 24:7a3906c2f5d5 43 fifo.pop();
narshu 24:7a3906c2f5d5 44 }
narshu 24:7a3906c2f5d5 45 return true;
narshu 24:7a3906c2f5d5 46 }
narshu 24:7a3906c2f5d5 47 }
narshu 24:7a3906c2f5d5 48 */
narshu 24:7a3906c2f5d5 49
narshu 24:7a3906c2f5d5 50 // Reads a byte of data from the receive buffer
narshu 24:7a3906c2f5d5 51 /*
narshu 24:7a3906c2f5d5 52 unsigned char RF12B::read() {
narshu 24:7a3906c2f5d5 53 if (available()) {
narshu 24:7a3906c2f5d5 54 unsigned char data = fifo.front();
narshu 24:7a3906c2f5d5 55 fifo.pop();
narshu 24:7a3906c2f5d5 56 return data;
narshu 24:7a3906c2f5d5 57 } else {
narshu 24:7a3906c2f5d5 58 return 0xFF; // Error val although could also be data...
narshu 24:7a3906c2f5d5 59 }
narshu 24:7a3906c2f5d5 60 }
narshu 24:7a3906c2f5d5 61 */
narshu 24:7a3906c2f5d5 62
narshu 24:7a3906c2f5d5 63 // Sends a packet of data to the RF module for transmission TODO: Make asych
narshu 24:7a3906c2f5d5 64 void RF12B::write(unsigned char *data, unsigned char length) {
narshu 24:7a3906c2f5d5 65 unsigned char crc = 0;
narshu 24:7a3906c2f5d5 66
narshu 24:7a3906c2f5d5 67 // Transmitter mode
narshu 24:7a3906c2f5d5 68 changeMode(TX);
narshu 24:7a3906c2f5d5 69
narshu 24:7a3906c2f5d5 70 writeCmd(0x0000);
narshu 24:7a3906c2f5d5 71 send(0xAA); // PREAMBLE
narshu 24:7a3906c2f5d5 72 send(0xAA);
narshu 24:7a3906c2f5d5 73 send(0xAA);
narshu 24:7a3906c2f5d5 74 send(0x2D); // SYNC
narshu 24:7a3906c2f5d5 75 send(0xD4);
narshu 24:7a3906c2f5d5 76 // Packet Length
narshu 24:7a3906c2f5d5 77 send(length);
narshu 24:7a3906c2f5d5 78 crc = crc8(crc, length);
narshu 24:7a3906c2f5d5 79 send(crc);
narshu 24:7a3906c2f5d5 80 crc = crc8(crc, crc);
narshu 24:7a3906c2f5d5 81 // Packet Data
narshu 24:7a3906c2f5d5 82 for (unsigned char i=0; i<length; i++) {
narshu 24:7a3906c2f5d5 83 send(data[i]);
narshu 24:7a3906c2f5d5 84 crc = crc8(crc, data[i]);
narshu 24:7a3906c2f5d5 85 }
narshu 24:7a3906c2f5d5 86 send(crc);
narshu 24:7a3906c2f5d5 87 send(0xAA); // DUMMY BYTES
narshu 24:7a3906c2f5d5 88 send(0xAA);
narshu 24:7a3906c2f5d5 89 send(0xAA);
narshu 24:7a3906c2f5d5 90
narshu 24:7a3906c2f5d5 91 // Back to receiver mode
narshu 24:7a3906c2f5d5 92 changeMode(RX);
narshu 24:7a3906c2f5d5 93 status();
narshu 24:7a3906c2f5d5 94
narshu 24:7a3906c2f5d5 95
narshu 24:7a3906c2f5d5 96 }
narshu 24:7a3906c2f5d5 97
narshu 24:7a3906c2f5d5 98 // Transmit a 1-byte data packet
narshu 24:7a3906c2f5d5 99 void RF12B::write(unsigned char data) {
narshu 24:7a3906c2f5d5 100 write(&data, 1);
narshu 24:7a3906c2f5d5 101 }
narshu 24:7a3906c2f5d5 102 /*
narshu 24:7a3906c2f5d5 103 void RF12B::write(queue<char> &data, int length) {
narshu 24:7a3906c2f5d5 104 char crc = 0;
narshu 24:7a3906c2f5d5 105 char length_byte = 0;
narshu 24:7a3906c2f5d5 106
narshu 24:7a3906c2f5d5 107 // -1 means try to transmit everything in the queue
narshu 24:7a3906c2f5d5 108 if (length == -1) {
narshu 24:7a3906c2f5d5 109 length = data.size();
narshu 24:7a3906c2f5d5 110 }
narshu 24:7a3906c2f5d5 111
narshu 24:7a3906c2f5d5 112 // max length of packet is 255
narshu 24:7a3906c2f5d5 113 length_byte = min(length, 255);
narshu 24:7a3906c2f5d5 114
narshu 24:7a3906c2f5d5 115 // Transmitter mode
narshu 24:7a3906c2f5d5 116 changeMode(TX);
narshu 24:7a3906c2f5d5 117
narshu 24:7a3906c2f5d5 118 writeCmd(0x0000);
narshu 24:7a3906c2f5d5 119 send(0xAA); // PREAMBLE
narshu 24:7a3906c2f5d5 120 send(0xAA);
narshu 24:7a3906c2f5d5 121 send(0xAA);
narshu 24:7a3906c2f5d5 122 send(0x2D); // SYNC
narshu 24:7a3906c2f5d5 123 send(0xD4);
narshu 24:7a3906c2f5d5 124 // Packet Length
narshu 24:7a3906c2f5d5 125 send(length_byte);
narshu 24:7a3906c2f5d5 126 crc = crc8(crc, length_byte);
narshu 24:7a3906c2f5d5 127 send(crc);
narshu 24:7a3906c2f5d5 128 crc = crc8(crc, crc);
narshu 24:7a3906c2f5d5 129 // Packet Data
narshu 24:7a3906c2f5d5 130 for (char i=0; i<length_byte; i++) {
narshu 24:7a3906c2f5d5 131 send(data.front());
narshu 24:7a3906c2f5d5 132 crc = crc8(crc, data.front());
narshu 24:7a3906c2f5d5 133 data.pop();
narshu 24:7a3906c2f5d5 134 }
narshu 24:7a3906c2f5d5 135 send(crc);
narshu 24:7a3906c2f5d5 136 send(0xAA); // DUMMY BYTES
narshu 24:7a3906c2f5d5 137 send(0xAA);
narshu 24:7a3906c2f5d5 138 send(0xAA);
narshu 24:7a3906c2f5d5 139
narshu 24:7a3906c2f5d5 140 // Back to receiver mode
narshu 24:7a3906c2f5d5 141 changeMode(RX);
narshu 24:7a3906c2f5d5 142 status();
narshu 24:7a3906c2f5d5 143 }
narshu 24:7a3906c2f5d5 144 */
narshu 24:7a3906c2f5d5 145 /**********************************************************************
narshu 24:7a3906c2f5d5 146 * PRIVATE FUNCTIONS
narshu 24:7a3906c2f5d5 147 *********************************************************************/
narshu 24:7a3906c2f5d5 148
narshu 24:7a3906c2f5d5 149 // Initialises the RF12B module
narshu 24:7a3906c2f5d5 150 void RF12B::init() {
narshu 24:7a3906c2f5d5 151 // writeCmd(0x80E7); //EL,EF,868band,12.0pF
narshu 24:7a3906c2f5d5 152 changeMode(RX);
narshu 24:7a3906c2f5d5 153 writeCmd(0xA640); //frequency select
narshu 24:7a3906c2f5d5 154 writeCmd(0xC647); //4.8kbps
narshu 24:7a3906c2f5d5 155 writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
narshu 24:7a3906c2f5d5 156 writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
narshu 24:7a3906c2f5d5 157 writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
narshu 24:7a3906c2f5d5 158 writeCmd(0xCED4); //SYNC=2DD4
narshu 24:7a3906c2f5d5 159 writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
narshu 24:7a3906c2f5d5 160 writeCmd(0x9850); //!mp,90kHz,MAX OUT
narshu 24:7a3906c2f5d5 161 writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT&#65533;CBW0
narshu 24:7a3906c2f5d5 162 writeCmd(0xE000); //NOT USED
narshu 24:7a3906c2f5d5 163 writeCmd(0xC800); //NOT USED
narshu 24:7a3906c2f5d5 164 writeCmd(0xC040); //1.66MHz,2.2V
narshu 24:7a3906c2f5d5 165
narshu 24:7a3906c2f5d5 166 writeCmd(
narshu 24:7a3906c2f5d5 167 RFM_CONFIG_EL |
narshu 24:7a3906c2f5d5 168 RFM_CONFIG_EF |
narshu 24:7a3906c2f5d5 169 RFM_CONFIG_BAND_433 //|
narshu 24:7a3906c2f5d5 170 //RFM_CONFIG_X_11_0pf // meh, using default
narshu 24:7a3906c2f5d5 171 );
narshu 24:7a3906c2f5d5 172
narshu 24:7a3906c2f5d5 173 // 2. Power Management Command
narshu 24:7a3906c2f5d5 174 // leave everything switched off for now
narshu 24:7a3906c2f5d5 175 /*
narshu 24:7a3906c2f5d5 176 writeCmd(
narshu 24:7a3906c2f5d5 177 RFM_POWER_MANAGEMENT // switch all off
narshu 24:7a3906c2f5d5 178 );
narshu 24:7a3906c2f5d5 179 */
narshu 24:7a3906c2f5d5 180
narshu 24:7a3906c2f5d5 181 // 3. Frequency Setting Command
narshu 24:7a3906c2f5d5 182 writeCmd(
narshu 24:7a3906c2f5d5 183 RFM_FREQUENCY |
narshu 24:7a3906c2f5d5 184 RFM_FREQ_433Band(435.7) //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
narshu 24:7a3906c2f5d5 185 );
narshu 24:7a3906c2f5d5 186
narshu 24:7a3906c2f5d5 187
narshu 24:7a3906c2f5d5 188 // 4. Data Rate Command
narshu 24:7a3906c2f5d5 189 //writeCmd(RFM_DATA_RATE_9600);
narshu 24:7a3906c2f5d5 190 writeCmd(RFM_DATA_RATE_57600);
narshu 24:7a3906c2f5d5 191
narshu 24:7a3906c2f5d5 192
narshu 24:7a3906c2f5d5 193 // 5. Receiver Control Command
narshu 24:7a3906c2f5d5 194 writeCmd(
narshu 24:7a3906c2f5d5 195 RFM_RX_CONTROL_P20_VDI |
narshu 24:7a3906c2f5d5 196 RFM_RX_CONTROL_VDI_FAST |
narshu 24:7a3906c2f5d5 197 //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
narshu 24:7a3906c2f5d5 198 RFM_RX_CONTROL_BW_134 | // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
narshu 24:7a3906c2f5d5 199 RFM_RX_CONTROL_GAIN_0 |
narshu 24:7a3906c2f5d5 200 RFM_RX_CONTROL_RSSI_103 // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
narshu 24:7a3906c2f5d5 201 );
narshu 24:7a3906c2f5d5 202
narshu 24:7a3906c2f5d5 203 // 6. Data Filter Command
narshu 24:7a3906c2f5d5 204 writeCmd(
narshu 24:7a3906c2f5d5 205 RFM_DATA_FILTER_AL |
narshu 24:7a3906c2f5d5 206 RFM_DATA_FILTER_ML |
narshu 24:7a3906c2f5d5 207 RFM_DATA_FILTER_DIG //|
narshu 24:7a3906c2f5d5 208 //RFM_DATA_FILTER_DQD(4)
narshu 24:7a3906c2f5d5 209 );
narshu 24:7a3906c2f5d5 210
narshu 24:7a3906c2f5d5 211 // 7. FIFO and Reset Mode Command
narshu 24:7a3906c2f5d5 212 writeCmd(
narshu 24:7a3906c2f5d5 213 RFM_FIFO_IT(8) |
narshu 24:7a3906c2f5d5 214 RFM_FIFO_DR |
narshu 24:7a3906c2f5d5 215 0x8 //turn on 16bit sync word
narshu 24:7a3906c2f5d5 216 );
narshu 24:7a3906c2f5d5 217
narshu 24:7a3906c2f5d5 218 // 8. FIFO Syncword
narshu 24:7a3906c2f5d5 219 // Leave as default: 0xD4
narshu 24:7a3906c2f5d5 220
narshu 24:7a3906c2f5d5 221 // 9. Receiver FIFO Read
narshu 24:7a3906c2f5d5 222 // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
narshu 24:7a3906c2f5d5 223 // result = RFM_READ_FIFO();
narshu 24:7a3906c2f5d5 224
narshu 24:7a3906c2f5d5 225 // 10. AFC Command
narshu 24:7a3906c2f5d5 226 writeCmd(
narshu 24:7a3906c2f5d5 227 //RFM_AFC_AUTO_VDI | //Note this might be changed to improve range. Refer to datasheet.
narshu 24:7a3906c2f5d5 228 RFM_AFC_AUTO_INDEPENDENT |
narshu 24:7a3906c2f5d5 229 RFM_AFC_RANGE_LIMIT_7_8 |
narshu 24:7a3906c2f5d5 230 RFM_AFC_EN |
narshu 24:7a3906c2f5d5 231 RFM_AFC_OE |
narshu 24:7a3906c2f5d5 232 RFM_AFC_FI
narshu 24:7a3906c2f5d5 233 );
narshu 24:7a3906c2f5d5 234
narshu 24:7a3906c2f5d5 235 // 11. TX Configuration Control Command
narshu 24:7a3906c2f5d5 236 writeCmd(
narshu 24:7a3906c2f5d5 237 RFM_TX_CONTROL_MOD_60 |
narshu 24:7a3906c2f5d5 238 RFM_TX_CONTROL_POW_0
narshu 24:7a3906c2f5d5 239 );
narshu 24:7a3906c2f5d5 240
narshu 24:7a3906c2f5d5 241
narshu 24:7a3906c2f5d5 242 // 12. PLL Setting Command
narshu 24:7a3906c2f5d5 243 writeCmd(
narshu 24:7a3906c2f5d5 244 0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
narshu 24:7a3906c2f5d5 245 // I think this will slow down the pll's reaction time. Not sure, check with someone!
narshu 24:7a3906c2f5d5 246 );
narshu 24:7a3906c2f5d5 247
narshu 24:7a3906c2f5d5 248 changeMode(RX);
narshu 24:7a3906c2f5d5 249 resetRX();
narshu 24:7a3906c2f5d5 250 status();
narshu 24:7a3906c2f5d5 251 }
narshu 24:7a3906c2f5d5 252
narshu 24:7a3906c2f5d5 253 /* Write a command to the RF Module */
narshu 24:7a3906c2f5d5 254 unsigned int RF12B::writeCmd(unsigned int cmd) {
narshu 24:7a3906c2f5d5 255 NCS = 0;
narshu 24:7a3906c2f5d5 256 unsigned int recv = spi.write(cmd);
narshu 24:7a3906c2f5d5 257 NCS = 1;
narshu 24:7a3906c2f5d5 258 return recv;
narshu 24:7a3906c2f5d5 259 }
narshu 24:7a3906c2f5d5 260
narshu 24:7a3906c2f5d5 261 /* Sends a byte of data across RF */
narshu 24:7a3906c2f5d5 262 void RF12B::send(unsigned char data) {
narshu 24:7a3906c2f5d5 263 while (NIRQ);
narshu 24:7a3906c2f5d5 264 writeCmd(0xB800 + data);
narshu 24:7a3906c2f5d5 265 }
narshu 24:7a3906c2f5d5 266
narshu 24:7a3906c2f5d5 267 /* Change the mode of the RF module to Transmitting or Receiving */
narshu 24:7a3906c2f5d5 268 void RF12B::changeMode(rfmode_t _mode) {
narshu 24:7a3906c2f5d5 269 mode = _mode;
narshu 24:7a3906c2f5d5 270 if (_mode == TX) {
narshu 24:7a3906c2f5d5 271 writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
narshu 24:7a3906c2f5d5 272 } else { /* mode == RX */
narshu 24:7a3906c2f5d5 273 writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
narshu 24:7a3906c2f5d5 274 }
narshu 24:7a3906c2f5d5 275 }
narshu 24:7a3906c2f5d5 276
narshu 24:7a3906c2f5d5 277 // Interrupt routine for data reception */
narshu 24:7a3906c2f5d5 278 void RF12B::rxISR() {
narshu 24:7a3906c2f5d5 279
narshu 24:7a3906c2f5d5 280 unsigned int data = 0;
narshu 24:7a3906c2f5d5 281 static int i = -2;
narshu 24:7a3906c2f5d5 282 static unsigned char packet_length = 0;
narshu 24:7a3906c2f5d5 283 static unsigned char crc = 0;
narshu 24:7a3906c2f5d5 284 // #ifdef ROBOT_SECONDARY
narshu 24:7a3906c2f5d5 285 static unsigned char temp;
narshu 24:7a3906c2f5d5 286 // #endif
narshu 24:7a3906c2f5d5 287
narshu 24:7a3906c2f5d5 288 //Loop while interrupt is asserted
narshu 24:7a3906c2f5d5 289 while (!NIRQ_in && mode == RX) {
narshu 24:7a3906c2f5d5 290
narshu 24:7a3906c2f5d5 291 // Grab the packet's length byte
narshu 24:7a3906c2f5d5 292 if (i == -2) {
narshu 24:7a3906c2f5d5 293 data = writeCmd(0x0000);
narshu 24:7a3906c2f5d5 294 if ( (data&0x8000) ) {
narshu 24:7a3906c2f5d5 295 data = writeCmd(0xB000);
narshu 24:7a3906c2f5d5 296 packet_length = (data&0x00FF);
narshu 24:7a3906c2f5d5 297 crc = crc8(crc, packet_length);
narshu 24:7a3906c2f5d5 298 i++;
narshu 24:7a3906c2f5d5 299 }
narshu 24:7a3906c2f5d5 300 }
narshu 24:7a3906c2f5d5 301
narshu 24:7a3906c2f5d5 302 //If we exhaust the interrupt, exit
narshu 24:7a3906c2f5d5 303 if (NIRQ_in)
narshu 24:7a3906c2f5d5 304 break;
narshu 24:7a3906c2f5d5 305
narshu 24:7a3906c2f5d5 306 // Check that packet length was correct
narshu 24:7a3906c2f5d5 307 if (i == -1) {
narshu 24:7a3906c2f5d5 308 data = writeCmd(0x0000);
narshu 24:7a3906c2f5d5 309 if ( (data&0x8000) ) {
narshu 24:7a3906c2f5d5 310 data = writeCmd(0xB000);
narshu 24:7a3906c2f5d5 311 unsigned char crcofsize = (data&0x00FF);
narshu 24:7a3906c2f5d5 312 if (crcofsize != crc) {
narshu 24:7a3906c2f5d5 313 //It was wrong, start over
narshu 24:7a3906c2f5d5 314 i = -2;
narshu 24:7a3906c2f5d5 315 packet_length = 0;
narshu 24:7a3906c2f5d5 316 crc = 0;
narshu 24:7a3906c2f5d5 317 //temp = queue<unsigned char>();
narshu 24:7a3906c2f5d5 318 resetRX();
narshu 24:7a3906c2f5d5 319 } else {
narshu 24:7a3906c2f5d5 320 crc = crc8(crc, crcofsize);
narshu 24:7a3906c2f5d5 321 i++;
narshu 24:7a3906c2f5d5 322 }
narshu 24:7a3906c2f5d5 323 }
narshu 24:7a3906c2f5d5 324 }
narshu 24:7a3906c2f5d5 325
narshu 24:7a3906c2f5d5 326 //If we exhaust the interrupt, exit
narshu 24:7a3906c2f5d5 327 if (NIRQ_in)
narshu 24:7a3906c2f5d5 328 break;
narshu 24:7a3906c2f5d5 329
narshu 24:7a3906c2f5d5 330 // Grab the packet's data
narshu 24:7a3906c2f5d5 331 if (i >= 0 && i < packet_length) {
narshu 24:7a3906c2f5d5 332 data = writeCmd(0x0000);
narshu 24:7a3906c2f5d5 333 if ( (data&0x8000) ) {
narshu 24:7a3906c2f5d5 334 data = writeCmd(0xB000);
narshu 24:7a3906c2f5d5 335 // #ifdef ROBOT_SECONDARY
narshu 24:7a3906c2f5d5 336 temp = data&0x00FF;
narshu 24:7a3906c2f5d5 337 // #endif
narshu 24:7a3906c2f5d5 338 //temp.push(data&0x00FF);
narshu 24:7a3906c2f5d5 339 crc = crc8(crc, (unsigned char)(data&0x00FF));
narshu 24:7a3906c2f5d5 340 i++;
narshu 24:7a3906c2f5d5 341 }
narshu 24:7a3906c2f5d5 342 }
narshu 24:7a3906c2f5d5 343
narshu 24:7a3906c2f5d5 344 //If we exhaust the interrupt, exit
narshu 24:7a3906c2f5d5 345 if (NIRQ_in)
narshu 24:7a3906c2f5d5 346 break;
narshu 24:7a3906c2f5d5 347
narshu 24:7a3906c2f5d5 348 if (i >= packet_length) {
narshu 24:7a3906c2f5d5 349 data = writeCmd(0x0000);
narshu 24:7a3906c2f5d5 350 if ( (data&0x8000) ) {
narshu 24:7a3906c2f5d5 351 data = writeCmd(0xB000);
narshu 24:7a3906c2f5d5 352 if ((unsigned char)(data & 0x00FF) == crc) {
narshu 24:7a3906c2f5d5 353 //If the checksum is correct, add our data to the end of the output buffer
narshu 24:7a3906c2f5d5 354 //while (!temp.empty()) {
narshu 24:7a3906c2f5d5 355 //fifo.push(temp);
narshu 24:7a3906c2f5d5 356 // temp.pop();
narshu 24:7a3906c2f5d5 357 //#ifdef ROBOT_SECONDARY
narshu 24:7a3906c2f5d5 358 if (callbackfunc)
narshu 24:7a3906c2f5d5 359 (*callbackfunc)(temp);
narshu 24:7a3906c2f5d5 360
narshu 24:7a3906c2f5d5 361 if (callbackobj && mcallbackfunc)
narshu 24:7a3906c2f5d5 362 (callbackobj->*mcallbackfunc)(temp);
narshu 24:7a3906c2f5d5 363 //#endif
narshu 24:7a3906c2f5d5 364 // }
narshu 24:7a3906c2f5d5 365 }
narshu 24:7a3906c2f5d5 366
narshu 24:7a3906c2f5d5 367 // Tell RF Module we are finished, and clean up
narshu 24:7a3906c2f5d5 368 i = -2;
narshu 24:7a3906c2f5d5 369 packet_length = 0;
narshu 24:7a3906c2f5d5 370 crc = 0;
narshu 24:7a3906c2f5d5 371 //temp = queue<unsigned char>();
narshu 24:7a3906c2f5d5 372 resetRX();
narshu 24:7a3906c2f5d5 373 }
narshu 24:7a3906c2f5d5 374 }
narshu 24:7a3906c2f5d5 375 }
narshu 24:7a3906c2f5d5 376
narshu 24:7a3906c2f5d5 377 }
narshu 24:7a3906c2f5d5 378
narshu 24:7a3906c2f5d5 379 unsigned int RF12B::status() {
narshu 24:7a3906c2f5d5 380 return writeCmd(0x0000);
narshu 24:7a3906c2f5d5 381 }
narshu 24:7a3906c2f5d5 382
narshu 24:7a3906c2f5d5 383 // Tell the RF Module this packet is received and wait for the next */
narshu 24:7a3906c2f5d5 384 void RF12B::resetRX() {
narshu 24:7a3906c2f5d5 385 writeCmd(0xCA81);
narshu 24:7a3906c2f5d5 386 writeCmd(0xCA83);
narshu 24:7a3906c2f5d5 387 };
narshu 24:7a3906c2f5d5 388
narshu 24:7a3906c2f5d5 389 // Calculate CRC8 */
narshu 24:7a3906c2f5d5 390 unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
narshu 24:7a3906c2f5d5 391 crc = crc ^ data;
narshu 24:7a3906c2f5d5 392 for (int i = 0; i < 8; i++) {
narshu 24:7a3906c2f5d5 393 if (crc & 0x01) {
narshu 24:7a3906c2f5d5 394 crc = (crc >> 1) ^ 0x8C;
narshu 24:7a3906c2f5d5 395 } else {
narshu 24:7a3906c2f5d5 396 crc >>= 1;
narshu 24:7a3906c2f5d5 397 }
narshu 24:7a3906c2f5d5 398 }
narshu 24:7a3906c2f5d5 399 return crc;
narshu 24:7a3906c2f5d5 400 }