src/main/rx/nrf24_h8_3d.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_H8_3D
  28
  29 #include "build/build_config.h"
  30
  31 #include "common/utils.h"
  32
  33 #include "drivers/rx_nrf24l01.h"
  34 #include "drivers/rx_xn297.h"
  35 #include "drivers/time.h"
  36
  37 #include "rx/rx.h"
  38 #include "rx/rx_spi.h"
  39 #include "rx/nrf24_h8_3d.h"
  40
  41
  42 /*
  43  * Deviation transmitter sends 345 bind packets, then starts sending data packets.
  44  * Packets are send at rate of at least one every 4 milliseconds, ie at least 250Hz.
  45  * This means binding phase lasts 1.4 seconds, the transmitter then enters the data phase.
  46  * Other transmitters may vary but should have similar characteristics.
  47  */
  48
  49
  50 /*
  51  * H8_3D Protocol
  52  * No auto acknowledgment
  53  * Payload size is 20, static
  54  * Data rate is 1Mbps
  55  * Bind Phase
  56  * uses address {0xab,0xac,0xad,0xae,0xaf}, converted by XN297 to {0x41, 0xbd, 0x42, 0xd4, 0xc2}
  57  * hops between 4 channels
  58  * Data Phase
  59  * uses same address as bind phase
  60  * hops between 4 channels generated from txId received in bind packets
  61  *
  62  */
  63 #define RC_CHANNEL_COUNT    14
  64
  65 #define FLAG_FLIP       0x01
  66 #define FLAG_RATE_MID   0x02
  67 #define FLAG_RATE_HIGH  0x04
  68 #define FLAG_HEADLESS   0x10 // RTH + headless on H8, headless on JJRC H20
  69 #define FLAG_RTH        0x20 // 360° flip mode on H8 3D, RTH on JJRC H20
  70 #define FLAG_PICTURE    0x40 // on payload[18]
  71 #define FLAG_VIDEO      0x80 // on payload[18]
  72 #define FLAG_CAMERA_UP  0x04 // on payload[18]
  73 #define FLAG_CAMERA_DOWN 0x08 // on payload[18]
  74
  75 typedef enum {
  76     STATE_BIND = 0,
  77     STATE_DATA
  78 } protocol_state_t;
  79
  80 STATIC_UNIT_TESTED protocol_state_t protocolState;
  81
  82 #define H8_3D_PROTOCOL_PAYLOAD_SIZE   20
  83 STATIC_UNIT_TESTED uint8_t payloadSize;
  84
  85 #define CRC_LEN 2
  86 #define RX_TX_ADDR_LEN     5
  87 STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address
  88 #define TX_ID_LEN 4
  89 STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN];
  90 static uint32_t *rxSpiIdPtr;
  91
  92 // radio channels for frequency hopping
  93 #define H8_3D_RF_CHANNEL_COUNT 4
  94 STATIC_UNIT_TESTED uint8_t h8_3dRfChannelCount = H8_3D_RF_CHANNEL_COUNT;
  95 STATIC_UNIT_TESTED uint8_t h8_3dRfChannelIndex;
  96 STATIC_UNIT_TESTED uint8_t h8_3dRfChannels[H8_3D_RF_CHANNEL_COUNT];
  97 // hop between these channels in the bind phase
  98 #define H8_3D_RF_BIND_CHANNEL_START 0x06
  99 #define H8_3D_RF_BIND_CHANNEL_END 0x26
 100
 101 #define DATA_HOP_TIMEOUT 5000 // 5ms
 102 #define BIND_HOP_TIMEOUT 1000 // 1ms, to find the bind channel as quickly as possible
 103 static timeUs_t hopTimeout = BIND_HOP_TIMEOUT;
 104 static timeUs_t timeOfLastHop;
 105
 106 STATIC_UNIT_TESTED bool h8_3dCheckBindPacket(const uint8_t *payload)
 107 {
 108     bool bindPacket = false;
 109     if ((payload[5] == 0x00) && (payload[6] == 0x00) && (payload[7] == 0x01)) {
 110         const uint32_t checkSumTxId = (payload[1] + payload[2] + payload[3] + payload[4]) & 0xff;
 111         if (checkSumTxId == payload[8]) {
 112             bindPacket = true;
 113             txId[0] = payload[1];
 114             txId[1] = payload[2];
 115             txId[2] = payload[3];
 116             txId[3] = payload[4];
 117             if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) {
 118                 // copy the txId so it can be saved
 119                 memcpy(rxSpiIdPtr, txId, sizeof(uint32_t));
 120             }
 121         }
 122     }
 123     return bindPacket;
 124 }
 125
 126 STATIC_UNIT_TESTED uint16_t h8_3dConvertToPwm(uint8_t val, int16_t _min, int16_t _max)
 127 {
 128 #define PWM_RANGE (PWM_RANGE_MAX - PWM_RANGE_MIN)
 129
 130     int32_t ret = val;
 131     const int32_t range = _max - _min;
 132     ret = PWM_RANGE_MIN + ((ret - _min) * PWM_RANGE)/range;
 133     return (uint16_t)ret;
 134 }
 135
 136 void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
 137 {
 138     rcData[RC_SPI_ROLL] = h8_3dConvertToPwm(payload[12], 0xbb, 0x43); // aileron
 139     rcData[RC_SPI_PITCH] = h8_3dConvertToPwm(payload[11], 0x43, 0xbb); // elevator
 140     rcData[RC_SPI_THROTTLE] = h8_3dConvertToPwm(payload[9], 0, 0xff); // throttle
 141     const int8_t yawByte = payload[10]; // rudder
 142     rcData[RC_SPI_YAW] = yawByte >= 0 ? h8_3dConvertToPwm(yawByte, -0x3c, 0x3c) : h8_3dConvertToPwm(yawByte, 0xbc, 0x44);
 143
 144     const uint8_t flags = payload[17];
 145     const uint8_t flags2 = payload[18];
 146     if (flags & FLAG_RATE_HIGH) {
 147         rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
 148     } else if (flags & FLAG_RATE_MID) {
 149         rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
 150     } else {
 151         rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
 152     }
 153
 154     rcData[RC_CHANNEL_FLIP] = flags & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 155     rcData[RC_CHANNEL_PICTURE] = flags2 & FLAG_PICTURE ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 156     rcData[RC_CHANNEL_VIDEO] = flags2 & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 157     rcData[RC_CHANNEL_HEADLESS] = flags & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 158     rcData[RC_CHANNEL_RTH] = flags & FLAG_RTH ? PWM_RANGE_MAX : PWM_RANGE_MIN;
 159
 160     if (flags2 & FLAG_CAMERA_UP) {
 161         rcData[RC_SPI_AUX7] = PWM_RANGE_MAX;
 162     } else if (flags2 & FLAG_CAMERA_DOWN) {
 163         rcData[RC_SPI_AUX7] = PWM_RANGE_MIN;
 164     } else {
 165         rcData[RC_SPI_AUX7] = PWM_RANGE_MIDDLE;
 166     }
 167     rcData[RC_SPI_AUX8] = h8_3dConvertToPwm(payload[14], 0x10, 0x30);
 168     rcData[RC_SPI_AUX9] = h8_3dConvertToPwm(payload[15], 0x30, 0x10);
 169     rcData[RC_SPI_AUX10] = h8_3dConvertToPwm(payload[16], 0x10, 0x30);
 170 }
 171
 172 static void h8_3dHopToNextChannel(void)
 173 {
 174     ++h8_3dRfChannelIndex;
 175     if (protocolState == STATE_BIND) {
 176         if (h8_3dRfChannelIndex > H8_3D_RF_BIND_CHANNEL_END) {
 177             h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
 178         }
 179         NRF24L01_SetChannel(h8_3dRfChannelIndex);
 180     } else {
 181         if (h8_3dRfChannelIndex >= h8_3dRfChannelCount) {
 182             h8_3dRfChannelIndex = 0;
 183         }
 184         NRF24L01_SetChannel(h8_3dRfChannels[h8_3dRfChannelIndex]);
 185     }
 186 }
 187
 188 // The hopping channels are determined by the txId
 189 static void h8_3dSetHoppingChannels(const uint8_t *txId)
 190 {
 191     for (int ii = 0; ii < H8_3D_RF_CHANNEL_COUNT; ++ii) {
 192         h8_3dRfChannels[ii] = 0x06 + (0x0f * ii) + ((txId[ii] >> 4) + (txId[ii] & 0x0f)) % 0x0f;
 193     }
 194 }
 195
 196 static void h8_3dSetBound(const uint8_t *txId)
 197 {
 198     protocolState = STATE_DATA;
 199     h8_3dSetHoppingChannels(txId);
 200     hopTimeout = DATA_HOP_TIMEOUT;
 201     timeOfLastHop = micros();
 202     h8_3dRfChannelIndex = 0;
 203     NRF24L01_SetChannel(h8_3dRfChannels[0]);
 204 }
 205
 206 static bool h8_3dCrcOK(uint16_t crc, const uint8_t *payload)
 207 {
 208     if (payload[payloadSize] != (crc >> 8)) {
 209         return false;
 210     }
 211     if (payload[payloadSize + 1] != (crc & 0xff)) {
 212         return false;
 213     }
 214     return true;
 215 }
 216
 217 /*
 218  * This is called periodically by the scheduler.
 219  * Returns NRF24L01_RECEIVED_DATA if a data packet was received.
 220  */
 221 rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload, uint16_t *linkQuality)
 222 {
 223     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 224     bool payloadReceived = false;
 225     if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + CRC_LEN)) {
 226         const uint16_t crc = XN297_UnscramblePayload(payload, payloadSize, rxTxAddrXN297);
 227         if (h8_3dCrcOK(crc, payload)) {
 228             payloadReceived = true;
 229         }
 230     }
 231     switch (protocolState) {
 232     case STATE_BIND:
 233         if (payloadReceived) {
 234             const bool bindPacket = h8_3dCheckBindPacket(payload);
 235             if (bindPacket) {
 236                 ret = RX_SPI_RECEIVED_BIND;
 237                 h8_3dSetBound(txId);
 238             }
 239         }
 240         break;
 241     case STATE_DATA:
 242         if (payloadReceived) {
 243             ret = RX_SPI_RECEIVED_DATA;
 244         }
 245         break;
 246     }
 247     const timeUs_t timeNowUs = micros();
 248     if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) {
 249         h8_3dHopToNextChannel();
 250         timeOfLastHop = timeNowUs;
 251     }
 252     return ret;
 253 }
 254
 255 static void h8_3dNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId)
 256 {
 257     UNUSED(protocol);
 258     protocolState = STATE_BIND;
 259
 260     NRF24L01_Initialize(0); // sets PWR_UP, no CRC - hardware CRC not used for XN297
 261     NRF24L01_SetupBasic();
 262
 263     NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 264     // RX_ADDR for pipes P1-P5 are left at default values
 265     NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrXN297, RX_TX_ADDR_LEN);
 266     rxSpiIdPtr = (uint32_t*)rxSpiId;
 267     if (rxSpiId == NULL || *rxSpiId == 0) {
 268         h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
 269         NRF24L01_SetChannel(H8_3D_RF_BIND_CHANNEL_START);
 270     } else {
 271         h8_3dSetBound((uint8_t*)rxSpiId);
 272     }
 273
 274     payloadSize = H8_3D_PROTOCOL_PAYLOAD_SIZE;
 275     NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize + CRC_LEN); // payload + 2 bytes CRC
 276
 277     NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
 278 }
 279
 280 void h8_3dNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 281 {
 282     rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
 283     h8_3dNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol, &rxConfig->rx_spi_id);
 284 }
 285 #endif