diff --git a/main/db_esp32_control.c b/main/db_esp32_control.c index 36579e8..3f46e67 100644 --- a/main/db_esp32_control.c +++ b/main/db_esp32_control.c @@ -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, @@ -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); } } } diff --git a/main/db_esp32_control.h b/main/db_esp32_control.h index 895f5fa..61ff6ab 100644 --- a/main/db_esp32_control.h +++ b/main/db_esp32_control.h @@ -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 diff --git a/main/db_mavlink_msgs.c b/main/db_mavlink_msgs.c index 74806a9..982c965 100644 --- a/main/db_mavlink_msgs.c +++ b/main/db_mavlink_msgs.c @@ -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. @@ -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 } @@ -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: { diff --git a/main/db_serial.c b/main/db_serial.c index 96d3461..6dbddda 100644 --- a/main/db_serial.c +++ b/main/db_serial.c @@ -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(<m_frame_buffer[ltm_frames_in_buffer_pnt], db_msp_ltm_port->ltm_frame_buffer, @@ -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; @@ -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!"); } @@ -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 @@ -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); @@ -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 } diff --git a/main/tcp_server.c b/main/tcp_server.c index 6c31f5f..37deebe 100644 --- a/main/tcp_server.c +++ b/main/tcp_server.c @@ -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); diff --git a/main/tcp_server.h b/main/tcp_server.h index 494d051..7d9e834 100644 --- a/main/tcp_server.h +++ b/main/tcp_server.h @@ -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