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);
}
}
}
}
}
// Update telemetry using the ACK packet payload
static void update_telemetry_ackpkt()
{
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) {
// 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) {
u32 vBat = 0;
memcpy(&vBat, &rx_packet[4], sizeof(u32));
Telemetry.value[TELEM_CFLIE_INTERNAL_VBAT] = (s32)(vBat / 10); // The log value expects centivolts
TELEMETRY_SetUpdated(TELEM_CFLIE_INTERNAL_VBAT);
}
}
}
}
END_TEST
/* Packet->ack */
START_TEST(test_packet_ack)
{
fail_unless((packet_ack(packet) == 2),
"packet_ack did not return 2");
}
MODULE_CALLTYPE
static u16 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);
break;
case PKT_TIMEOUT:
send_search_packet();
counter = 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
}
MODULE_CALLTYPE
static u16 cflie_callback()
{
switch (phase) {
case CFLIE_INIT_SEARCH:
send_search_packet();
phase = CFLIE_SEARCH;
break;
case CFLIE_INIT_TELEMETRY:
if (telemetry_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);
break;
case PKT_TIMEOUT:
send_search_packet();
counter = BIND_COUNT;
}
break;
case CFLIE_DATA:
update_telemetry();
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
}
/*
stateful function
if the funtion returns anything but 0
then a new packet is in the raw_packet_buffer
*/
uint8_t get_new_raw_data()
{
if ( NewSerial.available() == 0){
return 0;
}
uint8_t ch = (uint8_t)NewSerial.read();
switch (current_packet_mode){
case in_zeros:{
if ( ch != 0){
raw_packet_buffer[0] = ch;
packet_idx = 1;
current_packet_mode = in_packet;
}
return 0;
}
case in_packet:{
if ( ch != 0){
if ( packet_idx < max_packet_length){
raw_packet_buffer[packet_idx] = ch;
++packet_idx;
}else{
// error .. send nack?
packet_nack();
current_packet_mode = not_synced;
}
return 0;
}else{ // got a new packet
// return number of chars
// send ack?
packet_ack();
return packet_idx;
}
}
case not_synced:
default:{
if ( ch == 0){
current_packet_mode = in_zeros;
}
return 0;
}
}
}
/***************************************************************************//**
*
* uart_update - 从串口进行升级
*
* @param void
*
* @retval true 成功
* @retval false 失败
*
*******************************************************************************/
STATUS uart_update(void)
{
uint32 i, startAddress, transferSize = 0;
uint8 size;
uint8 status;
uint8 cmd, data[MAX_BUF_SIZE];
uart_printf("Press ESC to upgrade from serial port ");
// 检测是否要从串口进行升级
if (!escKeyDetect()) return (OK);
uart_printf("Receiving data ..");
// 过滤多余按键或串口切换产生的错误字节
while (!packet_hello())
{
}
startAddress = 0xffffffff;
status = COMMAND_RET_SUCCESS;
while (true)
{
// 如果未正确接收报文，放弃处理
if (!packet_receive(&cmd, data, &size))
{
packet_nak();
continue;
}
// 根据命令字进行处理
switch(cmd)
{
case COMMAND_PING: // 测试连接
status = COMMAND_RET_SUCCESS;
// 返回确认包
packet_ack();
break;
case COMMAND_DOWNLOAD: // 开始下载
status = COMMAND_RET_SUCCESS;
do
{
// 检查报文长度
if (size != 8)
{
status = COMMAND_RET_INVALID_CMD;
break;
}
// 起始地址
startAddress = (data[0] << 24)
| (data[1] << 16)
| (data[2] << 8)
| data[3];
/* 起始地址必须为0，由各BootLoader自行决定写入位置 */
if (startAddress != 0u)
{
status = COMMAND_RET_INVALID_CMD;
break;
}
startAddress = APP_START_ADDRESS;
// 下载字节数
transferSize = (data[4] << 24)
| (data[5] << 16)
| (data[6] << 8)
| data[7];
// 检查写入空间
if (!iflash_spaceCheck(startAddress, transferSize))
{
status = COMMAND_RET_INVALID_PARA;
break;
}
// 清除中断状态
iflash_errorClear();
// 按块擦除将要更新的地址区间
for (i = startAddress; i < startAddress + transferSize; i += FLASH_PAGE_SIZE)
iflash_erase(i);
// 检查中断状态，如果出现错误，则设置对应状态
if (iflash_errorCheck())
status = COMMAND_RET_FLASH_FAIL;
}
while(false);
if (status != COMMAND_RET_SUCCESS)
transferSize = 0;
// 确认报文已得到处理
packet_ack();
break;
case COMMAND_GET_STATUS: // 获取状态
// 返回确认包
packet_ack();
// 返回当前状态
packet_status(status);
break;
case COMMAND_SEND_DATA: // 数据
status = COMMAND_RET_SUCCESS;
// 注意:后台程序应保证size按字对齐，否则写入结果无法预料
// 检查是否还有数据未写入
if (transferSize >= size)
{
iflash_write(startAddress, data, size);
if (iflash_errorCheck())
status = COMMAND_RET_FLASH_FAIL;
else
{
transferSize -= size;
startAddress += size;
}
}
else status = COMMAND_RET_INVALID_PARA;
// 返回确认包
packet_ack();
break;
case COMMAND_RESET: // 复位
// 返回确认包
packet_ack();
// 等待UART完成发送
uart_flush();
delay(524288);
// 写入注册码及复位请求
HWREG(NVIC_APINT) = (NVIC_APINT_VECTKEY | NVIC_APINT_SYSRESETREQ);
// 死循环
while(true)
{
}
default: // 未知命令
// 返回确认包
packet_ack();
// 状态设为未知命令
status = COMMAND_RET_UNKNOWN_CMD;
}
}
}
// 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;
}