Multiprotocol/Propel_nrf24l01.ino

   1 /*
   2  This project is free software: you can redistribute it and/or modify
   3  it under the terms of the GNU General Public License as published by
   4  the Free Software Foundation, either version 3 of the License, or
   5  (at your option) any later version.
   6
   7 Multiprotocol is distributed in the hope that it will be useful,
   8  but WITHOUT ANY WARRANTY; without even the implied warranty of
   9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10  GNU General Public License for more details.
  11
  12  You should have received a copy of the GNU General Public License
  13  along with Multiprotocol.  If not, see <http://www.gnu.org/licenses/>.
  14  */
  15 // Compatible with PROPEL 74-Z Speeder Bike.
  16
  17 #if defined(PROPEL_NRF24L01_INO)
  18
  19 #include "iface_nrf24l01.h"
  20
  21 //#define PROPEL_FORCE_ID
  22
  23 #define PROPEL_INITIAL_WAIT             500
  24 #define PROPEL_PACKET_PERIOD    10000
  25 #define PROPEL_BIND_RF_CHANNEL  0x23
  26 #define PROPEL_PAYLOAD_SIZE             16
  27 #define PROPEL_SEARCH_PERIOD    50      //*10ms
  28 #define PROPEL_BIND_PERIOD              1500
  29 #define PROPEL_PACKET_SIZE              14
  30 #define PROPEL_RF_NUM_CHANNELS  4
  31 #define PROPEL_ADDRESS_LENGTH   5
  32 #define PROPEL_DEFAULT_PERIOD   20
  33
  34 enum {
  35         PROPEL_BIND1 = 0,
  36         PROPEL_BIND2,
  37         PROPEL_BIND3,
  38         PROPEL_DATA1,
  39 };
  40
  41 static uint16_t __attribute__((unused)) PROPEL_checksum()
  42 {
  43         typedef union {
  44                 struct {
  45                         uint8_t h:1;
  46                         uint8_t g:1;
  47                         uint8_t f:1;
  48                         uint8_t e:1;
  49                         uint8_t d:1;
  50                         uint8_t c:1;
  51                         uint8_t b:1;
  52                         uint8_t a:1;
  53                 } bits;
  54                 uint8_t byte:8;
  55         } byte_bits_t;
  56
  57     uint8_t sum = packet[0];
  58     for (uint8_t i = 1; i < PROPEL_PACKET_SIZE - 2; i++)
  59         sum += packet[i];
  60
  61     byte_bits_t in  = { .byte = sum };
  62     byte_bits_t out = { .byte = sum };
  63         out.byte ^= 0x0a;
  64     out.bits.d = !(in.bits.d ^ in.bits.h);
  65     out.bits.c = (!in.bits.c && !in.bits.d &&  in.bits.g)
  66               ||  (in.bits.c && !in.bits.d && !in.bits.g)
  67               || (!in.bits.c &&  in.bits.g && !in.bits.h)
  68               ||  (in.bits.c && !in.bits.g && !in.bits.h)
  69               ||  (in.bits.c &&  in.bits.d &&  in.bits.g &&  in.bits.h)
  70               || (!in.bits.c &&  in.bits.d && !in.bits.g &&  in.bits.h);
  71     out.bits.b = (!in.bits.b && !in.bits.c && !in.bits.d)
  72               ||  (in.bits.b &&  in.bits.c &&  in.bits.g)
  73               || (!in.bits.b && !in.bits.c && !in.bits.g)
  74               || (!in.bits.b && !in.bits.d && !in.bits.g)
  75               || (!in.bits.b && !in.bits.c && !in.bits.h)
  76               || (!in.bits.b && !in.bits.g && !in.bits.h)
  77               ||  (in.bits.b &&  in.bits.c &&  in.bits.d &&  in.bits.h)
  78               ||  (in.bits.b &&  in.bits.d &&  in.bits.g &&  in.bits.h);
  79     out.bits.a =  (in.bits.a && !in.bits.b)
  80               ||  (in.bits.a && !in.bits.c && !in.bits.d)
  81               ||  (in.bits.a && !in.bits.c && !in.bits.g)
  82               ||  (in.bits.a && !in.bits.d && !in.bits.g)
  83               ||  (in.bits.a && !in.bits.c && !in.bits.h)
  84               ||  (in.bits.a && !in.bits.g && !in.bits.h)
  85               || (!in.bits.a &&  in.bits.b &&  in.bits.c &&  in.bits.g)
  86               || (!in.bits.a &&  in.bits.b &&  in.bits.c &&  in.bits.d &&  in.bits.h)
  87               || (!in.bits.a &&  in.bits.b &&  in.bits.d &&  in.bits.g &&  in.bits.h);
  88
  89     return (sum << 8) | (out.byte & 0xff);
  90 }
  91
  92 static void __attribute__((unused)) PROPEL_bind_packet(bool valid_rx_id)
  93 {
  94         memset(packet, 0, PROPEL_PACKET_SIZE);
  95
  96         packet[0] = 0xD0;
  97         memcpy(&packet[1], rx_tx_addr, 4);  // only 4 bytes sent of 5-byte address
  98         if (valid_rx_id) memcpy(&packet[5], rx_id, 4);
  99         packet[9] = rf_ch_num;                          // hopping table to be used when switching to normal mode
 100         packet[11] = 0x05;                                      // unknown, 0x01 on TX2??
 101
 102         uint16_t check = PROPEL_checksum();
 103         packet[12] = check >> 8;
 104         packet[13] = check & 0xff;
 105
 106         NRF24L01_WriteReg(NRF24L01_07_STATUS, (_BV(NRF24L01_07_RX_DR) | _BV(NRF24L01_07_TX_DS) | _BV(NRF24L01_07_MAX_RT)));
 107         NRF24L01_FlushTx();
 108         NRF24L01_FlushRx();
 109         NRF24L01_WritePayload(packet, PROPEL_PACKET_SIZE);
 110 }
 111
 112 static void __attribute__((unused)) PROPEL_data_packet()
 113 {
 114         memset(packet, 0, PROPEL_PACKET_SIZE);
 115
 116         packet[0] = 0xC0;
 117         packet[1] = convert_channel_16b_limit(THROTTLE, 0x2f, 0xcf);
 118         packet[2] = convert_channel_16b_limit(RUDDER  , 0xcf, 0x2f);
 119         packet[3] = convert_channel_16b_limit(ELEVATOR, 0x2f, 0xcf);
 120         packet[4] = convert_channel_16b_limit(AILERON , 0xcf, 0x2f);
 121         packet[5] = 0x40;                                                       //might be trims but unsused
 122         packet[6] = 0x40;                                                       //might be trims but unsused
 123         packet[7] = 0x40;                                                       //might be trims but unsused
 124         packet[8] = 0x40;                                                       //might be trims but unsused
 125         if (bind_phase)
 126         {//need to send a couple of default packets after bind
 127                 bind_phase--;
 128                 packet[10] = 0x80;      // LEDs
 129         }
 130         else
 131         {
 132                 packet[9] = 0x02                                        // Always fast speed, slow=0x00, medium=0x01, fast=0x02, 0x03=flight training mode
 133                                         | GET_FLAG( CH14_SW, 0x03)      // Flight training mode
 134                                         | GET_FLAG( CH10_SW, 0x04)      // Calibrate
 135                                         | GET_FLAG( CH12_SW, 0x08)      // Take off
 136                                         | GET_FLAG( CH8_SW,  0x10)      // Fire
 137                                         | GET_FLAG( CH11_SW, 0x20)      // Altitude hold=0x20
 138                                         | GET_FLAG( CH6_SW,  0x40)      // Roll CW
 139                                         | GET_FLAG( CH7_SW,  0x80);     // Roll CCW
 140                 packet[10] =  GET_FLAG( CH13_SW, 0x20)  // Land
 141                                         | GET_FLAG( CH9_SW,  0x40)      // Weapon system activted=0x40
 142                                         | GET_FLAG(!CH5_SW,  0x80);     // LEDs
 143         }
 144         packet[11] = 5;                                                         // unknown, 0x01 on TX2??
 145
 146         uint16_t check = PROPEL_checksum();
 147         packet[12] = check >> 8;
 148         packet[13] = check & 0xff;
 149
 150         NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no++]);
 151         hopping_frequency_no &= 0x03;
 152         NRF24L01_SetPower();
 153         NRF24L01_WriteReg(NRF24L01_07_STATUS, (_BV(NRF24L01_07_RX_DR) | _BV(NRF24L01_07_TX_DS) | _BV(NRF24L01_07_MAX_RT)));
 154         NRF24L01_FlushTx();
 155         NRF24L01_WritePayload(packet, PROPEL_PACKET_SIZE);
 156 }
 157
 158 static void __attribute__((unused)) PROPEL_RF_init()
 159 {
 160         NRF24L01_Initialize();
 161
 162         NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x7f);
 163         NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x3f);                     // AA on all pipes
 164         NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x3f);         // Enable all pipes
 165         NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x36);        // retransmit 1ms, 6 times
 166         NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, (uint8_t *)"\x99\x77\x55\x33\x11", PROPEL_ADDRESS_LENGTH);  //Bind address
 167         NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR,    (uint8_t *)"\x99\x77\x55\x33\x11", PROPEL_ADDRESS_LENGTH);  //Bind address
 168         NRF24L01_WriteReg(NRF24L01_05_RF_CH, PROPEL_BIND_RF_CHANNEL);
 169         NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3f);                     // Enable dynamic payload length
 170         NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x07);           // Enable all features
 171
 172         NRF24L01_SetTxRxMode(TX_EN);                                            // Clear data ready, data sent, retransmit and enable CRC 16bits, ready for TX
 173 }
 174
 175 const uint8_t PROGMEM PROPEL_hopping []= { 0x47,0x36,0x27,0x44,0x33,0x0D,0x3C,0x2E,0x1B,0x39,0x2A,0x18 };
 176 static void __attribute__((unused)) PROPEL_initialize_txid()
 177 {
 178         //address last byte
 179         rx_tx_addr[4]=0x11;
 180
 181         //random hopping channel table
 182         rf_ch_num=random(0xfefefefe)&0x03;
 183         for(uint8_t i=0; i<3; i++)
 184                 hopping_frequency[i]=pgm_read_byte_near( &PROPEL_hopping[i + 3*rf_ch_num] );
 185         hopping_frequency[3]=0x23;
 186
 187 #ifdef PROPEL_FORCE_ID
 188         if(RX_num&1)
 189                 memcpy(rx_tx_addr, (uint8_t *)"\x73\xd3\x31\x30\x11", PROPEL_ADDRESS_LENGTH);   //TX1: 73 d3 31 30 11
 190         else
 191                 memcpy(rx_tx_addr, (uint8_t *)"\x94\xc5\x31\x30\x11", PROPEL_ADDRESS_LENGTH);   //TX2: 94 c5 31 30 11
 192         rf_ch_num = 0x03; //TX1
 193         memcpy(hopping_frequency,(uint8_t *)"\x39\x2A\x18\x23",PROPEL_RF_NUM_CHANNELS);         //TX1: 57,42,24,35
 194         rf_ch_num = 0x00; //TX2
 195         memcpy(hopping_frequency,(uint8_t *)"\x47\x36\x27\x23",PROPEL_RF_NUM_CHANNELS);         //TX2: 71,54,39,35
 196         rf_ch_num = 0x01; // Manual search
 197         memcpy(hopping_frequency,(uint8_t *)"\x44\x33\x0D\x23",PROPEL_RF_NUM_CHANNELS);         //Manual: 68,51,13,35
 198         rf_ch_num = 0x02; // Manual search
 199         memcpy(hopping_frequency,(uint8_t *)"\x3C\x2E\x1B\x23",PROPEL_RF_NUM_CHANNELS);         //Manual: 60,46,27,35
 200 #endif
 201 }
 202
 203 uint16_t PROPEL_callback()
 204 {
 205         uint8_t status;
 206
 207         switch (phase)
 208         {
 209                 case PROPEL_BIND1:
 210                         PROPEL_bind_packet(false);              //rx_id unknown
 211                         phase++;                                                //BIND2
 212                         return PROPEL_BIND_PERIOD;
 213
 214                 case PROPEL_BIND2:
 215                         status=NRF24L01_ReadReg(NRF24L01_07_STATUS);
 216                         if (status & _BV(NRF24L01_07_MAX_RT))
 217                         {// Max retry (6) reached
 218                                 phase = PROPEL_BIND1;
 219                                 return PROPEL_BIND_PERIOD;
 220                         }
 221                         if (!(_BV(NRF24L01_07_RX_DR) & status))
 222                                 return PROPEL_BIND_PERIOD;              // nothing received
 223                         // received frame, got rx_id, save it
 224                         NRF24L01_ReadPayload(packet_in, PROPEL_PACKET_SIZE);
 225                         memcpy(rx_id, &packet_in[1], 4);
 226                         PROPEL_bind_packet(true);               //send bind packet with rx_id
 227                         phase++;                                                //BIND3
 228                         break;
 229
 230                 case PROPEL_BIND3:
 231                         if (_BV(NRF24L01_07_RX_DR) & NRF24L01_ReadReg(NRF24L01_07_STATUS))
 232                         {
 233                                 NRF24L01_ReadPayload(packet_in, PROPEL_PACKET_SIZE);
 234                                 if (packet_in[0] == 0xa3 && memcmp(&packet_in[1],rx_id,4)==0)
 235                                 {//confirmation from the model
 236                                         phase++;                                //PROPEL_DATA1
 237                                         bind_phase=PROPEL_DEFAULT_PERIOD;
 238                                         packet_count=0;
 239                                         BIND_DONE;
 240                                         break;
 241                                 }
 242                         }
 243                         NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, PROPEL_ADDRESS_LENGTH);
 244                         NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, PROPEL_ADDRESS_LENGTH);
 245                         PROPEL_bind_packet(true);               //send bind packet with rx_id
 246                         break;
 247
 248                 case PROPEL_DATA1:
 249                         #ifdef PROPEL_HUB_TELEMETRY
 250                                 if (_BV(NRF24L01_07_RX_DR) & NRF24L01_ReadReg(NRF24L01_07_STATUS))
 251                                 {// data received from the model
 252                                         NRF24L01_ReadPayload(packet_in, PROPEL_PACKET_SIZE);
 253                                         #if 1
 254                                                 for(uint8_t i=0; i<PROPEL_PACKET_SIZE; i++)
 255                                                         debug("%02X ",packet_in[i]);
 256                                                 debugln("");
 257                                         #endif
 258                                         if (packet_in[0] == 0xa3 && memcmp(&packet_in[1],rx_id,3)==0)
 259                                         {
 260                                                 telemetry_counter++;    //LQI
 261                                                 v_lipo1=packet_in[5];   //number of life left?
 262                                                 v_lipo2=packet_in[4];   //bit mask: 0x80=flying, 0x08=taking off, 0x04=landing, 0x00=landed/crashed
 263                                                 if(telemetry_lost==0)
 264                                                         telemetry_link=1;
 265                                         }
 266                                 }
 267                                 packet_count++;
 268                                 if(packet_count>=100)
 269                                 {//LQI calculation
 270                                         packet_count=0;
 271                                         TX_LQI=telemetry_counter;
 272                                         RX_RSSI=telemetry_counter;
 273                                         telemetry_counter = 0;
 274                                         telemetry_lost=0;
 275                                 }
 276                         #endif
 277                         PROPEL_data_packet();
 278                         break;
 279         }
 280         return PROPEL_PACKET_PERIOD;
 281 }
 282
 283 void PROPEL_init()
 284 {
 285         BIND_IN_PROGRESS;       // autobind protocol
 286         PROPEL_initialize_txid();
 287         PROPEL_RF_init();
 288         hopping_frequency_no = 0;
 289         phase=PROPEL_BIND1;
 290 }
 291
 292 #endif
 293 // equations for checksum check byte from truth table
 294 // (1)  z =  a && !b
 295 //       ||  a && !c && !d
 296 //       ||  a && !c && !g
 297 //       ||  a && !d && !g
 298 //       ||  a && !c && !h
 299 //       ||  a && !g && !h
 300 //       || !a &&  b &&  c &&  g
 301 //       || !a &&  b &&  c &&  d &&  h
 302 //       || !a &&  b &&  d &&  g &&  h;
 303 //
 304 // (2)  y = !b && !c && !d
 305 //       ||  b &&  c &&  g
 306 //       || !b && !c && !g
 307 //       || !b && !d && !g
 308 //       || !b && !c && !h
 309 //       || !b && !g && !h
 310 //       ||  b &&  c &&  d &&  h
 311 //       ||  b &&  d &&  g &&  h;
 312 //
 313 // (3)  x = !c && !d &&  g
 314 //       ||  c && !d && !g
 315 //       || !c &&  g && !h
 316 //       ||  c && !g && !h
 317 //       ||  c &&  d &&  g &&  h
 318 //       || !c &&  d && !g &&  h;
 319 //
 320 // (4)  w =  d &&  h
 321 //       || !d && !h;
 322 //
 323 // (5)  v = !e;
 324 //
 325 // (6)  u =  f;
 326 //
 327 // (7)  t = !g;
 328 //
 329 // (8)  s =  h;