diff --git a/src/main/rx/nrf24_kn.c b/src/main/rx/nrf24_kn.c new file mode 100644 index 0000000000..1d78fa363f --- /dev/null +++ b/src/main/rx/nrf24_kn.c @@ -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 . + */ + +#include +#include +#include + +#include + +#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 diff --git a/src/main/rx/nrf24_kn.h b/src/main/rx/nrf24_kn.h new file mode 100644 index 0000000000..96a0bbc634 --- /dev/null +++ b/src/main/rx/nrf24_kn.h @@ -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 . + */ + +#pragma once + +#include +#include + +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); diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index 0dbe00a9a8..33c1990a24 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -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; diff --git a/src/main/rx/rx_spi.h b/src/main/rx/rx_spi.h index d882df4922..4abf9e8b7b 100644 --- a/src/main/rx/rx_spi.h +++ b/src/main/rx/rx_spi.h @@ -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,