@@ -193,7 +193,7 @@ int db_open_int_telemetry_udp_socket() {
193193 * @param data Buffer with the data to send
194194 * @param data_length Length of the data in the buffer
195195 */
196- void send_to_all_udp_clients (udp_conn_list_t * n_udp_conn_list , const uint8_t * data , uint data_length ) {
196+ void db_send_to_all_udp_clients (udp_conn_list_t * n_udp_conn_list , const uint8_t * data , uint data_length ) {
197197 for (int i = 0 ; i < n_udp_conn_list -> size ; i ++ ) { // send to all UDP clients
198198 int sent = sendto (n_udp_conn_list -> udp_socket , data , data_length , 0 ,
199199 (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
206206}
207207
208208/**
209+ * 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
210+ * and finally send it over the air)
211+ *
212+ * @param data Pointer to the payload buffer
213+ * @param data_length Length of the payload data. Must not be bigger than DB_ESPNOW_PAYLOAD_MAXSIZE - fails otherwise
214+ */
215+ void db_send_to_all_espnow (uint8_t data [], const uint16_t * data_length ) {
216+ db_espnow_queue_event_t evt ;
217+ evt .data = malloc (* data_length );
218+ memcpy (evt .data , data , * data_length );
219+ evt .data_len = * data_length ;
220+ evt .packet_type = DB_ESP_NOW_PACKET_TYPE_DATA ;
221+ if (xQueueSend (db_espnow_send_queue , & evt , ESPNOW_MAXDELAY ) != pdTRUE ) {
222+ ESP_LOGW (TAG , "Send to db_espnow_send_queue queue fail" );
223+ free (evt .data );
224+ } else {
225+ // all good
226+ }
227+ }
228+
229+ /**
230+ * Main call for sending anything over the air.
209231 * Send to all connected TCP & UDP clients or broadcast via ESP-NOW depending on the mode (DB_WIFI_MODE) we are currently in.
210232 * Typically called by a function that read from UART.
211233 *
234+ * When in ESP-NOW mode the packets will be split if they are bigger than DB_ESPNOW_PAYLOAD_MAXSIZE.
235+ *
212236 * @param tcp_clients Array of socket IDs for the TCP clients
213237 * @param udp_conn Structure handling the UDP connection
214238 * @param data payload to send
215239 * @param data_length Length of payload to send
216240 */
217- void send_to_all_clients (int tcp_clients [], udp_conn_list_t * n_udp_conn_list , uint8_t data [], uint data_length ) {
241+ void db_send_to_all_clients (int tcp_clients [], udp_conn_list_t * n_udp_conn_list , uint8_t data [], uint16_t data_length ) {
218242 if (DB_WIFI_MODE != DB_WIFI_MODE_ESPNOW_AIR && DB_WIFI_MODE != DB_WIFI_MODE_ESPNOW_GND ) {
219- send_to_all_tcp_clients (tcp_clients , data , data_length );
220- send_to_all_udp_clients (n_udp_conn_list , data , data_length );
243+ db_send_to_all_tcp_clients (tcp_clients , data , data_length );
244+ db_send_to_all_udp_clients (n_udp_conn_list , data , data_length );
221245 } else {
222246 // ESP-NOW mode
223- db_espnow_queue_event_t evt ;
224- evt .data = malloc (data_length );
225- memcpy (evt .data , data , data_length );
226- evt .data_len = data_length ;
227- evt .packet_type = DB_ESP_NOW_PACKET_TYPE_DATA ;
228- if (xQueueSend (db_espnow_send_queue , & evt , ESPNOW_MAXDELAY ) != pdTRUE ) {
229- ESP_LOGW (TAG , "Send to db_espnow_send_queue queue fail" );
230- free (evt .data );
247+ // Check if payload fits into one ESP-NOW packet
248+ if (data_length > DB_ESPNOW_PAYLOAD_MAXSIZE ) {
249+ // data not properly sized (MAVLink implementation already sends properly sized chunks but MSP parser will not)
250+ // split into multiple packets
251+ uint16_t sent_bytes = 0 ;
252+ uint16_t next_chunk_len = 0 ;
253+ do {
254+ next_chunk_len = data_length - sent_bytes ;
255+ if (next_chunk_len > DB_ESPNOW_PAYLOAD_MAXSIZE ) {
256+ next_chunk_len = DB_ESPNOW_PAYLOAD_MAXSIZE ;
257+ } else {
258+ // do nothing - chunk will fit into the ESP-NOW payload field
259+ }
260+ db_send_to_all_espnow (& data [sent_bytes ], & next_chunk_len );
261+ sent_bytes += next_chunk_len ;
262+ } while (sent_bytes < data_length );
231263 } else {
232- // all good
264+ // packet is properly sized - send to ESP-NOW outbound queue
265+ db_send_to_all_espnow (data , & data_length );
233266 }
234267 }
235268}
@@ -416,7 +449,7 @@ _Noreturn void control_module_esp_now(){
416449 if (db_uart_write_queue != NULL && xQueueReceive (db_uart_write_queue , & db_espnow_uart_evt , 0 ) == pdTRUE ) {
417450 if (DB_SERIAL_PROTOCOL == DB_SERIAL_PROTOCOL_MAVLINK ) {
418451 // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link.
419- // We can not write to serial first since we might inject packets and do not know when to do so to not "destroy" an existign packet
452+ // We can not write to serial first since we might inject packets and do not know when to do so to not "destroy" an existing packet
420453 db_parse_mavlink_from_radio (NULL , NULL , db_espnow_uart_evt .data , db_espnow_uart_evt .data_len );
421454 } else {
422455 // no parsing with any other protocol - transparent here - just pass through
0 commit comments