gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 22:38:14 +00:00
Code Issues Projects Releases Wiki Activity
DIY-Multiprotocol-TX-Module/Multiprotocol/CFlie_nrf24l01.ino
Pascal Langer 29d1fb00b1 More code optimization
2021-02-12 11:21:42 +01:00

845 lines
30 KiB
C++
Raw Blame History

/*
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 <http://www.gnu.org/licenses/>.
*/
// 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 CFLIE_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 packet
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 uint8_t data_rate;
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 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(packet, tx_payload_len);
// // Check and adjust transmission power.
NRF24L01_SetPower();
}
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_ch_num++ > 125)
{
rf_ch_num = 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_ch_num);
NRF24L01_WritePayload(buf, sizeof(buf));
}
// 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
packet[0] = crtp_create_header(CRTP_PORT_SETPOINT, 0); // Commander packet to channel 0
memcpy(&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
packet[0] = crtp_create_header(CRTP_PORT_SETPOINT_GENERIC, 0); // Generic setpoint packet to channel 0
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(&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;
packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
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;
packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
packet[1] = CRTP_LOG_TOC_CMD_ELEMENT;
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;
packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
packet[1] = CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK;
packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID
packet[3] = vbat_var_type; // Variable type
packet[4] = vbat_var_id; // ID of the VBAT variable
packet[5] = extvbat_var_type; // Variable type
packet[6] = extvbat_var_id; // ID of the ExtVBat variable
packet[7] = rssi_var_type; // Variable type
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;
packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
packet[1] = CRTP_LOG_SETTINGS_CMD_START_LOGGING;
packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID 1
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 void CFLIE_RF_init()
{
NRF24L01_Initialize();
// CRC, radio on
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x01); // Auto Acknowledgement for data pipe 0
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x13); // 3 retransmits, 500us delay
NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch_num); // Defined in initialize_rx_tx_addr
NRF24L01_SetBitrate(data_rate); // Defined in initialize_rx_tx_addr
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);
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
NRF24L01_SetTxRxMode(TX_EN); // Clear data ready, data sent, retransmit and enable CRC 16bits, ready for TX
}
// 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_ch_num); // 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_ch_num); // 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();
}
break;
case CFLIE_DATA:
#ifdef MULTI_SYNC
telemetry_set_input_sync(CFLIE_PACKET_PERIOD);
#endif
// 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 CFLIE_PACKET_PERIOD; // Packet at standard protocol interval
}
// Generate address to use from TX id and manufacturer id (STM32 unique id)
static uint8_t CFLIE_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_ch_num = 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_ch_num = 10;
// return CFLIE_INIT_SEARCH;
// }
// Default 1
data_rate = NRF24L01_BR_1M;
rf_ch_num = 10;
// Default 2
// data_rate = NRF24L01_BR_2M;
// rf_ch_num = 110;
return CFLIE_INIT_SEARCH;
}
void CFLIE_init(void)
{
BIND_IN_PROGRESS; // autobind protocol
phase = CFLIE_initialize_rx_tx_addr();
crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_INIT;
packet_count=0;
CFLIE_RF_init();
}
#endif
Reference in New Issue View Git Blame Copy Permalink