src/main/rx/nrf24_v202.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 // this file is copied with modifications from bradwii for jd385
  22 // see https://github.com/hackocopter/bradwii-jd385
  23
  24 #include <stdbool.h>
  25 #include <stdint.h>
  26 #include <stdlib.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_RX_V202
  31
  32 #include "build/build_config.h"
  33
  34 #include "common/utils.h"
  35
  36 #include "pg/rx.h"
  37
  38 #include "drivers/io.h"
  39 #include "drivers/rx/rx_nrf24l01.h"
  40 #include "drivers/time.h"
  41
  42 #include "rx/rx.h"
  43 #include "rx/rx_spi.h"
  44 #include "rx/nrf24_v202.h"
  45
  46 /*
  47  * V202 Protocol
  48  * No auto acknowledgment
  49  * Payload size is 16 and static
  50  * Data rate is 1Mbps, there is a 256Kbps data rate used by the Deviation transmitter implementation
  51  * Bind Phase
  52  * uses address {0x66, 0x88, 0x68, 0x68, 0x68}
  53  * uses channels from the frequency hopping table
  54  * Data phase
  55  * uses same address as bind phase
  56  * hops between 16 channels that are set using the txId sent in the bind packet and the frequency hopping table
  57  */
  58
  59 #define V2X2_PAYLOAD_SIZE 16
  60 #define V2X2_NFREQCHANNELS 16
  61 #define TXIDSIZE 3
  62 #define V2X2_RC_CHANNEL_COUNT 11
  63
  64 enum {
  65     // packet[14] flags
  66     V2X2_FLAG_CAMERA = 0x01, // also automatic Missile Launcher and Hoist in one direction
  67     V2X2_FLAG_VIDEO  = 0x02, // also Sprayer, Bubbler, Missile Launcher(1), and Hoist in the other dir.
  68     V2X2_FLAG_FLIP   = 0x04,
  69     V2X2_FLAG_UNK9   = 0x08,
  70     V2X2_FLAG_LED    = 0x10,
  71     V2X2_FLAG_UNK10  = 0x20,
  72     V2X2_FLAG_BIND   = 0xC0,
  73     // packet[10] flags
  74     V2X2_FLAG_HEADLESS  = 0x02,
  75     V2X2_FLAG_MAG_CAL_X = 0x08,
  76     V2X2_FLAG_MAG_CAL_Y = 0x20
  77 };
  78
  79 enum {
  80     PHASE_NOT_BOUND = 0,
  81     PHASE_BOUND
  82 };
  83
  84 // This is frequency hopping table for V202 protocol
  85 // The table is the first 4 rows of 32 frequency hopping
  86 // patterns, all other rows are derived from the first 4.
  87 // For some reason the protocol avoids channels, dividing
  88 // by 16 and replaces them by subtracting 3 from the channel
  89 // number in this case.
  90 // The pattern is defined by 5 least significant bits of
  91 // sum of 3 bytes comprising TX id
  92 static const uint8_t v2x2_freq_hopping[][V2X2_NFREQCHANNELS] = {
  93  { 0x27, 0x1B, 0x39, 0x28, 0x24, 0x22, 0x2E, 0x36,
  94    0x19, 0x21, 0x29, 0x14, 0x1E, 0x12, 0x2D, 0x18 }, //  00
  95  { 0x2E, 0x33, 0x25, 0x38, 0x19, 0x12, 0x18, 0x16,
  96    0x2A, 0x1C, 0x1F, 0x37, 0x2F, 0x23, 0x34, 0x10 }, //  01
  97  { 0x11, 0x1A, 0x35, 0x24, 0x28, 0x18, 0x25, 0x2A,
  98    0x32, 0x2C, 0x14, 0x27, 0x36, 0x34, 0x1C, 0x17 }, //  02
  99  { 0x22, 0x27, 0x17, 0x39, 0x34, 0x28, 0x2B, 0x1D,
 100    0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F }  //  03
 101 };
 102
 103 STATIC_UNIT_TESTED uint8_t rf_channels[V2X2_NFREQCHANNELS];
 104 STATIC_UNIT_TESTED uint8_t rf_ch_num;
 105 STATIC_UNIT_TESTED uint8_t bind_phase;
 106 static uint32_t packet_timer;
 107 STATIC_UNIT_TESTED uint8_t txid[TXIDSIZE];
 108 static uint32_t rx_timeout;
 109 extern uint16_t rxSpiRcData[];
 110
 111 static const unsigned char v2x2_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_YAW,RC_SPI_PITCH,RC_SPI_ROLL,
 112         RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4,RC_SPI_AUX5,RC_SPI_AUX6,RC_SPI_AUX7};
 113
 114 static void prepare_to_bind(void)
 115 {
 116     packet_timer = micros();
 117     for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
 118         rf_channels[i] = v2x2_freq_hopping[0][i];
 119     }
 120     rx_timeout = 1000L;
 121 }
 122
 123 static void switch_channel(void)
 124 {
 125     NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channels[rf_ch_num]);
 126     if (++rf_ch_num >= V2X2_NFREQCHANNELS) rf_ch_num = 0;
 127 }
 128
 129 static void v2x2_set_tx_id(uint8_t *id)
 130 {
 131     uint8_t sum;
 132     txid[0] = id[0];
 133     txid[1] = id[1];
 134     txid[2] = id[2];
 135     sum = id[0] + id[1] + id[2];
 136
 137     // Base row is defined by lowest 2 bits
 138     const uint8_t *fh_row = v2x2_freq_hopping[sum & 0x03];
 139     // Higher 3 bits define increment to corresponding row
 140     uint8_t increment = (sum & 0x1e) >> 2;
 141     for (int i = 0; i < V2X2_NFREQCHANNELS; ++i) {
 142         uint8_t val = fh_row[i] + increment;
 143         // Strange avoidance of channels divisible by 16
 144         rf_channels[i] = (val & 0x0f) ? val : val - 3;
 145     }
 146 }
 147
 148 static void decode_bind_packet(uint8_t *packet)
 149 {
 150     if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
 151         // Fill out rf_channels with bound protocol parameters
 152         v2x2_set_tx_id(&packet[7]);
 153         bind_phase = PHASE_BOUND;
 154         rx_timeout = 1000L; // find the channel as fast as possible
 155     }
 156 }
 157
 158 // Returns whether the data was successfully decoded
 159 static rx_spi_received_e decode_packet(uint8_t *packet)
 160 {
 161     if (bind_phase != PHASE_BOUND) {
 162         decode_bind_packet(packet);
 163         return RX_SPI_RECEIVED_BIND;
 164     }
 165     // Decode packet
 166     if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
 167         return RX_SPI_RECEIVED_BIND;
 168     }
 169     if (packet[7] != txid[0] ||
 170         packet[8] != txid[1] ||
 171         packet[9] != txid[2]) {
 172         return RX_SPI_RECEIVED_NONE;
 173     }
 174     // Restore regular interval
 175     rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
 176     // TREA order in packet to MultiWii order is handled by
 177     // correct assignment to channelindex
 178     // Throttle 0..255 to 1000..2000
 179     rxSpiRcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
 180     for (int i = 1; i < 4; ++i) {
 181         uint8_t a = packet[i];
 182         rxSpiRcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
 183     }
 184     const uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
 185     for (int i = 4; i < 8; ++i) {
 186         rxSpiRcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 187     }
 188     const uint8_t flags10[] = {V2X2_FLAG_HEADLESS, V2X2_FLAG_MAG_CAL_X, V2X2_FLAG_MAG_CAL_Y};
 189     for (int i = 8; i < 11; ++i) {
 190         rxSpiRcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 191     }
 192     packet_timer = micros();
 193     return RX_SPI_RECEIVED_DATA;
 194 }
 195
 196 void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
 197 {
 198     UNUSED(rcData);
 199     UNUSED(packet);
 200     // Ideally the decoding of the packet should be moved into here, to reduce the overhead of v202DataReceived function.
 201 }
 202
 203 static rx_spi_received_e readrx(uint8_t *packet)
 204 {
 205     if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BIT(NRF24L01_07_STATUS_RX_DR))) {
 206         uint32_t t = micros() - packet_timer;
 207         if (t > rx_timeout) {
 208             switch_channel();
 209             packet_timer = micros();
 210         }
 211         return RX_SPI_RECEIVED_NONE;
 212     }
 213     packet_timer = micros();
 214     NRF24L01_WriteReg(NRF24L01_07_STATUS, BIT(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag
 215     NRF24L01_ReadPayload(packet, V2X2_PAYLOAD_SIZE);
 216     NRF24L01_FlushRx();
 217
 218     switch_channel();
 219     return decode_packet(packet);
 220 }
 221
 222 /*
 223  * This is called periodically by the scheduler.
 224  * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
 225  */
 226 rx_spi_received_e v202Nrf24DataReceived(uint8_t *packet)
 227 {
 228     return readrx(packet);
 229 }
 230
 231 static void v202Nrf24Setup(rx_spi_protocol_e protocol)
 232 {
 233     NRF24L01_Initialize(BIT(NRF24L01_00_CONFIG_EN_CRC) | BIT(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC
 234
 235     NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00);      // No Auto Acknowledgment
 236     NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BIT(NRF24L01_02_EN_RXADDR_ERX_P0));  // Enable data pipe 0
 237     NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES);   // 5-byte RX/TX address
 238     NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
 239     if (protocol == RX_SPI_NRF24_V202_250K) {
 240         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 241     } else {
 242         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 243     }
 244     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
 245     NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, V2X2_PAYLOAD_SIZE);  // bytes of data payload for pipe 0
 246     NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
 247 #define RX_TX_ADDR_LEN 5
 248     const uint8_t rx_tx_addr[RX_TX_ADDR_LEN] = {0x66, 0x88, 0x68, 0x68, 0x68};
 249     NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, RX_TX_ADDR_LEN);
 250     NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, RX_TX_ADDR_LEN);
 251
 252     NRF24L01_FlushTx();
 253     NRF24L01_FlushRx();
 254
 255     rf_ch_num = 0;
 256     bind_phase = PHASE_NOT_BOUND;
 257     prepare_to_bind();
 258     switch_channel();
 259     NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
 260 }
 261
 262 bool v202Nrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 263 {
 264     UNUSED(extiConfig);
 265
 266     rxRuntimeState->channelCount = V2X2_RC_CHANNEL_COUNT;
 267     v202Nrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
 268
 269     return true;
 270 }
 271 #endif