diff --git a/.gitmodules b/.gitmodules
index 80a95d75ad..545c8e91f6 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,4 +1,3 @@
-[submodule "libopencm3"]
+[submodule "src/libopencm3"]
path = src/libopencm3
- url = https://github.com/DeviationTx/libopencm3.git
- branch = stm32f2_adc
+ url = https://github.com/libopencm3/libopencm3.git
diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json
new file mode 100644
index 0000000000..c7f6a46606
--- /dev/null
+++ b/src/.vscode/settings.json
@@ -0,0 +1,11 @@
+{
+ "files.associations": {
+ "limits": "cpp",
+ "protocol.h": "c",
+ "interface.h": "c",
+ "mixer.h": "c",
+ "model.h": "c",
+ "tx.h": "c",
+ "*.tcc": "c"
+ }
+}
\ No newline at end of file
diff --git a/src/libopencm3 b/src/libopencm3
index d55bbafddb..6763681c26 160000
--- a/src/libopencm3
+++ b/src/libopencm3
@@ -1 +1 @@
-Subproject commit d55bbafddb9768228748f48a82967f91a910806e
+Subproject commit 6763681c260cf280487d70ca0d1996a8b79fff30
diff --git a/src/protocol/Makefile.inc b/src/protocol/Makefile.inc
index 15c03d346e..a701442a67 100644
--- a/src/protocol/Makefile.inc
+++ b/src/protocol/Makefile.inc
@@ -60,6 +60,7 @@ PROTO_MODULES += $(ODIR)/protocol/pxx.mod
PROTO_MODULES += $(ODIR)/protocol/loli.mod
PROTO_MODULES += $(ODIR)/protocol/e016h.mod
PROTO_MODULES += $(ODIR)/protocol/sumd.mod
+PROTO_MODULES += $(ODIR)/protocol/omp.mod
ifeq "$(SUPPORT_SCANNER)" "1"
PROTO_MODULES += $(ODIR)/protocol/scancyrf.mod
endif
@@ -328,6 +329,10 @@ $(ODIR)/protocol/e010.mod : $(ODIR)/e010_cc2500.bin
@echo Building 'e010' module
/bin/mkdir -p $(ODIR)/protocol/ 2>/dev/null; /bin/cp $< $@
+$(ODIR)/protocol/omp.mod : $(ODIR)/omp_cc2500.bin
+ @echo Building 'omp' module
+ /bin/mkdir -p $(ODIR)/protocol/ 2>/dev/null; /bin/cp $< $@
+
$(ODIR)/protocol/v761.mod : $(ODIR)/v761_nrf24l01.bin
@echo Building 'v761' module
/bin/mkdir -p $(ODIR)/protocol/ 2>/dev/null; /bin/cp $< $@
diff --git a/src/protocol/e010_cc2500.c b/src/protocol/e010_cc2500.c
index c197f6ebf3..f0631916b7 100644
--- a/src/protocol/e010_cc2500.c
+++ b/src/protocol/e010_cc2500.c
@@ -106,6 +106,7 @@ e010_tx_rf_map[] = { {{0x4F, 0x1C}, {0x3A, 0x35, 0x4A, 0x45}},
{{0xFD, 0x4F}, {0x33, 0x3B, 0x43, 0x4B}},
{{0x86, 0x3C}, {0x34, 0x3E, 0x44, 0x4E}} };
+/*
// xn297 emulation
////////////////////
static u8 xn297_addr_len;
@@ -213,6 +214,7 @@ static void XN297L_WritePayload(const u8* msg, u8 len)
// end of xn297 emulation
///////////////////////////
+*/
// Bit vector from bit position
#define BV(bit) (1 << bit)
@@ -300,7 +302,7 @@ static void send_packet(u8 bind)
static void e010_init()
{
u8 rx_tx_addr[ADDRESS_LENGTH];
- XN297L_init(); // setup cc2500 for xn297L@250kbps emulation
+ XN297L_Configure(XN297L_SCRAMBLED, XN297L_CRC, PACKET_SIZE+8); // setup cc2500 for xn297L@250kbps emulation packet_size(16) + 5byte address + + 2byte crc + 1byte preamble (0x55）
CC2500_WriteReg(CC2500_0C_FSCTRL0, fine);
memcpy(rx_tx_addr, "\x6d\x6a\x77\x77\x77", sizeof(rx_tx_addr));
memcpy(rf_channels, "\x36\x3e\x46\x2e", sizeof(rf_channels));
diff --git a/src/protocol/iface_cc2500.h b/src/protocol/iface_cc2500.h
index 4e8f645671..f3a69acc9c 100644
--- a/src/protocol/iface_cc2500.h
+++ b/src/protocol/iface_cc2500.h
@@ -135,4 +135,24 @@ void CC2500_WriteData(u8 *packet, u8 length);
void CC2500_ReadData(u8 *dpbuffer, int len);
void CC2500_SetTxRxMode(enum TXRX_State);
void CC2500_SetPower(int power);
+
+void XN297L_Configure(u8 scramble_en, u8 crc_en, u8 cc2500_packet_len);
+void XN297L_SetTXAddr(const u8* addr, u8 len);
+void XN297L_SetRXAddr(const u8* addr, u8 len);
+void XN297L_WritePayload(u8* msg, u8 len);
+void XN297L_SetChannel(u8 ch);
+void XN297L_SetScrambledMode(const u8 mode);
+void XN297L_WriteEnhancedPayload(u8* msg, u8 len, u8 noack);
+u8 _xn297l_write_enhancedpayload(const u8* msg, u8 len, u8* out, u8 noack);
+u8 _xn297l_write_payload(const u8* msg, u8 len, u8* out);
+u8 XN297L_ReadPayload(u8* msg, u8 len);
+u8 XN297L_ReadEnhancedPayload(u8* msg, u8 len);
+#define XN297L_UNSCRAMBLED 0
+#define XN297L_SCRAMBLED 1
+#define XN297L_NOCRC 0
+#define XN297L_CRC 1
+
+u16 crc16_update(u16 crc, u8 a, u8 bits);
+u8 bit_reverse(u8 b_in);
+
#endif
diff --git a/src/protocol/iface_nrf24l01.h b/src/protocol/iface_nrf24l01.h
index 6039ae5354..050120f514 100644
--- a/src/protocol/iface_nrf24l01.h
+++ b/src/protocol/iface_nrf24l01.h
@@ -145,17 +145,20 @@ enum {
extern const u8 xn297_scramble[];
extern const u16 xn297_crc_xorout_scrambled[];
extern const u16 xn297_crc_xorout[];
-uint8_t bit_reverse(uint8_t b_in);
+extern const u16 xn297_crc_xorout_scrambled_enhanced[];
+extern const u16 xn297_crc_xorout_enhanced[];
-void XN297_SetTXAddr(const u8* addr, int len);
-void XN297_SetRXAddr(const u8* addr, int len);
+// uint8_t bit_reverse(uint8_t b_in);
+
+void XN297_SetTXAddr(const u8* addr, u8 len);
+void XN297_SetRXAddr(const u8* addr, u8 len);
void XN297_Configure(u8 flags);
void XN297_SetScrambledMode(const u8 mode);
u8 XN297_WritePayload(u8* msg, int len);
-u8 XN297_WriteEnhancedPayload(u8* msg, int len, int noack, u16 crc_xorout);
-u8 XN297_ReadPayload(u8* msg, int len);
-u8 XN297_ReadEnhancedPayload(u8* msg, int len);
-u16 crc16_update(u16 crc, u8 a, u8 bits);
+u8 XN297_WriteEnhancedPayload(u8* msg, u8 len, u8 noack, u16 crc_xorout);
+u8 XN297_ReadPayload(u8* msg, u8 len);
+u8 XN297_ReadEnhancedPayload(u8* msg, u8 len);
+// u16 crc16_update(u16 crc, u8 a, u8 bits);
// HS6200 emulation layer
void HS6200_SetTXAddr(const u8* addr, u8 len);
diff --git a/src/protocol/omp_cc2500.c b/src/protocol/omp_cc2500.c
new file mode 100755
index 0000000000..b285cd56f5
--- /dev/null
+++ b/src/protocol/omp_cc2500.c
@@ -0,0 +1,445 @@
+/*
+ This project 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.
+ Deviation 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 Deviation. If not, see .
+ */
+
+#include "common.h"
+#include "interface.h"
+#include "mixer.h"
+#include "config/model.h"
+#include "config/tx.h" // for Transmitter
+
+
+#ifdef PROTO_HAS_CC2500
+
+#ifdef EMULATOR
+#define USE_FIXED_MFGID
+#define OMP_BIND_COUNT 20
+#define OMP_PACKET_PERIOD 100
+#define dbgprintf printf
+#else
+#define OMP_BIND_COUNT 100
+#define OMP_PACKET_PERIOD 5000 // Timeout for callback in uSec
+// printf inside an interrupt handler is really dangerous
+// this shouldn't be enabled even in debug builds without explicitly
+// turning it on
+#define dbgprintf if (0) printf
+#endif
+
+#define OMP_PACKET_SIZE 16
+#define OMP_RF_BIND_CHANNEL 35
+#define OMP_NUM_RF_CHANNELS 8
+#define OMP_ADDR_LEN 5
+
+
+static const char * const omp_opts[] = {
+ _tr_noop("Freq-Fine"), "-127", "127", NULL,
+ _tr_noop("Telemetry"), _tr_noop("On"), _tr_noop("Off"), NULL,
+ NULL
+};
+
+enum {
+ PROTOOPTS_FREQFINE = 0,
+ PROTOOPTS_TELEMETRY,
+ LAST_PROTO_OPT,
+};
+
+ctassert(LAST_PROTO_OPT <= NUM_PROTO_OPTS, too_many_protocol_opts);
+
+#define TELEM_ON 0
+#define TELEM_OFF 1
+
+
+static u8 tx_power;
+static u8 packet[16];
+static u8 hopping_frequency_no = 0;
+static u8 rx_tx_addr[5];
+static u8 hopping_frequency[OMP_NUM_RF_CHANNELS];
+static u16 bind_counter;
+static u8 phase;
+static u8 calibration[OMP_NUM_RF_CHANNELS];
+static u8 calibration_fscal2;
+static u8 calibration_fscal3;
+static s8 fine;
+static u8 telm_req = 0;
+static u16 tx_wait = 0;
+static u8 last_good_v_lipo = 0;
+
+
+enum{
+ OMP_BIND,
+ OMP_DATA,
+ OMP_PACKET_SEND,
+};
+
+enum {
+ CHANNEL1 = 0,
+ CHANNEL2,
+ CHANNEL3,
+ CHANNEL4,
+ CHANNEL5,
+ CHANNEL6,
+ CHANNEL7,
+};
+
+// Bit vector from bit position
+#define BV(bit) (1 << bit)
+
+#define CHAN_RANGE (CHAN_MAX_VALUE - CHAN_MIN_VALUE)
+static u16 scale_channel(u8 ch, u16 destMin, u16 destMax)
+{
+ s32 chanval = Channels[ch];
+ s32 range = (s32) destMax - (s32) destMin;
+
+ if (chanval < CHAN_MIN_VALUE)
+ chanval = CHAN_MIN_VALUE;
+ else if (chanval > CHAN_MAX_VALUE)
+ chanval = CHAN_MAX_VALUE;
+ return (range * (chanval - CHAN_MIN_VALUE)) / CHAN_RANGE + destMin;
+}
+
+#define GET_FLAG(ch, mask) (Channels[ch] > 0 ? mask : 0)
+
+
+// calibrate used RF channels for faster hopping
+static void calibrate_rf_chans()
+{
+ for (int c = 0; c < OMP_NUM_RF_CHANNELS; c++) {
+ CLOCK_ResetWatchdog();
+ CC2500_Strobe(CC2500_SIDLE);
+ XN297L_SetChannel(hopping_frequency[c]);
+ CC2500_Strobe(CC2500_SCAL);
+ usleep(900);
+ calibration[c] = CC2500_ReadReg(CC2500_25_FSCAL1);
+ }
+ calibration_fscal3 = CC2500_ReadReg(CC2500_23_FSCAL3); // only needs to be done once
+ calibration_fscal2 = CC2500_ReadReg(CC2500_24_FSCAL2); // only needs to be done once
+ CC2500_Strobe(CC2500_SIDLE);
+}
+
+static void calc_fh_channels(u8 num_ch)
+{
+ u8 idx = 0;
+ u32 rnd = Model.fixed_id;
+ u8 max = (num_ch/3)+2;
+
+ while (idx < num_ch)
+ {
+ u8 i;
+ u8 count_2_26 = 0, count_27_50 = 0, count_51_74 = 0;
+
+ rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
+ // Use least-significant byte. 73 is prime, so channels 76..77 are unused
+ u8 next_ch = ((rnd >> 8) % 73) + 2;
+ // Keep a distance of 5 between consecutive channels
+ if (idx !=0)
+ {
+ if (hopping_frequency[idx-1] > next_ch)
+ {
+ if (hopping_frequency[idx-1] - next_ch < 5)
+ continue;
+ }
+ else
+ if (next_ch-hopping_frequency[idx-1] < 5)
+ continue;
+ }
+ // Check that it's not duplicated and spread uniformly
+ for (i = 0; i < idx; i++) {
+ if (hopping_frequency[i] == next_ch)
+ break;
+ if (hopping_frequency[i] <= 26)
+ count_2_26++;
+ else if (hopping_frequency[i] <= 50)
+ count_27_50++;
+ else
+ count_51_74++;
+ }
+ if (i != idx)
+ continue;
+ if ( (next_ch <= 26 && count_2_26 < max) || (next_ch >= 27 && next_ch <= 50 && count_27_50 < max) || (next_ch >= 51 && count_51_74 < max) )
+ hopping_frequency[idx++] = next_ch; // find hopping frequency
+ }
+}
+
+static void omp_update_telemetry()
+{
+// raw receive data first byte should be 0x55 (last xn297l preamble word) and then 5 byte address + 2byte PCF + 16byte payload +2 byte crc, total=1+5+2+16+2=26
+// packet_in = 01 00 98 2C 03 19 19 F0 49 02 00 00 00 00 00 00
+// all bytes are fixed and unknown except 2 and 3 which represent the battery voltage: packet_in[3]*256+packet_in[2]=lipo voltage*100 in V
+ const u8 *update = NULL;
+ static const u8 omp_telem[] = { TELEM_DEVO_VOLT1, TELEM_DEVO_RPM1, TELEM_DEVO_RPM2, 0 }; // use TELEM_DEVO_RPM1 for LRSSI, TELEM_DEVO_RPM2 for LQI
+
+ u16 V = 0;
+ u8 telem_len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
+
+ if (telem_len == 28) // 26 plus 2byte append rx_status(RSSI+LQI)
+ {
+ u8 crc_ok = XN297L_ReadEnhancedPayload(packet, telem_len);
+
+ if (crc_ok)
+ { // packet with good CRC and length
+ V = ((packet[3] << 8) + packet[2]) / 100;
+ last_good_v_lipo = V;
+ Telemetry.value[TELEM_DEVO_VOLT1] = V;
+ Telemetry.value[TELEM_DEVO_RPM1] = ((s8)crc_ok) / 2 - 72; // rssi to dbm
+ update = omp_telem;
+ }
+ else
+ { // As soon as the motor spins the telem packets are becoming really bad and the CRC throws most of them in error as it should but...
+ if (packet[0] == 0x01 && packet[1] == 0x00)
+ { // the start of the packet looks ok...
+ V = ((packet[3] << 8) + packet[2]) / 100;
+ if (V < 130 && V > 60)
+ { // voltage is less than 13V and more than 6V (3V/element)
+ u16 v1 = V - last_good_v_lipo;
+ if (v1&0x8000) v1 = -v1;
+ if (v1 < 10) // the batt voltage is within 1V from a good reading...
+ {
+ Telemetry.value[TELEM_DEVO_VOLT1] = V;
+ update = omp_telem;
+ }
+ }
+ }
+ }
+ Telemetry.value[TELEM_DEVO_RPM2] = CC2500_ReadReg(CC2500_33_LQI | CC2500_READ_BURST) & 0x7F;
+ update = omp_telem;
+ }
+ CC2500_Strobe(CC2500_SFRX);
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_SetTxRxMode(TXRX_OFF);
+ if (update)
+ {
+ while (*update) {
+ TELEMETRY_SetUpdated(*update++);
+ }
+ }
+}
+
+static void OMP_send_packet(u8 bind)
+{
+ CC2500_SetTxRxMode(TX_EN);
+ CLOCK_ResetWatchdog();
+ CLOCK_RunMixer();
+ if (!bind)
+ {
+ memset(packet, 0x00, OMP_PACKET_SIZE);
+ packet[0] = hopping_frequency_no;
+ telm_req++;
+ telm_req %= OMP_NUM_RF_CHANNELS-1; // Change telem RX channels every time
+
+ if (telm_req == 0)
+ packet[0] |= 0x40;
+ CC2500_WriteReg(CC2500_23_FSCAL3, calibration_fscal3);
+ CC2500_WriteReg(CC2500_24_FSCAL2, calibration_fscal2);
+ CC2500_WriteReg(CC2500_25_FSCAL1, calibration[hopping_frequency_no]);
+ XN297L_SetChannel(hopping_frequency[hopping_frequency_no]);
+ hopping_frequency_no++;
+ hopping_frequency_no &= OMP_NUM_RF_CHANNELS-1;
+
+ packet[1] = 0x08 // unknown
+ | GET_FLAG(CHANNEL5, 0x20); // HOLD
+ packet[2] = 0x40; // unknown
+ u16 ch = scale_channel(CHANNEL6, 0, 0x7FF); // throttle
+ if (ch > 0x600)
+ packet[2] |= 0x20; // IDLE2
+ else if (ch > 0x200)
+ packet[1] |= 0x40; // IDLE1
+ ch = scale_channel(CHANNEL7, 0, 0x7FF); // throttle
+ if (ch > 0x600)
+ packet[2] |= 0x08; // 3D
+ else if (ch > 0x200)
+ packet[2] |= 0x04; // ATTITUDE
+ ch = scale_channel(CHANNEL3, 0, 0x7FF); // throttle
+ packet[7] = ch;
+ packet[8] = ch >> 8;
+ ch = scale_channel(CHANNEL1, 0, 0x7FF); // aileron
+ packet[8] |= ch << 3;
+ packet[9] = ch >> 5;
+ ch = scale_channel(CHANNEL2, 0, 0x7FF); // elevator
+ packet[9] |= ch << 6;
+ packet[10] = ch >> 2;
+ packet[11] = ch >> 10;
+ ch = scale_channel(CHANNEL4, 0, 0x7FF); // rudder
+ packet[11] |= ch << 1;
+ packet[12] = ch >> 7;
+ packet[15] = 0x04;
+ }
+
+ XN297L_WriteEnhancedPayload(packet, OMP_PACKET_SIZE, telm_req != 0); // ack/8packet
+
+ if (tx_power != Model.tx_power) // Keep transmit power updated
+ {
+ tx_power = Model.tx_power;
+ CC2500_SetPower(tx_power);
+ }
+}
+
+
+static u16 OMP_callback()
+{
+ u16 timeout = OMP_PACKET_PERIOD;
+ if (fine != (s8)Model.proto_opts[PROTOOPTS_FREQFINE])
+ {
+ fine = (s8)Model.proto_opts[PROTOOPTS_FREQFINE];
+ CC2500_WriteReg(CC2500_0C_FSCTRL0, fine);
+ }
+ switch (phase) {
+ case OMP_BIND:
+ if (bind_counter == 0)
+ {
+ PROTOCOL_SetBindState(0);
+ XN297L_SetTXAddr(rx_tx_addr, 5);
+ phase = OMP_DATA;
+ }
+ else
+ {
+ OMP_send_packet(1);
+ bind_counter--;
+ }
+ break;
+
+ case OMP_DATA:
+ OMP_send_packet(0);
+ phase = OMP_PACKET_SEND;
+ timeout = 1250;
+ tx_wait = 0;
+ break;
+
+ case OMP_PACKET_SEND:
+ if (CC2500_ReadReg(CC2500_35_MARCSTATE | CC2500_READ_BURST) == 0x13)
+ {
+ timeout = 50;
+ tx_wait += 50;
+ if (tx_wait > 1000)
+ {
+ phase = OMP_DATA;
+ timeout = OMP_PACKET_PERIOD-2250;
+ }
+ break;
+ }
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_SetTxRxMode(TXRX_OFF);
+ timeout = OMP_PACKET_PERIOD-1250-tx_wait;
+ phase = OMP_DATA;
+
+ if (Model.proto_opts[PROTOOPTS_TELEMETRY])
+ break;
+
+ switch (telm_req)
+ {
+ case 0:
+ CC2500_SetTxRxMode(RX_EN);
+ CC2500_Strobe(CC2500_SFRX);
+ CC2500_Strobe(CC2500_SRX);
+ break;
+ case 1:
+ omp_update_telemetry();
+ timeout -= 50;
+ break;
+ default:
+ break;
+ }
+ break;
+ }
+
+ return timeout;
+}
+
+static void OMP_init()
+{
+ // setup cc2500 for xn297L@250kbps emulation, scrambled, crc enabled
+ XN297L_Configure(XN297L_SCRAMBLED, XN297L_CRC, OMP_PACKET_SIZE+10); // packet_size + 5byte address + 2 byte pcf + 2byte crc + 1byte preamble
+ calibrate_rf_chans();
+ CC2500_SetPower(tx_power);
+}
+
+static void OMP_initialize_txid()
+{
+ u32 lfsr = 0xb2c54a2ful;
+ u8 i, j;
+
+ if (Model.fixed_id) {
+ for (i = 0, j = 0; i < sizeof(Model.fixed_id); ++i, j += 8)
+ rand32_r(&lfsr, (Model.fixed_id >> j) & 0xff);
+ }
+ // Pump zero bytes for LFSR to diverge more
+ for (i = 0; i < sizeof(lfsr); ++i) rand32_r(&lfsr, 0);
+
+ for (i=0, j=0; i < 4; i++, j+=8)
+ rx_tx_addr[i] = (lfsr >> j) & 0xff;
+
+ rand32_r(&lfsr, 0);
+ rx_tx_addr[4] = lfsr & 0xff;
+ // channels
+ calc_fh_channels(OMP_NUM_RF_CHANNELS);
+}
+
+static void initialize(u8 bind)
+{
+ CLOCK_StopTimer();
+ OMP_initialize_txid();
+
+ tx_power = Model.tx_power;
+ OMP_init();
+
+ fine = (s8)Model.proto_opts[PROTOOPTS_FREQFINE];
+ CC2500_WriteReg(CC2500_0C_FSCTRL0, fine);
+
+ if (bind)
+ {
+ bind_counter = OMP_BIND_COUNT;
+ PROTOCOL_SetBindState(OMP_BIND_COUNT * OMP_PACKET_PERIOD / 1000);
+ phase = OMP_BIND;
+ XN297L_SetTXAddr((u8*)"FLPBD", OMP_ADDR_LEN);
+ XN297L_SetChannel(OMP_RF_BIND_CHANNEL);
+ CC2500_Strobe(CC2500_SCAL);
+ usleep(900);
+ CC2500_Strobe(CC2500_SIDLE);
+ memset(packet, 0x00, OMP_PACKET_SIZE);
+ memcpy(packet, "BND", 3);
+ memcpy(&packet[3], rx_tx_addr, 5);
+ memcpy(&packet[8], hopping_frequency, 8);
+ }
+ else
+ {
+ XN297L_SetTXAddr(rx_tx_addr, OMP_ADDR_LEN);
+ XN297L_SetRXAddr(rx_tx_addr, OMP_ADDR_LEN);
+ phase = OMP_DATA;
+ }
+
+ CLOCK_StartTimer(OMP_PACKET_PERIOD, OMP_callback);
+}
+
+uintptr_t OMP_Cmds(enum ProtoCmds cmd)
+{
+ switch (cmd) {
+ case PROTOCMD_INIT: initialize(0); return 0;
+ case PROTOCMD_DEINIT:
+ case PROTOCMD_RESET:
+ CLOCK_StopTimer();
+ return (CC2500_Reset() ? 1 : -1);
+ case PROTOCMD_CHECK_AUTOBIND: return 0;
+ case PROTOCMD_BIND: initialize(1); return 0;
+ case PROTOCMD_NUMCHAN: return 7;
+ case PROTOCMD_DEFAULT_NUMCHAN: return 7;
+ case PROTOCMD_CURRENT_ID: return Model.fixed_id;
+ case PROTOCMD_GETOPTIONS: return (uintptr_t)omp_opts;
+ case PROTOCMD_TELEMETRYSTATE:
+ return (Model.proto_opts[PROTOOPTS_TELEMETRY] != TELEM_OFF ? PROTO_TELEM_ON : PROTO_TELEM_OFF);
+ case PROTOCMD_TELEMETRYTYPE:
+ return TELEM_DEVO;
+ case PROTOCMD_CHANNELMAP: return AETRG;
+ default: break;
+ }
+ return 0;
+}
+
+#endif
diff --git a/src/protocol/protocol.h b/src/protocol/protocol.h
index b33a8d66e0..75bf177822 100644
--- a/src/protocol/protocol.h
+++ b/src/protocol/protocol.h
@@ -30,6 +30,7 @@ PROTODEF(PROTOCOL_SFHSS, CC2500, AETRG, SFHSS_Cmds, "S-FHSS")
PROTODEF(PROTOCOL_CORONA, CC2500, AETRG, Corona_Cmds, "Corona")
PROTODEF(PROTOCOL_HITEC, CC2500, AETRG, Hitec_Cmds, "Hitec")
PROTODEF(PROTOCOL_E010, CC2500, AETRG, E010_Cmds, "E010")
+PROTODEF(PROTOCOL_OMP, CC2500, AETRG, OMP_Cmds, "OMP")
#endif //PROTO_HAS_CC2500
#ifdef PROTO_HAS_NRF24L01
PROTODEF(PROTOCOL_V202, NRF24L01, AETRG, V202_Cmds, "V202")
diff --git a/src/protocol/spi/cc2500.c b/src/protocol/spi/cc2500.c
index ae6ec9f1f3..813515d3c2 100644
--- a/src/protocol/spi/cc2500.c
+++ b/src/protocol/spi/cc2500.c
@@ -135,4 +135,389 @@ int CC2500_Reset()
CC2500_SetTxRxMode(TXRX_OFF);
return CC2500_ReadReg(CC2500_0E_FREQ1) == 0xC4;
}
+
+// XN297 EMU
+
+u8 xn297_crc;
+u8 xn297_scramble_enabled;
+u8 xn297_addr_len;
+u8 xn297_tx_addr[5];
+u8 xn297_rx_addr[5];
+
+void XN297L_Configure(u8 scramble_en, u8 crc_en, u8 cc2500_packet_len)
+{
+ // Address Config = check for (0x55)
+ // Base Frequency = 2400
+ // CRC Autoflush = false
+ // CRC Enable = false
+ // Carrier Frequency = 2400
+ // Channel Number = 0
+ // Channel Spacing = 333.251953
+ // Data Format = Normal mode
+ // Data Rate = 249.939
+ // Deviation = 126.953125
+ // Device Address = 0
+ // Manchester Enable = false
+ // Modulated = true
+ // Modulation Format = GFSK
+ // Packet Length = fix len
+ // Packet Length Mode = Variable packet length mode. Packet length configured by the first byte after sync word
+ // Preamble Count = 1
+ // RX Filter BW = 464kHz
+ // !!! channel filter bandwidth should be selected so that the signal bandwidth occupies at most 80% of the channel filter bandwidth.
+ // The channel centre tolerance due to crystal accuracy should also be subtracted from the signal bandwidth.
+ // Sync Word Qualifier Mode = preamble (0x71 0x0F)
+ // TX Power = 0
+ // Whitening = false
+
+ CC2500_Reset();
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_WriteReg(CC2500_04_SYNC1, 0x71); // Sync word, high byte (Sync word = 0x71,0x0F,address=0x55)
+ CC2500_WriteReg(CC2500_05_SYNC0, 0x0F); // Sync word, low byte
+ CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x05); // Packet Automation Control, address check true auto append RSSI & LQI
+ CC2500_WriteReg(CC2500_06_PKTLEN, cc2500_packet_len); // Packet len, fix packet len
+ CC2500_WriteReg(CC2500_08_PKTCTRL0, 0x00); // Packet Automation Control, fix packet len
+ CC2500_WriteReg(CC2500_09_ADDR, 0x55); // Set addr to 0x55 (Sync word = 0x71,0x0F,address=0x55)
+ CC2500_WriteReg(CC2500_0B_FSCTRL1, 0x0A); // Frequency Synthesizer Control (IF Frequency)
+ CC2500_WriteReg(CC2500_0C_FSCTRL0, 0x00); // Frequency Synthesizer Control
+ CC2500_WriteReg(CC2500_0D_FREQ2, 0x5C); // Frequency Control Word, High Byte
+ CC2500_WriteReg(CC2500_0E_FREQ1, 0x4E); // Frequency Control Word, Middle Byte
+ CC2500_WriteReg(CC2500_0F_FREQ0, 0xC5); // Frequency Control Word, Low Byte
+ CC2500_WriteReg(CC2500_10_MDMCFG4, 0x3D); // Modem Configuration Set to 406kHz BW filter
+ CC2500_WriteReg(CC2500_11_MDMCFG3, 0x3B); // Modem Configuration
+ CC2500_WriteReg(CC2500_12_MDMCFG2, 0x12); // Modem Configuration 16/16 Sync Word detect
+ CC2500_WriteReg(CC2500_13_MDMCFG1, 0x03); // Modem Configuration
+ CC2500_WriteReg(CC2500_14_MDMCFG0, 0xA4); // Modem Configuration
+ CC2500_WriteReg(CC2500_15_DEVIATN, 0x62); // Modem Deviation Setting
+ // CC2500_WriteReg(CC2500_16_MCSM2, 0x07); // Main Radio Control State Machine Configuration
+ // CC2500_WriteReg(CC2500_17_MCSM1, 0x30); // Main Radio Control State Machine Configuration
+ CC2500_WriteReg(CC2500_18_MCSM0, 0x28); // Main Radio Control State Machine Configuration
+ CC2500_WriteReg(CC2500_19_FOCCFG, 0x1D); // Frequency Offset Compensation Configuration
+ CC2500_WriteReg(CC2500_1A_BSCFG, 0x1C); // Bit Synchronization Configuration
+ CC2500_WriteReg(CC2500_1B_AGCCTRL2, 0xC7); // AGC Control
+ CC2500_WriteReg(CC2500_1C_AGCCTRL1, 0x00); // AGC Control
+ CC2500_WriteReg(CC2500_1D_AGCCTRL0, 0xB0); // AGC Control
+ CC2500_WriteReg(CC2500_21_FREND1, 0xB6); // Front End RX Configuration
+ CC2500_WriteReg(CC2500_23_FSCAL3, 0xEA); // Frequency Synthesizer Calibration
+ CC2500_WriteReg(CC2500_25_FSCAL1, 0x00); // Frequency Synthesizer Calibration
+ CC2500_WriteReg(CC2500_26_FSCAL0, 0x11); // Frequency Synthesizer Calibration
+
+ XN297L_SetScrambledMode(scramble_en);
+ xn297_crc = crc_en;
+}
+
+void XN297L_SetTXAddr(const u8* addr, u8 len)
+{
+ if (len > 5) len = 5;
+ if (len < 3) len = 3;
+ xn297_addr_len = len;
+ memcpy(xn297_tx_addr, addr, len);
+}
+
+void XN297L_SetRXAddr(const u8* addr, u8 len)
+{
+ if (len > 5) len = 5;
+ if (len < 3) len = 3;
+ xn297_addr_len = len;
+ memcpy(xn297_rx_addr, addr, len);
+}
+
+void XN297L_SetChannel(u8 ch)
+{
+ if (ch > 85)
+ ch = 85;
+ // channel spacing is 333.25 MHz
+ CC2500_WriteReg(CC2500_0A_CHANNR, ch * 3);
+}
+
+const u8 xn297_scramble[] = {
+ 0xE3, 0xB1, 0x4B, 0xEA, 0x85, 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, 0xAA, 0x16, 0xF3, 0x95 };
+
+const u16 xn297_crc_xorout_scrambled[] = {
+ 0x0000, 0x3448, 0x9BA7, 0x8BBB, 0x85E1, 0x3E8C,
+ 0x451E, 0x18E6, 0x6B24, 0xE7AB, 0x3828, 0x814B,
+ 0xD461, 0xF494, 0x2503, 0x691D, 0xFE8B, 0x9BA7,
+ 0x8B17, 0x2920, 0x8B5F, 0x61B1, 0xD391, 0x7401,
+ 0x2138, 0x129F, 0xB3A0, 0x2988, 0x23CA, 0xC0CB,
+ 0x0C6C, 0xB329, 0xA0A1, 0x0A16, 0xA9D0 };
+
+const u16 xn297_crc_xorout[] = {
+ 0x0000, 0x3D5F, 0xA6F1, 0x3A23, 0xAA16, 0x1CAF,
+ 0x62B2, 0xE0EB, 0x0821, 0xBE07, 0x5F1A, 0xAF15,
+ 0x4F0A, 0xAD24, 0x5E48, 0xED34, 0x068C, 0xF2C9,
+ 0x1852, 0xDF36, 0x129D, 0xB17C, 0xD5F5, 0x70D7,
+ 0xB798, 0x5133, 0x67DB, 0xD94E, 0x0A5B, 0xE445,
+ 0xE6A5, 0x26E7, 0xBDAB, 0xC379, 0x8E20 };
+
+const u16 xn297_crc_xorout_scrambled_enhanced[] = {
+ 0x0000, 0x7EBF, 0x3ECE, 0x07A4, 0xCA52, 0x343B,
+ 0x53F8, 0x8CD0, 0x9EAC, 0xD0C0, 0x150D, 0x5186,
+ 0xD251, 0xA46F, 0x8435, 0xFA2E, 0x7EBD, 0x3C7D,
+ 0x94E0, 0x3D5F, 0xA685, 0x4E47, 0xF045, 0xB483,
+ 0x7A1F, 0xDEA2, 0x9642, 0xBF4B, 0x032F, 0x01D2,
+ 0xDC86, 0x92A5, 0x183A, 0xB760, 0xA953 };
+
+const u16 xn297_crc_xorout_enhanced[] = {
+ 0x0000, 0x8BE6, 0xD8EC, 0xB87A, 0x42DC, 0xAA89,
+ 0x83AF, 0x10E4, 0xE83E, 0x5C29, 0xAC76, 0x1C69,
+ 0xA4B2, 0x5961, 0xB4D3, 0x2A50, 0xCB27, 0x5128,
+ 0x7CDB, 0x7A14, 0xD5D2, 0x57D7, 0xE31D, 0xCE42,
+ 0x648D, 0xBF2D, 0x653B, 0x190C, 0x9117, 0x9A97,
+ 0xABFC, 0xE68E, 0x0DE7, 0x28A2, 0x1965 };
+
+
+#if defined(__GNUC__) && defined(__ARM_ARCH_ISA_THUMB) && (__ARM_ARCH_ISA_THUMB == 2)
+// rbit instruction works on cortex m3
+u32 __RBIT_(u32 in)
+{
+ u32 out = 0;
+ __asm volatile ("rbit %0, %1" : "=r" (out) : "r" (in) );
+ return(out);
+}
+
+u8 bit_reverse(u8 b_in)
+{
+ return __RBIT_( (unsigned int) b_in)>>24;
+}
+#else
+u8 bit_reverse(u8 b_in)
+{
+ u8 b_out = 0;
+ for (int i = 0; i < 8; ++i) {
+ b_out = (b_out << 1) | (b_in & 1);
+ b_in >>= 1;
+ }
+ return b_out;
+}
+#endif
+
+const u16 polynomial = 0x1021;
+
+u16 crc16_update(u16 crc, u8 a, u8 bits)
+{
+ crc ^= a << 8;
+ while (bits--) {
+ if (crc & 0x8000) {
+ crc = (crc << 1) ^ polynomial;
+ } else {
+ crc = crc << 1;
+ }
+ }
+ return crc;
+}
+
+void XN297L_SetScrambledMode(const u8 mode)
+{
+ xn297_scramble_enabled = mode;
+}
+
+u8 _xn297l_write_payload(const u8* msg, u8 len, u8* out)
+{
+ u8 last = 0;
+ u8 i;
+
+ for (i = 0; i < xn297_addr_len; ++i) {
+ out[last] = xn297_tx_addr[xn297_addr_len-i-1];
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[i];
+ last++;
+ }
+
+ for (i = 0; i < len; ++i) {
+ // bit-reverse bytes in packet
+ u8 b_out = bit_reverse(msg[i]);
+ out[last] = b_out;
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[xn297_addr_len+i];
+ last++;
+ }
+
+ if (xn297_crc) {
+ u16 crc = 0xb5d2;
+ for (i = 0; i < last; ++i) {
+ crc = crc16_update(crc, out[i], 8);
+ }
+ if (xn297_scramble_enabled)
+ crc ^= xn297_crc_xorout_scrambled[xn297_addr_len - 3 + len];
+ else
+ crc ^= xn297_crc_xorout[xn297_addr_len - 3 + len];
+ out[last++] = crc >> 8;
+ out[last++] = crc & 0xff;
+ }
+ return last;
+}
+
+u8 _xn297l_write_enhancedpayload(const u8* msg, u8 len, u8* out, u8 noack)
+{
+ u8 scramble_index = 0;
+ u8 last = 0;
+ static u8 pid = 0;
+ u8 i;
+ // address
+ for ( i = 0; i < xn297_addr_len; ++i ) {
+ out[last] = xn297_tx_addr[xn297_addr_len-i-1];
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[scramble_index++];
+ last++;
+ }
+ // pcf
+ out[last] = (len << 1) | (pid>>1);
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[scramble_index++];
+ last++;
+ out[last] = (pid << 7) | (noack << 6);
+ // payload
+ out[last]|= bit_reverse(msg[0]) >> 2; // first 6 bit of payload
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[scramble_index++];
+
+ for ( i = 0; i < len-1; ++i ) {
+ last++;
+ out[last] = (bit_reverse(msg[i]) << 6) | (bit_reverse(msg[i+1]) >> 2);
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[scramble_index++];
+ }
+
+ last++;
+ out[last] = bit_reverse(msg[len-1]) << 6; // last 2 bit of payload
+ if (xn297_scramble_enabled)
+ out[last] ^= xn297_scramble[scramble_index++] & 0xc0;
+
+ // crc
+ if (xn297_crc) {
+ u16 crc = 0xb5d2;
+ for ( i = 0; i < last; ++i)
+ crc = crc16_update(crc, out[i], 8);
+ crc = crc16_update(crc, out[last] & 0xc0, 2);
+
+ if (xn297_scramble_enabled)
+ crc ^= xn297_crc_xorout_scrambled_enhanced[xn297_addr_len - 3+len];
+ else
+ crc ^= xn297_crc_xorout_enhanced[xn297_addr_len - 3 + len];
+
+ out[last++] |= (crc >> 8) >> 2;
+ out[last++] = ((crc >> 8) << 6) | ((crc & 0xff) >> 2);
+ out[last++] = (crc & 0xff) << 6;
+ }
+ pid++;
+ pid &= 3;
+ return last;
+}
+
+void XN297L_WritePayload(u8* msg, u8 len)
+{
+ u8 buf[32];
+ u8 count = _xn297l_write_payload(msg, len, buf);
+ // halt Tx/Rx
+ CC2500_Strobe(CC2500_SIDLE);
+ // flush tx FIFO
+ CC2500_Strobe(CC2500_SFTX);
+ // set cc2500 packet length
+ // CC2500_WriteReg(CC2500_3F_TXFIFO, count+3);
+ // XN297L preamble
+ CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, (u8*)"\x55", 1); // preamble was writen in cc2500 sync word & preamble.
+ // packet
+ CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, buf, count);
+ // transmit
+ CC2500_Strobe(CC2500_STX);
+}
+
+void XN297L_WriteEnhancedPayload(u8* msg, u8 len, u8 noack)
+{
+ u8 buf[32];
+ u8 count = _xn297l_write_enhancedpayload(msg, len, buf, noack);
+ // halt Tx/Rx
+ CC2500_Strobe(CC2500_SIDLE);
+ // flush tx FIFO
+ CC2500_Strobe(CC2500_SFTX);
+ // set cc2500 packet length
+ // CC2500_WriteReg(CC2500_3F_TXFIFO, count + 3);
+ // XN297L preamble
+ CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, (u8*)"\x55", 1); // preamble was writen in cc2500 sync word & preamble.
+ // packet
+ CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, buf, count);
+ // transmit
+ CC2500_Strobe(CC2500_STX);
+}
+
+u8 XN297L_ReadPayload(u8* msg, u8 len) // just what it should be, no tested yeah.
+{ // cc2500 receive 1byte address(0x55) + 5byte xnl297 address + 16byte payload +2byte crc
+ u8 buf[32];
+ u8 rx_fifo = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
+ CC2500_ReadData(buf, rx_fifo);
+ // Decode payload
+ for (u8 i = 0; i < len; i++)
+ {
+ u8 b_in = buf[i+6];
+ if (xn297_scramble_enabled)
+ b_in ^= xn297_scramble[i+xn297_addr_len];
+ msg[i] = bit_reverse(b_in);
+ }
+ if (!xn297_crc)
+ return 1; // No CRC so OK by default...
+
+ // Calculate CRC
+ u16 crc = 0xb5d2;
+ // process address
+ for (u8 i = 0; i < xn297_addr_len; ++i)
+ {
+ u8 b_in = xn297_rx_addr[xn297_addr_len-i-1];
+ if (xn297_scramble_enabled)
+ b_in ^= xn297_scramble[i];
+ crc = crc16_update(crc, b_in, 8);
+ }
+ // process payload
+ for (u8 i = 0; i < len; ++i)
+ crc = crc16_update(crc, buf[i+6], 8);
+ // xorout
+ if (xn297_scramble_enabled)
+ crc ^= xn297_crc_xorout_scrambled[xn297_addr_len - 3 + len];
+ else
+ crc ^= xn297_crc_xorout[xn297_addr_len - 3 + len];
+ // test
+ if ((crc >> 8) == buf[len+6] && (crc & 0xff) == buf[len+7])
+ return 1; // CRC OK
+ return 0; // CRC NOK
+}
+
+u8 XN297L_ReadEnhancedPayload(u8* msg, u8 len)
+{ // cc2500 receive 1byte preamble(0x55) + 5byte xnl297 address + 2byte pcf + 16byte payload +2byte crc +2byte rx_status
+ u8 buffer[32];
+ u8 pcf_size; // pcf payload size
+ CC2500_ReadData(buffer, len);
+ len = len -2; // 2byte append rx_status
+ pcf_size = buffer[6]; // 0x55+5byte addr
+ if (xn297_scramble_enabled)
+ pcf_size ^= xn297_scramble[xn297_addr_len];
+ pcf_size = pcf_size >> 1;
+ for (u8 i=0; i < len-10; i++) // no include preamble(1byte) xnl297 address(5byte) PCF(2byte) crc(2byte)
+ {
+ msg[i] = bit_reverse((buffer[i+7] << 2) | (buffer[i+8] >> 6));
+ if (xn297_scramble_enabled)
+ msg[i] ^= bit_reverse((xn297_scramble[xn297_addr_len+i+1] << 2) |
+ (xn297_scramble[xn297_addr_len+i+2] >> 6));
+ }
+
+ if (!xn297_crc)
+ return pcf_size; // No CRC so OK by default...
+
+ // Calculate CRC
+ u16 crc = 0xb5d2;
+ for (u8 i = 0; i < len-4; ++i)
+ crc = crc16_update(crc, buffer[i+1], 8);
+ crc = crc16_update(crc, buffer[len-3] & 0xc0, 2);
+ // xorout
+ if (xn297_scramble_enabled)
+ crc ^= xn297_crc_xorout_scrambled_enhanced[xn297_addr_len-3+16];
+
+ u16 crcxored = (buffer[len-3] << 10)|(buffer[len-2] << 2)|(buffer[len-1] >> 6);
+ if (crc == crcxored)
+ return buffer[len]; // CRC OK, return RSSI
+ return 0; // CRC NOK
+}
+
#endif
diff --git a/src/protocol/spi/nrf24l01.c b/src/protocol/spi/nrf24l01.c
index 78be9c6485..ae58c16b2a 100644
--- a/src/protocol/spi/nrf24l01.c
+++ b/src/protocol/spi/nrf24l01.c
@@ -273,28 +273,11 @@ static u8 xn297_tx_addr[5];
static u8 xn297_rx_addr[5];
static u8 xn297_crc = 0;
-const uint8_t xn297_scramble[] = {
- 0xe3, 0xb1, 0x4b, 0xea, 0x85, 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};
-
-const u16 xn297_crc_xorout_scrambled[] = {
- 0x0000, 0x3448, 0x9BA7, 0x8BBB, 0x85E1, 0x3E8C,
- 0x451E, 0x18E6, 0x6B24, 0xE7AB, 0x3828, 0x814B,
- 0xD461, 0xF494, 0x2503, 0x691D, 0xFE8B, 0x9BA7,
- 0x8B17, 0x2920, 0x8B5F, 0x61B1, 0xD391, 0x7401,
- 0x2138, 0x129F, 0xB3A0, 0x2988};
-
-const u16 xn297_crc_xorout[] = {
- 0x0000, 0x3d5f, 0xa6f1, 0x3a23, 0xaa16, 0x1caf,
- 0x62b2, 0xe0eb, 0x0821, 0xbe07, 0x5f1a, 0xaf15,
- 0x4f0a, 0xad24, 0x5e48, 0xed34, 0x068c, 0xf2c9,
- 0x1852, 0xdf36, 0x129d, 0xb17c, 0xd5f5, 0x70d7,
- 0xb798, 0x5133, 0x67db, 0xd94e};
-
+extern const u8 xn297_scramble[];
+extern const u16 xn297_crc_xorout_scrambled[];
+extern const u16 xn297_crc_xorout[];
+/*
#if defined(__GNUC__) && defined(__ARM_ARCH_ISA_THUMB) && (__ARM_ARCH_ISA_THUMB==2)
// rbit instruction works on cortex m3
uint32_t __RBIT_(uint32_t in)
@@ -335,9 +318,11 @@ u16 crc16_update(u16 crc, u8 a, u8 bits)
}
return crc;
}
+*/
+static const uint16_t initial = 0xb5d2;
-void XN297_SetTXAddr(const u8* addr, int len)
+void XN297_SetTXAddr(const u8* addr, u8 len)
{
if (len > 5) len = 5;
if (len < 3) len = 3;
@@ -359,7 +344,7 @@ void XN297_SetTXAddr(const u8* addr, int len)
}
-void XN297_SetRXAddr(const u8* addr, int len)
+void XN297_SetRXAddr(const u8* addr, u8 len)
{
if (len > 5) len = 5;
if (len < 3) len = 3;
@@ -433,7 +418,7 @@ u8 XN297_WritePayload(u8* msg, int len)
return res;
}
-u8 XN297_WriteEnhancedPayload(u8* msg, int len, int noack, u16 crc_xorout)
+u8 XN297_WriteEnhancedPayload(u8* msg, u8 len, u8 noack, u16 crc_xorout)
{
u8 packet[32];
u8 scramble_index=0;
@@ -501,35 +486,95 @@ u8 XN297_WriteEnhancedPayload(u8* msg, int len, int noack, u16 crc_xorout)
return res;
}
-u8 XN297_ReadPayload(u8* msg, int len)
-{
- // TODO: if xn297_crc==1, check CRC before filling *msg
- u8 res = NRF24L01_ReadPayload(msg, len);
- for(u8 i=0; i> 8) == buf[len] && (crc & 0xff) == buf[len+1])
+ return 1; // CRC OK
+ return 0; // CRC NOK
}
-u8 XN297_ReadEnhancedPayload(u8* msg, int len)
-{
+u8 XN297_ReadEnhancedPayload(u8* msg, u8 len)
+{ //!!! Don't forget do a +2 and if using CRC add +4 on any of the used NRF24L01_11_RX_PW_Px !!!
u8 buffer[32];
u8 pcf_size; // pcf payload size
- NRF24L01_ReadPayload(buffer, len+2); // pcf + payload
+ if (xn297_crc)
+ NRF24L01_ReadPayload(buffer, len+4); // Read pcf + payload + CRC
+ else
+ NRF24L01_ReadPayload(buffer, len+2); // Read pcf + payload
pcf_size = buffer[0];
- if(xn297_scramble_enabled)
+ if (xn297_scramble_enabled)
pcf_size ^= xn297_scramble[xn297_addr_len];
pcf_size = pcf_size >> 1;
- for(int i=0; i> 6));
- if(xn297_scramble_enabled)
+ if (xn297_scramble_enabled)
msg[i] ^= bit_reverse((xn297_scramble[xn297_addr_len+i+1] << 2) |
(xn297_scramble[xn297_addr_len+i+2] >> 6));
}
- return pcf_size;
+
+ if (!xn297_crc)
+ return pcf_size; // No CRC so OK by default...
+
+ // Calculate CRC
+ u16 crc = 0xb5d2;
+ // process address
+ for (u8 i = 0; i < xn297_addr_len; ++i)
+ {
+ u8 b_in = xn297_rx_addr[xn297_addr_len-i-1];
+ if (xn297_scramble_enabled)
+ b_in ^= xn297_scramble[i];
+ crc = crc16_update(crc, b_in, 8);
+ }
+ // process payload
+ for (u8 i = 0; i < len+1; ++i)
+ crc = crc16_update(crc, buffer[i], 8);
+ crc = crc16_update(crc, buffer[len+1] & 0xc0, 2);
+ // xorout
+ if (xn297_scramble_enabled)
+ crc ^= xn297_crc_xorout_scrambled_enhanced[xn297_addr_len-3+len];
+
+ u16 crcxored = (buffer[len+1] << 10)|(buffer[len+2] << 2)|(buffer[len+3] >> 6);
+ if (crc == crcxored)
+ return pcf_size; // CRC OK
+ return 0; // CRC NOK
}
+
//
// End of XN297 emulation
///////////////////////////
diff --git a/src/target/drivers/mcu/stm32/f1/pwr.h b/src/target/drivers/mcu/stm32/f1/pwr.h
index 2c52c8bad5..7ec1502cfe 100644
--- a/src/target/drivers/mcu/stm32/f1/pwr.h
+++ b/src/target/drivers/mcu/stm32/f1/pwr.h
@@ -5,7 +5,7 @@ static inline void _pwr_init()
{
SCB_VTOR = VECTOR_TABLE_LOCATION;
SCB_SCR &= ~SCB_SCR_SLEEPONEXIT; // sleep immediate on WFI
- rcc_clock_setup_in_hse_8mhz_out_72mhz();
+ rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE8_72MHZ]);
}
static inline void _pwr_shutdown()