Skip to content

Commit

Permalink
Fixed MSP over ESP-NOW bug, minor refractoring
Browse files Browse the repository at this point in the history 
MSP parser was sending too big packets via ESP-NOW, split packets into multiple packets.
  • Loading branch information
@seeul8er
seeul8er committed Oct 14, 2024
1 parent 71fc51c commit 366f4e4
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 32 deletions.
59 changes: 46 additions & 13 deletions main/db_esp32_control.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,7 +193,7 @@ int db_open_int_telemetry_udp_socket() {
* @param data Buffer with the data to send
* @param data_length Length of the data in the buffer
*/
void send_to_all_udp_clients(udp_conn_list_t *n_udp_conn_list, const uint8_t *data, uint data_length) {
void db_send_to_all_udp_clients(udp_conn_list_t *n_udp_conn_list, const uint8_t *data, uint data_length) {
for (int i = 0; i < n_udp_conn_list->size; i++) { // send to all UDP clients
int sent = sendto(n_udp_conn_list->udp_socket, data, data_length, 0,
(struct sockaddr *) &n_udp_conn_list->db_udp_clients[i].udp_client,
Expand All @@ -206,30 +206,63 @@ void send_to_all_udp_clients(udp_conn_list_t *n_udp_conn_list, const uint8_t *da
}

/**
* Adds a payload to be sent via ESP-NOW to the ESP-NOW queue (where the esp-now task will pick it up, encrypt, package
* and finally send it over the air)
*
* @param data Pointer to the payload buffer
* @param data_length Length of the payload data. Must not be bigger than DB_ESPNOW_PAYLOAD_MAXSIZE - fails otherwise
*/
void db_send_to_all_espnow(uint8_t data[], const uint16_t *data_length) {
db_espnow_queue_event_t evt;
evt.data = malloc(*data_length);
memcpy(evt.data, data, *data_length);
evt.data_len = *data_length;
evt.packet_type = DB_ESP_NOW_PACKET_TYPE_DATA;
if (xQueueSend(db_espnow_send_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) {
ESP_LOGW(TAG, "Send to db_espnow_send_queue queue fail");
free(evt.data);
} else {
// all good
}
}

/**
* Main call for sending anything over the air.
* Send to all connected TCP & UDP clients or broadcast via ESP-NOW depending on the mode (DB_WIFI_MODE) we are currently in.
* Typically called by a function that read from UART.
*
* When in ESP-NOW mode the packets will be split if they are bigger than DB_ESPNOW_PAYLOAD_MAXSIZE.
*
* @param tcp_clients Array of socket IDs for the TCP clients
* @param udp_conn Structure handling the UDP connection
* @param data payload to send
* @param data_length Length of payload to send
*/
void send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, uint8_t data[], uint data_length) {
void db_send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, uint8_t data[], uint16_t data_length) {
if (DB_WIFI_MODE != DB_WIFI_MODE_ESPNOW_AIR && DB_WIFI_MODE != DB_WIFI_MODE_ESPNOW_GND) {
send_to_all_tcp_clients(tcp_clients, data, data_length);
send_to_all_udp_clients(n_udp_conn_list, data, data_length);
db_send_to_all_tcp_clients(tcp_clients, data, data_length);
db_send_to_all_udp_clients(n_udp_conn_list, data, data_length);
} else {
// ESP-NOW mode
db_espnow_queue_event_t evt;
evt.data = malloc(data_length);
memcpy(evt.data, data, data_length);
evt.data_len = data_length;
evt.packet_type = DB_ESP_NOW_PACKET_TYPE_DATA;
if (xQueueSend(db_espnow_send_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) {
ESP_LOGW(TAG, "Send to db_espnow_send_queue queue fail");
free(evt.data);
// Check if payload fits into one ESP-NOW packet
if (data_length > DB_ESPNOW_PAYLOAD_MAXSIZE) {
// data not properly sized (MAVLink implementation already sends properly sized chunks but MSP parser will not)
// split into multiple packets
uint16_t sent_bytes = 0;
uint16_t next_chunk_len = 0;
do {
next_chunk_len = data_length - sent_bytes;
if (next_chunk_len > DB_ESPNOW_PAYLOAD_MAXSIZE) {
next_chunk_len = DB_ESPNOW_PAYLOAD_MAXSIZE;
} else {
// do nothing - chunk will fit into the ESP-NOW payload field
}
db_send_to_all_espnow(&data[sent_bytes], &next_chunk_len);
sent_bytes += next_chunk_len;
} while (sent_bytes < data_length);
} else {
// all good
// packet is properly sized - send to ESP-NOW outbound queue
db_send_to_all_espnow(data, &data_length);
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion main/db_esp32_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void control_module();
udp_conn_list_t *udp_client_list_create();
void udp_client_list_destroy(udp_conn_list_t *n_udp_conn_list);
bool add_to_known_udp_clients(udp_conn_list_t *n_udp_conn_list, struct db_udp_client_t new_db_udp_client, bool save_to_nvm);
void send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, uint8_t data[], uint data_length);
void db_send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, uint8_t data[], uint16_t data_length);
bool remove_from_known_udp_clients(udp_conn_list_t *n_udp_conn_list, struct db_udp_client_t new_db_udp_client);

#endif //DB_ESP32_DB_ESP32_CONTROL_H
6 changes: 3 additions & 3 deletions main/db_mavlink_msgs.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -350,7 +350,7 @@ void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_
uint16_t len = fmav_msg_radio_status_encode_to_frame_buf(buff, db_get_mav_sys_id(),
db_get_mav_comp_id(), &payload_r,
fmav_status);
send_to_all_clients(tcp_clients, udp_conns, buff, len);
db_send_to_all_clients(tcp_clients, udp_conns, buff, len);
} else if (DB_WIFI_MODE == DB_WIFI_MODE_AP && wifi_sta_list.num > 0) {
// we assume ESP32 is not used in DB_WIFI_MODE_AP on the ground but only on the drone side
// ToDo: Only the RSSI of the first client is considered.
Expand All @@ -359,7 +359,7 @@ void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_
uint16_t len = fmav_msg_radio_status_encode_to_frame_buf(buff, db_get_mav_sys_id(),
db_get_mav_comp_id(), &payload_r,
fmav_status);
send_to_all_clients(tcp_clients, udp_conns, buff, len);
db_send_to_all_clients(tcp_clients, udp_conns, buff, len);
} else {
// In AP LR mode the clients will send the info to the GCS
}
Expand All @@ -369,7 +369,7 @@ void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_
// ToDo: Check if that is a good idea or push to extra thread
uint16_t length = db_create_heartbeat(buff, fmav_status);
// Send heartbeat to GND clients: Every ESP32 no matter its role or mode is emitting a heartbeat
send_to_all_clients(tcp_clients, udp_conns, buff, length);
db_send_to_all_clients(tcp_clients, udp_conns, buff, length);
} // do not react to heartbeats received via wireless interface - reaction to serial is sufficient
break;
case FASTMAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
Expand Down
27 changes: 14 additions & 13 deletions main/db_serial.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ void db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_
if (parse_msp_ltm_byte(db_msp_ltm_port, serial_byte)) {
msp_message_buffer[(*serial_read_bytes - 1)] = serial_byte;
if (db_msp_ltm_port->parse_state == MSP_PACKET_RECEIVED) {
send_to_all_clients(tcp_clients, udp_connection, msp_message_buffer, *serial_read_bytes);
db_send_to_all_clients(tcp_clients, udp_connection, msp_message_buffer, *serial_read_bytes);
*serial_read_bytes = 0;
} else if (db_msp_ltm_port->parse_state == LTM_PACKET_RECEIVED) {
memcpy(&ltm_frame_buffer[ltm_frames_in_buffer_pnt], db_msp_ltm_port->ltm_frame_buffer,
Expand All @@ -189,7 +189,7 @@ void db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_
ltm_frames_in_buffer++;
if (ltm_frames_in_buffer == DB_LTM_FRAME_NUM_BUFFER &&
(DB_LTM_FRAME_NUM_BUFFER <= MAX_LTM_FRAMES_IN_BUFFER)) {
send_to_all_clients(tcp_clients, udp_connection, ltm_frame_buffer, *serial_read_bytes);
db_send_to_all_clients(tcp_clients, udp_connection, ltm_frame_buffer, *serial_read_bytes);
ESP_LOGD(TAG, "Sent %i LTM message(s) to telemetry port!", DB_LTM_FRAME_NUM_BUFFER);
ltm_frames_in_buffer = 0;
ltm_frames_in_buffer_pnt = 0;
Expand All @@ -216,7 +216,7 @@ void db_route_mavlink_response(uint8_t *buffer, uint16_t length, enum DB_MAVLINK
if (origin == DB_MAVLINK_DATA_ORIGIN_SERIAL) {
write_to_serial(buffer, length);
} else if (origin == DB_MAVLINK_DATA_ORIGIN_RADIO) {
send_to_all_clients(tcp_clients, udp_conns, buffer, length);
db_send_to_all_clients(tcp_clients, udp_conns, buffer, length);
} else {
ESP_LOGE(TAG, "Unknown msg origin. Do not know on which link to respond!");
}
Expand Down Expand Up @@ -309,7 +309,7 @@ void db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns,
last_tick = current_tick; // reset timeout
// flush buffer to air interface -> send what we have in the buffer (already parsed)
if (*serial_buff_pos > 0) {
send_to_all_clients(tcp_clients, udp_conns, serial_buffer, *serial_buff_pos);
db_send_to_all_clients(tcp_clients, udp_conns, serial_buffer, *serial_buff_pos);
*serial_buff_pos = 0;
} else {
// do nothing since buffer is empty anyway
Expand All @@ -333,20 +333,21 @@ void db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns,
mav_msg_counter++;
// Check if the new message will fit in the buffer
if (*serial_buff_pos == 0 && result.frame_len > DB_TRANS_BUF_SIZE) {
// frame_len is bigger than DB_TRANS_BUF_SIZE -> Split into multiple messages since e.g. ESP-NOW can only handle 250 bytes which is less than MAVLink max msg length
// frame_len is bigger than DB_TRANS_BUF_SIZE -> Split into multiple messages since
// e.g. ESP-NOW can only handle DB_ESPNOW_PAYLOAD_MAXSIZE bytes which is less than MAVLink max msg length
uint16_t sent_bytes = 0;
uint16_t next_chuck_len = 0;
uint16_t next_chunk_len = 0;
do {
next_chuck_len = result.frame_len - sent_bytes;
if (next_chuck_len > DB_TRANS_BUF_SIZE) {
next_chuck_len = DB_TRANS_BUF_SIZE;
next_chunk_len = result.frame_len - sent_bytes;
if (next_chunk_len > DB_TRANS_BUF_SIZE) {
next_chunk_len = DB_TRANS_BUF_SIZE;
} else {}
send_to_all_clients(tcp_clients, udp_conns, &mav_parser_rx_buf[sent_bytes], next_chuck_len);
sent_bytes += next_chuck_len;
db_send_to_all_clients(tcp_clients, udp_conns, &mav_parser_rx_buf[sent_bytes], next_chunk_len);
sent_bytes += next_chunk_len;
} while (sent_bytes < result.frame_len);
} else if (*serial_buff_pos + result.frame_len > DB_TRANS_BUF_SIZE) {
// New message won't fit into the buffer, send buffer first
send_to_all_clients(tcp_clients, udp_conns, serial_buffer, *serial_buff_pos);
db_send_to_all_clients(tcp_clients, udp_conns, serial_buffer, *serial_buff_pos);
*serial_buff_pos = 0;
// copy the new message to the uart send buffer and set buffer position
memcpy(&serial_buffer[*serial_buff_pos], mav_parser_rx_buf, result.frame_len);
Expand Down Expand Up @@ -413,7 +414,7 @@ void db_read_serial_parse_transparent(int tcp_clients[], udp_conn_list_t *udp_co
}
// send serial data over the air interface
if (*serial_read_bytes >= DB_TRANS_BUF_SIZE || (serial_read_timeout_reached && *serial_read_bytes > 0)) {
send_to_all_clients(tcp_clients, udp_connection, serial_buffer, *serial_read_bytes);
db_send_to_all_clients(tcp_clients, udp_connection, serial_buffer, *serial_read_bytes);
*serial_read_bytes = 0; // reset buffer position
serial_read_timeout_reached = false; // reset serial read timeout
}
Expand Down
2 changes: 1 addition & 1 deletion main/tcp_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ int open_tcp_server(int port) {
return listen_sock;
}

void send_to_all_tcp_clients(const int tcp_clients[], uint8_t data[], uint data_length) {
void db_send_to_all_tcp_clients(const int tcp_clients[], uint8_t data[], uint data_length) {
for (int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) {
if (tcp_clients[i] > 0) {
ESP_LOGD(TCP_TAG, "Sending %i bytes", data_length);
Expand Down
2 changes: 1 addition & 1 deletion main/tcp_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,6 @@
#define TCP_BUFF_SIZ 4096

int open_tcp_server(int port);
void send_to_all_tcp_clients(const int tcp_clients[], uint8_t data[], uint data_length);
void db_send_to_all_tcp_clients(const int tcp_clients[], uint8_t data[], uint data_length);

#endif //DB_ESP32_TCP_SERVER_H

0 comments on commit 366f4e4

Please sign in to comment.