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