diff --git a/Makefile b/Makefile
index 167e187d45..b7243be322 100644
--- a/Makefile
+++ b/Makefile
@@ -62,11 +62,16 @@ FEATURES =
ALT_TARGETS = $(sort $(filter-out target, $(basename $(notdir $(wildcard $(ROOT)/src/main/target/*/*.mk)))))
OPBL_TARGETS = $(filter %_OPBL, $(ALT_TARGETS))
+<<<<<<< HEAD
#VALID_TARGETS = $(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS)
VALID_TARGETS = $(dir $(wildcard $(ROOT)/src/main/target/*/target.mk))
VALID_TARGETS := $(subst /,, $(subst ./src/main/target/,, $(VALID_TARGETS)))
VALID_TARGETS := $(VALID_TARGETS) $(ALT_TARGETS)
VALID_TARGETS := $(sort $(VALID_TARGETS))
+=======
+# Valid targets for OP BootLoader support
+OPBL_TARGETS = CC3D_OPBL CC3D_OPBL_NRF24
+>>>>>>> Tidied protocols and added H8_3D protocol.
ifeq ($(filter $(TARGET),$(ALT_TARGETS)), $(TARGET))
BASE_TARGET := $(firstword $(subst /,, $(subst ./src/main/target/,, $(dir $(wildcard $(ROOT)/src/main/target/*/$(TARGET).mk)))))
@@ -332,8 +337,31 @@ ifneq ($(FLASH_SIZE),)
DEVICE_FLAGS := $(DEVICE_FLAGS) -DFLASH_SIZE=$(FLASH_SIZE)
endif
+<<<<<<< HEAD
TARGET_DIR = $(ROOT)/src/main/target/$(BASE_TARGET)
TARGET_DIR_SRC = $(notdir $(wildcard $(TARGET_DIR)/*.c))
+=======
+ifeq ($(TARGET),$(filter $(TARGET), $(CC3D_TARGETS)))
+TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D
+ifeq ($(TARGET),CC3D_PPM1)
+TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D_PPM1
+CC3D_PPM1_SRC = $(CC3D_SRC)
+endif
+ifeq ($(TARGET),CC3D_OPBL)
+TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D_OPBL
+CC3D_OPBL_SRC = $(CC3D_SRC)
+endif
+ifeq ($(TARGET),CC3D_OPBL_NRF24)
+TARGET_FLAGS := $(TARGET_FLAGS) -DCC3D_OPBL -DUSE_RX_NRF24
+CC3D_OPBL_NRF24_SRC = $(CC3D_SRC)
+endif
+TARGET_DIR = $(ROOT)/src/main/target/CC3D
+endif
+
+ifneq ($(filter $(TARGET),$(OPBL_TARGETS)),)
+OPBL=yes
+endif
+>>>>>>> Tidied protocols and added H8_3D protocol.
ifeq ($(OPBL),yes)
TARGET_FLAGS := -DOPBL $(TARGET_FLAGS)
@@ -418,72 +446,6 @@ COMMON_SRC = \
sensors/initialisation.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC)
-=======
-INCLUDE_DIRS := $(INCLUDE_DIRS) \
- $(TARGET_DIR)
-
-VPATH := $(VPATH):$(TARGET_DIR)
-
-COMMON_SRC = build_config.c \
- debug.c \
- version.c \
- $(TARGET_SRC) \
- config/config.c \
- config/runtime_config.c \
- common/maths.c \
- common/printf.c \
- common/typeconversion.c \
- common/encoding.c \
- common/filter.c \
- scheduler/scheduler.c \
- scheduler/scheduler_tasks.c \
- main.c \
- mw.c \
- flight/failsafe.c \
- flight/pid.c \
- flight/imu.c \
- flight/hil.c \
- flight/mixer.c \
- drivers/bus_i2c_soft.c \
- drivers/serial.c \
- drivers/sound_beeper.c \
- drivers/system.c \
- drivers/gps_i2cnav.c \
- drivers/gyro_sync.c \
- drivers/buf_writer.c \
- drivers/rx_nrf24l01.c \
- io/beeper.c \
- io/rc_controls.c \
- io/rc_curves.c \
- io/serial.c \
- io/serial_4way.c \
- io/serial_4way_avrootloader.c \
- io/serial_4way_stk500v2.c \
- io/serial_cli.c \
- io/serial_msp.c \
- io/statusindicator.c \
- rx/rx.c \
- rx/pwm.c \
- rx/msp.c \
- rx/sbus.c \
- rx/sumd.c \
- rx/sumh.c \
- rx/spektrum.c \
- rx/xbus.c \
- rx/ibus.c \
- rx/nrf24.c \
- rx/nrf24_cx10.c \
- rx/nrf24_syma.c \
- rx/nrf24_v202.c \
- sensors/acceleration.c \
- sensors/battery.c \
- sensors/boardalignment.c \
- sensors/compass.c \
- sensors/gyro.c \
- sensors/initialisation.c \
- $(CMSIS_SRC) \
- $(DEVICE_STDPERIPH_SRC)
->>>>>>> NRF24 support for iNav.
HIGHEND_SRC = \
blackbox/blackbox.c \
@@ -521,7 +483,6 @@ VCP_SRC = \
drivers/serial_usb_vcp.c
else
VCP_SRC = \
-<<<<<<< HEAD
vcp/hw_config.c \
vcp/stm32_it.c \
vcp/usb_desc.c \
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 3a9e524e55..5b24fe83bf 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -389,7 +389,8 @@ static const char * const lookupTableNRF24RX[] = {
"SYMA_X",
"SYMA_X5C",
"CX10",
- "CX10A"
+ "CX10A",
+ "H8_3D",
};
#endif
@@ -649,7 +650,7 @@ const clivalue_t valueTable[] = {
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX }, 0 },
#endif
#ifdef USE_RX_NRF24
- { "nrf24rx_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.nrf24rx_protocol, .config.lookup = { TABLE_NRF24_RX }, 0 },
+ { "nrf24rx_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.nrf24rx_protocol, .config.lookup = { TABLE_NRF24_RX }, 0 },
#endif
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, 0 },
diff --git a/src/main/rx/nrf24.c b/src/main/rx/nrf24.c
index d4294be4e2..dc758a0ad8 100644
--- a/src/main/rx/nrf24.c
+++ b/src/main/rx/nrf24.c
@@ -28,6 +28,7 @@
#include "rx/nrf24_cx10.h"
#include "rx/nrf24_syma.h"
#include "rx/nrf24_v202.h"
+#include "rx/nrf24_h8_3d.h"
uint16_t nrf24RcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
@@ -80,6 +81,13 @@ STATIC_UNIT_TESTED bool rxNrf24SetProtocol(nrf24_protocol_t protocol)
protocolDataReceived = cx10DataReceived;
protocolSetRcDataFromPayload = cx10SetRcDataFromPayload;
break;
+#endif
+#ifdef USE_RX_H8_3D
+ case NRF24RX_H8_3D:
+ protocolInit = h8_3dInit;
+ protocolDataReceived = h8_3dDataReceived;
+ protocolSetRcDataFromPayload = h8_3dSetRcDataFromPayload;
+ break;
#endif
default:
return false;
diff --git a/src/main/rx/nrf24.h b/src/main/rx/nrf24.h
index 930a955178..8769f8e715 100644
--- a/src/main/rx/nrf24.h
+++ b/src/main/rx/nrf24.h
@@ -29,6 +29,7 @@ typedef enum {
NRF24RX_SYMA_X5C,
NRF24RX_CX10,
NRF24RX_CX10A,
+ NRF24RX_H8_3D,
NRF24RX_PROTOCOL_COUNT
} nrf24_protocol_t;
diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c
index 070fc6d3d1..b7472a1b09 100644
--- a/src/main/rx/nrf24_cx10.c
+++ b/src/main/rx/nrf24_cx10.c
@@ -94,9 +94,9 @@ STATIC_UNIT_TESTED uint8_t rxAddr[RX_TX_ADDR_LEN] = {0x49, 0x26, 0x87, 0x7d, 0x2
#define TX_ID_LEN 4
STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN];
-STATIC_UNIT_TESTED uint8_t rfChannelIndex = 0;
+STATIC_UNIT_TESTED uint8_t cx10RfChannelIndex = 0;
#define RF_CHANNEL_COUNT 4
-STATIC_UNIT_TESTED uint8_t rfChannels[RF_CHANNEL_COUNT]; // channels are set using txId from bind packet
+STATIC_UNIT_TESTED uint8_t cx10RfChannels[RF_CHANNEL_COUNT]; // channels are set using txId from bind packet
static uint32_t timeOfLastHop;
static const uint32_t hopTimeout = 5000; // 5ms
@@ -131,7 +131,7 @@ void cx10Nrf24Init(nrf24_protocol_t protocol)
/*
* Returns true if it is a bind packet.
*/
-STATIC_UNIT_TESTED bool checkBindPacket(const uint8_t *packet)
+STATIC_UNIT_TESTED bool cx10CheckBindPacket(const uint8_t *packet)
{
const bool bindPacket = (packet[0] == 0xaa);
if (bindPacket) {
@@ -144,7 +144,7 @@ STATIC_UNIT_TESTED bool checkBindPacket(const uint8_t *packet)
return false;
}
-STATIC_UNIT_TESTED uint16_t convertToPwmUnsigned(const uint8_t* pVal)
+STATIC_UNIT_TESTED uint16_t cx10ConvertToPwmUnsigned(const uint8_t* pVal)
{
uint16_t ret = (*(pVal + 1)) & 0x7f; // mask out top bit which is used for a flag for the rudder
ret = (ret << 8) | *pVal;
@@ -154,10 +154,10 @@ STATIC_UNIT_TESTED uint16_t convertToPwmUnsigned(const uint8_t* pVal)
void cx10SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
{
const uint8_t offset = (cx10Protocol == NRF24RX_CX10) ? 0 : 4;
- rcData[NRF24_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - convertToPwmUnsigned(&payload[5 + offset]); // aileron
- rcData[NRF24_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - convertToPwmUnsigned(&payload[7 + offset]); // elevator
- rcData[NRF24_THROTTLE] = convertToPwmUnsigned(&payload[9 + offset]); // throttle
- rcData[NRF24_YAW] = convertToPwmUnsigned(&payload[11 + offset]); // rudder
+ rcData[NRF24_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron
+ rcData[NRF24_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator
+ rcData[NRF24_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle
+ rcData[NRF24_YAW] = cx10ConvertToPwmUnsigned(&payload[11 + offset]); // rudder
const uint8_t flags1 = payload[13 + offset];
const uint8_t rate = flags1 & FLAG_MODE_MASK; // takes values 0, 1, 2
if (rate == 0) {
@@ -177,21 +177,21 @@ void cx10SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
static void hopToNextChannel(void)
{
- ++rfChannelIndex;
- if (rfChannelIndex >= RF_CHANNEL_COUNT) {
- rfChannelIndex = 0;
+ ++cx10RfChannelIndex;
+ if (cx10RfChannelIndex >= RF_CHANNEL_COUNT) {
+ cx10RfChannelIndex = 0;
}
- NRF24L01_SetChannel(rfChannels[rfChannelIndex]);
+ NRF24L01_SetChannel(cx10RfChannels[cx10RfChannelIndex]);
}
// The hopping channels are determined by the txId
STATIC_UNIT_TESTED void setHoppingChannels(const uint8_t* txId)
{
- rfChannelIndex = 0;
- rfChannels[0] = 0x03 + (txId[0] & 0x0F);
- rfChannels[1] = 0x16 + (txId[0] >> 4);
- rfChannels[2] = 0x2D + (txId[1] & 0x0F);
- rfChannels[3] = 0x40 + (txId[1] >> 4);
+ cx10RfChannelIndex = 0;
+ cx10RfChannels[0] = 0x03 + (txId[0] & 0x0F);
+ cx10RfChannels[1] = 0x16 + (txId[0] >> 4);
+ cx10RfChannels[2] = 0x2D + (txId[1] & 0x0F);
+ cx10RfChannels[3] = 0x40 + (txId[1] >> 4);
}
/*
@@ -209,7 +209,7 @@ nrf24_received_t cx10DataReceived(uint8_t *payload)
case STATE_BIND:
if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + 2)) {
XN297_UnscramblePayload(payload, payloadSize + 2);
- const bool bindPacket = checkBindPacket(payload);
+ const bool bindPacket = cx10CheckBindPacket(payload);
if (bindPacket) {
// set the hopping channels as determined by the txId received in the bind packet
setHoppingChannels(txId);
@@ -237,7 +237,7 @@ nrf24_received_t cx10DataReceived(uint8_t *payload)
}
// send out an ACK on each of the hopping channels, required by CX10 transmitter
for (uint8_t ii = 0; ii < RF_CHANNEL_COUNT; ++ii) {
- NRF24L01_SetChannel(rfChannels[ii]);
+ NRF24L01_SetChannel(cx10RfChannels[ii]);
XN297_WritePayload(payload, payloadSize);
NRF24L01_SetTxMode();// enter transmit mode to send the packet
// wait for the ACK packet to send before changing channel
@@ -252,7 +252,7 @@ nrf24_received_t cx10DataReceived(uint8_t *payload)
}
NRF24L01_SetRxMode();//reenter receive mode after sending ACKs
if (ackCount > ACK_TO_SEND_COUNT) {
- NRF24L01_SetChannel(rfChannels[0]);
+ NRF24L01_SetChannel(cx10RfChannels[0]);
// and go into data state to wait for first data packet
protocolState = STATE_DATA;
}
diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c
new file mode 100644
index 0000000000..0dca1d33cc
--- /dev/null
+++ b/src/main/rx/nrf24_h8_3d.c
@@ -0,0 +1,287 @@
+/*
+ * 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 .
+ */
+
+// This file borrows heavily from project Deviation,
+// see http://deviationtx.com
+
+#include
+#include
+#include
+
+#include
+#include "build_config.h"
+
+#ifdef USE_RX_H8_3D
+
+#include "drivers/rx_nrf24l01.h"
+#include "drivers/system.h"
+
+#include "rx/nrf24.h"
+#include "rx/nrf24_h8_3d.h"
+
+
+/*
+ * Deviation transmitter sends 345 bind packets, then starts sending data packets.
+ * Packets are send at rate of at least one every 4 milliseconds, ie at least 250Hz.
+ * This means binding phase lasts 1.4 seconds, the transmitter then enters the data phase.
+ * Other transmitters may vary but should have similar characteristics.
+ */
+
+
+/*
+ * H8_3D Protocol
+ * No auto acknowledgment
+ * Payload size is 20, static
+ * Data rate is 1Kbps
+ * Bind Phase
+ * uses address {0xab,0xac,0xad,0xae,0xaf}, converted by XN297 to {0x41, 0xbd, 0x42, 0xd4, 0xc2}
+ * hops between 4 channels
+ * Data Phase
+ * uses same address as bind phase
+ * hops between 4 channels generated from txId received in bind packets
+ *
+ */
+#define H8_3D_RC_CHANNEL_COUNT 14
+
+#define H8_3D_X_PROTOCOL_PAYLOAD_SIZE 20
+
+#define H8_3D_RF_CHANNEL_COUNT 4
+
+#define FLAG_FLIP 0x01
+#define FLAG_RATE_MID 0x02
+#define FLAG_RATE_HIGH 0x04
+#define FLAG_HEADLESS 0x10 // RTH + headless on H8, headless on JJRC H20
+#define FLAG_RTH 0x20 // 360° flip mode on H8 3D, RTH on JJRC H20
+
+// XN297 emulation layer
+static void XN297_UnscramblePayload(uint8_t* data, int len);
+
+typedef enum {
+ STATE_BIND = 0,
+ STATE_DATA
+} protocol_state_t;
+
+STATIC_UNIT_TESTED protocol_state_t protocolState;
+
+STATIC_UNIT_TESTED uint8_t payloadSize;
+
+#define RX_TX_ADDR_LEN 5
+//STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0xc4, 0x57, 0x09, 0x65, 0x21};
+STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address
+#define TX_ID_LEN 4
+STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN];
+
+// radio channels for frequency hopping
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannelIndex;
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannelCount = H8_3D_RF_CHANNEL_COUNT;
+STATIC_UNIT_TESTED uint8_t h8_3dRfChannels[H8_3D_RF_CHANNEL_COUNT];
+#define H8_3D_RF_BIND_CHANNEL_START 0x06
+#define H8_3D_RF_BIND_CHANNEL_END 0x26
+
+#define DATA_HOP_TIMEOUT 5000 // 5ms
+#define BIND_HOP_TIMEOUT 1000 // 1ms, to find the bind channel as quickly as possible
+static uint32_t hopTimeout = BIND_HOP_TIMEOUT;
+static uint32_t timeOfLastHop;
+
+void setBound(const uint8_t* txId);
+
+void h8_3dNrf24Init(nrf24_protocol_t protocol, const uint8_t* nrf24_id)
+{
+ UNUSED(protocol);
+ UNUSED(nrf24_id);
+ protocolState = STATE_BIND;
+
+ NRF24L01_Initialize(0); // sets PWR_UP, no CRC
+
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0); // No auto acknowledgment
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0));
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm);
+ // RX_ADDR for pipes P1-P5 are left at default values
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrXN297, RX_TX_ADDR_LEN);
+ if ((nrf24_id[0] | nrf24_id[1] | nrf24_id[2] | nrf24_id[3]) == 0) {
+ h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
+ NRF24L01_SetChannel(H8_3D_RF_BIND_CHANNEL_START);
+ } else {
+ setBound(nrf24_id);
+ }
+
+ NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00);
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+
+ payloadSize = H8_3D_X_PROTOCOL_PAYLOAD_SIZE + 2; // payload + 2 bytes CRC
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, payloadSize); // payload + 2 bytes CRC
+
+ NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
+}
+
+STATIC_UNIT_TESTED bool h8_3dCheckBindPacket(const uint8_t *payload)
+{
+ bool bindPacket = false;
+ if ((payload[5] == 0x00) && (payload[6] == 0x00) && (payload[7] == 0x01)) {
+ bindPacket = true;
+ txId[0] = payload[1];
+ txId[1] = payload[2];
+ txId[2] = payload[3];
+ txId[3] = payload[4];
+ }
+ return bindPacket;
+}
+
+STATIC_UNIT_TESTED uint16_t h8_3dConvertToPwm(uint8_t val, int16_t _min, int16_t _max)
+{
+#define PWM_RANGE (PWM_RANGE_MAX - PWM_RANGE_MIN)
+
+ int32_t ret = val;
+ const int32_t range = _max - _min;
+ ret = PWM_RANGE_MIN + ((ret - _min) * PWM_RANGE)/range;
+ return (uint16_t)ret;
+}
+
+void h8_3dSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
+{
+ rcData[NRF24_THROTTLE] = h8_3dConvertToPwm(payload[9], 0, 0xff);
+ rcData[NRF24_ROLL] = h8_3dConvertToPwm(payload[12], 0xbb, 0x43); // aileron
+ rcData[NRF24_PITCH] = h8_3dConvertToPwm(payload[11], 0x43, 0xbb); // elevator
+ const int8_t yawByte = payload[10];
+ rcData[NRF24_YAW] = yawByte >= 0 ? h8_3dConvertToPwm(yawByte, -0x3c, 0x3c) : h8_3dConvertToPwm(yawByte, 0xbc, 0x44);
+
+ const uint8_t flags = payload[17];
+ if (flags & FLAG_RATE_HIGH) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MAX;
+ } else if (flags & FLAG_RATE_MID) {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIDDLE;
+ } else {
+ rcData[NRF24_AUX1] = PWM_RANGE_MIN;
+ }
+ rcData[NRF24_AUX2] = flags & FLAG_FLIP ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX3] = PWM_RANGE_MIN; // picture
+ rcData[NRF24_AUX4] = PWM_RANGE_MIN; // video
+ rcData[NRF24_AUX5] = flags & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX6] = flags & FLAG_RTH ? PWM_RANGE_MAX : PWM_RANGE_MIN;
+ rcData[NRF24_AUX7] = h8_3dConvertToPwm(payload[13], 0x43, 0xbb);
+ rcData[NRF24_AUX9] = h8_3dConvertToPwm(payload[14], 0x43, 0xbb);
+ rcData[NRF24_AUX9] = h8_3dConvertToPwm(payload[15], 0x43, 0xbb);
+ rcData[NRF24_AUX10] = h8_3dConvertToPwm(payload[16], 0x43, 0xbb);
+
+ rcData[NRF24_AUX1] = 1000+payload[0];
+ rcData[NRF24_AUX2] = 1000+payload[1];
+ rcData[NRF24_AUX3] = 1000+payload[2];
+ rcData[NRF24_AUX4] = 1000+payload[3];
+ rcData[NRF24_AUX5] = 1000+payload[4];
+ rcData[NRF24_AUX6] = 1000+payload[5];
+}
+
+static void hopToNextChannel(void)
+{
+ ++h8_3dRfChannelIndex;
+ if (protocolState == STATE_BIND) {
+ if (h8_3dRfChannelIndex > H8_3D_RF_BIND_CHANNEL_END) {
+ h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START;
+ }
+ NRF24L01_SetChannel(h8_3dRfChannelIndex);
+ } else {
+ if (h8_3dRfChannelIndex >= h8_3dRfChannelCount) {
+ h8_3dRfChannelIndex = 0;
+ }
+ NRF24L01_SetChannel(h8_3dRfChannels[h8_3dRfChannelIndex]);
+ }
+}
+
+// The hopping channels are determined by the txId
+void setHoppingChannels(const uint8_t* txId)
+{
+ h8_3dRfChannels[0] = 0x06 + ((txId[0] & 0x0f) % 0x0f);
+ h8_3dRfChannels[1] = 0x15 + ((txId[1] & 0x0f) % 0x0f);
+ h8_3dRfChannels[2] = 0x24 + ((txId[2] & 0x0f) % 0x0f);
+ h8_3dRfChannels[3] = 0x33 + ((txId[3] & 0x0f) % 0x0f);
+}
+
+void setBound(const uint8_t* txId)
+{
+ setHoppingChannels(txId);
+ protocolState = STATE_DATA;
+ hopTimeout = DATA_HOP_TIMEOUT;
+ h8_3dRfChannelIndex = 0;
+ NRF24L01_SetChannel(h8_3dRfChannels[0]);
+}
+/*
+ * This is called periodically by the scheduler.
+ * Returns NRF24L01_RECEIVED_DATA if a data packet was received.
+ */
+nrf24_received_t h8_3dDataReceived(uint8_t *payload)
+{
+ nrf24_received_t ret = NRF24_RECEIVED_NONE;
+ const uint32_t timeNow = micros();
+ if (timeNow > timeOfLastHop + hopTimeout) {
+ hopToNextChannel();
+ timeOfLastHop = timeNow;
+ }
+ switch (protocolState) {
+ case STATE_BIND:
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ XN297_UnscramblePayload(payload, payloadSize);
+ const bool bindPacket = h8_3dCheckBindPacket(payload);
+ if (bindPacket) {
+ ret = NRF24_RECEIVED_BIND;
+ setBound(txId);
+ }
+ }
+ break;
+ case STATE_DATA:
+ // read the payload, processing of payload is deferred
+ if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
+ XN297_UnscramblePayload(payload, payloadSize);
+ hopToNextChannel();
+ timeOfLastHop = timeNow;
+ ret = NRF24_RECEIVED_DATA;
+ }
+ break;
+ }
+ return ret;
+}
+
+void h8_3dInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ rxRuntimeConfig->channelCount = H8_3D_RC_CHANNEL_COUNT;
+ h8_3dNrf24Init((nrf24_protocol_t)rxConfig->nrf24rx_protocol, rxConfig->nrf24rx_id);
+}
+
+static const uint8_t xn297_data_scramble[30] = {
+ 0xbc, 0xe5, 0x66, 0x0d, 0xae, 0x8c, 0x88, 0x12,
+ 0x69, 0xee, 0x1f, 0xc7, 0x62, 0x97, 0xd5, 0x0b,
+ 0x79, 0xca, 0xcc, 0x1b, 0x5d, 0x19, 0x10, 0x24,
+ 0xd3, 0xdc, 0x3f, 0x8e, 0xc5, 0x2f
+};
+
+static uint8_t bitReverse(uint8_t bIn)
+{
+ uint8_t bOut = 0;
+ for (int ii = 0; ii < 8; ++ii) {
+ bOut = (bOut << 1) | (bIn & 1);
+ bIn >>= 1;
+ }
+ return bOut;
+}
+
+static void XN297_UnscramblePayload(uint8_t* data, int len)
+{
+ for (uint8_t ii = 0; ii < len; ++ii) {
+ data[ii] = bitReverse(data[ii] ^ xn297_data_scramble[ii]);
+ }
+}
+#endif
diff --git a/src/main/rx/nrf24_h8_3d.h b/src/main/rx/nrf24_h8_3d.h
new file mode 100644
index 0000000000..0ab6839278
--- /dev/null
+++ b/src/main/rx/nrf24_h8_3d.h
@@ -0,0 +1,25 @@
+/*
+ * 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
+
+void h8_3dInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+void h8_3dSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
+nrf24_received_t h8_3dDataReceived(uint8_t *payload);
diff --git a/src/main/rx/nrf24_syma.c b/src/main/rx/nrf24_syma.c
index 0ac8bc3216..eceb913f3d 100644
--- a/src/main/rx/nrf24_syma.c
+++ b/src/main/rx/nrf24_syma.c
@@ -106,11 +106,11 @@ STATIC_UNIT_TESTED const uint8_t rxTxAddrX5C[RX_TX_ADDR_LEN] = {0x6d, 0x6a, 0x73
// radio channels for frequency hopping
static int packetCount = 0;
-STATIC_UNIT_TESTED uint8_t rfChannelIndex = 0;
-STATIC_UNIT_TESTED uint8_t rfChannelCount = SYMA_X_RF_CHANNEL_COUNT;
+STATIC_UNIT_TESTED uint8_t symaRfChannelIndex = 0;
+STATIC_UNIT_TESTED uint8_t symaRfChannelCount = SYMA_X_RF_CHANNEL_COUNT;
// set rfChannels to SymaX bind channels, reserve enough space for SymaX5C channels
-STATIC_UNIT_TESTED uint8_t rfChannels[SYMA_X5C_RF_BIND_CHANNEL_COUNT] = {0x4b, 0x30, 0x40, 0x20};
-STATIC_UNIT_TESTED const uint8_t rfChannelsX5C[SYMA_X5C_RF_CHANNEL_COUNT] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
+STATIC_UNIT_TESTED uint8_t symaRfChannels[SYMA_X5C_RF_BIND_CHANNEL_COUNT] = {0x4b, 0x30, 0x40, 0x20};
+STATIC_UNIT_TESTED const uint8_t symaRfChannelsX5C[SYMA_X5C_RF_CHANNEL_COUNT] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
static uint32_t timeOfLastHop;
static uint32_t hopTimeout = 10000; // 10ms
@@ -136,10 +136,10 @@ void symaNrf24Init(nrf24_protocol_t protocol)
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrX5C, RX_TX_ADDR_LEN);
// just go straight into data mode, since the SYMA_X5C protocol does not actually require binding
protocolState = STATE_DATA;
- rfChannelCount = SYMA_X5C_RF_CHANNEL_COUNT;
- memcpy(rfChannels, rfChannelsX5C, SYMA_X5C_RF_CHANNEL_COUNT);
+ symaRfChannelCount = SYMA_X5C_RF_CHANNEL_COUNT;
+ memcpy(symaRfChannels, symaRfChannelsX5C, SYMA_X5C_RF_CHANNEL_COUNT);
}
- NRF24L01_SetChannel(rfChannels[0]);
+ NRF24L01_SetChannel(symaRfChannels[0]);
NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00);
NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
@@ -149,14 +149,14 @@ void symaNrf24Init(nrf24_protocol_t protocol)
NRF24L01_SetRxMode(); // enter receive mode to start listening for packets
}
-STATIC_UNIT_TESTED uint16_t convertToPwmUnsigned(uint8_t val)
+STATIC_UNIT_TESTED uint16_t symaConvertToPwmUnsigned(uint8_t val)
{
uint32_t ret = val;
ret = ret * (PWM_RANGE_MAX - PWM_RANGE_MIN) / UINT8_MAX + PWM_RANGE_MIN;
return (uint16_t)ret;
}
-STATIC_UNIT_TESTED uint16_t convertToPwmSigned(uint8_t val)
+STATIC_UNIT_TESTED uint16_t symaConvertToPwmSigned(uint8_t val)
{
int32_t ret = val & 0x7f;
ret = (ret * (PWM_RANGE_MAX - PWM_RANGE_MIN)) / (2 * INT8_MAX);
@@ -166,7 +166,7 @@ STATIC_UNIT_TESTED uint16_t convertToPwmSigned(uint8_t val)
return (uint16_t)(PWM_RANGE_MIDDLE + ret);
}
-STATIC_UNIT_TESTED bool checkBindPacket(const uint8_t *packet)
+STATIC_UNIT_TESTED bool symaCheckBindPacket(const uint8_t *packet)
{
bool bindPacket = false;
if (symaProtocol == NRF24RX_SYMA_X) {
@@ -188,11 +188,11 @@ STATIC_UNIT_TESTED bool checkBindPacket(const uint8_t *packet)
void symaSetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
{
- rcData[NRF24_THROTTLE] = convertToPwmUnsigned(packet[0]); // throttle
- rcData[NRF24_ROLL] = convertToPwmSigned(packet[3]); // aileron
+ rcData[NRF24_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle
+ rcData[NRF24_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron
if (symaProtocol == NRF24RX_SYMA_X) {
- rcData[NRF24_PITCH] = convertToPwmSigned(packet[1]); // elevator
- rcData[NRF24_YAW] = convertToPwmSigned(packet[2]); // rudder
+ rcData[NRF24_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator
+ rcData[NRF24_YAW] = symaConvertToPwmSigned(packet[2]); // rudder
const uint8_t rate = (packet[5] & 0xc0) >> 6; // takes values 0, 1, 2
if (rate == 0) {
rcData[NRF24_AUX1] = PWM_RANGE_MIN;
@@ -206,8 +206,8 @@ void symaSetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet)
rcData[NRF24_AUX4] = packet[4] & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN;
rcData[NRF24_AUX5] = packet[14] & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN;
} else {
- rcData[NRF24_PITCH] = convertToPwmSigned(packet[2]); // elevator
- rcData[NRF24_YAW] = convertToPwmSigned(packet[1]); // rudder
+ rcData[NRF24_PITCH] = symaConvertToPwmSigned(packet[2]); // elevator
+ rcData[NRF24_YAW] = symaConvertToPwmSigned(packet[1]); // rudder
const uint8_t flags = packet[14];
rcData[NRF24_AUX1] = flags & FLAG_RATE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
rcData[NRF24_AUX2] = flags & FLAG_FLIP_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN;
@@ -221,12 +221,12 @@ static void hopToNextChannel(void)
// hop channel every second packet
++packetCount;
if ((packetCount & 0x01) == 0) {
- ++rfChannelIndex;
- if (rfChannelIndex >= rfChannelCount) {
- rfChannelIndex = 0;
+ ++symaRfChannelIndex;
+ if (symaRfChannelIndex >= symaRfChannelCount) {
+ symaRfChannelIndex = 0;
}
}
- NRF24L01_SetChannel(rfChannels[rfChannelIndex]);
+ NRF24L01_SetChannel(symaRfChannels[symaRfChannelIndex]);
}
// The SymaX hopping channels are determined by the low bits of rxTxAddress
@@ -237,7 +237,7 @@ void setSymaXHoppingChannels(uint32_t addr)
addr = 0x07;
}
const uint32_t inc = (addr << 24) | (addr << 16) | (addr << 8) | addr;
- uint32_t * const prfChannels = (uint32_t *)rfChannels;
+ uint32_t * const prfChannels = (uint32_t *)symaRfChannels;
if (addr == 0x16) {
*prfChannels = 0x28481131;
} else if (addr == 0x1e) {
@@ -261,7 +261,7 @@ nrf24_received_t symaDataReceived(uint8_t *payload)
switch (protocolState) {
case STATE_BIND:
if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) {
- const bool bindPacket = checkBindPacket(payload);
+ const bool bindPacket = symaCheckBindPacket(payload);
if (bindPacket) {
ret = NRF24_RECEIVED_BIND;
protocolState = STATE_DATA;
@@ -271,8 +271,8 @@ nrf24_received_t symaDataReceived(uint8_t *payload)
// set the NRF24 to use the rxTxAddr received in the bind packet
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddr, RX_TX_ADDR_LEN);
packetCount = 0;
- rfChannelIndex = 0;
- NRF24L01_SetChannel(rfChannels[0]);
+ symaRfChannelIndex = 0;
+ NRF24L01_SetChannel(symaRfChannels[0]);
}
}
break;
diff --git a/src/main/rx/nrf24_v202.c b/src/main/rx/nrf24_v202.c
index 78aa023891..83b4a38727 100644
--- a/src/main/rx/nrf24_v202.c
+++ b/src/main/rx/nrf24_v202.c
@@ -15,19 +15,6 @@
* along with Cleanflight. If not, see .
*/
-/*
- * v202 Protocol
- * No auto acknowledgment
- * Payload size is 16, static
- * Data rate is 250Kbps or 1Mbps depending on variant
- * Bind Phase
- * uses address {0x66, 0x88, 0x68, 0x68, 0x68}
- * hops between 16 channels
- * Data Phase
- * uses same address as bind phase
- * hops between 16 channels generated from address received in bind packets
- */
-
// this file is copied with modifications from bradwii for jd385
// see https://github.com/hackocopter/bradwii-jd385
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 54b1798faa..561997d2be 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -32,6 +32,7 @@
#include "drivers/serial.h"
#include "drivers/adc.h"
+
#include "io/serial.h"
#include "io/rc_controls.h"
@@ -41,6 +42,7 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/system.h"
+
#include "rx/pwm.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 1f15dc930b..33ea542c3d 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -108,10 +108,12 @@ typedef struct rxChannelRangeConfiguration_s {
uint16_t max;
} rxChannelRangeConfiguration_t;
+#define NRF24RX_ID_LEN 5
typedef struct rxConfig_s {
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
uint8_t nrf24rx_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
+ uint8_t nrf24rx_id[NRF24RX_ID_LEN];
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
uint8_t rssi_channel;
uint8_t rssi_scale;
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 5e7f192c02..9c58aa4f64 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -93,8 +93,6 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11
-#define USE_RX_NRF24
-
#ifdef USE_RX_NRF24
#undef SERIAL_RX
#define SKIP_RX_MSP
@@ -104,8 +102,10 @@
#define USE_RX_V202
#define USE_RX_SYMA
#define USE_RX_CX10
-#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C
+#define USE_RX_H8_3D
//#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M
+//#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C
+#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D
#define USE_SOFTSPI
#define USE_NRF24_SOFTSPI