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