src/main/rx/nrf24_inav.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 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_RX_INAV
  28
  29 #include "build/build_config.h"
  30 #include "build/debug.h"
  31
  32 #include "common/utils.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_inav.h"
  43
  44 #include "telemetry/ltm.h"
  45
  46 // debug build flags
  47 //#define DEBUG_NRF24_INAV
  48 //#define NO_RF_CHANNEL_HOPPING
  49 //#define USE_BIND_ADDRESS_FOR_DATA_STATE
  50
  51
  52 #define USE_AUTO_ACKKNOWLEDGEMENT
  53
  54 /*
  55  * iNav Protocol
  56  * Data rate is 250Kbps - lower data rate for better reliability and range
  57  *
  58  * Uses auto acknowledgment and dynamic payload size
  59  *     ACK payload is used for handshaking in bind phase and telemetry in data phase
  60  *
  61  * Bind payload size is 16 bytes
  62  * Data payload size is 8, 16 or 18 bytes dependent on variant of protocol, (small payload is read more quickly (marginal benefit))
  63  *
  64  * Bind Phase
  65  * uses address {0x4b,0x5c,0x6d,0x7e,0x8f}
  66  * uses channel 0x4c (76)
  67  *
  68  * Data Phase
  69  * 1) Uses the address received in bind packet
  70  *
  71  * 2) Hops between RF channels generated from the address received in bind packet.
  72  *    The number of RF hopping channels is set during bind handshaking:
  73  *        the transmitter requests a number of bind channels in payload[7]
  74  *        the receiver sets ackPayload[7] with the number of hopping channels actually allocated - the transmitter must
  75  *        use this value.
  76  *    All receiver variants must support the 16 byte payload. Support for the 8 and 18 byte payload is optional.
  77  *
  78  * 3) Uses the payload size negotiated in the bind phase, payload size may be 8, 16 or 18 bytes
  79  * a) For 8 byte payload there are 6 channels: AETR with resolution of 1 (10-bits are used for the channel data), and AUX1
  80  *    and AUX2 with resolution of 4 (8-bits are used for the channel data)
  81  * b) For 16 byte payload there are 16 channels: eight 10-bit analog channels, two 8-bit analog channels, and six digital channels as follows:
  82  *    Channels 0 to 3, are the AETR channels, values 1000 to 2000 with resolution of 1 (10-bit channels)
  83  *    Channel AUX1 by deviation convention is used for rate, values 1000, 1500, 2000
  84  *    Channels AUX2 to AUX6 are binary channels, values 1000 or 2000,
  85  *        by deviation convention these channels are used for: flip, picture, video, headless, and return to home
  86  *    Channels AUX7 to AUX10 are analog channels, values 1000 to 2000 with resolution of 1 (10-bit channels)
  87  *    Channels AUX11 and AUX12 are analog channels, values 1000 to 2000 with resolution of 4 (8-bit channels)
  88  * c) For 18 byte payload there are 18 channels, the first 16 channelsar are as for 16 byte payload, and then there are two
  89  *    additional channels: AUX13 and AUX14 both with resolution of 4 (8-bit channels)
  90  */
  91
  92 #define RC_CHANNEL_COUNT 16 // standard variant of the protocol has 16 RC channels
  93 #define RC_CHANNEL_COUNT_MAX MAX_SUPPORTED_RC_CHANNEL_COUNT // up to 18 RC channels are supported
  94
  95 enum {
  96     RATE_LOW = 0,
  97     RATE_MID = 1,
  98     RATE_HIGH = 2
  99 };
 100
 101 enum {
 102     FLAG_FLIP     = 0x01,
 103     FLAG_PICTURE  = 0x02,
 104     FLAG_VIDEO    = 0x04,
 105     FLAG_RTH      = 0x08,
 106     FLAG_HEADLESS = 0x10
 107 };
 108
 109 typedef enum {
 110     STATE_BIND = 0,
 111     STATE_DATA
 112 } protocol_state_t;
 113
 114 STATIC_UNIT_TESTED protocol_state_t protocolState;
 115
 116 STATIC_UNIT_TESTED uint8_t ackPayload[NRF24L01_MAX_PAYLOAD_SIZE];
 117 #define BIND_PAYLOAD0 0xae // 10101110
 118 #define BIND_PAYLOAD1 0xc9 // 11001001
 119 #define BIND_ACK_PAYLOAD0 0x83 // 10000111
 120 #define BIND_ACK_PAYLOAD1 0xa5 // 10100101
 121
 122 #define INAV_PROTOCOL_PAYLOAD_SIZE_MIN 8
 123 #define INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT 16
 124 #define INAV_PROTOCOL_PAYLOAD_SIZE_MAX 18
 125 STATIC_UNIT_TESTED const uint8_t payloadSize = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT;
 126 uint8_t receivedPowerSnapshot;
 127
 128 #define RX_TX_ADDR_LEN 5
 129 // set rxTxAddr to the bind address
 130 STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0x4b,0x5c,0x6d,0x7e,0x8f};
 131 static uint32_t *rxSpiIdPtr;
 132 #define RX_TX_ADDR_4 0xD2 // rxTxAddr[4] always set to this value
 133
 134 // radio channels for frequency hopping
 135 #define INAV_RF_CHANNEL_COUNT_MAX 8
 136 #define INAV_RF_CHANNEL_HOPPING_COUNT_DEFAULT 4
 137 STATIC_UNIT_TESTED const uint8_t inavRfChannelHoppingCount = INAV_RF_CHANNEL_HOPPING_COUNT_DEFAULT;
 138 STATIC_UNIT_TESTED uint8_t inavRfChannelCount;
 139 STATIC_UNIT_TESTED uint8_t inavRfChannelIndex;
 140 STATIC_UNIT_TESTED uint8_t inavRfChannels[INAV_RF_CHANNEL_COUNT_MAX];
 141 #define INAV_RF_BIND_CHANNEL 0x4c
 142
 143 static uint32_t timeOfLastHop;
 144 static const uint32_t hopTimeout = 5000; // 5ms
 145
 146 STATIC_UNIT_TESTED bool inavCheckBindPacket(const uint8_t *payload)
 147 {
 148     bool bindPacket = false;
 149     if (payload[0] == BIND_PAYLOAD0  && payload[1] == BIND_PAYLOAD1) {
 150         bindPacket = true;
 151         if (protocolState ==STATE_BIND) {
 152             rxTxAddr[0] = payload[2];
 153             rxTxAddr[1] = payload[3];
 154             rxTxAddr[2] = payload[4];
 155             rxTxAddr[3] = payload[5];
 156             rxTxAddr[4] = payload[6];
 157             /*inavRfChannelHoppingCount = payload[7]; // !!TODO not yet implemented on transmitter
 158             if (inavRfChannelHoppingCount > INAV_RF_CHANNEL_COUNT_MAX) {
 159                 inavRfChannelHoppingCount = INAV_RF_CHANNEL_COUNT_MAX;
 160             }*/
 161             if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) {
 162                 // copy the rxTxAddr so it can be saved
 163                 memcpy(rxSpiIdPtr, rxTxAddr, sizeof(uint32_t));
 164             }
 165         }
 166     }
 167     return bindPacket;
 168 }
 169
 170 void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
 171 {
 172     memset(rcData, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT * sizeof(uint16_t));
 173     // payload[0] and payload[1] are zero in DATA state
 174     // the AETR channels have 10 bit resolution
 175     uint8_t lowBits = payload[6]; // least significant bits for AETR
 176     rcData[RC_SPI_ROLL]     = PWM_RANGE_MIN + ((payload[2] << 2) | (lowBits & 0x03)); // Aileron
 177     lowBits >>= 2;
 178     rcData[RC_SPI_PITCH]    = PWM_RANGE_MIN + ((payload[3] << 2) | (lowBits & 0x03)); // Elevator
 179     lowBits >>= 2;
 180     rcData[RC_SPI_THROTTLE] = PWM_RANGE_MIN + ((payload[4] << 2) | (lowBits & 0x03)); // Throttle
 181     lowBits >>= 2;
 182     rcData[RC_SPI_YAW]      = PWM_RANGE_MIN + ((payload[5] << 2) | (lowBits & 0x03)); // Rudder
 183
 184     if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MIN) {
 185         // small payload variant of protocol, supports 6 channels
 186         rcData[RC_SPI_AUX1] = PWM_RANGE_MIN + (payload[7] << 2);
 187         rcData[RC_SPI_AUX2] = PWM_RANGE_MIN + (payload[1] << 2);
 188     } else {
 189         // channel AUX1 is used for rate, as per the deviation convention
 190         const uint8_t rate = payload[7];
 191         // AUX1
 192         if (rate == RATE_HIGH) {
 193             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MAX;
 194         } else if (rate == RATE_MID) {
 195             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIDDLE;
 196         } else {
 197             rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN;
 198         }
 199
 200         // channels AUX2 to AUX7 use the deviation convention
 201         const uint8_t flags = payload[8];
 202         rcData[RC_CHANNEL_FLIP]= (flags & FLAG_FLIP) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX2
 203         rcData[RC_CHANNEL_PICTURE]= (flags & FLAG_PICTURE) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX3
 204         rcData[RC_CHANNEL_VIDEO]= (flags & FLAG_VIDEO) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX4
 205         rcData[RC_CHANNEL_HEADLESS]= (flags & FLAG_HEADLESS) ? PWM_RANGE_MAX : PWM_RANGE_MIN; //AUX5
 206         rcData[RC_CHANNEL_RTH]= (flags & FLAG_RTH) ? PWM_RANGE_MAX : PWM_RANGE_MIN; // AUX6
 207
 208         // channels AUX7 to AUX10 have 10 bit resolution
 209         lowBits = payload[13]; // least significant bits for AUX7 to AUX10
 210         rcData[RC_SPI_AUX7] = PWM_RANGE_MIN + ((payload[9] << 2) | (lowBits & 0x03));
 211         lowBits >>= 2;
 212         rcData[RC_SPI_AUX8] = PWM_RANGE_MIN + ((payload[10] << 2) | (lowBits & 0x03));
 213         lowBits >>= 2;
 214         rcData[RC_SPI_AUX9] = PWM_RANGE_MIN + ((payload[11] << 2) | (lowBits & 0x03));
 215         lowBits >>= 2;
 216         rcData[RC_SPI_AUX10] = PWM_RANGE_MIN + ((payload[12] << 2) | (lowBits & 0x03));
 217         lowBits >>= 2;
 218
 219         // channels AUX11 and AUX12 have 8 bit resolution
 220         rcData[RC_SPI_AUX11] = PWM_RANGE_MIN + (payload[14] << 2);
 221         rcData[RC_SPI_AUX12] = PWM_RANGE_MIN + (payload[15] << 2);
 222     }
 223     if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MAX) {
 224         // large payload variant of protocol
 225         // channels AUX13 to AUX16 have 8 bit resolution
 226         rcData[RC_SPI_AUX13] = PWM_RANGE_MIN + (payload[16] << 2);
 227         rcData[RC_SPI_AUX14] = PWM_RANGE_MIN + (payload[17] << 2);
 228     }
 229 }
 230
 231 static void inavHopToNextChannel(void)
 232 {
 233     ++inavRfChannelIndex;
 234     if (inavRfChannelIndex >= inavRfChannelCount) {
 235         inavRfChannelIndex = 0;
 236     }
 237     NRF24L01_SetChannel(inavRfChannels[inavRfChannelIndex]);
 238 #ifdef DEBUG_NRF24_INAV
 239     debug[0] = inavRfChannels[inavRfChannelIndex];
 240 #endif
 241 }
 242
 243 // The hopping channels are determined by the low bits of rxTxAddr
 244 STATIC_UNIT_TESTED void inavSetHoppingChannels(void)
 245 {
 246 #ifdef NO_RF_CHANNEL_HOPPING
 247      // just stay on bind channel, useful for debugging
 248     inavRfChannelCount = 1;
 249     inavRfChannels[0] = INAV_RF_BIND_CHANNEL;
 250 #else
 251     inavRfChannelCount = inavRfChannelHoppingCount;
 252     const uint8_t addr = rxTxAddr[0];
 253     uint8_t ch = 0x10 + (addr & 0x07);
 254     for (int ii = 0; ii < INAV_RF_CHANNEL_COUNT_MAX; ++ii) {
 255         inavRfChannels[ii] = ch;
 256         ch += 0x0c;
 257     }
 258 #endif
 259 }
 260
 261 static void inavSetBound(void)
 262 {
 263     protocolState = STATE_DATA;
 264     NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
 265     NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rxTxAddr, RX_TX_ADDR_LEN);
 266
 267     timeOfLastHop = micros();
 268     inavRfChannelIndex = 0;
 269     inavSetHoppingChannels();
 270     NRF24L01_SetChannel(inavRfChannels[0]);
 271 #ifdef DEBUG_NRF24_INAV
 272     debug[0] = inavRfChannels[inavRfChannelIndex];
 273 #endif
 274 }
 275
 276 static void writeAckPayload(const uint8_t *data, uint8_t length)
 277 {
 278     NRF24L01_WriteReg(NRF24L01_07_STATUS, BIT(NRF24L01_07_STATUS_MAX_RT));
 279     NRF24L01_WriteAckPayload(data, length, NRF24L01_PIPE0);
 280 }
 281
 282 static void writeTelemetryAckPayload(void)
 283 {
 284 #ifdef USE_TELEMETRY_NRF24_LTM
 285     // set up telemetry data, send back telemetry data in the ACK packet
 286     static uint8_t sequenceNumber = 0;
 287     static ltm_frame_e ltmFrameType = LTM_FRAME_START;
 288
 289     ackPayload[0] = sequenceNumber++;
 290     const int ackPayloadSize = getLtmFrame(&ackPayload[1], ltmFrameType) + 1;
 291
 292     ++ltmFrameType;
 293     if (ltmFrameType > LTM_FRAME_COUNT) {
 294         ltmFrameType = LTM_FRAME_START;
 295     }
 296     writeAckPayload(ackPayload, ackPayloadSize);
 297 #ifdef DEBUG_NRF24_INAV
 298     debug[1] = ackPayload[0]; // sequenceNumber
 299     debug[2] = ackPayload[1]; // frame type, 'A', 'S' etc
 300     debug[3] = ackPayload[2]; // pitch for AFrame
 301 #endif
 302 #endif
 303 }
 304
 305 static void writeBindAckPayload(uint8_t *payload)
 306 {
 307 #ifdef USE_AUTO_ACKKNOWLEDGEMENT
 308     // send back the payload with the first two bytes set to zero as the ack
 309     payload[0] = BIND_ACK_PAYLOAD0;
 310     payload[1] = BIND_ACK_PAYLOAD1;
 311     // respond to request for rfChannelCount;
 312     payload[7] = inavRfChannelHoppingCount;
 313     // respond to request for payloadSize
 314     switch (payloadSize) {
 315     case INAV_PROTOCOL_PAYLOAD_SIZE_MIN:
 316     case INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT:
 317     case INAV_PROTOCOL_PAYLOAD_SIZE_MAX:
 318         payload[8] = payloadSize;
 319         break;
 320     default:
 321         payload[8] = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT;
 322         break;
 323     }
 324     writeAckPayload(payload, payloadSize);
 325 #else
 326     UNUSED(payload);
 327 #endif
 328 }
 329
 330 /*
 331  * This is called periodically by the scheduler.
 332  * Returns RX_SPI_RECEIVED_DATA if a data packet was received.
 333  */
 334 rx_spi_received_e inavNrf24DataReceived(uint8_t *payload)
 335 {
 336     rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
 337     uint32_t timeNowUs;
 338     switch (protocolState) {
 339     case STATE_BIND:
 340         if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
 341             const bool bindPacket = inavCheckBindPacket(payload);
 342             if (bindPacket) {
 343                 ret = RX_SPI_RECEIVED_BIND;
 344                 writeBindAckPayload(payload);
 345                 // got a bind packet, so set the hopping channels and the rxTxAddr and start listening for data
 346                 inavSetBound();
 347             }
 348         }
 349         break;
 350     case STATE_DATA:
 351         timeNowUs = micros();
 352         // read the payload, processing of payload is deferred
 353         if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
 354             receivedPowerSnapshot = NRF24L01_ReadReg(NRF24L01_09_RPD); // set to 1 if received power > -64dBm
 355             const bool bindPacket = inavCheckBindPacket(payload);
 356             if (bindPacket) {
 357                 // transmitter may still continue to transmit bind packets after we have switched to data mode
 358                 ret = RX_SPI_RECEIVED_BIND;
 359                 writeBindAckPayload(payload);
 360             } else {
 361                 ret = RX_SPI_RECEIVED_DATA;
 362                 writeTelemetryAckPayload();
 363             }
 364         }
 365         if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) {
 366             inavHopToNextChannel();
 367             timeOfLastHop = timeNowUs;
 368         }
 369         break;
 370     }
 371     return ret;
 372 }
 373
 374 static void inavNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId, int rfChannelHoppingCount)
 375 {
 376     UNUSED(protocol);
 377     UNUSED(rfChannelHoppingCount);
 378
 379     // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC, only get IRQ pin interrupt on RX_DR
 380     NRF24L01_Initialize(BIT(NRF24L01_00_CONFIG_EN_CRC) | BIT(NRF24L01_00_CONFIG_CRCO) | BIT(NRF24L01_00_CONFIG_MASK_MAX_RT) | BIT(NRF24L01_00_CONFIG_MASK_TX_DS));
 381
 382 #ifdef USE_AUTO_ACKKNOWLEDGEMENT
 383     NRF24L01_WriteReg(NRF24L01_01_EN_AA, BIT(NRF24L01_01_EN_AA_ENAA_P0)); // auto acknowledgment on P0
 384     NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BIT(NRF24L01_02_EN_RXADDR_ERX_P0));
 385     NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
 386     NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0);
 387     NRF24L01_Activate(0x73); // activate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers
 388     NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BIT(NRF24L01_1D_FEATURE_EN_ACK_PAY) | BIT(NRF24L01_1D_FEATURE_EN_DPL));
 389     NRF24L01_WriteReg(NRF24L01_1C_DYNPD, BIT(NRF24L01_1C_DYNPD_DPL_P0)); // enable dynamic payload length on P0
 390     //NRF24L01_Activate(0x73); // deactivate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers
 391
 392     NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rxTxAddr, RX_TX_ADDR_LEN);
 393 #else
 394     NRF24L01_SetupBasic();
 395 #endif
 396
 397     NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
 398     // RX_ADDR for pipes P1-P5 are left at default values
 399     NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
 400     NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize);
 401
 402 #ifdef USE_BIND_ADDRESS_FOR_DATA_STATE
 403     inavSetBound();
 404     UNUSED(rxSpiId);
 405 #else
 406     rxSpiId = NULL; // !!TODO remove this once  configurator supports setting rx_id
 407     if (rxSpiId == NULL || *rxSpiId == 0) {
 408         rxSpiIdPtr = NULL;
 409         protocolState = STATE_BIND;
 410         inavRfChannelCount = 1;
 411         inavRfChannelIndex = 0;
 412         NRF24L01_SetChannel(INAV_RF_BIND_CHANNEL);
 413     } else {
 414         rxSpiIdPtr = (uint32_t*)rxSpiId;
 415         // use the rxTxAddr provided and go straight into DATA_STATE
 416         memcpy(rxTxAddr, rxSpiId, sizeof(uint32_t));
 417         rxTxAddr[4] = RX_TX_ADDR_4;
 418         inavSetBound();
 419     }
 420 #endif
 421
 422     NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
 423     // put a null packet in the transmit buffer to be sent as ACK on first receive
 424     writeAckPayload(ackPayload, payloadSize);
 425 }
 426
 427 bool inavNrf24Init(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeState, rxSpiExtiConfig_t *extiConfig)
 428 {
 429     UNUSED(extiConfig);
 430
 431     rxRuntimeState->channelCount = RC_CHANNEL_COUNT_MAX;
 432     inavNrf24Setup((rx_spi_protocol_e)rxSpiConfig->rx_spi_protocol, &rxSpiConfig->rx_spi_id, rxSpiConfig->rx_spi_rf_channel_count);
 433
 434     return true;
 435 }
 436 #endif