KN protocol over NRF24 (#4994)
* Added KN protocol support * Added KN protocol support * Added KN protocol support * Added KN protocol support * Update nrf24_kn.c * Update rx_spi.h * Update nrf24_kn.c
This commit is contained in:
fgiudice98
Michael Keller
parent
d284ae31ad
commit
f992b839c1
|
|
@ -0,0 +1,211 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#ifdef USE_RX_KN
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rx/rx_nrf24l01.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/rx_spi.h"
|
||||
#include "rx/nrf24_kn.h"
|
||||
|
||||
/*
|
||||
* KN Protocol
|
||||
*/
|
||||
|
||||
#define KN_PAYLOAD_SIZE 16
|
||||
#define KN_NFREQCHANNELS 4
|
||||
#define RX_TX_ADDR_LEN 5
|
||||
#define KN_RC_CHANNEL_COUNT 8
|
||||
|
||||
enum {
|
||||
// packet[12] flags
|
||||
KN_FLAG_DR = 0x01,
|
||||
KN_FLAG_TRHOLD = 0x02,
|
||||
KN_FLAG_IDLEUP = 0x04,
|
||||
KN_FLAG_TD = 0x40
|
||||
};
|
||||
|
||||
enum {
|
||||
PHASE_NOT_BOUND = 0,
|
||||
PHASE_RECEIVED,
|
||||
PHASE_BOUND
|
||||
};
|
||||
|
||||
STATIC_UNIT_TESTED uint8_t rf_ch_num;
|
||||
STATIC_UNIT_TESTED uint8_t bind_phase;
|
||||
static uint32_t packet_timer;
|
||||
STATIC_UNIT_TESTED uint8_t txid[RX_TX_ADDR_LEN];
|
||||
STATIC_UNIT_TESTED uint8_t kn_freq_hopping[KN_NFREQCHANNELS];
|
||||
static uint32_t rx_timeout;
|
||||
extern uint16_t rxSpiRcData[];
|
||||
|
||||
static const unsigned char kn_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_ROLL,RC_SPI_PITCH,RC_SPI_YAW,
|
||||
RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4};
|
||||
|
||||
static void prepare_to_bind(void)
|
||||
{
|
||||
packet_timer = micros();
|
||||
for (int i = 0; i < KN_NFREQCHANNELS; ++i) {
|
||||
kn_freq_hopping[i] = 0;
|
||||
}
|
||||
rx_timeout = 1000L;
|
||||
}
|
||||
|
||||
static void switch_channel(void)
|
||||
{
|
||||
NRF24L01_WriteReg(NRF24L01_05_RF_CH, kn_freq_hopping[rf_ch_num]);
|
||||
if (++rf_ch_num >= KN_NFREQCHANNELS) rf_ch_num = 0;
|
||||
}
|
||||
|
||||
static void decode_bind_packet(uint8_t *packet)
|
||||
{
|
||||
if (packet[0]==0x4b && packet[1]==0x4e && packet[2]==0x44 && packet[3]==0x5a) {
|
||||
txid[0] = packet[4];
|
||||
txid[1] = packet[5];
|
||||
txid[2] = packet[6];
|
||||
txid[3] = packet[7];
|
||||
txid[4] = 0x4b;
|
||||
|
||||
kn_freq_hopping[0] = packet[8];
|
||||
kn_freq_hopping[1] = packet[9];
|
||||
kn_freq_hopping[2] = packet[10];
|
||||
kn_freq_hopping[3] = packet[11];
|
||||
|
||||
if (packet[15]==0x01) {
|
||||
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
|
||||
} else {
|
||||
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_250Kbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
|
||||
}
|
||||
|
||||
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, txid, RX_TX_ADDR_LEN);
|
||||
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, txid, RX_TX_ADDR_LEN);
|
||||
|
||||
bind_phase = PHASE_BOUND;
|
||||
rx_timeout = 1000L; // find the channel as fast as possible
|
||||
}
|
||||
}
|
||||
|
||||
// Returns whether the data was successfully decoded
|
||||
static rx_spi_received_e decode_packet(uint8_t *packet)
|
||||
{
|
||||
if (bind_phase == PHASE_NOT_BOUND) {
|
||||
decode_bind_packet(packet);
|
||||
return RX_SPI_RECEIVED_BIND;
|
||||
}
|
||||
// Decode packet
|
||||
// Restore regular interval
|
||||
rx_timeout = 13000L; // 13ms if data received
|
||||
bind_phase = PHASE_RECEIVED;
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
uint16_t a = packet[i*2];
|
||||
uint16_t b = packet[(i*2)+1];
|
||||
rxSpiRcData[kn_channelindex[i]] = ((uint16_t)(a<<8)+b) * 1000 / 1024 + 1000;
|
||||
}
|
||||
const uint8_t flags[] = {KN_FLAG_DR, KN_FLAG_TRHOLD, KN_FLAG_IDLEUP, KN_FLAG_TD};
|
||||
for (int i = 4; i < 8; ++i) {
|
||||
rxSpiRcData[kn_channelindex[i]] = (packet[12] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
|
||||
}
|
||||
packet_timer = micros();
|
||||
return RX_SPI_RECEIVED_DATA;
|
||||
}
|
||||
|
||||
void knNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
|
||||
{
|
||||
UNUSED(rcData);
|
||||
UNUSED(packet);
|
||||
}
|
||||
|
||||
static rx_spi_received_e readrx(uint8_t *packet)
|
||||
{
|
||||
if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_STATUS_RX_DR))) {
|
||||
uint32_t t = micros() - packet_timer;
|
||||
if (t > rx_timeout) {
|
||||
if (bind_phase == PHASE_RECEIVED) {
|
||||
switch_channel();
|
||||
}
|
||||
packet_timer = micros();
|
||||
rx_timeout = 10000L; // 10ms if data not received
|
||||
}
|
||||
return RX_SPI_RECEIVED_NONE;
|
||||
}
|
||||
packet_timer = micros();
|
||||
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag
|
||||
NRF24L01_ReadPayload(packet, KN_PAYLOAD_SIZE);
|
||||
NRF24L01_FlushRx();
|
||||
|
||||
switch_channel();
|
||||
return decode_packet(packet);
|
||||
}
|
||||
|
||||
/*
|
||||
* This is called periodically by the scheduler.
|
||||
* Returns RX_SPI_RECEIVED_DATA if a data packet was received.
|
||||
*/
|
||||
rx_spi_received_e knNrf24DataReceived(uint8_t *packet)
|
||||
{
|
||||
return readrx(packet);
|
||||
}
|
||||
|
||||
static void knNrf24Setup(rx_spi_protocol_e protocol)
|
||||
{
|
||||
NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC
|
||||
|
||||
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgment
|
||||
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); // Enable data pipe 0
|
||||
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
|
||||
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x00);
|
||||
NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
|
||||
NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR) | BV(NRF24L01_07_STATUS_TX_DS) | BV(NRF24L01_07_STATUS_MAX_RT)); // Clear data ready, data sent, and retransmit
|
||||
NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, KN_PAYLOAD_SIZE); // bytes of data payload for pipe 0
|
||||
NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
|
||||
|
||||
const uint8_t rx_tx_addr[RX_TX_ADDR_LEN] = {0x4b, 0x4e, 0x44, 0x5a, 0x4b};
|
||||
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, RX_TX_ADDR_LEN);
|
||||
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, RX_TX_ADDR_LEN);
|
||||
|
||||
NRF24L01_FlushTx();
|
||||
NRF24L01_FlushRx();
|
||||
|
||||
rf_ch_num = 0;
|
||||
bind_phase = PHASE_NOT_BOUND;
|
||||
prepare_to_bind();
|
||||
NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x53);// switch to channel 83
|
||||
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
|
||||
}
|
||||
|
||||
bool knNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
rxRuntimeConfig->channelCount = KN_RC_CHANNEL_COUNT;
|
||||
knNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol);
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
struct rxConfig_s;
|
||||
struct rxRuntimeConfig_s;
|
||||
bool knNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
||||
void knNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||
rx_spi_received_e knNrf24DataReceived(uint8_t *payload);
|
||||
|
|
@ -41,6 +41,7 @@
|
|||
#include "rx/nrf24_v202.h"
|
||||
#include "rx/nrf24_h8_3d.h"
|
||||
#include "rx/nrf24_inav.h"
|
||||
#include "rx/nrf24_kn.h"
|
||||
#include "rx/flysky.h"
|
||||
|
||||
|
||||
|
|
@ -105,6 +106,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
|||
protocolSetRcDataFromPayload = h8_3dNrf24SetRcDataFromPayload;
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_RX_KN
|
||||
case RX_SPI_NRF24_KN:
|
||||
protocolInit = knNrf24Init;
|
||||
protocolDataReceived = knNrf24DataReceived;
|
||||
protocolSetRcDataFromPayload = knNrf24SetRcDataFromPayload;
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_RX_INAV
|
||||
case RX_SPI_NRF24_INAV:
|
||||
protocolInit = inavNrf24Init;
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ typedef enum {
|
|||
RX_SPI_NRF24_CX10A,
|
||||
RX_SPI_NRF24_H8_3D,
|
||||
RX_SPI_NRF24_INAV,
|
||||
RX_SPI_NRF24_KN,
|
||||
RX_SPI_FRSKY_D,
|
||||
RX_SPI_FRSKY_X,
|
||||
RX_SPI_A7105_FLYSKY,
|
||||
|
|
|
|||
Loading…
Reference in New Issue