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