src/main/rx/nrf24_kn.c

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