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