diff --git a/Multiprotocol/CFlie_nrf24l01.ino b/Multiprotocol/CFlie_nrf24l01.ino
new file mode 100644
index 0000000..7c94a5e
--- /dev/null
+++ b/Multiprotocol/CFlie_nrf24l01.ino
@@ -0,0 +1,908 @@
+/*
+ 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.
+
+ Multiprotocol 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 Multiprotocol. If not, see .
+ */
+
+// Most of this code was ported from theseankelly's related DeviationTX work.
+
+#if defined(CFLIE_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define CFLIE_BIND_COUNT 60
+
+//=============================================================================
+// CRTP (Crazy RealTime Protocol) Implementation
+//=============================================================================
+
+// Port IDs
+enum {
+ CRTP_PORT_CONSOLE = 0x00,
+ CRTP_PORT_PARAM = 0x02,
+ CRTP_PORT_SETPOINT = 0x03,
+ CRTP_PORT_MEM = 0x04,
+ CRTP_PORT_LOG = 0x05,
+ CRTP_PORT_POSITION = 0x06,
+ CRTP_PORT_SETPOINT_GENERIC = 0x07,
+ CRTP_PORT_PLATFORM = 0x0D,
+ CRTP_PORT_LINK = 0x0F,
+};
+
+// Channel definitions for the LOG port
+enum {
+ CRTP_LOG_CHAN_TOC = 0x00,
+ CRTP_LOG_CHAN_SETTINGS = 0x01,
+ CRTP_LOG_CHAN_LOGDATA = 0x02,
+};
+
+// Command definitions for the LOG port's TOC channel
+enum {
+ CRTP_LOG_TOC_CMD_ELEMENT = 0x00,
+ CRTP_LOG_TOC_CMD_INFO = 0x01,
+};
+
+// Command definitions for the LOG port's CMD channel
+enum {
+ CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK = 0x00,
+ CRTP_LOG_SETTINGS_CMD_APPEND_BLOCK = 0x01,
+ CRTP_LOG_SETTINGS_CMD_DELETE_BLOCK = 0x02,
+ CRTP_LOG_SETTINGS_CMD_START_LOGGING = 0x03,
+ CRTP_LOG_SETTINGS_CMD_STOP_LOGGING = 0x04,
+ CRTP_LOG_SETTINGS_CMD_RESET_LOGGING = 0x05,
+};
+
+// Log variables types
+enum {
+ LOG_UINT8 = 0x01,
+ LOG_UINT16 = 0x02,
+ LOG_UINT32 = 0x03,
+ LOG_INT8 = 0x04,
+ LOG_INT16 = 0x05,
+ LOG_INT32 = 0x06,
+ LOG_FLOAT = 0x07,
+ LOG_FP16 = 0x08,
+};
+
+#define CFLIE_TELEM_LOG_BLOCK_ID 0x01
+#define CFLIE_TELEM_LOG_BLOCK_PERIOD_10MS 50 // 50*10 = 500ms
+
+// Setpoint type definitions for the generic setpoint channel
+enum {
+ CRTP_SETPOINT_GENERIC_STOP_TYPE = 0x00,
+ CRTP_SETPOINT_GENERIC_VELOCITY_WORLD_TYPE = 0x01,
+ CRTP_SETPOINT_GENERIC_Z_DISTANCE_TYPE = 0x02,
+ CRTP_SETPOINT_GENERIC_CPPM_EMU_TYPE = 0x03,
+};
+
+static inline uint8_t crtp_create_header(uint8_t port, uint8_t channel)
+{
+ return ((port)&0x0F)<<4 | (channel & 0x03);
+}
+
+//=============================================================================
+// End CRTP implementation
+//=============================================================================
+
+// Address size
+#define TX_ADDR_SIZE 5
+
+// Timeout for callback in uSec, 10ms=10000us for Crazyflie
+#define PACKET_PERIOD 10000
+
+#define MAX_PACKET_SIZE 32 // CRTP is 32 bytes
+
+// CPPM CRTP supports up to 10 aux channels but deviation only
+// supports a total of 12 channels. R,P,Y,T leaves 8 aux channels left
+#define MAX_CPPM_AUX_CHANNELS 8
+
+static uint8_t tx_payload_len = 0; // Length of the packet stored in tx_packet
+static uint8_t tx_packet[MAX_PACKET_SIZE]; // For writing Tx payloads
+static uint8_t rx_payload_len = 0; // Length of the packet stored in rx_packet
+static uint8_t rx_packet[MAX_PACKET_SIZE]; // For reading in ACK payloads
+
+static uint16_t cflie_counter;
+static uint32_t packet_counter;
+static uint8_t tx_power, data_rate, rf_channel;
+
+enum {
+ CFLIE_INIT_SEARCH = 0,
+ CFLIE_INIT_CRTP_LOG,
+ CFLIE_INIT_DATA,
+ CFLIE_SEARCH,
+ CFLIE_DATA
+};
+
+static uint8_t crtp_log_setup_state;
+enum {
+ CFLIE_CRTP_LOG_SETUP_STATE_INIT = 0,
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO,
+ CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_INFO,
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM,
+ CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_ITEM,
+ // It might be a good idea to add a state here
+ // to send the command to reset the logging engine
+ // to avoid log block ID conflicts. However, there
+ // is not a conflict with the current defaults in
+ // cfclient and I'd rather be able to log from the Tx
+ // and cfclient simultaneously
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK,
+ CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_CREATE_BLOCK,
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK,
+ CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_START_BLOCK,
+ CFLIE_CRTP_LOG_SETUP_STATE_COMPLETE,
+};
+
+// State variables for the crtp_log_setup_state_machine
+static uint8_t toc_size; // Size of the TOC read from the crazyflie
+static uint8_t next_toc_variable; // State variable keeping track of the next var to read
+static uint8_t vbat_var_id; // ID of the vbatMV variable
+static uint8_t extvbat_var_id; // ID of the extVbatMV variable
+static uint8_t rssi_var_id; // ID of the RSSI variable
+
+// Constants used for finding var IDs from the toc
+static const char* pm_group_name = "pm";
+static const char* vbat_var_name = "vbatMV";
+static const uint8_t vbat_var_type = LOG_UINT16;
+static const char* extvbat_var_name = "extVbatMV";
+static const uint8_t extvbat_var_type = LOG_UINT16;
+static const char* radio_group_name = "radio";
+static const char* rssi_var_name = "rssi";
+static const uint8_t rssi_var_type = LOG_UINT8;
+
+// Repurposing DSM Telemetry fields
+#define TELEM_CFLIE_INTERNAL_VBAT TELEM_DSM_FLOG_VOLT2 // Onboard voltage
+#define TELEM_CFLIE_EXTERNAL_VBAT TELEM_DSM_FLOG_VOLT1 // Voltage from external pin (BigQuad)
+#define TELEM_CFLIE_RSSI TELEM_DSM_FLOG_FADESA // Repurpose FADESA for RSSI
+
+enum {
+ PROTOOPTS_TELEMETRY = 0,
+ PROTOOPTS_CRTP_MODE = 1,
+ LAST_PROTO_OPT,
+};
+
+#define TELEM_OFF 0
+#define TELEM_ON_ACKPKT 1
+#define TELEM_ON_CRTPLOG 2
+
+#define CRTP_MODE_RPYT 0
+#define CRTP_MODE_CPPM 1
+
+// Bit vector from bit position
+#define BV(bit) (1 << bit)
+
+#define PACKET_CHKTIME 500 // time to wait if packet not yet acknowledged or timed out
+
+// Helper for sending a packet
+// Assumes packet data has been put in tx_packet
+// and tx_payload_len has been set correctly
+static void send_packet()
+{
+ // clear packet status bits and Tx/Rx FIFOs
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, (_BV(NRF24L01_07_TX_DS) | _BV(NRF24L01_07_MAX_RT)));
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+
+ // Transmit the payload
+ NRF24L01_WritePayload(tx_packet, tx_payload_len);
+
+ ++packet_counter;
+
+ // // Check and adjust transmission power. We do this after
+ // // transmission to not bother with timeout after power
+ // // settings change - we have plenty of time until next
+ // // packet.
+ // if (tx_power != Model.tx_power) {
+ // //Keep transmit power updated
+ // tx_power = Model.tx_power;
+ // NRF24L01_SetPower(tx_power);
+ // }
+ NRF24L01_SetPower(); // hack? not sure if required...
+}
+
+static uint16_t dbg_cnt = 0;
+static uint8_t packet_ack()
+{
+ if (++dbg_cnt > 50) {
+ // debugln("S: %02x\n", NRF24L01_ReadReg(NRF24L01_07_STATUS));
+ dbg_cnt = 0;
+ }
+ switch (NRF24L01_ReadReg(NRF24L01_07_STATUS) & (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT))) {
+ case BV(NRF24L01_07_TX_DS):
+ rx_payload_len = NRF24L01_GetDynamicPayloadSize();
+ if (rx_payload_len > MAX_PACKET_SIZE) {
+ rx_payload_len = MAX_PACKET_SIZE;
+ }
+ NRF24L01_ReadPayload(rx_packet, rx_payload_len);
+ return PKT_ACKED;
+ case BV(NRF24L01_07_MAX_RT):
+ return PKT_TIMEOUT;
+ }
+ return PKT_PENDING;
+}
+
+static void set_rate_channel(uint8_t rate, uint8_t channel)
+{
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, channel); // Defined by model id
+ NRF24L01_SetBitrate(rate); // Defined by model id
+}
+
+static void send_search_packet()
+{
+ uint8_t buf[1];
+ buf[0] = 0xff;
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT)));
+ NRF24L01_FlushTx();
+
+ if (rf_channel++ > 125) {
+ rf_channel = 0;
+ switch(data_rate) {
+ case NRF24L01_BR_250K:
+ data_rate = NRF24L01_BR_1M;
+ break;
+ case NRF24L01_BR_1M:
+ data_rate = NRF24L01_BR_2M;
+ break;
+ case NRF24L01_BR_2M:
+ data_rate = NRF24L01_BR_250K;
+ break;
+ }
+ }
+
+ set_rate_channel(data_rate, rf_channel);
+
+ NRF24L01_WritePayload(buf, sizeof(buf));
+
+ ++packet_counter;
+}
+
+// Frac 16.16
+#define FRAC_MANTISSA 16 // This means, not IEEE 754...
+#define FRAC_SCALE (1 << FRAC_MANTISSA)
+
+// Convert fractional 16.16 to float32
+static void frac2float(int32_t n, float* res)
+{
+ if (n == 0) {
+ *res = 0.0;
+ return;
+ }
+ uint32_t m = n < 0 ? -n : n; // Figure out mantissa?
+ int i;
+ for (i = (31-FRAC_MANTISSA); (m & 0x80000000) == 0; i--, m <<= 1);
+ m <<= 1; // Clear implicit leftmost 1
+ m >>= 9;
+ uint32_t e = 127 + i;
+ if (n < 0) m |= 0x80000000;
+ m |= e << 23;
+ *((uint32_t *) res) = m;
+}
+
+static void send_crtp_rpyt_packet()
+{
+ int32_t f_roll;
+ int32_t f_pitch;
+ int32_t f_yaw;
+ uint16_t thrust;
+
+ uint16_t val;
+
+ struct CommanderPacketRPYT
+ {
+ float roll;
+ float pitch;
+ float yaw;
+ uint16_t thrust;
+ }__attribute__((packed)) cpkt;
+
+ // Channels in AETR order
+ // Roll, aka aileron, float +- 50.0 in degrees
+ // float roll = -(float) Channels[0]*50.0/10000;
+ val = convert_channel_16b_limit(AILERON, -10000, 10000);
+ // f_roll = -Channels[0] * FRAC_SCALE / (10000 / 50);
+ f_roll = val * FRAC_SCALE / (10000 / 50);
+
+ frac2float(f_roll, &cpkt.roll); // TODO: Remove this and use the correct Mode switch below...
+ // debugln("Roll: raw, converted: %d, %d, %d, %0.2f", Channel_data[AILERON], val, f_roll, cpkt.roll);
+
+ // Pitch, aka elevator, float +- 50.0 degrees
+ //float pitch = -(float) Channels[1]*50.0/10000;
+ val = convert_channel_16b_limit(ELEVATOR, -10000, 10000);
+ // f_pitch = -Channels[1] * FRAC_SCALE / (10000 / 50);
+ f_pitch = -val * FRAC_SCALE / (10000 / 50);
+
+ frac2float(f_pitch, &cpkt.pitch); // TODO: Remove this and use the correct Mode switch below...
+ // debugln("Pitch: raw, converted: %d, %d, %d, %0.2f", Channel_data[ELEVATOR], val, f_pitch, cpkt.pitch);
+
+ // Thrust, aka throttle 0..65535, working range 5535..65535
+ // Android Crazyflie app puts out a throttle range of 0-80%: 0..52000
+ thrust = convert_channel_16b_limit(THROTTLE, 0, 32767) * 2;
+
+ // Crazyflie needs zero thrust to unlock
+ if (thrust < 900)
+ cpkt.thrust = 0;
+ else
+ cpkt.thrust = thrust;
+
+ // debugln("Thrust: raw, converted: %d, %u, %u", Channel_data[THROTTLE], thrust, cpkt.thrust);
+
+ // Yaw, aka rudder, float +- 400.0 deg/s
+ // float yaw = -(float) Channels[3]*400.0/10000;
+ val = convert_channel_16b_limit(RUDDER, -10000, 10000);
+ // f_yaw = - Channels[3] * FRAC_SCALE / (10000 / 400);
+ f_yaw = val * FRAC_SCALE / (10000 / 400);
+ frac2float(f_yaw, &cpkt.yaw);
+
+ // debugln("Yaw: raw, converted: %d, %d, %d, %0.2f", Channel_data[RUDDER], val, f_yaw, cpkt.yaw);
+
+ // Switch on/off?
+ // TODO: Get X or + mode working again:
+ // if (Channels[4] >= 0) {
+ // frac2float(f_roll, &cpkt.roll);
+ // frac2float(f_pitch, &cpkt.pitch);
+ // } else {
+ // // Rotate 45 degrees going from X to + mode or opposite.
+ // // 181 / 256 = 0.70703125 ~= sqrt(2) / 2
+ // int32_t f_x_roll = (f_roll + f_pitch) * 181 / 256;
+ // frac2float(f_x_roll, &cpkt.roll);
+ // int32_t f_x_pitch = (f_pitch - f_roll) * 181 / 256;
+ // frac2float(f_x_pitch, &cpkt.pitch);
+ // }
+
+ // Construct and send packet
+ tx_packet[0] = crtp_create_header(CRTP_PORT_SETPOINT, 0); // Commander packet to channel 0
+ memcpy(&tx_packet[1], (char*) &cpkt, sizeof(cpkt));
+ tx_payload_len = 1 + sizeof(cpkt);
+ send_packet();
+}
+
+static void send_crtp_cppm_emu_packet()
+{
+ struct CommanderPacketCppmEmu {
+ struct {
+ uint8_t numAuxChannels : 4; // Set to 0 through MAX_AUX_RC_CHANNELS
+ uint8_t reserved : 4;
+ } hdr;
+ uint16_t channelRoll;
+ uint16_t channelPitch;
+ uint16_t channelYaw;
+ uint16_t channelThrust;
+ uint16_t channelAux[10];
+ } __attribute__((packed)) cpkt;
+
+ // To emulate PWM RC signals, rescale channels from (-10000,10000) to (1000,2000)
+ // This is done by dividing by 20 to get a total range of 1000 (-500,500)
+ // and then adding 1500 to to rebase the offset
+#define RESCALE_RC_CHANNEL_TO_PWM(chan) ((chan / 20) + 1500)
+
+ // Make sure the number of aux channels in use is capped to MAX_CPPM_AUX_CHANNELS
+ // uint8_t numAuxChannels = Model.num_channels - 4;
+ uint8_t numAuxChannels = 2; // TODO: Figure this out correctly
+ if(numAuxChannels > MAX_CPPM_AUX_CHANNELS)
+ {
+ numAuxChannels = MAX_CPPM_AUX_CHANNELS;
+ }
+
+ cpkt.hdr.numAuxChannels = numAuxChannels;
+
+ // Remap AETR to AERT (RPYT)
+ cpkt.channelRoll = convert_channel_16b_limit(AILERON,1000,2000);
+ cpkt.channelPitch = convert_channel_16b_limit(ELEVATOR,1000,2000);
+ // Note: T & R Swapped:
+ cpkt.channelYaw = convert_channel_16b_limit(RUDDER, 1000, 2000);
+ cpkt.channelThrust = convert_channel_16b_limit(THROTTLE, 1000, 2000);
+
+ // Rescale the rest of the aux channels - RC channel 4 and up
+ for (uint8_t i = 4; i < 14; i++)
+ {
+ cpkt.channelAux[i] = convert_channel_16b_limit(i, 1000, 2000);
+ }
+
+ // Total size of the commander packet is a 1-byte header, 4 2-byte channels and
+ // a variable number of 2-byte auxiliary channels
+ uint8_t commanderPacketSize = 1 + 8 + (2*numAuxChannels);
+
+ // Construct and send packet
+ tx_packet[0] = crtp_create_header(CRTP_PORT_SETPOINT_GENERIC, 0); // Generic setpoint packet to channel 0
+ tx_packet[1] = CRTP_SETPOINT_GENERIC_CPPM_EMU_TYPE;
+
+ // Copy the header (1) plus 4 2-byte channels (8) plus whatever number of 2-byte aux channels are in use
+ memcpy(&tx_packet[2], (char*)&cpkt, commanderPacketSize); // Why not use sizeof(cpkt) here??
+ tx_payload_len = 2 + commanderPacketSize; // CRTP header, commander type, and packet
+ send_packet();
+}
+
+static void send_cmd_packet()
+{
+ // TODO: Fix this so we can actually configure the packet type
+ // switch(Model.proto_opts[PROTOOPTS_CRTP_MODE])
+ // {
+ // case CRTP_MODE_CPPM:
+ // send_crtp_cppm_emu_packet();
+ // break;
+ // case CRTP_MODE_RPYT:
+ // send_crtp_rpyt_packet();
+ // break;
+ // default:
+ // send_crtp_rpyt_packet();
+ // }
+
+ // send_crtp_cppm_emu_packet(); // oh maAAAn
+ send_crtp_rpyt_packet();
+}
+
+// State machine for setting up CRTP logging
+// returns 1 when the state machine has completed, 0 otherwise
+static uint8_t crtp_log_setup_state_machine()
+{
+ uint8_t state_machine_completed = 0;
+ // A note on the design of this state machine:
+ //
+ // Responses from the crazyflie come in the form of ACK payloads.
+ // There is no retry logic associated with ACK payloads, so it is possible
+ // to miss a response from the crazyflie. To avoid this, the request
+ // packet must be re-sent until the expected response is received. However,
+ // re-sending the same request generates another response in the crazyflie
+ // Rx queue, which can produce large backlogs of duplicate responses.
+ //
+ // To avoid this backlog but still guard against dropped ACK payloads,
+ // transmit cmd packets (which don't generate responses themselves)
+ // until an empty ACK payload is received (the crazyflie alternates between
+ // 0xF3 and 0xF7 for empty ACK payloads) which indicates the Rx queue on the
+ // crazyflie has been drained. If the queue has been drained and the
+ // desired ACK has still not been received, it was likely dropped and the
+ // request should be re-transmit.
+
+ switch (crtp_log_setup_state) {
+ case CFLIE_CRTP_LOG_SETUP_STATE_INIT:
+ toc_size = 0;
+ next_toc_variable = 0;
+ vbat_var_id = 0;
+ extvbat_var_id = 0;
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO;
+ // fallthrough
+ case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO:
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_INFO;
+ tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
+ tx_packet[1] = CRTP_LOG_TOC_CMD_INFO;
+ tx_payload_len = 2;
+ send_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_INFO:
+ if (packet_ack() == PKT_ACKED) {
+ if (rx_payload_len >= 3
+ && rx_packet[0] == crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC)
+ && rx_packet[1] == CRTP_LOG_TOC_CMD_INFO) {
+ // Received the ACK payload. Save the toc_size
+ // and advance to the next state
+ toc_size = rx_packet[2];
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM;
+ return state_machine_completed;
+ } else if (rx_packet[0] == 0xF3 || rx_packet[0] == 0xF7) {
+ // "empty" ACK packet received - likely missed the ACK
+ // payload we are waiting for.
+ // return to the send state and retransmit the request
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO;
+ return state_machine_completed;
+ }
+ }
+
+ // Otherwise, send a cmd packet to get the next ACK in the Rx queue
+ send_cmd_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM:
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_ITEM;
+ tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
+ tx_packet[1] = CRTP_LOG_TOC_CMD_ELEMENT;
+ tx_packet[2] = next_toc_variable;
+ tx_payload_len = 3;
+ send_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_ITEM:
+ if (packet_ack() == PKT_ACKED) {
+ if (rx_payload_len >= 3
+ && rx_packet[0] == crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC)
+ && rx_packet[1] == CRTP_LOG_TOC_CMD_ELEMENT
+ && rx_packet[2] == next_toc_variable) {
+ // For every element in the TOC we must compare its
+ // type (rx_packet[3]), group and name (back to back
+ // null terminated strings starting with the fifth byte)
+ // and see if it matches any of the variables we need
+ // for logging
+ //
+ // Currently enabled for logging:
+ // - vbatMV (LOG_UINT16)
+ // - extVbatMV (LOG_UINT16)
+ // - rssi (LOG_UINT8)
+ if(rx_packet[3] == vbat_var_type
+ && (0 == strcmp((char*)&rx_packet[4], pm_group_name))
+ && (0 == strcmp((char*)&rx_packet[4 + strlen(pm_group_name) + 1], vbat_var_name))) {
+ // Found the vbat element - save it for later
+ vbat_var_id = next_toc_variable;
+ }
+
+ if(rx_packet[3] == extvbat_var_type
+ && (0 == strcmp((char*)&rx_packet[4], pm_group_name))
+ && (0 == strcmp((char*)&rx_packet[4 + strlen(pm_group_name) + 1], extvbat_var_name))) {
+ // Found the extvbat element - save it for later
+ extvbat_var_id = next_toc_variable;
+ }
+
+ if(rx_packet[3] == rssi_var_type
+ && (0 == strcmp((char*)&rx_packet[4], radio_group_name))
+ && (0 == strcmp((char*)&rx_packet[4 + strlen(radio_group_name) + 1], rssi_var_name))) {
+ // Found the rssi element - save it for later
+ rssi_var_id = next_toc_variable;
+ }
+
+ // Advance the toc variable counter
+ // If there are more variables, read them
+ // If not, move on to the next state
+ next_toc_variable += 1;
+ if(next_toc_variable >= toc_size) {
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK;
+ } else {
+ // There are more TOC elements to get
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM;
+ }
+ return state_machine_completed;
+ } else if (rx_packet[0] == 0xF3 || rx_packet[0] == 0xF7) {
+ // "empty" ACK packet received - likely missed the ACK
+ // payload we are waiting for.
+ // return to the send state and retransmit the request
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO;
+ return state_machine_completed;
+ }
+ }
+
+ // Otherwise, send a cmd packet to get the next ACK in the Rx queue
+ send_cmd_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK:
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_CREATE_BLOCK;
+ tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
+ tx_packet[1] = CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK;
+ tx_packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID
+ tx_packet[3] = vbat_var_type; // Variable type
+ tx_packet[4] = vbat_var_id; // ID of the VBAT variable
+ tx_packet[5] = extvbat_var_type; // Variable type
+ tx_packet[6] = extvbat_var_id; // ID of the ExtVBat variable
+ tx_packet[7] = rssi_var_type; // Variable type
+ tx_packet[8] = rssi_var_id; // ID of the RSSI variable
+ tx_payload_len = 9;
+ send_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_CREATE_BLOCK:
+ if (packet_ack() == PKT_ACKED) {
+ if (rx_payload_len >= 2
+ && rx_packet[0] == crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS)
+ && rx_packet[1] == CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK) {
+ // Received the ACK payload. Advance to the next state
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK;
+ return state_machine_completed;
+ } else if (rx_packet[0] == 0xF3 || rx_packet[0] == 0xF7) {
+ // "empty" ACK packet received - likely missed the ACK
+ // payload we are waiting for.
+ // return to the send state and retransmit the request
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK;
+ return state_machine_completed;
+ }
+ }
+
+ // Otherwise, send a cmd packet to get the next ACK in the Rx queue
+ send_cmd_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK:
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_START_BLOCK;
+ tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
+ tx_packet[1] = CRTP_LOG_SETTINGS_CMD_START_LOGGING;
+ tx_packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID 1
+ tx_packet[3] = CFLIE_TELEM_LOG_BLOCK_PERIOD_10MS; // Log frequency in 10ms units
+ tx_payload_len = 4;
+ send_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_START_BLOCK:
+ if (packet_ack() == PKT_ACKED) {
+ if (rx_payload_len >= 2
+ && rx_packet[0] == crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS)
+ && rx_packet[1] == CRTP_LOG_SETTINGS_CMD_START_LOGGING) {
+ // Received the ACK payload. Advance to the next state
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_COMPLETE;
+ return state_machine_completed;
+ } else if (rx_packet[0] == 0xF3 || rx_packet[0] == 0xF7) {
+ // "empty" ACK packet received - likely missed the ACK
+ // payload we are waiting for.
+ // return to the send state and retransmit the request
+ crtp_log_setup_state =
+ CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK;
+ return state_machine_completed;
+ }
+ }
+
+ // Otherwise, send a cmd packet to get the next ACK in the Rx queue
+ send_cmd_packet();
+ break;
+
+ case CFLIE_CRTP_LOG_SETUP_STATE_COMPLETE:
+ state_machine_completed = 1;
+ return state_machine_completed;
+ break;
+ }
+
+ return state_machine_completed;
+}
+
+static int cflie_init()
+{
+ NRF24L01_Initialize();
+
+ // CRC, radio on
+ NRF24L01_SetTxRxMode(TX_EN);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, _BV(NRF24L01_00_EN_CRC) | _BV(NRF24L01_00_CRCO) | _BV(NRF24L01_00_PWR_UP));
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x01); // Auto Acknowledgement for data pipe 0
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, TX_ADDR_SIZE-2); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x13); // 3 retransmits, 500us delay
+
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // Defined in initialize_rx_tx_addr
+ NRF24L01_SetBitrate(data_rate); // Defined in initialize_rx_tx_addr
+
+ NRF24L01_SetPower();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, TX_ADDR_SIZE);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, TX_ADDR_SIZE);
+
+ // this sequence necessary for module from stock tx
+ NRF24L01_ReadReg(NRF24L01_1D_FEATURE);
+ NRF24L01_Activate(0x73); // Activate feature register
+ NRF24L01_ReadReg(NRF24L01_1D_FEATURE);
+
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x01); // Enable Dynamic Payload Length on pipe 0
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x06); // Enable Dynamic Payload Length, enable Payload with ACK
+
+ // Check for Beken BK2421/BK2423 chip
+ // It is done by using Beken specific activate code, 0x53
+ // and checking that status register changed appropriately
+ // There is no harm to run it on nRF24L01 because following
+ // closing activate command changes state back even if it
+ // does something on nRF24L01
+ NRF24L01_Activate(0x53); // magic for BK2421 bank switch
+ // debugln("CFlie: Trying to switch banks\n");
+ if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x80) {
+ // debugln("CFlie: BK2421 detected\n");
+ long nul = 0;
+ // Beken registers don't have such nice names, so we just mention
+ // them by their numbers
+ // It's all magic, eavesdropped from real transfer and not even from the
+ // data sheet - it has slightly different values
+ NRF24L01_WriteRegisterMulti(0x00, (uint8_t *) "\x40\x4B\x01\xE2", 4);
+ NRF24L01_WriteRegisterMulti(0x01, (uint8_t *) "\xC0\x4B\x00\x00", 4);
+ NRF24L01_WriteRegisterMulti(0x02, (uint8_t *) "\xD0\xFC\x8C\x02", 4);
+ NRF24L01_WriteRegisterMulti(0x03, (uint8_t *) "\xF9\x00\x39\x21", 4);
+ NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4);
+ NRF24L01_WriteRegisterMulti(0x05, (uint8_t *) "\x24\x06\x7F\xA6", 4);
+ NRF24L01_WriteRegisterMulti(0x06, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x07, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x08, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x09, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x0A, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x0B, (uint8_t *) &nul, 4);
+ NRF24L01_WriteRegisterMulti(0x0C, (uint8_t *) "\x00\x12\x73\x00", 4);
+ NRF24L01_WriteRegisterMulti(0x0D, (uint8_t *) "\x46\xB4\x80\x00", 4);
+ NRF24L01_WriteRegisterMulti(0x0E, (uint8_t *) "\x41\x10\x04\x82\x20\x08\x08\xF2\x7D\xEF\xFF", 11);
+ NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC7\x96\x9A\x1B", 4);
+ NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4);
+ } else {
+ // debugln("CFlie: nRF24L01 detected");
+ }
+ NRF24L01_Activate(0x53); // switch bank back
+
+ // 50ms delay in callback
+ return 50000;
+}
+
+// TODO: Fix telemetry
+
+// Update telemetry using the CRTP logging framework
+// static void update_telemetry_crtplog()
+// {
+// static uint8_t frameloss = 0;
+
+// // Read and reset count of dropped packets
+// frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4;
+// NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // reset packet loss counter
+// Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss;
+// TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS);
+
+// if (packet_ack() == PKT_ACKED) {
+// // See if the ACK packet is a cflie log packet
+// // A log data packet is a minimum of 5 bytes. Ignore anything less.
+// if (rx_payload_len >= 5) {
+// // Port 5 = log, Channel 2 = data
+// if (rx_packet[0] == crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_LOGDATA)) {
+// // The log block ID
+// if (rx_packet[1] == CFLIE_TELEM_LOG_BLOCK_ID) {
+// // Bytes 5 and 6 are the Vbat in mV units
+// uint16_t vBat;
+// memcpy(&vBat, &rx_packet[5], sizeof(uint16_t));
+// Telemetry.value[TELEM_CFLIE_INTERNAL_VBAT] = (int32_t) (vBat / 10); // The log value expects centivolts
+// TELEMETRY_SetUpdated(TELEM_CFLIE_INTERNAL_VBAT);
+
+// // Bytes 7 and 8 are the ExtVbat in mV units
+// uint16_t extVBat;
+// memcpy(&extVBat, &rx_packet[7], sizeof(uint16_t));
+// Telemetry.value[TELEM_CFLIE_EXTERNAL_VBAT] = (int32_t) (extVBat / 10); // The log value expects centivolts
+// TELEMETRY_SetUpdated(TELEM_CFLIE_EXTERNAL_VBAT);
+
+// // Byte 9 is the RSSI
+// Telemetry.value[TELEM_CFLIE_RSSI] = rx_packet[9];
+// TELEMETRY_SetUpdated(TELEM_CFLIE_RSSI);
+// }
+// }
+// }
+// }
+// }
+
+// // Update telemetry using the ACK packet payload
+// static void update_telemetry_ackpkt()
+// {
+// static uint8_t frameloss = 0;
+
+// // Read and reset count of dropped packets
+// frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4;
+// NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // reset packet loss counter
+// Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss;
+// TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS);
+
+// if (packet_ack() == PKT_ACKED) {
+// // Make sure this is an ACK packet (first byte will alternate between 0xF3 and 0xF7
+// if (rx_packet[0] == 0xF3 || rx_packet[0] == 0xF7) {
+// // If ACK packet contains RSSI (proper length and byte 1 is 0x01)
+// if(rx_payload_len >= 3 && rx_packet[1] == 0x01) {
+// Telemetry.value[TELEM_CFLIE_RSSI] = rx_packet[2];
+// TELEMETRY_SetUpdated(TELEM_CFLIE_RSSI);
+// }
+// // If ACK packet contains VBAT (proper length and byte 3 is 0x02)
+// if(rx_payload_len >= 8 && rx_packet[3] == 0x02) {
+// uint32_t vBat = 0;
+// memcpy(&vBat, &rx_packet[4], sizeof(uint32_t));
+// Telemetry.value[TELEM_CFLIE_INTERNAL_VBAT] = (int32_t)(vBat / 10); // The log value expects centivolts
+// TELEMETRY_SetUpdated(TELEM_CFLIE_INTERNAL_VBAT);
+// }
+// }
+// }
+// }
+
+static uint16_t cflie_callback()
+{
+ switch (phase) {
+ case CFLIE_INIT_SEARCH:
+ send_search_packet();
+ phase = CFLIE_SEARCH;
+ break;
+ case CFLIE_INIT_CRTP_LOG:
+ if (crtp_log_setup_state_machine()) {
+ phase = CFLIE_INIT_DATA;
+ }
+ break;
+ case CFLIE_INIT_DATA:
+ send_cmd_packet();
+ phase = CFLIE_DATA;
+ break;
+ case CFLIE_SEARCH:
+ switch (packet_ack()) {
+ case PKT_PENDING:
+ return PACKET_CHKTIME; // packet send not yet complete
+ case PKT_ACKED:
+ phase = CFLIE_DATA;
+ // PROTOCOL_SetBindState(0);
+ // MUSIC_Play(MUSIC_DONE_BINDING);
+ BIND_DONE;
+ break;
+ case PKT_TIMEOUT:
+ send_search_packet();
+ cflie_counter = CFLIE_BIND_COUNT;
+ }
+ break;
+
+ case CFLIE_DATA:
+ // if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_CRTPLOG) {
+ // update_telemetry_crtplog();
+ // } else if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_ACKPKT) {
+ // update_telemetry_ackpkt();
+ // }
+
+ if (packet_ack() == PKT_PENDING)
+ return PACKET_CHKTIME; // packet send not yet complete
+ send_cmd_packet();
+ break;
+ }
+ return PACKET_PERIOD; // Packet at standard protocol interval
+}
+
+// Generate address to use from TX id and manufacturer id (STM32 unique id)
+static uint8_t initialize_rx_tx_addr()
+{
+ rx_tx_addr[0] =
+ rx_tx_addr[1] =
+ rx_tx_addr[2] =
+ rx_tx_addr[3] =
+ rx_tx_addr[4] = 0xE7; // CFlie uses fixed address
+
+ // if (Model.fixed_id) {
+ // rf_channel = Model.fixed_id % 100;
+ // switch (Model.fixed_id / 100) {
+ // case 0:
+ // data_rate = NRF24L01_BR_250K;
+ // break;
+ // case 1:
+ // data_rate = NRF24L01_BR_1M;
+ // break;
+ // case 2:
+ // data_rate = NRF24L01_BR_2M;
+ // break;
+ // default:
+ // break;
+ // }
+
+ // if (Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_ON_CRTPLOG) {
+ // return CFLIE_INIT_CRTP_LOG;
+ // } else {
+ // return CFLIE_INIT_DATA;
+ // }
+ // } else {
+ // data_rate = NRF24L01_BR_250K;
+ // rf_channel = 10;
+ // return CFLIE_INIT_SEARCH;
+ // }
+
+ // Default 1
+ data_rate = NRF24L01_BR_1M;
+ rf_channel = 10;
+
+ // Default 2
+ // data_rate = NRF24L01_BR_2M;
+ // rf_channel = 110;
+ return CFLIE_INIT_SEARCH;
+}
+
+uint16_t initCFlie(void)
+{
+ BIND_IN_PROGRESS; // autobind protocol
+
+ phase = initialize_rx_tx_addr();
+ crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_INIT;
+ packet_count=0;
+
+ int delay = cflie_init();
+
+ // debugln("CFlie init!");
+
+ return delay;
+}
+
+#endif
\ No newline at end of file
diff --git a/Multiprotocol/Multi.txt b/Multiprotocol/Multi.txt
index fe0a574..b37d78a 100644
--- a/Multiprotocol/Multi.txt
+++ b/Multiprotocol/Multi.txt
@@ -35,4 +35,5 @@
35,ESKY150
36,H8_3D,H8_3D,H20H,H20Mini,H30Mini
37,CORONA,COR_V1,COR_V2
+38,CFlie
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index bd4a7db..46b2cf6 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,8 @@
#define VERSION_MAJOR 1
#define VERSION_MINOR 2
#define VERSION_REVISION 0
-#define VERSION_PATCH_LEVEL 19
+#define VERSION_PATCH_LEVEL 20
+
//******************
// Protocols
//******************
@@ -63,6 +64,7 @@ enum PROTOCOLS
PROTO_ESKY150 = 35, // =>NRF24L01
PROTO_H8_3D = 36, // =>NRF24L01
PROTO_CORONA = 37, // =>CC2500
+ PROTO_CFLIE = 38, // =>NRF24L01
};
enum Flysky
@@ -556,6 +558,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p --
ESKY150 35
H8_3D 36
CORONA 37
+ CFlie 38
BindBit=> 0x80 1=Bind/0=No
AutoBindBit=> 0x40 1=Yes /0=No
RangeCheck=> 0x20 1=Yes /0=No
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 536b9c7..d01379f 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -23,7 +23,7 @@
#include
//#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle
-//#define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial
+// #define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial
#ifdef __arm__ // Let's automatically select the board if arm is selected
#define STM32_BOARD
@@ -1150,6 +1150,12 @@ static void protocol_init()
remote_callback = H8_3D_callback;
break;
#endif
+ #if defined(CFLIE_NRF24L01_INO)
+ case PROTO_CFLIE:
+ next_callback=initCFlie();
+ remote_callback = cflie_callback;
+ break;
+ #endif
#endif
}
}
diff --git a/Multiprotocol/NRF24l01_SPI.ino b/Multiprotocol/NRF24l01_SPI.ino
index c66f14b..881e0a9 100644
--- a/Multiprotocol/NRF24l01_SPI.ino
+++ b/Multiprotocol/NRF24l01_SPI.ino
@@ -108,7 +108,7 @@ static uint8_t __attribute__((unused)) NRF24L01_GetStatus()
return SPI_Read();
}
-static uint8_t __attribute__((unused)) NRF24L01_GetDynamicPayloadSize()
+static uint8_t NRF24L01_GetDynamicPayloadSize()
{
NRF_CSN_off;
SPI_Write(R_RX_PL_WID);
diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h
index e732468..6252ee9 100644
--- a/Multiprotocol/Validate.h
+++ b/Multiprotocol/Validate.h
@@ -171,6 +171,7 @@
#undef CABELL_NRF24L01_INO
#undef ESKY150_NRF24L01_INO
#undef H8_3D_NRF24L01_INO
+ #undef CFLIE_NRF24L01_INO
#endif
//Make sure telemetry is selected correctly
diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h
index 5c9b5b3..06cfb90 100644
--- a/Multiprotocol/_Config.h
+++ b/Multiprotocol/_Config.h
@@ -188,6 +188,7 @@
#define CABELL_NRF24L01_INO
#define ESKY150_NRF24L01_INO
#define H8_3D_NRF24L01_INO
+#define CFLIE_NRF24L01_INO
/**************************/
@@ -543,6 +544,8 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]= {
PROTO_CORONA
COR_V1
COR_V2
+ PROTO_CFLIE
+ NONE
*/
// RX_Num is used for TX & RX match. Using different RX_Num values for each receiver will prevent starting a model with the false config loaded...