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); } }