Multiprotocol/KN_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 // Last sync with hexfet new_protocols/KN_nrf24l01.c dated 2015-11-09
  16
  17 #if defined(KN_NRF24L01_INO)
  18
  19 #include "iface_nrf24l01.h"
  20
  21 // Wait for RX chip stable - 10ms
  22 #define KN_INIT_WAIT_MS  10000
  23
  24 //Payload(16 bytes) plus overhead(10 bytes) is 208 bits, takes about 0.4ms or 0.2ms
  25 //to send for the rate of 500kb/s and 1Mb/s respectively.
  26
  27 // Callback timeout period for sending bind packets, minimum 250
  28 #define KN_BINDING_PACKET_PERIOD  1000
  29
  30 // Timeout for sending data packets, in uSec, KN protocol requires 2ms
  31 #define KN_WL_SENDING_PACKET_PERIOD  2000
  32 // Timeout for sending data packets, in uSec, KNFX protocol requires 1.2 ms
  33 #define KN_FX_SENDING_PACKET_PERIOD  1200
  34
  35 // packets to be sent during binding, last 0.5 seconds in WL Toys and 0.2 seconds in Feilun
  36 #define KN_WL_BIND_COUNT 500
  37 #define KN_FX_BIND_COUNT 200
  38
  39 #define KN_PAYLOADSIZE 16
  40
  41 //24L01 has 126 RF channels, can we use all of them?
  42 #define KN_MAX_RF_CHANNEL 73
  43
  44 //KN protocol for WL Toys changes RF frequency every 10 ms, repeats with only 4 channels.
  45 //Feilun variant uses only 2 channels, so we will just repeat the hopping channels later
  46 #define KN_RF_CH_COUNT 4
  47
  48 //KN protocol for WL Toys sends 4 data packets every 2ms per frequency, plus a 2ms gap.
  49 #define KN_WL_PACKET_SEND_COUNT 5
  50 //KN protocol for Feilun sends 8 data packets every 1.2ms per frequency, plus a 0.3ms gap.
  51 #define KN_FX_PACKET_SEND_COUNT 8
  52
  53 #define KN_TX_ADDRESS_SIZE 5
  54
  55 enum {
  56     KN_PHASE_PRE_BIND,
  57     KN_PHASE_BINDING,
  58     KN_PHASE_PRE_SEND,
  59     KN_PHASE_SENDING,
  60 };
  61
  62 enum {
  63         KN_FLAG_DR     = 0x01, // Dual Rate: 1 - full range
  64         KN_FLAG_TH     = 0x02, // Throttle Hold: 1 - hold
  65         KN_FLAG_IDLEUP = 0x04, // Idle up: 1 - 3D
  66         KN_FLAG_RES1   = 0x08, // HoverDebugging
  67         KN_FLAG_RES2   = 0x10,
  68         KN_FLAG_RES3   = 0x20,
  69         KN_FLAG_GYRO3  = 0x40, // 0 - 6G mode, 1 - 3G mode
  70         KN_FLAG_GYROR  = 0x80  // Always 0 so far
  71 };
  72
  73 //-------------------------------------------------------------------------------------------------
  74 // This function init 24L01 regs and packet data for binding
  75 // Send tx address, hopping table (for Wl Toys), and data rate to the KN receiver during binding.
  76 // It seems that KN can remember these parameters, no binding needed after power up.
  77 // Bind uses fixed TX address "KNDZK", 1 Mbps data rate and channel 83
  78 //-------------------------------------------------------------------------------------------------
  79 static void __attribute__((unused)) kn_bind_init()
  80 {
  81         NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t*)"KNDZK", 5);
  82         packet[0]  = 'K';
  83         packet[1]  = 'N';
  84         packet[2]  = 'D';
  85         packet[3]  = 'Z';
  86         //Use first four bytes of tx_addr
  87         packet[4]  = rx_tx_addr[0];
  88         packet[5]  = rx_tx_addr[1];
  89         packet[6]  = rx_tx_addr[2];
  90         packet[7]  = rx_tx_addr[3];
  91
  92         if(sub_protocol==WLTOYS)
  93         {
  94                 packet[8]  = hopping_frequency[0];
  95                 packet[9]  = hopping_frequency[1];
  96                 packet[10] = hopping_frequency[2];
  97                 packet[11] = hopping_frequency[3];
  98         }
  99         else
 100         {
 101                 packet[8]  = 0x00;
 102                 packet[9]  = 0x00;
 103                 packet[10] = 0x00;
 104                 packet[11] = 0x00;
 105         }
 106         packet[12] = 0x00;
 107         packet[13] = 0x00;
 108         packet[14] = 0x00;
 109         packet[15] = 0x01;      //(USE1MBPS_YES) ? 0x01 : 0x00;
 110
 111         //Set RF channel
 112         NRF24L01_WriteReg(NRF24L01_05_RF_CH, 83);
 113 }
 114
 115 //-------------------------------------------------------------------------------------------------
 116 // Update control data to be sent
 117 // Do it once per frequency, so the same values will be sent 4 times
 118 // KN uses 4 10-bit data channels plus a 8-bit switch channel
 119 //
 120 // The packet[0] is used for pitch/throttle, the relation is hard coded, not changeable.
 121 // We can change the throttle/pitch range though.
 122 //
 123 // How to use trim? V977 stock controller can trim 6-axis mode to eliminate the drift.
 124 //-------------------------------------------------------------------------------------------------
 125 static void __attribute__((unused)) kn_update_packet_control_data()
 126 {
 127         uint16_t value;
 128         value = convert_channel_10b(THROTTLE, false);
 129         packet[0]  = (value >> 8) & 0xFF;
 130         packet[1]  = value & 0xFF;
 131         value = convert_channel_10b(AILERON, false);
 132         packet[2]  = (value >> 8) & 0xFF;
 133         packet[3]  = value & 0xFF;
 134         value = convert_channel_10b(ELEVATOR, false);
 135         packet[4]  = (value >> 8) & 0xFF;
 136         packet[5]  = value & 0xFF;
 137         value = convert_channel_10b(RUDDER, false);
 138         packet[6]  = (value >> 8) & 0xFF;
 139         packet[7]  = value & 0xFF;
 140         // Trims, middle is 0x64 (100) range 0-200
 141         packet[8]  = convert_channel_16b_limit(CH9,0,200); // 0x64; // T
 142         packet[9]  = convert_channel_16b_limit(CH10,0,200); // 0x64; // A
 143         packet[10] = convert_channel_16b_limit(CH11,0,200); // 0x64; // E
 144         packet[11] = convert_channel_16b_limit(CH12,0,200); // 0x64; // R
 145
 146         packet[12] = GET_FLAG(CH5_SW,  KN_FLAG_DR)
 147                                 |GET_FLAG(CH6_SW,  KN_FLAG_TH)
 148                                 |GET_FLAG(CH7_SW,  KN_FLAG_IDLEUP)
 149                                 |GET_FLAG(CH8_SW,  KN_FLAG_GYRO3)
 150                                 |GET_FLAG(CH13_SW, KN_FLAG_RES1);       //Hover debugging
 151
 152         packet[13] = 0x00;
 153         if(sub_protocol==WLTOYS)
 154                 packet[13] = (packet_sent << 5) | (hopping_frequency_no << 2);
 155
 156         packet[14] = 0x00;
 157         packet[15] = 0x00;
 158
 159         NRF24L01_SetPower();
 160 }
 161
 162
 163 //-------------------------------------------------------------------------------------------------
 164 // This function generate RF TX packet address
 165 // V977 can remember the binding parameters; we do not need rebind when power up.
 166 // This requires the address must be repeatable for a specific RF ID at power up.
 167 //-------------------------------------------------------------------------------------------------
 168 static void __attribute__((unused)) kn_calculate_tx_addr()
 169 {
 170         if(sub_protocol==FEILUN)
 171         {
 172                 uint8_t addr2;
 173                 // Generate TXID with sum of minimum 256 and maximum 256+MAX_RF_CHANNEL-32
 174                 rx_tx_addr[1] = 1 + rx_tx_addr[0] % (KN_MAX_RF_CHANNEL-33);
 175                 addr2 = 1 + rx_tx_addr[2] % (KN_MAX_RF_CHANNEL-33);
 176                 if ((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1]) < 256)
 177                         rx_tx_addr[2] = addr2;
 178                 else
 179                         rx_tx_addr[2] = 0x00;
 180                 rx_tx_addr[3] = 0x00;
 181                 while((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]) < 257)
 182                         rx_tx_addr[3] += addr2;
 183         }
 184     //The 5th byte is a constant, must be 'K'
 185     rx_tx_addr[4] = 'K';
 186 }
 187
 188 //-------------------------------------------------------------------------------------------------
 189 // This function generates "random" RF hopping frequency channel numbers.
 190 // These numbers must be repeatable for a specific seed
 191 // The generated number range is from 0 to MAX_RF_CHANNEL. No repeat or adjacent numbers
 192 //
 193 // For Feilun variant, the channels are calculated from TXID, and since only 2 channels are used
 194 // we copy them to fill up to MAX_RF_CHANNEL
 195 //-------------------------------------------------------------------------------------------------
 196 static void __attribute__((unused)) kn_calculate_freqency_hopping_channels()
 197 {
 198         if(sub_protocol==WLTOYS)
 199         {
 200                 uint8_t idx = 0;
 201                 uint32_t rnd = MProtocol_id;
 202                 while (idx < KN_RF_CH_COUNT)
 203                 {
 204                         uint8_t i;
 205                         rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
 206
 207                         // Use least-significant byte. 73 is prime, so channels 76..77 are unused
 208                         uint8_t next_ch = ((rnd >> 8) % KN_MAX_RF_CHANNEL) + 2;
 209                         // Keep the distance 2 between the channels - either odd or even
 210                         if (((next_ch ^ MProtocol_id) & 0x01 )== 0)
 211                                 continue;
 212                         // Check that it's not duplicate and spread uniformly
 213                         for (i = 0; i < idx; i++)
 214                                 if(hopping_frequency[i] == next_ch)
 215                                         break;
 216                         if (i != idx)
 217                                 continue;
 218                         hopping_frequency[idx++] = next_ch;
 219                 }
 220         }
 221         else
 222         {//FEILUN
 223                 hopping_frequency[0] = rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]; // - 256; ???
 224                 hopping_frequency[1] = hopping_frequency[0] + 32;
 225                 hopping_frequency[2] = hopping_frequency[0];
 226                 hopping_frequency[3] = hopping_frequency[1];
 227         }
 228 }
 229
 230 //-------------------------------------------------------------------------------------------------
 231 // This function setup 24L01
 232 // V977 uses one way communication, receiving only. 24L01 RX is never enabled.
 233 // V977 needs payload length in the packet. We should configure 24L01 to enable Packet Control Field(PCF)
 234 //   Some RX reg settings are actually for enable PCF
 235 //-------------------------------------------------------------------------------------------------
 236 static void __attribute__((unused)) KN_RF_init()
 237 {
 238         kn_calculate_tx_addr();
 239         kn_calculate_freqency_hopping_channels();
 240
 241         NRF24L01_Initialize();
 242
 243         NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20);   // bytes of data payload for pipe 0
 244         NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 1); // Dynamic payload for data pipe 0
 245         // Enable: Dynamic Payload Length to enable PCF
 246         NRF24L01_WriteReg(NRF24L01_1D_FEATURE, _BV(NRF2401_1D_EN_DPL));
 247
 248         NRF24L01_SetTxRxMode(TX_EN);                                            // Clear data ready, data sent, retransmit and enable CRC 16bits, ready for TX
 249 }
 250
 251 //================================================================================================
 252 // Private Functions
 253 //================================================================================================
 254 void KN_init()
 255 {
 256         if(sub_protocol==WLTOYS)
 257         {
 258                 packet_period = KN_WL_SENDING_PACKET_PERIOD;
 259                 bind_counter  = KN_WL_BIND_COUNT;
 260                 packet_count  = KN_WL_PACKET_SEND_COUNT;
 261                 seed = KN_WL_PACKET_SEND_COUNT * KN_WL_SENDING_PACKET_PERIOD;
 262         }
 263         else
 264         {
 265                 packet_period = KN_FX_SENDING_PACKET_PERIOD;
 266                 bind_counter  = KN_FX_BIND_COUNT;
 267                 packet_count  = KN_FX_PACKET_SEND_COUNT;
 268                 seed = KN_FX_PACKET_SEND_COUNT * KN_FX_SENDING_PACKET_PERIOD;
 269         }
 270         KN_RF_init();
 271         phase = IS_BIND_IN_PROGRESS ? KN_PHASE_PRE_BIND : KN_PHASE_PRE_SEND;
 272 }
 273
 274 uint16_t KN_callback()
 275 {
 276         switch (phase)
 277         {
 278                 case KN_PHASE_PRE_BIND:
 279                         kn_bind_init();
 280                         phase=KN_PHASE_BINDING;
 281                         //Do once, no break needed
 282                 case KN_PHASE_BINDING:
 283                         if(bind_counter>0)
 284                         {
 285                                 bind_counter--;
 286                                 NRF24L01_WritePayload(packet, KN_PAYLOADSIZE);
 287                                 return KN_BINDING_PACKET_PERIOD;
 288                         }
 289                         BIND_DONE;
 290                         //Continue
 291                 case KN_PHASE_PRE_SEND:
 292                         packet_sent = 0;
 293                         hopping_frequency_no = 0;
 294                         NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, KN_TX_ADDRESS_SIZE);
 295                         phase = KN_PHASE_SENDING;
 296                         //Do once, no break needed
 297                 case KN_PHASE_SENDING:
 298                         if(packet_sent >= packet_count)
 299                         {
 300                                 #ifdef MULTI_SYNC
 301                                         telemetry_set_input_sync(seed);
 302                                 #endif
 303                                 packet_sent = 0;
 304                                 hopping_frequency_no++;
 305                                 if(hopping_frequency_no >= KN_RF_CH_COUNT) hopping_frequency_no = 0;
 306                                 kn_update_packet_control_data();
 307                                 NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
 308                         }
 309                         else
 310                         {
 311                                 // Update sending count and RF channel index.
 312                                 // The protocol sends 4 data packets every 2ms per frequency, plus a 2ms gap.
 313                                 // Each data packet need a packet number and RF channel index
 314                                 packet[13] = 0x00;
 315                                 if(sub_protocol==WLTOYS)
 316                                         packet[13] = (packet_sent << 5) | (hopping_frequency_no << 2);
 317                         }
 318                         NRF24L01_WritePayload(packet, KN_PAYLOADSIZE);
 319                         packet_sent++;
 320                         return packet_period;
 321         } //switch
 322
 323     //Bad things happened, reset
 324     packet_sent = 0;
 325     phase = KN_PHASE_PRE_SEND;
 326     return packet_period;
 327 }
 328
 329 #endif