src/main/rx/nrf24_syma.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 borrows heavily from project Deviation,
  22 // see http://deviationtx.com
  23
  24 #include <stdbool.h>
  25 #include <stdint.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_RX_SYMA
  31
  32 #include "build/build_config.h"
  33
  34 #include "pg/rx.h"
  35
  36 #include "drivers/io.h"
  37 #include "drivers/rx/rx_nrf24l01.h"
  38 #include "drivers/time.h"
  39
  40 #include "rx/rx.h"
  41 #include "rx/rx_spi.h"
  42 #include "rx/nrf24_syma.h"
  43
  44 /*
  45  * Deviation transmitter sends 345 bind packets, then starts sending data packets.
  46  * Packets are send at rate of at least one every 4 milliseconds, ie at least 250Hz.
  47  * This means binding phase lasts 1.4 seconds, the transmitter then enters the data phase.
  48  * Other transmitters may vary but should have similar characteristics.
  49  */
  50
  51 /*
  52  * SymaX Protocol
  53  * No auto acknowledgment
  54  * Data rate is 250Kbps
  55  * Payload size is 10, static
  56  * Bind Phase
  57  * uses address {0xab,0xac,0xad,0xae,0xaf}
  58  * hops between 4 channels {0x4b, 0x30, 0x40, 0x20}
  59  * Data Phase
  60  * uses address received in bind packets
  61  * hops between 4 channels generated from address received in bind packets
  62  *
  63  * SymaX5C Protocol
  64  * No auto acknowledgment
  65  * Payload size is 16, static
  66  * Data rate is 1Mbps
  67  * Bind Phase
  68  * uses address {0x6d,0x6a,0x73,0x73,0x73}
  69  * hops between 16 channels {0x27, 0x1b, 0x39, 0x28, 0x24, 0x22, 0x2e, 0x36, 0x19, 0x21, 0x29, 0x14, 0x1e, 0x12, 0x2d, 0x18};
  70  * Data phase
  71  * uses same address as bind phase
  72  * hops between 15 channels {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
  73  * (common channels between both phases are: 0x27, 0x39, 0x24, 0x22, 0x2d)
  74  */
  75
  76 #define RC_CHANNEL_COUNT 9
  77
  78 enum {
  79     RATE_LOW = 0,
  80     RATE_MID = 1,
  81     RATE_HIGH= 2
  82 };
  83
  84 #define FLAG_PICTURE    0x40
  85 #define FLAG_VIDEO      0x80
  86 #define FLAG_FLIP       0x40
  87 #define FLAG_HEADLESS   0x80
  88
  89 #define FLAG_FLIP_X5C   0x01
  90 #define FLAG_PICTURE_X5C 0x08
  91 #define FLAG_VIDEO_X5C  0x10
  92 #define FLAG_RATE_X5C   0x04
  93
  94 STATIC_UNIT_TESTED rx_spi_protocol_e symaProtocol;
  95
  96 typedef enum {
  97     STATE_BIND = 0,
  98     STATE_DATA
  99 } protocol_state_t;
 100
 101 STATIC_UNIT_TESTED protocol_state_t protocolState;
 102
 103 // X11, X12, X5C-1 have 10-byte payload, X5C has 16-byte payload
 104 #define SYMA_X_PROTOCOL_PAYLOAD_SIZE      10
 105 #define SYMA_X5C_PROTOCOL_PAYLOAD_SIZE    16
 106 STATIC_UNIT_TESTED uint8_t payloadSize;
 107
 108 #define RX_TX_ADDR_LEN     5
 109 // set rxTxAddr to SymaX bind values
 110 STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0xab, 0xac, 0xad, 0xae, 0xaf};
 111 STATIC_UNIT_TESTED const uint8_t rxTxAddrX5C[RX_TX_ADDR_LEN] = {0x6d, 0x6a, 0x73, 0x73, 0x73};   // X5C uses same address for bind and data
 112
 113 // radio channels for frequency hopping
 114 #define SYMA_X_RF_BIND_CHANNEL             8
 115 #define SYMA_X_RF_CHANNEL_COUNT            4
 116 #define SYMA_X5C_RF_BIND_CHANNEL_COUNT    16
 117 #define SYMA_X5C_RF_CHANNEL_COUNT         15
 118
 119 STATIC_UNIT_TESTED uint8_t symaRfChannelCount = SYMA_X_RF_CHANNEL_COUNT;
 120 STATIC_UNIT_TESTED uint8_t symaRfChannelIndex = 0;
 121 // set rfChannels to SymaX bind channels, reserve enough space for SymaX5C channels
 122 STATIC_UNIT_TESTED uint8_t symaRfChannels[SYMA_X5C_RF_BIND_CHANNEL_COUNT]  = {0x4b, 0x30, 0x40, 0x20};
 123 STATIC_UNIT_TESTED const uint8_t symaRfChannelsX5C[SYMA_X5C_RF_CHANNEL_COUNT] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
 124
 125 static uint32_t packetCount = 0;
 126 static uint32_t timeOfLastHop;
 127 static uint32_t hopTimeout = 10000; // 10ms
 128
 129 STATIC_UNIT_TESTED bool symaCheckBindPacket(const uint8_t *packet)
 130 {
 131     bool bindPacket = false;
 132     if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
 133         if ((packet[5] == 0xaa) && (packet[6] == 0xaa) && (packet[7] == 0xaa)) {
 134             bindPacket = true;
 135             rxTxAddr[4] = packet[0];
 136             rxTxAddr[3] = packet[1];
 137             rxTxAddr[2] = packet[2];
 138             rxTxAddr[1] = packet[3];
 139             rxTxAddr[0] = packet[4];
 140         }
 141     } else {
 142         if ((packet[0] == 0) && (packet[1] == 0) && (packet[14] == 0xc0) && (packet[15] == 0x17)) {
 143             bindPacket = true;
 144         }
 145     }
 146     return bindPacket;
 147 }
 148
 149 STATIC_UNIT_TESTED uint16_t symaConvertToPwmUnsigned(uint8_t val)
 150 {
 151     uint32_t ret = val;
 152     ret = ret * PWM_RANGE / UINT8_MAX + PWM_RANGE_MIN;
 153     return (uint16_t)ret;
 154 }
 155
 156 STATIC_UNIT_TESTED uint16_t symaConvertToPwmSigned(uint8_t val)
 157 {
 158     int32_t ret = val & 0x7f;
 159     ret = ret * PWM_RANGE / (2 * INT8_MAX);
 160     if (val & 0x80) {// sign bit set
 161         ret = -ret;
 162     }
 163     return (uint16_t)(PWM_RANGE_MIDDLE + ret);
 164 }
 165
 166 void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
 167 {
 168     rcData[RC_SPI_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle
 169     rcData[RC_SPI_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron
 170     if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
 171         rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator
 172         rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[2]); // rudder
 173         const uint8_t rate = (packet[5] & 0xc0) >> 6;
 174         if (rate == RATE_LOW) {
 175             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
 176         } else if (rate == RATE_MID) {
 177             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
 178         } else {
 179             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
 180         }
 181         rcData[RC_CHANNEL_FLIP] = packet[6] & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 182         rcData[RC_CHANNEL_PICTURE] = packet[4] & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 183         rcData[RC_CHANNEL_VIDEO] = packet[4] & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 184         rcData[RC_CHANNEL_HEADLESS] = packet[14] & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 185     } else {
 186         rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[2]); // elevator
 187         rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[1]); // rudder
 188         const uint8_t flags = packet[14];
 189         rcData[RC_CHANNEL_RATE] = flags & FLAG_RATE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 190         rcData[RC_CHANNEL_FLIP] = flags & FLAG_FLIP_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 191         rcData[RC_CHANNEL_PICTURE] = flags & FLAG_PICTURE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 192         rcData[RC_CHANNEL_VIDEO] = flags & FLAG_VIDEO_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 193     }
 194 }
 195
 196 static void symaHopToNextChannel(void)
 197 {
 198     // hop channel every second packet
 199     ++packetCount;
 200     if ((packetCount & 0x01) == 0) {
 201         ++symaRfChannelIndex;
 202         if (symaRfChannelIndex >= symaRfChannelCount) {
 203             symaRfChannelIndex = 0;
 204         }
 205     }
 206     NRF24L01_SetChannel(symaRfChannels[symaRfChannelIndex]);
 207 }
 208
 209 // The SymaX hopping channels are determined by the low bits of rxTxAddress
 210 static void setSymaXHoppingChannels(uint32_t addr)
 211 {
 212     addr = addr & 0x1f;
 213     if (addr == 0x06) {
 214         addr = 0x07;
 215     }
 216     const uint32_t inc = (addr << 24) | (addr << 16) | (addr << 8) | addr;
 217     uint32_t * const prfChannels = (uint32_t *)symaRfChannels;
 218     if (addr == 0x16) {
 219         *prfChannels = 0x28481131;
 220     } else if (addr == 0x1e) {
 221         *prfChannels = 0x38184121;
 222     } else if (addr < 0x10) {
 223         *prfChannels = 0x3A2A1A0A + inc;
 224     } else if (addr < 0x18) {
 225         *prfChannels = 0x1231FA1A + inc;
 226     } else {
 227         *prfChannels = 0x19FA2202 + inc;
 228     }
 229 }
 230
 231 /*
 232  * This is called periodically by the scheduler.
 233  * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
 234  */
 235 rx_spi_received_e symaNrf24DataReceived(uint8_t *payload)
 236 {
 237     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 238
 239     switch (protocolState) {
 240     case STATE_BIND:
 241         if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
 242             const bool bindPacket = symaCheckBindPacket(payload);
 243             if (bindPacket) {
 244                 ret = RX_SPI_RECEIVED_BIND;
 245                 protocolState = STATE_DATA;
 246                 // using protocol NRF24L01_SYMA_X, since NRF24L01_SYMA_X5C went straight into data mode
 247                 // set the hopping channels as determined by the rxTxAddr received in the bind packet
 248                 setSymaXHoppingChannels(rxTxAddr[0]);
 249                 // set the NRF24 to use the rxTxAddr received in the bind packet
 250                 NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
 251                 packetCount = 0;
 252                 symaRfChannelIndex = 0;
 253                 NRF24L01_SetChannel(symaRfChannels[0]);
 254             }
 255         }
 256         break;
 257     case STATE_DATA:
 258         // read the payload, processing of payload is deferred
 259         if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
 260             symaHopToNextChannel();
 261             timeOfLastHop = micros();
 262             ret = RX_SPI_RECEIVED_DATA;
 263         }
 264         if (micros() > timeOfLastHop + hopTimeout) {
 265             symaHopToNextChannel();
 266             timeOfLastHop = micros();
 267         }
 268         break;
 269     }
 270     return ret;
 271 }
 272
 273 static void symaNrf24Setup(rx_spi_protocol_e protocol)
 274 {
 275     symaProtocol = protocol;
 276     NRF24L01_Initialize(BIT(NRF24L01_00_CONFIG_EN_CRC) | BIT( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC
 277     NRF24L01_SetupBasic();
 278
 279     if (symaProtocol == RX_SPI_NRF24_SYMA_X) {
 280         payloadSize = SYMA_X_PROTOCOL_PAYLOAD_SIZE;
 281         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 282         protocolState = STATE_BIND;
 283         // RX_ADDR for pipes P1-P5 are left at default values
 284         NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
 285     } else {
 286         payloadSize = SYMA_X5C_PROTOCOL_PAYLOAD_SIZE;
 287         NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 288         // RX_ADDR for pipes P1-P5 are left at default values
 289         NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrX5C, RX_TX_ADDR_LEN);
 290         // just go straight into data mode, since the SYMA_X5C protocol does not actually require binding
 291         protocolState = STATE_DATA;
 292         symaRfChannelCount = SYMA_X5C_RF_CHANNEL_COUNT;
 293         memcpy(symaRfChannels, symaRfChannelsX5C, SYMA_X5C_RF_CHANNEL_COUNT);
 294     }
 295     NRF24L01_SetChannel(symaRfChannels[0]);
 296     NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize);
 297
 298     NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
 299 }
 300
 301 bool symaNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 302 {
 303     UNUSED(extiConfig);
 304
 305     rxRuntimeState->channelCount = RC_CHANNEL_COUNT;
 306     symaNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol);
 307
 308     return true;
 309 }
 310 #endif