src/main/rx/nrf24_kn.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_RX_KN
  28
  29 #include "build/build_config.h"
  30
  31 #include "common/utils.h"
  32
  33 #include "drivers/io.h"
  34 #include "drivers/rx/rx_nrf24l01.h"
  35 #include "drivers/time.h"
  36
  37 #include "rx/rx.h"
  38 #include "rx/rx_spi.h"
  39 #include "rx/nrf24_kn.h"
  40
  41 /*
  42  * KN Protocol
  43  */
  44
  45 #define KN_PAYLOAD_SIZE 16
  46 #define KN_NFREQCHANNELS 4
  47 #define RX_TX_ADDR_LEN 5
  48 #define KN_RC_CHANNEL_COUNT 8
  49
  50 enum {
  51     // packet[12] flags
  52     KN_FLAG_DR = 0x01,
  53     KN_FLAG_TRHOLD  = 0x02,
  54     KN_FLAG_IDLEUP   = 0x04,
  55     KN_FLAG_TD   = 0x40
  56 };
  57
  58 enum {
  59     PHASE_NOT_BOUND = 0,
  60     PHASE_RECEIVED,
  61     PHASE_BOUND
  62 };
  63
  64 STATIC_UNIT_TESTED uint8_t rf_ch_num;
  65 STATIC_UNIT_TESTED uint8_t bind_phase;
  66 static uint32_t packet_timer;
  67 STATIC_UNIT_TESTED uint8_t txid[RX_TX_ADDR_LEN];
  68 STATIC_UNIT_TESTED uint8_t kn_freq_hopping[KN_NFREQCHANNELS];
  69 static uint32_t rx_timeout;
  70 extern uint16_t rxSpiRcData[];
  71
  72 static const unsigned char kn_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_ROLL,RC_SPI_PITCH,RC_SPI_YAW,
  73         RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4};
  74
  75 static void prepare_to_bind(void)
  76 {
  77     packet_timer = micros();
  78     for (int i = 0; i < KN_NFREQCHANNELS; ++i) {
  79         kn_freq_hopping[i] = 0;
  80     }
  81     rx_timeout = 1000L;
  82 }
  83
  84 static void switch_channel(void)
  85 {
  86     NRF24L01_WriteReg(NRF24L01_05_RF_CH, kn_freq_hopping[rf_ch_num]);
  87     if (++rf_ch_num >= KN_NFREQCHANNELS) rf_ch_num = 0;
  88 }
  89
  90 static void decode_bind_packet(uint8_t *packet)
  91 {
  92         if (packet[0]==0x4b && packet[1]==0x4e && packet[2]==0x44 && packet[3]==0x5a) {
  93                 txid[0] = packet[4];
  94                 txid[1] = packet[5];
  95                 txid[2] = packet[6];
  96                 txid[3] = packet[7];
  97                 txid[4] = 0x4b;
  98
  99                 kn_freq_hopping[0] = packet[8];
 100                 kn_freq_hopping[1] = packet[9];
 101                 kn_freq_hopping[2] = packet[10];
 102                 kn_freq_hopping[3] = packet[11];
 103
 104                 if (packet[15]==0x01) {
 105                         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 106                 } else {
 107                         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 108                 }
 109
 110                 NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, txid, RX_TX_ADDR_LEN);
 111                 NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, txid, RX_TX_ADDR_LEN);
 112
 113         bind_phase = PHASE_BOUND;
 114         rx_timeout = 1000L; // find the channel as fast as possible
 115     }
 116 }
 117
 118 // Returns whether the data was successfully decoded
 119 static rx_spi_received_e decode_packet(uint8_t *packet)
 120 {
 121     if (bind_phase == PHASE_NOT_BOUND) {
 122         decode_bind_packet(packet);
 123         return RX_SPI_RECEIVED_BIND;
 124     }
 125     // Decode packet
 126     // Restore regular interval
 127     rx_timeout = 13000L; // 13ms if data received
 128     bind_phase = PHASE_RECEIVED;
 129
 130     for (int i = 0; i < 4; ++i) {
 131         uint16_t a = packet[i*2];
 132         uint16_t b = packet[(i*2)+1];
 133         rxSpiRcData[kn_channelindex[i]] = ((uint16_t)(a<<8)+b) * 1000 / 1024 + 1000;
 134     }
 135     const uint8_t flags[] = {KN_FLAG_DR, KN_FLAG_TRHOLD, KN_FLAG_IDLEUP, KN_FLAG_TD};
 136     for (int i = 4; i < 8; ++i) {
 137         rxSpiRcData[kn_channelindex[i]] = (packet[12] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 138     }
 139     packet_timer = micros();
 140     return RX_SPI_RECEIVED_DATA;
 141 }
 142
 143 void knNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
 144 {
 145     UNUSED(rcData);
 146     UNUSED(packet);
 147 }
 148
 149 static rx_spi_received_e readrx(uint8_t *packet)
 150 {
 151     if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BIT(NRF24L01_07_STATUS_RX_DR))) {
 152         uint32_t t = micros() - packet_timer;
 153         if (t > rx_timeout) {
 154                         if (bind_phase == PHASE_RECEIVED) {
 155                                 switch_channel();
 156                         }
 157             packet_timer = micros();
 158                         rx_timeout = 10000L; // 10ms if data not received
 159         }
 160         return RX_SPI_RECEIVED_NONE;
 161     }
 162     packet_timer = micros();
 163     NRF24L01_WriteReg(NRF24L01_07_STATUS, BIT(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag
 164     NRF24L01_ReadPayload(packet, KN_PAYLOAD_SIZE);
 165     NRF24L01_FlushRx();
 166
 167     switch_channel();
 168     return decode_packet(packet);
 169 }
 170
 171 /*
 172  * This is called periodically by the scheduler.
 173  * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
 174  */
 175 rx_spi_received_e knNrf24DataReceived(uint8_t *packet)
 176 {
 177     return readrx(packet);
 178 }
 179
 180 static void knNrf24Setup(rx_spi_protocol_e protocol)
 181 {
 182     NRF24L01_Initialize(BIT(NRF24L01_00_CONFIG_EN_CRC) | BIT(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC
 183
 184     NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00);      // No Auto Acknowledgment
 185     NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BIT(NRF24L01_02_EN_RXADDR_ERX_P0));  // Enable data pipe 0
 186     NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES);   // 5-byte RX/TX address
 187     NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x00);
 188     NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 189     NRF24L01_WriteReg(NRF24L01_07_STATUS, BIT(NRF24L01_07_STATUS_RX_DR) | BIT(NRF24L01_07_STATUS_TX_DS) | BIT(NRF24L01_07_STATUS_MAX_RT));     // Clear data ready, data sent, and retransmit
 190     NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, KN_PAYLOAD_SIZE);  // bytes of data payload for pipe 0
 191     NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
 192
 193     const uint8_t rx_tx_addr[RX_TX_ADDR_LEN] = {0x4b, 0x4e, 0x44, 0x5a, 0x4b};
 194     NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, RX_TX_ADDR_LEN);
 195     NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, RX_TX_ADDR_LEN);
 196
 197     NRF24L01_FlushTx();
 198     NRF24L01_FlushRx();
 199
 200     rf_ch_num = 0;
 201     bind_phase = PHASE_NOT_BOUND;
 202     prepare_to_bind();
 203     NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x53);// switch to channel 83
 204     NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
 205 }
 206
 207 bool knNrf24Init(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 208 {
 209     UNUSED(extiConfig);
 210
 211     rxRuntimeState->channelCount = KN_RC_CHANNEL_COUNT;
 212     knNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
 213
 214     return true;
 215 }
 216 #endif