From 07adeaf60d96170a1e65ad99bb427446b73a9c47 Mon Sep 17 00:00:00 2001 From: James Hagerman Date: Fri, 4 May 2018 07:12:04 -0700 Subject: [PATCH] Adding CFlie protocol (#159) --- Multiprotocol/CFlie_nrf24l01.ino | 908 +++++++++++++++++++++++++++++++ Multiprotocol/Multi.txt | 1 + Multiprotocol/Multiprotocol.h | 5 +- Multiprotocol/Multiprotocol.ino | 8 +- Multiprotocol/NRF24l01_SPI.ino | 2 +- Multiprotocol/Validate.h | 1 + Multiprotocol/_Config.h | 3 + 7 files changed, 925 insertions(+), 3 deletions(-) create mode 100644 Multiprotocol/CFlie_nrf24l01.ino 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...