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