static void update_telemetry()
{
static u8 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 6 and 7 are the Vbat in mV units
u16 vBat;
memcpy(&vBat, &rx_packet[5], sizeof(u16));
Telemetry.value[TELEM_DSM_FLOG_VOLT2] = (s32) (vBat / 10); // The log value expects tenths of volts
TELEMETRY_SetUpdated(TELEM_DSM_FLOG_VOLT2);
// Bytes 8 and 9 are the ExtVbat in mV units
u16 extVBat;
memcpy(&extVBat, &rx_packet[7], sizeof(u16));
Telemetry.value[TELEM_DSM_FLOG_VOLT1] = (s32) (extVBat / 10); // The log value expects tenths of volts
TELEMETRY_SetUpdated(TELEM_DSM_FLOG_VOLT1);
}
}
}
}
}
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;
if(numAuxChannels > MAX_CPPM_AUX_CHANNELS)
{
numAuxChannels = MAX_CPPM_AUX_CHANNELS;
}
cpkt.hdr.numAuxChannels = numAuxChannels;
// Remap AETR to AERT (RPYT)
cpkt.channelRoll = RESCALE_RC_CHANNEL_TO_PWM(Channels[0]);
cpkt.channelPitch = RESCALE_RC_CHANNEL_TO_PWM(Channels[1]);
cpkt.channelYaw = RESCALE_RC_CHANNEL_TO_PWM(Channels[3]); // T & R Swapped
cpkt.channelThrust = RESCALE_RC_CHANNEL_TO_PWM(Channels[2]);
// Rescale the rest of the aux channels - RC channel 4 and up
uint8_t i;
for (i = 0; i < numAuxChannels; i++)
{
cpkt.channelAux[i] = RESCALE_RC_CHANNEL_TO_PWM(Channels[i + 4]);
}
// 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);
tx_payload_len = 2 + commanderPacketSize; // CRTP header, commander type, and packet
send_packet();
}
// State machine for setting up telemetry
// returns 1 when the state machine has completed, 0 otherwise
static u8 telemetry_setup_state_machine()
{
u8 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 (telemetry_setup_state) {
case CFLIE_TELEM_SETUP_STATE_INIT:
toc_size = 0;
next_toc_variable = 0;
vbat_var_id = 0;
extvbat_var_id = 0;
telemetry_setup_state = CFLIE_TELEM_SETUP_STATE_SEND_CMD_GET_INFO;
// fallthrough
case CFLIE_TELEM_SETUP_STATE_SEND_CMD_GET_INFO:
telemetry_setup_state = CFLIE_TELEM_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_TELEM_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];
telemetry_setup_state =
CFLIE_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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_TELEM_SETUP_STATE_SEND_CMD_GET_ITEM:
telemetry_setup_state = CFLIE_TELEM_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_TELEM_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)
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;
}
// 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) {
telemetry_setup_state = CFLIE_TELEM_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK;
} else {
// There are more TOC elements to get
telemetry_setup_state = CFLIE_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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_TELEM_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK:
telemetry_setup_state = CFLIE_TELEM_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_payload_len = 7;
send_packet();
break;
case CFLIE_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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_TELEM_SETUP_STATE_SEND_CONTROL_START_BLOCK:
telemetry_setup_state = CFLIE_TELEM_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_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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
telemetry_setup_state =
CFLIE_TELEM_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_TELEM_SETUP_STATE_COMPLETE:
state_machine_completed = 1;
return state_machine_completed;
break;
}
return state_machine_completed;
}
static void send_cmd_packet()
{
s32 f_roll;
s32 f_pitch;
s32 f_yaw;
s32 thrust_truncated;
u16 thrust;
struct CommanderPacker
{
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;
f_roll = -Channels[0] * FRAC_SCALE / (10000 / 50);
// Pitch, aka elevator, float +- 50.0 degrees
//float pitch = -(float) Channels[1]*50.0/10000;
f_pitch = -Channels[1] * FRAC_SCALE / (10000 / 50);
// Thrust, aka throttle 0..65535, working range 5535..65535
// No space for overshoot here, hard limit Channel3 by -10000..10000
thrust_truncated = Channels[2];
if (thrust_truncated < CHAN_MIN_VALUE) {
thrust_truncated = CHAN_MIN_VALUE;
} else if (thrust_truncated > CHAN_MAX_VALUE) {
thrust_truncated = CHAN_MAX_VALUE;
}
thrust = thrust_truncated*3L + 35535L;
// Crazyflie needs zero thrust to unlock
if (thrust < 6000)
cpkt.thrust = 0;
else
cpkt.thrust = thrust;
// Yaw, aka rudder, float +- 400.0 deg/s
// float yaw = -(float) Channels[3]*400.0/10000;
f_yaw = - Channels[3] * FRAC_SCALE / (10000 / 400);
frac2float(f_yaw, &cpkt.yaw);
// Switch on/off?
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
s32 f_x_roll = (f_roll + f_pitch) * 181 / 256;
frac2float(f_x_roll, &cpkt.roll);
s32 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_COMMANDER, 0); // Commander packet to channel 0
memcpy(&tx_packet[1], (char*) &cpkt, sizeof(cpkt));
tx_payload_len = 1 + sizeof(cpkt);
send_packet();
// Print channels every 2 seconds or so
if ((packet_counter & 0xFF) == 1) {
dbgprintf("Raw channels: %d, %d, %d, %d, %d, %d, %d, %d\n",
Channels[0], Channels[1], Channels[2], Channels[3],
Channels[4], Channels[5], Channels[6], Channels[7]);
dbgprintf("Roll %d, pitch %d, yaw %d, thrust %d\n",
(int) f_roll*100/FRAC_SCALE, (int) f_pitch*100/FRAC_SCALE, (int) f_yaw*100/FRAC_SCALE, (int) thrust);
}
}
