|
|
|
@ -38,6 +38,143 @@ CLte_dev_t lte_dev = { |
|
|
|
|
|
|
|
lte_diag_t lte_diag = {0}; |
|
|
|
static bool lte_log_clock_query_done = false; |
|
|
|
static rt_uint8_t pdpdeact_match_position = 0; |
|
|
|
|
|
|
|
typedef struct { |
|
|
|
rt_uint16_t remaining; |
|
|
|
rt_uint16_t checksum; |
|
|
|
rt_uint16_t received_checksum; |
|
|
|
rt_uint8_t magic; |
|
|
|
rt_uint8_t phase; |
|
|
|
rt_uint8_t position; |
|
|
|
rt_uint8_t payload_length; |
|
|
|
rt_uint8_t incompat_flags; |
|
|
|
bool heartbeat; |
|
|
|
} mavlink_diag_parser_t; |
|
|
|
|
|
|
|
static mavlink_diag_parser_t fmu_mavlink_parser; |
|
|
|
static mavlink_diag_parser_t server_mavlink_parser; |
|
|
|
|
|
|
|
static int parse_decimal(const char **text) { |
|
|
|
int value = 0; |
|
|
|
bool found = false; |
|
|
|
const char *p = *text; |
|
|
|
|
|
|
|
while (*p == ' ') p++; |
|
|
|
while (*p >= '0' && *p <= '9') { |
|
|
|
found = true; |
|
|
|
value = value * 10 + (*p++ - '0'); |
|
|
|
} |
|
|
|
*text = p; |
|
|
|
return found ? value : -1; |
|
|
|
} |
|
|
|
|
|
|
|
static void copy_csv_token(const char **text, char *output, rt_size_t capacity) { |
|
|
|
const char *p = *text; |
|
|
|
rt_size_t used = 0; |
|
|
|
char terminator = ','; |
|
|
|
|
|
|
|
if (capacity == 0) return; |
|
|
|
while (*p == ' ' || *p == ',') p++; |
|
|
|
if (*p == '"') { |
|
|
|
terminator = '"'; |
|
|
|
p++; |
|
|
|
} |
|
|
|
while (*p != '\0' && *p != '\r' && *p != '\n' && *p != terminator) { |
|
|
|
if (used + 1 < capacity) output[used++] = *p; |
|
|
|
p++; |
|
|
|
} |
|
|
|
while (used > 0 && output[used - 1] == ' ') used--; |
|
|
|
output[used] = '\0'; |
|
|
|
if (*p == terminator) p++; |
|
|
|
*text = p; |
|
|
|
} |
|
|
|
|
|
|
|
static void mavlink_crc_accumulate(uint8_t data, rt_uint16_t *checksum) { |
|
|
|
uint8_t tmp = data ^ (uint8_t)(*checksum & 0xff); |
|
|
|
tmp ^= tmp << 4; |
|
|
|
*checksum = (*checksum >> 8) ^ ((rt_uint16_t)tmp << 8) ^ |
|
|
|
((rt_uint16_t)tmp << 3) ^ ((rt_uint16_t)tmp >> 4); |
|
|
|
} |
|
|
|
|
|
|
|
static void mavlink_diag_start(mavlink_diag_parser_t *parser, uint8_t magic) { |
|
|
|
parser->magic = magic; |
|
|
|
parser->phase = 1; |
|
|
|
parser->position = 0; |
|
|
|
parser->remaining = 0; |
|
|
|
parser->checksum = 0xffff; |
|
|
|
parser->received_checksum = 0; |
|
|
|
parser->payload_length = 0; |
|
|
|
parser->incompat_flags = 0; |
|
|
|
parser->heartbeat = false; |
|
|
|
} |
|
|
|
|
|
|
|
static void mavlink_diag_feed(mavlink_diag_parser_t *parser, |
|
|
|
const uint8_t *data, rt_size_t size, |
|
|
|
rt_uint32_t *heartbeat_count) { |
|
|
|
rt_size_t i; |
|
|
|
|
|
|
|
for (i = 0; i < size; i++) { |
|
|
|
uint8_t ch = data[i]; |
|
|
|
|
|
|
|
if (parser->phase == 0) { |
|
|
|
if (ch == 0xFE || ch == 0xFD) mavlink_diag_start(parser, ch); |
|
|
|
continue; |
|
|
|
} |
|
|
|
if (parser->phase == 5) { |
|
|
|
parser->remaining--; |
|
|
|
if (parser->remaining == 0) parser->phase = 0; |
|
|
|
continue; |
|
|
|
} |
|
|
|
if (parser->phase == 2) { |
|
|
|
mavlink_crc_accumulate(ch, &parser->checksum); |
|
|
|
parser->remaining--; |
|
|
|
if (parser->remaining == 0) parser->phase = 3; |
|
|
|
continue; |
|
|
|
} |
|
|
|
if (parser->phase == 3) { |
|
|
|
parser->received_checksum = ch; |
|
|
|
parser->phase = 4; |
|
|
|
continue; |
|
|
|
} |
|
|
|
if (parser->phase == 4) { |
|
|
|
parser->received_checksum |= (rt_uint16_t)ch << 8; |
|
|
|
mavlink_crc_accumulate(50, &parser->checksum); |
|
|
|
if (parser->heartbeat && |
|
|
|
parser->checksum == parser->received_checksum) { |
|
|
|
(*heartbeat_count)++; |
|
|
|
} |
|
|
|
if (parser->magic == 0xFD && |
|
|
|
(parser->incompat_flags & 0x01) != 0) { |
|
|
|
parser->phase = 5; |
|
|
|
parser->remaining = 13; |
|
|
|
} else { |
|
|
|
parser->phase = 0; |
|
|
|
} |
|
|
|
continue; |
|
|
|
} |
|
|
|
|
|
|
|
mavlink_crc_accumulate(ch, &parser->checksum); |
|
|
|
if (parser->position == 0) parser->payload_length = ch; |
|
|
|
if (parser->magic == 0xFE && parser->position == 4) { |
|
|
|
parser->heartbeat = parser->payload_length == 9 && ch == 0; |
|
|
|
parser->remaining = parser->payload_length; |
|
|
|
parser->phase = parser->remaining > 0 ? 2 : 3; |
|
|
|
} else if (parser->magic == 0xFD) { |
|
|
|
if (parser->position == 1) parser->incompat_flags = ch; |
|
|
|
if (parser->position == 6) parser->heartbeat = ch == 0; |
|
|
|
if (parser->position == 7) |
|
|
|
parser->heartbeat = parser->heartbeat && ch == 0; |
|
|
|
if (parser->position == 8) { |
|
|
|
parser->heartbeat = parser->payload_length == 9 && |
|
|
|
parser->heartbeat && ch == 0; |
|
|
|
parser->remaining = parser->payload_length; |
|
|
|
parser->phase = parser->remaining > 0 ? 2 : 3; |
|
|
|
} |
|
|
|
} |
|
|
|
parser->position++; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_at_begin(const char *cmd) { |
|
|
|
rt_size_t i; |
|
|
|
@ -86,6 +223,42 @@ void lte_diag_fmu_rx(rt_size_t size) { |
|
|
|
lte_diag.last_fmu_rx_tick = rt_tick_get(); |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_fmu_data(const uint8_t *data, rt_size_t size) { |
|
|
|
if (data == RT_NULL || size == 0) return; |
|
|
|
lte_diag_fmu_rx(size); |
|
|
|
mavlink_diag_feed(&fmu_mavlink_parser, data, size, <e_diag.fmu_heartbeats); |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_server_data(const uint8_t *data, rt_size_t size) { |
|
|
|
if (data == RT_NULL || size == 0) return; |
|
|
|
lte_diag.server_rx_bytes += size; |
|
|
|
lte_diag.server_rx_read_events++; |
|
|
|
lte_diag.last_server_rx_tick = rt_tick_get(); |
|
|
|
mavlink_diag_feed(&server_mavlink_parser, data, size, |
|
|
|
<e_diag.server_heartbeats); |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_modem_text(const uint8_t *data, rt_size_t size) { |
|
|
|
static const char pattern[] = "pdpdeact"; |
|
|
|
rt_size_t i; |
|
|
|
|
|
|
|
if (data == RT_NULL) return; |
|
|
|
for (i = 0; i < size; i++) { |
|
|
|
char ch = (char)data[i]; |
|
|
|
if (ch >= 'A' && ch <= 'Z') ch = (char)(ch - 'A' + 'a'); |
|
|
|
if (ch == pattern[pdpdeact_match_position]) { |
|
|
|
pdpdeact_match_position++; |
|
|
|
if (pdpdeact_match_position == sizeof(pattern) - 1) { |
|
|
|
if (!lte_dev.socket_pdpdeact_pending) lte_diag.pdpdeact_count++; |
|
|
|
lte_dev.socket_pdpdeact_pending = true; |
|
|
|
pdpdeact_match_position = 0; |
|
|
|
} |
|
|
|
} else { |
|
|
|
pdpdeact_match_position = ch == pattern[0] ? 1 : 0; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_fmu_tx(rt_size_t size) { |
|
|
|
lte_diag.fmu_tx_bytes += size; |
|
|
|
lte_diag.last_fmu_tx_tick = rt_tick_get(); |
|
|
|
@ -101,6 +274,16 @@ void lte_diag_connect_attempt(void) { |
|
|
|
void lte_diag_connect_success(void) { |
|
|
|
lte_diag.connect_tick = rt_tick_get(); |
|
|
|
lte_diag.last_server_rx_tick = lte_diag.connect_tick; |
|
|
|
lte_diag.conn_fmu_rx_base = lte_diag.fmu_rx_bytes; |
|
|
|
lte_diag.conn_fmu_tx_base = lte_diag.fmu_tx_bytes; |
|
|
|
lte_diag.conn_server_rx_base = lte_diag.server_rx_bytes; |
|
|
|
lte_diag.conn_server_events_base = lte_diag.server_rx_read_events; |
|
|
|
lte_diag.conn_fmu_heartbeat_base = lte_diag.fmu_heartbeats; |
|
|
|
lte_diag.conn_server_heartbeat_base = lte_diag.server_heartbeats; |
|
|
|
lte_dev.socket_pdpdeact_pending = false; |
|
|
|
pdpdeact_match_position = 0; |
|
|
|
rt_memset(&fmu_mavlink_parser, 0, sizeof(fmu_mavlink_parser)); |
|
|
|
rt_memset(&server_mavlink_parser, 0, sizeof(server_mavlink_parser)); |
|
|
|
} |
|
|
|
|
|
|
|
void lte_diag_module_reset(void) { |
|
|
|
@ -151,6 +334,103 @@ void BSP_LTE_InitLogClock(void) { |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void parse_cereg_snapshot(const char *response) { |
|
|
|
const char *p = rt_strstr(response, "+CEREG:"); |
|
|
|
int value; |
|
|
|
|
|
|
|
if (p == RT_NULL) return; |
|
|
|
p += rt_strlen("+CEREG:"); |
|
|
|
value = parse_decimal(&p); |
|
|
|
if (value < 0 || *p != ',') return; |
|
|
|
p++; |
|
|
|
value = parse_decimal(&p); |
|
|
|
if (value < 0) return; |
|
|
|
lte_diag.network.cereg = (int8_t)value; |
|
|
|
if (*p == ',') { |
|
|
|
copy_csv_token(&p, lte_diag.network.tac, |
|
|
|
sizeof(lte_diag.network.tac)); |
|
|
|
copy_csv_token(&p, lte_diag.network.cell, |
|
|
|
sizeof(lte_diag.network.cell)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void parse_cgatt_snapshot(const char *response) { |
|
|
|
const char *p = rt_strstr(response, "+CGATT:"); |
|
|
|
int value; |
|
|
|
|
|
|
|
if (p == RT_NULL) return; |
|
|
|
p += rt_strlen("+CGATT:"); |
|
|
|
value = parse_decimal(&p); |
|
|
|
if (value >= 0) lte_diag.network.cgatt = (int8_t)value; |
|
|
|
} |
|
|
|
|
|
|
|
static void parse_qiact_snapshot(const char *response) { |
|
|
|
const char *p = rt_strstr(response, "+QIACT:"); |
|
|
|
int context; |
|
|
|
int state; |
|
|
|
|
|
|
|
if (p == RT_NULL) { |
|
|
|
lte_diag.network.pdp = 0; |
|
|
|
return; |
|
|
|
} |
|
|
|
p += rt_strlen("+QIACT:"); |
|
|
|
context = parse_decimal(&p); |
|
|
|
if (context < 0 || *p != ',') return; |
|
|
|
p++; |
|
|
|
state = parse_decimal(&p); |
|
|
|
if (context == 1 && state >= 0) lte_diag.network.pdp = (int8_t)state; |
|
|
|
} |
|
|
|
|
|
|
|
static void parse_qnwinfo_snapshot(const char *response) { |
|
|
|
const char *p = rt_strstr(response, "+QNWINFO:"); |
|
|
|
char channel[12]; |
|
|
|
|
|
|
|
if (p == RT_NULL) return; |
|
|
|
p += rt_strlen("+QNWINFO:"); |
|
|
|
copy_csv_token(&p, lte_diag.network.rat, sizeof(lte_diag.network.rat)); |
|
|
|
copy_csv_token(&p, lte_diag.network.oper, sizeof(lte_diag.network.oper)); |
|
|
|
copy_csv_token(&p, lte_diag.network.band, sizeof(lte_diag.network.band)); |
|
|
|
copy_csv_token(&p, channel, sizeof(channel)); |
|
|
|
p = channel; |
|
|
|
{ |
|
|
|
int value = parse_decimal(&p); |
|
|
|
if (value >= 0) lte_diag.network.channel = (rt_uint32_t)value; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void BSP_LTE_CollectNetworkSnapshot(void) { |
|
|
|
lte_network_snapshot_t *network = <e_diag.network; |
|
|
|
|
|
|
|
rt_memset(network, 0, sizeof(*network)); |
|
|
|
network->cereg = -1; |
|
|
|
network->cgatt = -1; |
|
|
|
network->pdp = -1; |
|
|
|
network->csq = -1; |
|
|
|
|
|
|
|
BSP_LTE_SendCmd(AT_CEREG2, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL); |
|
|
|
if (BSP_LTE_SendCmd(AT_CEREG, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL) == 0) { |
|
|
|
parse_cereg_snapshot((const char *)lte_dev.at_buf); |
|
|
|
} |
|
|
|
BSP_LTE_SendCmd(AT_CEREG0, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL); |
|
|
|
if (BSP_LTE_SendCmd(AT_CGATT, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL) == 0) { |
|
|
|
parse_cgatt_snapshot((const char *)lte_dev.at_buf); |
|
|
|
} |
|
|
|
if (BSP_LTE_SendCmd(AT_QIACT, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL) == 0) { |
|
|
|
parse_qiact_snapshot((const char *)lte_dev.at_buf); |
|
|
|
} |
|
|
|
if (BSP_LTE_SendCmd(AT_QNWINFO, DEFAULT_EXPECT, DEFAULT_ERROR, |
|
|
|
LTE_CMD_WAIT_NORMAL) == 0) { |
|
|
|
parse_qnwinfo_snapshot((const char *)lte_dev.at_buf); |
|
|
|
} |
|
|
|
network->csq = BSP_LTE_GetCSQ(); |
|
|
|
if (network->csq >= 0) lte_dev.csq = network->csq; |
|
|
|
} |
|
|
|
|
|
|
|
int16_t BSP_LTE_SendCmd(const char *cmd, const char *expect, const char *error, |
|
|
|
uint32_t waittime) { |
|
|
|
lte_diag_at_begin(cmd); |
|
|
|
@ -167,6 +447,7 @@ int16_t BSP_LTE_SendCmd(const char *cmd, const char *expect, const char *error, |
|
|
|
sizeof(lte_dev.at_buf)); |
|
|
|
if (read_len > 0) { |
|
|
|
lte_diag_uart2_rx((rt_size_t)read_len); |
|
|
|
lte_diag_modem_text(lte_dev.at_buf, (rt_size_t)read_len); |
|
|
|
} |
|
|
|
|
|
|
|
LOG_I("%s\n", lte_dev.at_buf); |
|
|
|
|