diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..d90731a --- /dev/null +++ b/.clang-format @@ -0,0 +1,79 @@ +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: false +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: TopLevel +AlwaysBreakAfterReturnType: TopLevelDefinitions +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: true +BinPackArguments: false +BinPackParameters: true +BraceWrapping: + AfterClass: true + AfterControlStatement: false + AfterEnum: true + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: false + BeforeElse: true + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Custom +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +ColumnLimit: 79 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IndentCaseLabels: false +IndentWidth: 2 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: false +PointerAlignment: Right +ReflowComments: true +SortIncludes: false +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 8 +UseTab: Never \ No newline at end of file diff --git a/main/db_ble.c b/main/db_ble.c index 61d4276..c3e7a91 100644 --- a/main/db_ble.c +++ b/main/db_ble.c @@ -1,5 +1,4 @@ -#include -/*************************************************************************************************************************** +/****************************************************************************** * @file db_ble.c * @brief DroneBridge ESP32 BLE Source File * @@ -7,7 +6,8 @@ * * This file contains the NimBLE Initialisation functions to start the BLE Host * stack and start advertising the BLE Service - * @authors Witty-Wizard further modified by Wolfgang Christl + * @authors Witty-Wizard further modified by + *Wolfgang Christl * @license Apache License, Version 2.0 * * Licensed under the Apache License, Version 2.0 (the "License"); @@ -21,21 +21,23 @@ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - **************************************************************************************************************************/ + *******************************************************************************/ -/*************************************************************************************************************************** +/****************************************************************************** * Header Inclusion - **************************************************************************************************************************/ + *******************************************************************************/ #include "db_ble.h" +#include -/*************************************************************************************************************************** +/****************************************************************************** * ESP-IDF APIs - **************************************************************************************************************************/ + *******************************************************************************/ #include "esp_check.h" -/*************************************************************************************************************************** +/****************************************************************************** * NimBLE Host APIs - **************************************************************************************************************************/ + *******************************************************************************/ +#include "globals.h" #include "host/ble_hs.h" #include "host/ble_uuid.h" #include "host/util/util.h" @@ -44,25 +46,24 @@ #include "nimble/nimble_port_freertos.h" #include "services/gap/ble_svc_gap.h" #include "services/gatt/ble_svc_gatt.h" -#include "globals.h" -/*************************************************************************************************************************** +/****************************************************************************** * MACROS - **************************************************************************************************************************/ + *******************************************************************************/ #define TAG "DB_BLE" -/*************************************************************************************************************************** +/****************************************************************************** * Public Variables - **************************************************************************************************************************/ + *******************************************************************************/ QueueHandle_t db_uart_write_queue_ble; QueueHandle_t db_uart_read_queue_ble; -/*************************************************************************************************************************** +/****************************************************************************** * Private Variables - **************************************************************************************************************************/ + *******************************************************************************/ static uint16_t active_conn_handle = BLE_HS_CONN_HANDLE_NONE; static uint8_t own_addr_type; -static uint8_t addr_val[6] = {0}; +static uint8_t addr_val[6] = { 0 }; static bool conn_handle_subs[CONFIG_BT_NIMBLE_MAX_CONNECTIONS + 1]; static char *DEVICE_NAME = "DroneBridge"; static uint16_t ble_spp_svc_gatt_notify_val_handle; @@ -71,28 +72,28 @@ static uint16_t ble_spp_svc_gatt_notify_cmd_handle; static uint16_t ble_spp_svc_gatt_write_cmd_handle; static uint16_t ble_mtu_size = 23; -/*************************************************************************************************************************** +/****************************************************************************** * Library Function Declaration - **************************************************************************************************************************/ + *******************************************************************************/ -/*************************************************************************************************************************** +/****************************************************************************** * @brief Library function, the function is defined in source file and not the * header file for the linker to work this function needs to be defined. */ void ble_store_config_init(); -/*************************************************************************************************************************** +/****************************************************************************** * Private Function Declaration - **************************************************************************************************************************/ + *******************************************************************************/ static int gap_event_handler(struct ble_gap_event *event, void *arg); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Starts the BLE advertising process. - **************************************************************************************************************************/ + *******************************************************************************/ static void start_advertising(void); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Format Address to string * * @warning Does not check for pointer overflow, make sure to allocate enough @@ -102,57 +103,58 @@ static void start_advertising(void); * convert to character array of the form xx:xx:xx:xx:xx:xx. * * @param [out] addr_str pointer to the output character array - **************************************************************************************************************************/ + *******************************************************************************/ inline static void format_addr(char *addr_str, uint8_t addr[]); -/*************************************************************************************************************************** +/****************************************************************************** * @brief FreeRTOS task to run NimBLE port * * @param [in] params Input parameters to the FreeRTOS Task - **************************************************************************************************************************/ + *******************************************************************************/ static void nimble_host_task(void *params); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Function to initialize NimBLE host config - **************************************************************************************************************************/ + *******************************************************************************/ static void nimble_host_config_init(); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Function to initialize gatt service - **************************************************************************************************************************/ + *******************************************************************************/ static int gatt_svr_init(); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Function to initialize advertising - **************************************************************************************************************************/ + *******************************************************************************/ static void adv_init(void); -/*************************************************************************************************************************** +/****************************************************************************** * Callback Function Declaration - **************************************************************************************************************************/ + *******************************************************************************/ -/*************************************************************************************************************************** +/****************************************************************************** * @brief Callback Function called when the BLE host stack is reset - **************************************************************************************************************************/ + *******************************************************************************/ static void on_stack_reset(int reason); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Callback Function called when the BLE host stack synchronises - **************************************************************************************************************************/ + *******************************************************************************/ static void on_stack_sync(void); -/*************************************************************************************************************************** +/****************************************************************************** * @brief GATT service register callback function - **************************************************************************************************************************/ -static void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); + *******************************************************************************/ +static void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, + void *arg); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Callback function to handle gap events - **************************************************************************************************************************/ + *******************************************************************************/ static int gap_event_handler(struct ble_gap_event *event, void *arg); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Handler for BLE GATT service. * * This Callback function handles BLE GATT service events. @@ -163,575 +165,680 @@ static int gap_event_handler(struct ble_gap_event *event, void *arg); * @param arg Additional arguments. * * @return Status code indicating the result of the handler. - **************************************************************************************************************************/ -static int -ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg); + *******************************************************************************/ +static int ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg); -/*************************************************************************************************************************** +/****************************************************************************** * BLE GATT service table - **************************************************************************************************************************/ + *******************************************************************************/ static const struct ble_gatt_svc_def new_ble_svc_gatt_defs[] = { + { + /*** Service: Serial Interface */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_UUID16), + .characteristics = + (struct ble_gatt_chr_def[]){ + { + /* Write-only characteristic */ + .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_CHR_WRITE_UUID16), + .access_cb = ble_svc_gatt_handler, + .val_handle = &ble_spp_svc_gatt_write_val_handle, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ, + }, { - /*** Service: Serial Interface */ - .type = BLE_GATT_SVC_TYPE_PRIMARY, - .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_UUID16), - .characteristics = - (struct ble_gatt_chr_def[]) { - { - /* Write-only characteristic */ - .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_CHR_WRITE_UUID16), - .access_cb = ble_svc_gatt_handler, - .val_handle = &ble_spp_svc_gatt_write_val_handle, - .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ, - }, - { - /* Notify-only characteristic */ - .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_CHR_NOTIFY_UUID16), - .access_cb = ble_svc_gatt_handler, // No direct access required, only notify - .val_handle = &ble_spp_svc_gatt_notify_val_handle, - .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, - }, - { - /* Write-only characteristic */ - .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CMD_WRITE_UUID16), - .access_cb = ble_svc_gatt_handler, // No direct access required, only notify - .val_handle = &ble_spp_svc_gatt_write_cmd_handle, - .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ, - }, - { - /* Notify-only characteristic */ - .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CMD_NOTIFY_UUID16), - .access_cb = ble_svc_gatt_handler, // No direct access required, only notify - .val_handle = &ble_spp_svc_gatt_notify_cmd_handle, - .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, - }, - {0}, /* No more characteristics */ - }, + /* Notify-only characteristic */ + .uuid = BLE_UUID16_DECLARE(BLE_SVC_SERIAL_CHR_NOTIFY_UUID16), + .access_cb = ble_svc_gatt_handler, // No direct access + // required, only notify + .val_handle = &ble_spp_svc_gatt_notify_val_handle, + .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, }, - {0}, /* No more services. */ + { + /* Write-only characteristic */ + .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CMD_WRITE_UUID16), + .access_cb = ble_svc_gatt_handler, // No direct access + // required, only notify + .val_handle = &ble_spp_svc_gatt_write_cmd_handle, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ, + }, + { + /* Notify-only characteristic */ + .uuid = BLE_UUID16_DECLARE(BLE_SVC_SPP_CMD_NOTIFY_UUID16), + .access_cb = ble_svc_gatt_handler, // No direct access + // required, only notify + .val_handle = &ble_spp_svc_gatt_notify_cmd_handle, + .flags = BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ, + }, + { 0 }, /* No more characteristics */ + }, + }, + { 0 }, /* No more services. */ }; -/*************************************************************************************************************************** - * Private Function Definition - **************************************************************************************************************************/ +/****************************************************************************** + * Private Function + *Definitionsssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssssss + *******************************************************************************/ /** * Sends supplied buffer via BLE notification to all connected clients * @param data Pointer to send buffer * @param data_len Length of send buffer - * @return 0 on success for all clients. -1 on failure to allocate buffer or ble_gatts_notify_custom return value + * @return 0 on success for all clients. -1 on failure to allocate buffer or + * ble_gatts_notify_custom return value */ - int db_ble_send_data(const uint8_t *data, uint16_t data_len) { - for (int i = 0; i < CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) { - /* Check if the client has subscribed to notifications */ - if (conn_handle_subs[i]) { - - /* Write to the characteristics */ - int rc = ble_gattc_write_flat(i,ble_spp_svc_gatt_notify_val_handle, - data, /* Data pointer */ - data_len, /* Data length */ - NULL, /* Callback function */ - NULL /* Callback function arguments */ - ); - if (rc != 0) { - ESP_LOGE(TAG, "Failed to write characteristic value (rc = %d)", rc); - return rc; - } - - /* Now send a notification with the updated characteristic value */ - rc = ble_gatts_notify(i, ble_spp_svc_gatt_notify_val_handle); - if (rc != 0) { - ESP_LOGE(TAG, "Error sending BLE notification rc = %d", rc); - return rc; - } - } +int +db_ble_send_data(const uint8_t *data, uint16_t data_len) +{ + for(int i = 0; i < CONFIG_BT_NIMBLE_MAX_CONNECTIONS; i++) { + /* Check if the client has subscribed to notifications */ + if(conn_handle_subs[i]) { + + /* Write to the characteristics */ + int rc = ble_gattc_write_flat(i, + ble_spp_svc_gatt_notify_val_handle, + data, /* Data pointer */ + data_len, /* Data length */ + NULL, /* Callback function */ + NULL /* Callback function arguments */ + ); + if(rc != 0) { + ESP_LOGE(TAG, "Failed to write characteristic value (rc = %d)", rc); + return rc; + } + + /* Now send a notification with the updated characteristic value */ + rc = ble_gatts_notify(i, ble_spp_svc_gatt_notify_val_handle); + if(rc != 0) { + ESP_LOGE(TAG, "Error sending BLE notification rc = %d", rc); + return rc; + } } - return 0; + } + return 0; } /** - * @brief Sends BLE data, automatically splitting it into chunks if it exceeds the MTU limit. + * @brief Sends BLE data, automatically splitting it into chunks if it exceeds + * the MTU limit. * * @param data Pointer to the data buffer to send. * @param data_len Length of the data in the buffer. - * @return 0 on success, negative error code on failure during sending. Returns 1 if data_len is 0. + * @return 0 on success, negative error code on failure during sending. Returns + * 1 if data_len is 0. */ -int send_ble_data_chunked(const uint8_t *data, uint16_t data_len) { - // NimBLE requires 3 bytes overhead for notifications (ATT header) - const uint16_t max_payload_size = ble_mtu_size > 3 ? ble_mtu_size - 3 : 0; - int ret = 0; - - if (max_payload_size == 0) { - ESP_LOGE(TAG, "Cannot send data, effective MTU is zero (ble_mtu_size: %d)", ble_mtu_size); - return -1; // Indicate error: MTU too small - } - - if (data == NULL || data_len == 0) { - ESP_LOGW(TAG, "No data provided to send_ble_data_chunked."); - return 1; // Indicate nothing was sent, but not necessarily an error state. +int +send_ble_data_chunked(const uint8_t *data, uint16_t data_len) +{ + // NimBLE requires 3 bytes overhead for notifications (ATT header) + const uint16_t max_payload_size = ble_mtu_size > 3 ? ble_mtu_size - 3 : 0; + int ret = 0; + + if(max_payload_size == 0) { + ESP_LOGE(TAG, + "Cannot send data, effective MTU is zero (ble_mtu_size: %d)", + ble_mtu_size); + return -1; // Indicate error: MTU too small + } + + if(data == NULL || data_len == 0) { + ESP_LOGW(TAG, "No data provided to send_ble_data_chunked."); + return 1; // Indicate nothing was sent, but not necessarily an error state. + } + + if(data_len <= max_payload_size) { + // Data fits in a single packet + ESP_LOGD(TAG, + "Sending single packet, len: %d, max_payload: %d", + data_len, + max_payload_size); + ret = db_ble_send_data(data, data_len); + if(ret != 0) { + ESP_LOGE(TAG, "Failed to send single packet, error: %d", ret); } - - if (data_len <= max_payload_size) { - // Data fits in a single packet - ESP_LOGD(TAG, "Sending single packet, len: %d, max_payload: %d", data_len, max_payload_size); - ret = db_ble_send_data(data, data_len); - if (ret != 0) { - ESP_LOGE(TAG, "Failed to send single packet, error: %d", ret); - } - } else { - // Data needs to be split into multiple packets - ESP_LOGD(TAG, "Splitting data (len: %d) into chunks (max_payload: %d)", data_len, max_payload_size); - uint16_t offset = 0; - while (offset < data_len) { - // Calculate the size of the current chunk - uint16_t chunk_size = (data_len - offset > max_payload_size) ? - max_payload_size : - (data_len - offset); - - ESP_LOGD(TAG, "Sending chunk, offset: %d, chunk_size: %d", offset, chunk_size); - - // Send the current chunk (pointer arithmetic to get the chunk start) - ret = db_ble_send_data(data + offset, chunk_size); - if (ret != 0) { - ESP_LOGE(TAG, "Failed to send chunk at offset %d, error: %d", offset, ret); - break; // Stop sending further chunks on error - } - - // Move to the next chunk - offset += chunk_size; - } + } + else { + // Data needs to be split into multiple packets + ESP_LOGD(TAG, + "Splitting data (len: %d) into chunks (max_payload: %d)", + data_len, + max_payload_size); + uint16_t offset = 0; + while(offset < data_len) { + // Calculate the size of the current chunk + uint16_t chunk_size = (data_len - offset > max_payload_size) + ? max_payload_size + : (data_len - offset); + + ESP_LOGD( + TAG, "Sending chunk, offset: %d, chunk_size: %d", offset, chunk_size); + + // Send the current chunk (pointer arithmetic to get the chunk start) + ret = db_ble_send_data(data + offset, chunk_size); + if(ret != 0) { + ESP_LOGE( + TAG, "Failed to send chunk at offset %d, error: %d", offset, ret); + break; // Stop sending further chunks on error + } + + // Move to the next chunk + offset += chunk_size; } - return ret; // Return the status of the last send operation + } + return ret; // Return the status of the last send operation } /** * Main sending task for BLE data */ -_Noreturn static void db_ble_server_uart_task() { - DB_RADIO_IS_OFF = false; // we are about to send stuff - update this parameter to indicate to others - ESP_LOGI(TAG, "BLE server UART_task started"); - int delay_timer_cnt = 0; - db_ble_queue_event_t bleData; - while (true) { - // Waiting for UART event. - if (xQueueReceive(db_uart_read_queue_ble, &bleData, 0)) { - // Split into multiple packets if MTU is bigger than data to send - // Instead, we could also change DB_PARAM_SERIAL_PACK_SIZE dynamically based on the BLE MTU, but that can - // result in very small buffers for the UART task to read which may cause performance issues. - // The serial packets arrive here with the length of DB_PARAM_SERIAL_PACK_SIZE. Fragment again if necessary. - send_ble_data_chunked(bleData.data, bleData.data_len); - free(bleData.data); - } - if (delay_timer_cnt == 5000) { - /* all actions are non-blocking so allow some delay so that the IDLE task of FreeRTOS and the watchdog can run - read: https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines for reference */ - db_ble_request_rssi(&db_esp_signal_quality.air_rssi); // use this timer to get the RSSI updated - vTaskDelay(10 / portTICK_PERIOD_MS); - delay_timer_cnt = 0; - } else { - delay_timer_cnt++; - } +_Noreturn static void +db_ble_server_uart_task() +{ + DB_RADIO_IS_OFF = false; // we are about to send stuff - update this + // parameter to indicate to others + ESP_LOGI(TAG, "BLE server UART_task started"); + int delay_timer_cnt = 0; + db_ble_queue_event_t bleData; + while(true) { + // Waiting for UART event. + if(xQueueReceive(db_uart_read_queue_ble, &bleData, 0)) { + // Split into multiple packets if MTU is bigger than data to send + // Instead, we could also change DB_PARAM_SERIAL_PACK_SIZE dynamically + // based on the BLE MTU, but that can result in very small buffers for + // the UART task to read which may cause performance issues. The serial + // packets arrive here with the length of DB_PARAM_SERIAL_PACK_SIZE. + // Fragment again if necessary. + send_ble_data_chunked(bleData.data, bleData.data_len); + free(bleData.data); } - vTaskDelete(NULL); -} - -static int gap_event_handler(struct ble_gap_event *event, void *arg) { - struct ble_gap_conn_desc desc; - int rc = 0; - - switch (event->type) { - case BLE_GAP_EVENT_CONNECT: - /* A new connection was established or a connection attempt failed. */ - ESP_LOGI(TAG, "connection %s; status=%d", event->connect.status == 0 ? "established" : "failed", - event->connect.status); - if (event->connect.status == 0) { - active_conn_handle = event->connect.conn_handle; - ESP_LOGI(TAG, "Connected, conn_handle=%d", active_conn_handle); - rc = ble_gap_conn_find(active_conn_handle, &desc); - assert(rc == 0); - // print_conn_desc(&desc); - } - ESP_LOGI(TAG, "\n"); - if (event->connect.status != 0 || CONFIG_BT_NIMBLE_MAX_CONNECTIONS > 1) { - /* Connection failed or if multiple connection allowed; resume - * advertising. */ - start_advertising(); - } - break; - - case BLE_GAP_EVENT_DISCONNECT: - ESP_LOGI(TAG, "disconnect; reason=%d", event->disconnect.reason); - conn_handle_subs[event->disconnect.conn.conn_handle] = false; - // Mark connection handle as invalid - active_conn_handle = BLE_HS_CONN_HANDLE_NONE; - /* Connection terminated; resume advertising. */ - start_advertising(); - break; - - case BLE_GAP_EVENT_CONN_UPDATE: - /* The central has updated the connection parameters. */ - ESP_LOGI(TAG, "connection updated; status=%d ", event->conn_update.status); - rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc); - assert(rc == 0); - break; - - case BLE_GAP_EVENT_ADV_COMPLETE: - ESP_LOGI(TAG, "advertise complete; reason=%d", event->adv_complete.reason); - start_advertising(); - break; - - case BLE_GAP_EVENT_MTU: - ESP_LOGI(TAG, "mtu update event; conn_handle=%d cid=%d mtu=%d", event->mtu.conn_handle, - event->mtu.channel_id, - event->mtu.value); - ble_mtu_size = event->mtu.value; // ToDo: Support more than one MTU for all connections - break; - - case BLE_GAP_EVENT_SUBSCRIBE: - ESP_LOGI(TAG, "subscribe event; conn_handle=%d attr_handle=%d reason=%d prevn=%d curn=%d previ=%d curi=%d", - event->subscribe.conn_handle, event->subscribe.attr_handle, event->subscribe.reason, - event->subscribe.prev_notify, event->subscribe.cur_notify, event->subscribe.prev_indicate, - event->subscribe.cur_indicate); - conn_handle_subs[event->subscribe.conn_handle] = true; - break; - - default: - break; + if(delay_timer_cnt == 5000) { + /* all actions are non-blocking so allow some delay so that the IDLE task + of FreeRTOS and the watchdog can run read: + https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines + for reference */ + db_ble_request_rssi( + &db_esp_signal_quality + .air_rssi); // use this timer to get the RSSI updated + vTaskDelay(10 / portTICK_PERIOD_MS); + delay_timer_cnt = 0; } - return rc; + else { + delay_timer_cnt++; + } + } + vTaskDelete(NULL); } -static void start_advertising(void) { - struct ble_gap_adv_params adv_params; - struct ble_hs_adv_fields fields; - const char *name; - int rc; - - /** - * Set the advertisement data included in our advertisements: - * o Flags (indicates advertisement type and other general info). - * o Advertising tx power. - * o Device name. - * o 16-bit service UUIDs (alert notifications). - */ - - memset(&fields, 0, sizeof fields); - - /* Advertise two flags: - * o Discoverability in forthcoming advertisement (general) - * o BLE-only (BR/EDR unsupported). - */ - fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP; - - /* Indicate that the TX power level field should be included; have the - * stack fill this value automatically. This is done by assigning the - * special value BLE_HS_ADV_TX_PWR_LVL_AUTO. - */ - fields.tx_pwr_lvl_is_present = 1; - fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO; - - name = ble_svc_gap_device_name(); - fields.name = (uint8_t *) name; - fields.name_len = strlen(name); - fields.name_is_complete = 1; - - fields.uuids16 = (ble_uuid16_t[]) {BLE_UUID16_INIT(BLE_SVC_SERIAL_UUID16)}; - fields.num_uuids16 = 1; - fields.uuids16_is_complete = 1; - - rc = ble_gap_adv_set_fields(&fields); - if (rc != 0) { - ESP_LOGE(TAG, "error setting advertisement data; rc=%d\n", rc); - return; +static int +gap_event_handler(struct ble_gap_event *event, void *arg) +{ + struct ble_gap_conn_desc desc; + int rc = 0; + + switch(event->type) { + case BLE_GAP_EVENT_CONNECT: + /* A new connection was established or a connection attempt failed. */ + ESP_LOGI(TAG, + "connection %s; status=%d", + event->connect.status == 0 ? "established" : "failed", + event->connect.status); + if(event->connect.status == 0) { + active_conn_handle = event->connect.conn_handle; + ESP_LOGI(TAG, "Connected, conn_handle=%d", active_conn_handle); + rc = ble_gap_conn_find(active_conn_handle, &desc); + assert(rc == 0); + // print_conn_desc(&desc); } - - /* Begin advertising. */ - memset(&adv_params, 0, sizeof adv_params); - adv_params.conn_mode = BLE_GAP_CONN_MODE_UND; - adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN; - rc = ble_gap_adv_start(own_addr_type, // Type of address that stack should use - NULL, // Peer address for direct advertising, null for in-direct advertising - BLE_HS_FOREVER, // Duration of advertisement, BLE_HS_FOREVER for no expiration - &adv_params, // Additional arguments for advertismenet - gap_event_handler, // Gap event handler callback - NULL); // Callback arguments - if (rc != 0) { - ESP_LOGE(TAG, "error enabling advertisement; rc=%d\n", rc); - return; + ESP_LOGI(TAG, "\n"); + if(event->connect.status != 0 || CONFIG_BT_NIMBLE_MAX_CONNECTIONS > 1) { + /* Connection failed or if multiple connection allowed; resume + * advertising. */ + start_advertising(); } -} + break; + + case BLE_GAP_EVENT_DISCONNECT: + ESP_LOGI(TAG, "disconnect; reason=%d", event->disconnect.reason); + conn_handle_subs[event->disconnect.conn.conn_handle] = false; + // Mark connection handle as invalid + active_conn_handle = BLE_HS_CONN_HANDLE_NONE; + /* Connection terminated; resume advertising. */ + start_advertising(); + break; + + case BLE_GAP_EVENT_CONN_UPDATE: + /* The central has updated the connection parameters. */ + ESP_LOGI(TAG, "connection updated; status=%d ", event->conn_update.status); + rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc); + assert(rc == 0); + break; -inline static void format_addr(char *addr_str, uint8_t addr[]) { - sprintf(addr_str, "%02X:%02X:%02X:%02X:%02X:%02X", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + case BLE_GAP_EVENT_ADV_COMPLETE: + ESP_LOGI(TAG, "advertise complete; reason=%d", event->adv_complete.reason); + start_advertising(); + break; + + case BLE_GAP_EVENT_MTU: + ESP_LOGI(TAG, + "mtu update event; conn_handle=%d cid=%d mtu=%d", + event->mtu.conn_handle, + event->mtu.channel_id, + event->mtu.value); + ble_mtu_size = + event->mtu.value; // ToDo: Support more than one MTU for all connections + break; + + case BLE_GAP_EVENT_SUBSCRIBE: + ESP_LOGI(TAG, + "subscribe event; conn_handle=%d attr_handle=%d reason=%d " + "prevn=%d curn=%d previ=%d curi=%d", + event->subscribe.conn_handle, + event->subscribe.attr_handle, + event->subscribe.reason, + event->subscribe.prev_notify, + event->subscribe.cur_notify, + event->subscribe.prev_indicate, + event->subscribe.cur_indicate); + conn_handle_subs[event->subscribe.conn_handle] = true; + break; + + default: + break; + } + return rc; } -static int -ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { - switch (ctxt->op) { - case BLE_GATT_ACCESS_OP_READ_CHR: - ESP_LOGI(TAG, "Callback for read"); - break; - - case BLE_GATT_ACCESS_OP_WRITE_CHR: - ESP_LOGD(TAG, "Data received in write event,conn_handle = %x,attr_handle = %x", conn_handle, attr_handle); - struct os_mbuf *om = ctxt->om; // Get the received data buffer - int len = OS_MBUF_PKTLEN(om); // Get total length of received data - db_ble_queue_event_t bleData; // Create a struct instance - bleData.data = malloc(len); // Allocate memory for the data buffer - bleData.data_len = len; // Ensure len does not exceed buffer size - - if (bleData.data == NULL) { - ESP_LOGE(TAG, "Failed to allocate memory for BLE data of length %d", len); - return BLE_ATT_ERR_UNLIKELY; - } - - if (os_mbuf_copydata(om, 0, bleData.data_len, bleData.data) != 0) { - ESP_LOGE(TAG, "Failed to copy data from os_mbuf"); - free(bleData.data); - return BLE_ATT_ERR_UNLIKELY; - } - - // Send the received data to the FreeRTOS queue - if (xQueueSend(db_uart_write_queue_ble, &bleData, portMAX_DELAY) != pdPASS) { - ESP_LOGE(TAG, "Failed to send BLE data to queue"); - free(bleData.data); - } - break; - - default: - ESP_LOGI(TAG, "Default Callback"); - break; - } - return 0; +static void +start_advertising(void) +{ + struct ble_gap_adv_params adv_params; + struct ble_hs_adv_fields fields; + const char *name; + int rc; + + /** + * Set the advertisement data included in our advertisements: + * o Flags (indicates advertisement type and other general info). + * o Advertising tx power. + * o Device name. + * o 16-bit service UUIDs (alert notifications). + */ + + memset(&fields, 0, sizeof fields); + + /* Advertise two flags: + * o Discoverability in forthcoming advertisement (general) + * o BLE-only (BR/EDR unsupported). + */ + fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP; + + /* Indicate that the TX power level field should be included; have the + * stack fill this value automatically. This is done by assigning the + * special value BLE_HS_ADV_TX_PWR_LVL_AUTO. + */ + fields.tx_pwr_lvl_is_present = 1; + fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO; + + name = ble_svc_gap_device_name(); + fields.name = (uint8_t *)name; + fields.name_len = strlen(name); + fields.name_is_complete = 1; + + fields.uuids16 = (ble_uuid16_t[]){ BLE_UUID16_INIT(BLE_SVC_SERIAL_UUID16) }; + fields.num_uuids16 = 1; + fields.uuids16_is_complete = 1; + + rc = ble_gap_adv_set_fields(&fields); + if(rc != 0) { + ESP_LOGE(TAG, "error setting advertisement data; rc=%d\n", rc); + return; + } + + /* Begin advertising. */ + memset(&adv_params, 0, sizeof adv_params); + adv_params.conn_mode = BLE_GAP_CONN_MODE_UND; + adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN; + rc = + ble_gap_adv_start(own_addr_type, // Type of address that stack should use + NULL, // Peer address for direct advertising, null for + // in-direct advertising + BLE_HS_FOREVER, // Duration of advertisement, + // BLE_HS_FOREVER for no expiration + &adv_params, // Additional arguments for advertismenet + gap_event_handler, // Gap event handler callback + NULL); // Callback arguments + if(rc != 0) { + ESP_LOGE(TAG, "error enabling advertisement; rc=%d\n", rc); + return; + } } -static void adv_init(void) { - /* Local variables */ - int rc = 0; - char addr_str[18] = {0}; +inline static void +format_addr(char *addr_str, uint8_t addr[]) +{ + sprintf(addr_str, + "%02X:%02X:%02X:%02X:%02X:%02X", + addr[0], + addr[1], + addr[2], + addr[3], + addr[4], + addr[5]); +} - /* Make sure we have proper BT identity address set */ - rc = ble_hs_util_ensure_addr(0); - if (rc != 0) { - ESP_LOGE(TAG, "device does not have any available bt address!"); - return; +static int +ble_svc_gatt_handler(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + switch(ctxt->op) { + case BLE_GATT_ACCESS_OP_READ_CHR: + ESP_LOGI(TAG, "Callback for read"); + break; + + case BLE_GATT_ACCESS_OP_WRITE_CHR: + ESP_LOGD(TAG, + "Data received in write event,conn_handle = %x,attr_handle = %x", + conn_handle, + attr_handle); + struct os_mbuf *om = ctxt->om; // Get the received data buffer + int len = OS_MBUF_PKTLEN(om); // Get total length of received data + db_ble_queue_event_t bleData; // Create a struct instance + bleData.data = malloc(len); // Allocate memory for the data buffer + bleData.data_len = len; // Ensure len does not exceed buffer size + + if(bleData.data == NULL) { + ESP_LOGE( + TAG, "Failed to allocate memory for BLE data of length %d", len); + return BLE_ATT_ERR_UNLIKELY; } - /* Figure out BT address to use while advertising */ - rc = ble_hs_id_infer_auto(0, &own_addr_type); - if (rc != 0) { - ESP_LOGE(TAG, "failed to infer address type, error code: %d", rc); - return; + if(os_mbuf_copydata(om, 0, bleData.data_len, bleData.data) != 0) { + ESP_LOGE(TAG, "Failed to copy data from os_mbuf"); + free(bleData.data); + return BLE_ATT_ERR_UNLIKELY; } - /* Copy device address to addr_val */ - rc = ble_hs_id_copy_addr(own_addr_type, addr_val, NULL); - if (rc != 0) { - ESP_LOGE(TAG, "failed to copy device address, error code: %d", rc); - return; + // Send the received data to the FreeRTOS queue + if(xQueueSend(db_uart_write_queue_ble, &bleData, portMAX_DELAY) != + pdPASS) { + ESP_LOGE(TAG, "Failed to send BLE data to queue"); + free(bleData.data); } - format_addr(addr_str, addr_val); - ESP_LOGI(TAG, "device address: %s", addr_str); + break; - /* Start advertising. */ - start_advertising(); + default: + ESP_LOGI(TAG, "Default Callback"); + break; + } + return 0; } -int gap_init() { - /* Local Variable to store the error code */ - int rc = 0; +static void +adv_init(void) +{ + /* Local variables */ + int rc = 0; + char addr_str[18] = { 0 }; + + /* Make sure we have proper BT identity address set */ + rc = ble_hs_util_ensure_addr(0); + if(rc != 0) { + ESP_LOGE(TAG, "device does not have any available bt address!"); + return; + } + + /* Figure out BT address to use while advertising */ + rc = ble_hs_id_infer_auto(0, &own_addr_type); + if(rc != 0) { + ESP_LOGE(TAG, "failed to infer address type, error code: %d", rc); + return; + } + + /* Copy device address to addr_val */ + rc = ble_hs_id_copy_addr(own_addr_type, addr_val, NULL); + if(rc != 0) { + ESP_LOGE(TAG, "failed to copy device address, error code: %d", rc); + return; + } + format_addr(addr_str, addr_val); + ESP_LOGI(TAG, "device address: %s", addr_str); + + /* Start advertising. */ + start_advertising(); +} - /* Initialize GAP Server*/ - ble_svc_gap_init(); +int +gap_init() +{ + /* Local Variable to store the error code */ + int rc = 0; - /* Set GAP Device name*/ - rc = ble_svc_gap_device_name_set(DEVICE_NAME); + /* Initialize GAP Server*/ + ble_svc_gap_init(); - /* check for if any error occured */ - if (rc != 0) { - ESP_LOGE(TAG, "failed to set device name to %s, error code: %d", DEVICE_NAME, rc); - return rc; - } + /* Set GAP Device name*/ + rc = ble_svc_gap_device_name_set(DEVICE_NAME); - /* set the ble gap appearance tag */ - rc = ble_svc_gap_device_appearance_set(BLE_GAP_APPEARANCE_GENERIC_TAG); + /* check for if any error occured */ + if(rc != 0) { + ESP_LOGE( + TAG, "failed to set device name to %s, error code: %d", DEVICE_NAME, rc); + return rc; + } - /* check for error and return */ - if (rc != 0) { - ESP_LOGE(TAG, "failed to set device appearance, error code: %d", rc); - return rc; - } + /* set the ble gap appearance tag */ + rc = ble_svc_gap_device_appearance_set(BLE_GAP_APPEARANCE_GENERIC_TAG); - // no errors return + /* check for error and return */ + if(rc != 0) { + ESP_LOGE(TAG, "failed to set device appearance, error code: %d", rc); return rc; + } + + // no errors return + return rc; } -static int gatt_svr_init() { - int rc = 0; - ble_svc_gatt_init(); +static int +gatt_svr_init() +{ + int rc = 0; + ble_svc_gatt_init(); - rc = ble_gatts_count_cfg(new_ble_svc_gatt_defs); + rc = ble_gatts_count_cfg(new_ble_svc_gatt_defs); - if (rc != 0) { - return rc; - } + if(rc != 0) { + return rc; + } - rc = ble_gatts_add_svcs(new_ble_svc_gatt_defs); - if (rc != 0) { - return rc; - } + rc = ble_gatts_add_svcs(new_ble_svc_gatt_defs); + if(rc != 0) { + return rc; + } - return 0; + return 0; } -static void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) { - char buf[BLE_UUID_STR_LEN]; - - switch (ctxt->op) { - case BLE_GATT_REGISTER_OP_SVC: - ESP_LOGD(TAG, "registered service %s with handle=%d\n", ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf), - ctxt->svc.handle); - break; - - case BLE_GATT_REGISTER_OP_CHR: - ESP_LOGD(TAG,"registering characteristic %s with def_handle=%d val_handle=%d\n", - ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf), ctxt->chr.def_handle, ctxt->chr.val_handle); - break; - - case BLE_GATT_REGISTER_OP_DSC: - ESP_LOGD(TAG, "registering descriptor %s with handle=%d\n", - ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf), - ctxt->dsc.handle); - break; - - default: - assert(0); - break; - } +static void +gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) +{ + char buf[BLE_UUID_STR_LEN]; + + switch(ctxt->op) { + case BLE_GATT_REGISTER_OP_SVC: + ESP_LOGD(TAG, + "registered service %s with handle=%d\n", + ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf), + ctxt->svc.handle); + break; + + case BLE_GATT_REGISTER_OP_CHR: + ESP_LOGD( + TAG, + "registering characteristic %s with def_handle=%d val_handle=%d\n", + ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf), + ctxt->chr.def_handle, + ctxt->chr.val_handle); + break; + + case BLE_GATT_REGISTER_OP_DSC: + ESP_LOGD(TAG, + "registering descriptor %s with handle=%d\n", + ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf), + ctxt->dsc.handle); + break; + + default: + assert(0); + break; + } } -static void nimble_host_config_init() { - // Set Host callbacks - ble_hs_cfg.reset_cb = on_stack_reset; - ble_hs_cfg.sync_cb = on_stack_sync; - ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb; - ble_hs_cfg.store_status_cb = ble_store_util_status_rr; - ble_hs_cfg.sm_sc = 1; // enable secure connection - - /** WittyWizard: - * TO DO: Figure out what they do later, code works without them, but what they do I have no idea - * */ - - // ble_hs_cfg.sm_io_cap = CONFIG_EXAMPLE_IO_TYPE; - // #ifdef CONFIG_EXAMPLE_BONDING - // ble_hs_cfg.sm_bonding = 1; - // #endif - // #ifdef CONFIG_EXAMPLE_MITM - // ble_hs_cfg.sm_mitm = 1; - // #endif - // #ifdef CONFIG_EXAMPLE_USE_SC - // ble_hs_cfg.sm_sc = 1; - // #else - // ble_hs_cfg.sm_sc = 0; - // #endif - // #ifdef CONFIG_EXAMPLE_BONDING - // ble_hs_cfg.sm_our_key_dist = 1; - // ble_hs_cfg.sm_their_key_dist = 1; - // #endif - - // Store host configuration - ble_store_config_init(); +static void +nimble_host_config_init() +{ + // Set Host callbacks + ble_hs_cfg.reset_cb = on_stack_reset; + ble_hs_cfg.sync_cb = on_stack_sync; + ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb; + ble_hs_cfg.store_status_cb = ble_store_util_status_rr; + ble_hs_cfg.sm_sc = 1; // enable secure connection + + /** WittyWizard: + * TO DO: Figure out what they do later, code works without them, but what + * they do I have no idea + * */ + + // ble_hs_cfg.sm_io_cap = CONFIG_EXAMPLE_IO_TYPE; + // #ifdef CONFIG_EXAMPLE_BONDING + // ble_hs_cfg.sm_bonding = 1; + // #endif + // #ifdef CONFIG_EXAMPLE_MITM + // ble_hs_cfg.sm_mitm = 1; + // #endif + // #ifdef CONFIG_EXAMPLE_USE_SC + // ble_hs_cfg.sm_sc = 1; + // #else + // ble_hs_cfg.sm_sc = 0; + // #endif + // #ifdef CONFIG_EXAMPLE_BONDING + // ble_hs_cfg.sm_our_key_dist = 1; + // ble_hs_cfg.sm_their_key_dist = 1; + // #endif + + // Store host configuration + ble_store_config_init(); } -static void on_stack_reset(int reason) { - // On reset, print reset reason to console - ESP_LOGI(TAG, "nimble stack reset, reset reason: %d", reason); +static void +on_stack_reset(int reason) +{ + // On reset, print reset reason to console + ESP_LOGI(TAG, "nimble stack reset, reset reason: %d", reason); } -static void on_stack_sync(void) { - // Initialize advertising - adv_init(); +static void +on_stack_sync(void) +{ + // Initialize advertising + adv_init(); } -static void nimble_host_task(void *param) { - /* Task entry log */ - ESP_LOGI(TAG, "nimble host task has been started!"); +static void +nimble_host_task(void *param) +{ + /* Task entry log */ + ESP_LOGI(TAG, "nimble host task has been started!"); - /* This function won't return until nimble_port_stop() is executed */ - nimble_port_run(); + /* This function won't return until nimble_port_stop() is executed */ + nimble_port_run(); - /* Clean up at exit */ - vTaskDelete(NULL); + /* Clean up at exit */ + vTaskDelete(NULL); } -/*************************************************************************************************************************** +/****************************************************************************** * Public Function Definition - **************************************************************************************************************************/ + *******************************************************************************/ /** * Gets the RSSI of the connection - * @param rssi Pointer to the RSSI variable to be filled. The value is in dBm (I suspect) + * @param rssi Pointer to the RSSI variable to be filled. The value is in dBm + * (I suspect) */ -void db_ble_request_rssi(int8_t *rssi) { - if (active_conn_handle != BLE_HS_CONN_HANDLE_NONE && !DB_RADIO_IS_OFF) { - int rc = ble_gap_conn_rssi(active_conn_handle, rssi); - if (rc != 0) { - ESP_LOGE(TAG, "Failed to read BLE RSSI; rc=%d", rc); - } +void +db_ble_request_rssi(int8_t *rssi) +{ + if(active_conn_handle != BLE_HS_CONN_HANDLE_NONE && !DB_RADIO_IS_OFF) { + int rc = ble_gap_conn_rssi(active_conn_handle, rssi); + if(rc != 0) { + ESP_LOGE(TAG, "Failed to read BLE RSSI; rc=%d", rc); } + } } -void db_ble_init() { - ESP_LOGI(TAG, "Initializing BLE stack"); +void +db_ble_init() +{ + ESP_LOGI(TAG, "Initializing BLE stack"); - /* NimBLE host stack initialization */ - ESP_ERROR_CHECK(nimble_port_init()); + /* NimBLE host stack initialization */ + ESP_ERROR_CHECK(nimble_port_init()); - /* GAP service initialization */ - ESP_ERROR_CHECK(gap_init()); + /* GAP service initialization */ + ESP_ERROR_CHECK(gap_init()); - /* GAP service initialization */ - ESP_ERROR_CHECK(gatt_svr_init()); + /* GAP service initialization */ + ESP_ERROR_CHECK(gatt_svr_init()); - /* NimBLE host configuration initialization */ - nimble_host_config_init(); + /* NimBLE host configuration initialization */ + nimble_host_config_init(); - /* Start NimBLE Notify Task*/ - xTaskCreate(db_ble_server_uart_task, /**< Task Function */ - "BLETask", /**< Task Name */ - 4096, /**< Stack size */ - NULL, /**< Parameters, NULL as no parameters required */ - 5, /**< Task Priority */ - NULL /**< Task reference, used to change behaviour of task from another task */ - ); + /* Start NimBLE Notify Task*/ + xTaskCreate(db_ble_server_uart_task, /**< Task Function */ + "BLETask", /**< Task Name */ + 4096, /**< Stack size */ + NULL, /**< Parameters, NULL as no parameters required */ + 5, /**< Task Priority */ + NULL /**< Task reference, used to change behaviour of task from + another task */ + ); - /* Start NimBLE host task thread and return */ - nimble_port_freertos_init(nimble_host_task); + /* Start NimBLE host task thread and return */ + nimble_port_freertos_init(nimble_host_task); } -void db_ble_deinit() { - ESP_LOGI(TAG, "Deinitializing BLE stack"); - if (ble_gap_adv_stop() != 0) { - ESP_LOGE(TAG, "Failed to stop advertising"); - } - int rc = nimble_port_stop(); - if (rc == 0) { - nimble_port_deinit(); - } else { - ESP_LOGI(TAG, "Nimble port stop failed, rc = %d", rc); - } +void +db_ble_deinit() +{ + ESP_LOGI(TAG, "Deinitializing BLE stack"); + if(ble_gap_adv_stop() != 0) { + ESP_LOGE(TAG, "Failed to stop advertising"); + } + int rc = nimble_port_stop(); + if(rc == 0) { + nimble_port_deinit(); + } + else { + ESP_LOGI(TAG, "Nimble port stop failed, rc = %d", rc); + } } -void db_ble_queue_init() { - db_uart_write_queue_ble = xQueueCreate(5, sizeof(db_ble_queue_event_t)); - db_uart_read_queue_ble = xQueueCreate(5, sizeof(db_ble_queue_event_t)); - - if (db_uart_write_queue_ble == NULL) { - ESP_LOGI(TAG, "Failed to create queue, you are on your own, KABOOM!"); - return; - } - if (db_uart_read_queue_ble == NULL) { - ESP_LOGI(TAG, "Failed to create another queue, you are definitely on your own, KABOOM! KABOOM!"); - return; - } +void +db_ble_queue_init() +{ + db_uart_write_queue_ble = xQueueCreate(5, sizeof(db_ble_queue_event_t)); + db_uart_read_queue_ble = xQueueCreate(5, sizeof(db_ble_queue_event_t)); + + if(db_uart_write_queue_ble == NULL) { + ESP_LOGI(TAG, "Failed to create queue, you are on your own, KABOOM!"); + return; + } + if(db_uart_read_queue_ble == NULL) { + ESP_LOGI(TAG, + "Failed to create another queue, you are definitely on your own, " + "KABOOM! KABOOM!"); + return; + } } diff --git a/main/db_ble.h b/main/db_ble.h index d713cc2..9778947 100644 --- a/main/db_ble.h +++ b/main/db_ble.h @@ -1,10 +1,11 @@ -/*************************************************************************************************************************** +/****************************************************************************** * @file db_ble.h * @brief DroneBridge ESP32 BLE Header file * * This file is part of DroneBridge and CosmicBridge * - * @authors Witty-Wizard modified for DroneBridge for ESP32 by Wolfgang Christl + * @authors Witty-Wizard modified for + *DroneBridge for ESP32 by Wolfgang Christl * @license Apache License, Version 2.0 * * Licensed under the Apache License, Version 2.0 (the "License"); @@ -18,69 +19,83 @@ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - **************************************************************************************************************************/ + *******************************************************************************/ #ifndef DB_BLE_H #define DB_BLE_H -/*************************************************************************************************************************** +/****************************************************************************** * Standard & System Headers - **************************************************************************************************************************/ + *******************************************************************************/ #include #include #include #include -/*************************************************************************************************************************** +/****************************************************************************** * MACROS - **************************************************************************************************************************/ -#define BLE_SVC_SERIAL_UUID16 0xDB32 ///< 16-bit UUID for the Serial service. -#define BLE_SVC_SERIAL_CHR_WRITE_UUID16 0xDB33 ///< 16-bit UUID for the Serial Receive service characteristic. -#define BLE_SVC_SERIAL_CHR_NOTIFY_UUID16 0xDB34 ///< 16-bit UUID for the Serial Send service characteristic. -// ToDo: Implement Control/Management Plane Interface via BLE - Below char. are not used -#define BLE_SVC_SPP_CMD_WRITE_UUID16 0xABF3 ///< 16-bit UUID for the - Receive command characteristic. -#define BLE_SVC_SPP_CMD_NOTIFY_UUID16 0xABF4 ///< 16-bit UUID for the - Send command characteristic. -#define BLE_GAP_APPEARANCE_GENERIC_TAG 0x0180 ///< Generic tag appearance value for BLE GAP. -#define BLE_GAP_LE_ROLE_PERIPHERAL 0x00 ///< LE role value indicating a peripheral device. + *******************************************************************************/ +#define BLE_SVC_SERIAL_UUID16 0xDB32 ///< 16-bit UUID for the Serial service. +#define BLE_SVC_SERIAL_CHR_WRITE_UUID16 \ + 0xDB33 ///< 16-bit UUID for the Serial Receive service characteristic. +#define BLE_SVC_SERIAL_CHR_NOTIFY_UUID16 \ + 0xDB34 ///< 16-bit UUID for the Serial Send service characteristic. +// ToDo: Implement Control/Management Plane Interface via BLE - Below char. are +// not used +#define BLE_SVC_SPP_CMD_WRITE_UUID16 \ + 0xABF3 ///< 16-bit UUID for the - Receive command characteristic. +#define BLE_SVC_SPP_CMD_NOTIFY_UUID16 \ + 0xABF4 ///< 16-bit UUID for the - Send command characteristic. +#define BLE_GAP_APPEARANCE_GENERIC_TAG \ + 0x0180 ///< Generic tag appearance value for BLE GAP. +#define BLE_GAP_LE_ROLE_PERIPHERAL \ + 0x00 ///< LE role value indicating a peripheral device. -/*************************************************************************************************************************** +/****************************************************************************** * @brief Structure to hold BLE data. - **************************************************************************************************************************/ -typedef struct { + *******************************************************************************/ +typedef struct +{ uint16_t data_len; /**< Length of the data. */ - uint8_t *data; /**< Pointer to the data. */ + uint8_t *data; /**< Pointer to the data. */ } __attribute__((__packed__)) db_ble_queue_event_t; -/*************************************************************************************************************************** +/****************************************************************************** * Queue Handles - **************************************************************************************************************************/ + *******************************************************************************/ -extern QueueHandle_t db_uart_write_queue_ble; /** Queue for data to be written to UART, filled by wireless - communication interface task */ extern QueueHandle_t - db_uart_read_queue_ble; /** Queue for data to be written to wireless communication interface - used by BLE, filled by UART */ + db_uart_write_queue_ble; /** Queue for data to be written to UART, filled by + wireless communication interface task */ +extern QueueHandle_t + db_uart_read_queue_ble; /** Queue for data to be written to wireless + communication interface - used by BLE, filled by + UART */ -/*************************************************************************************************************************** +/****************************************************************************** * Public Function Declaration - **************************************************************************************************************************/ + *******************************************************************************/ -/*************************************************************************************************************************** +/****************************************************************************** * @brief Drone Bridge BLE initialisation function - **************************************************************************************************************************/ + *******************************************************************************/ void db_ble_init(); -/*************************************************************************************************************************** +/****************************************************************************** * @brief Initializes the global UART read and write queues. * * This function creates two FreeRTOS queues using xQueueCreate(). - * - `db_uart_write_queue_ble`: Used to store data that needs to be written to UART. - * - `db_uart_read_queue_ble`: Used to store data read from UART for further processing. + * - `db_uart_write_queue_ble`: Used to store data that needs to be written to + *UART. + * - `db_uart_read_queue_ble`: Used to store data read from UART for further + *processing. * * The queue size is set to 5, with each element being the size of `BleData_t`. - * If queue creation fails, appropriate error messages are logged using `ESP_LOGI()`. + * If queue creation fails, appropriate error messages are logged using + *`ESP_LOGI()`. * * @return void - **************************************************************************************************************************/ + *******************************************************************************/ void db_ble_queue_init(); /** diff --git a/main/db_comm_protocol.h b/main/db_comm_protocol.h index 42fbd73..8dd85ac 100644 --- a/main/db_comm_protocol.h +++ b/main/db_comm_protocol.h @@ -20,7 +20,6 @@ #ifndef DB_ESP32_DB_COMM_PROTOCOL_H #define DB_ESP32_DB_COMM_PROTOCOL_H - #define DB_COMM_ORIGIN_GND "groundstation" #define DB_COMM_ORIGIN_UAV "drone" #define DB_COMM_TYPE_SETTINGS_SUCCESS "settingssuccess" @@ -45,13 +44,12 @@ #define DB_COMM_DST_GCS 4 #define DB_COMM_DST_UAV 5 - #define DB_COMM_KEY_TYPE "type" #define DB_COMM_KEY_ORIGIN "origin" #define DB_COMM_KEY_ID "id" #define DB_COMM_KEY_DEST "destination" -#define DB_COMM_KEY_HARDWID "HID" // Hardware ID -#define DB_COMM_KEY_FIRMWID "FID" // Firmware version +#define DB_COMM_KEY_HARDWID "HID" // Hardware ID +#define DB_COMM_KEY_FIRMWID "FID" // Firmware version #define DB_COMM_KEY_MSG "message" #define DB_COMM_CHANGE_DB "db" @@ -59,4 +57,4 @@ #define DB_ESP32_FID 101 -#endif //DB_ESP32_DB_COMM_PROTOCOL_H +#endif // DB_ESP32_DB_COMM_PROTOCOL_H diff --git a/main/db_crc.c b/main/db_crc.c index f347749..cbb7620 100644 --- a/main/db_crc.c +++ b/main/db_crc.c @@ -35,26 +35,30 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ -uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a) +uint8_t +crc8_dvb_s2(uint8_t crc, unsigned char a) { - crc ^= a; - for (int ii = 0; ii < 8; ++ii) { - if (crc & 0x80) { - crc = (crc << 1) ^ 0xD5; - } else { - crc = crc << 1; - } + crc ^= a; + for(int ii = 0; ii < 8; ++ii) { + if(crc & 0x80) { + crc = (crc << 1) ^ 0xD5; } - return crc; + else { + crc = crc << 1; + } + } + return crc; } /** - * Does the same as crc8_dvb_s2 just way more efficient (CPU) but with a little bit more RAM usage + * Does the same as crc8_dvb_s2 just way more efficient (CPU) but with a little + * bit more RAM usage * @param crc The current crc value * @param a The next byte * @return The new crc value */ -uint8_t crc8_dvb_s2_table(uint8_t crc, unsigned char a) +uint8_t +crc8_dvb_s2_table(uint8_t crc, unsigned char a) { - return (uint8_t) (crc_dvb_s2_table[(crc ^ a)] & 0xff); + return (uint8_t)(crc_dvb_s2_table[(crc ^ a)] & 0xff); } \ No newline at end of file diff --git a/main/db_crc.h b/main/db_crc.h index fa65eaa..d303053 100644 --- a/main/db_crc.h +++ b/main/db_crc.h @@ -74,7 +74,6 @@ extern "C" { */ #define CRC_ALGO_TABLE_DRIVEN 1 - /** * The type of the CRC values. * @@ -92,22 +91,26 @@ typedef uint_fast8_t crc_t; * - ReflectOut = False */ static const crc_t crc_dvb_s2_table[256] = { - 0x00, 0xd5, 0x7f, 0xaa, 0xfe, 0x2b, 0x81, 0x54, 0x29, 0xfc, 0x56, 0x83, 0xd7, 0x02, 0xa8, 0x7d, - 0x52, 0x87, 0x2d, 0xf8, 0xac, 0x79, 0xd3, 0x06, 0x7b, 0xae, 0x04, 0xd1, 0x85, 0x50, 0xfa, 0x2f, - 0xa4, 0x71, 0xdb, 0x0e, 0x5a, 0x8f, 0x25, 0xf0, 0x8d, 0x58, 0xf2, 0x27, 0x73, 0xa6, 0x0c, 0xd9, - 0xf6, 0x23, 0x89, 0x5c, 0x08, 0xdd, 0x77, 0xa2, 0xdf, 0x0a, 0xa0, 0x75, 0x21, 0xf4, 0x5e, 0x8b, - 0x9d, 0x48, 0xe2, 0x37, 0x63, 0xb6, 0x1c, 0xc9, 0xb4, 0x61, 0xcb, 0x1e, 0x4a, 0x9f, 0x35, 0xe0, - 0xcf, 0x1a, 0xb0, 0x65, 0x31, 0xe4, 0x4e, 0x9b, 0xe6, 0x33, 0x99, 0x4c, 0x18, 0xcd, 0x67, 0xb2, - 0x39, 0xec, 0x46, 0x93, 0xc7, 0x12, 0xb8, 0x6d, 0x10, 0xc5, 0x6f, 0xba, 0xee, 0x3b, 0x91, 0x44, - 0x6b, 0xbe, 0x14, 0xc1, 0x95, 0x40, 0xea, 0x3f, 0x42, 0x97, 0x3d, 0xe8, 0xbc, 0x69, 0xc3, 0x16, - 0xef, 0x3a, 0x90, 0x45, 0x11, 0xc4, 0x6e, 0xbb, 0xc6, 0x13, 0xb9, 0x6c, 0x38, 0xed, 0x47, 0x92, - 0xbd, 0x68, 0xc2, 0x17, 0x43, 0x96, 0x3c, 0xe9, 0x94, 0x41, 0xeb, 0x3e, 0x6a, 0xbf, 0x15, 0xc0, - 0x4b, 0x9e, 0x34, 0xe1, 0xb5, 0x60, 0xca, 0x1f, 0x62, 0xb7, 0x1d, 0xc8, 0x9c, 0x49, 0xe3, 0x36, - 0x19, 0xcc, 0x66, 0xb3, 0xe7, 0x32, 0x98, 0x4d, 0x30, 0xe5, 0x4f, 0x9a, 0xce, 0x1b, 0xb1, 0x64, - 0x72, 0xa7, 0x0d, 0xd8, 0x8c, 0x59, 0xf3, 0x26, 0x5b, 0x8e, 0x24, 0xf1, 0xa5, 0x70, 0xda, 0x0f, - 0x20, 0xf5, 0x5f, 0x8a, 0xde, 0x0b, 0xa1, 0x74, 0x09, 0xdc, 0x76, 0xa3, 0xf7, 0x22, 0x88, 0x5d, - 0xd6, 0x03, 0xa9, 0x7c, 0x28, 0xfd, 0x57, 0x82, 0xff, 0x2a, 0x80, 0x55, 0x01, 0xd4, 0x7e, 0xab, - 0x84, 0x51, 0xfb, 0x2e, 0x7a, 0xaf, 0x05, 0xd0, 0xad, 0x78, 0xd2, 0x07, 0x53, 0x86, 0x2c, 0xf9 + 0x00, 0xd5, 0x7f, 0xaa, 0xfe, 0x2b, 0x81, 0x54, 0x29, 0xfc, 0x56, 0x83, 0xd7, + 0x02, 0xa8, 0x7d, 0x52, 0x87, 0x2d, 0xf8, 0xac, 0x79, 0xd3, 0x06, 0x7b, 0xae, + 0x04, 0xd1, 0x85, 0x50, 0xfa, 0x2f, 0xa4, 0x71, 0xdb, 0x0e, 0x5a, 0x8f, 0x25, + 0xf0, 0x8d, 0x58, 0xf2, 0x27, 0x73, 0xa6, 0x0c, 0xd9, 0xf6, 0x23, 0x89, 0x5c, + 0x08, 0xdd, 0x77, 0xa2, 0xdf, 0x0a, 0xa0, 0x75, 0x21, 0xf4, 0x5e, 0x8b, 0x9d, + 0x48, 0xe2, 0x37, 0x63, 0xb6, 0x1c, 0xc9, 0xb4, 0x61, 0xcb, 0x1e, 0x4a, 0x9f, + 0x35, 0xe0, 0xcf, 0x1a, 0xb0, 0x65, 0x31, 0xe4, 0x4e, 0x9b, 0xe6, 0x33, 0x99, + 0x4c, 0x18, 0xcd, 0x67, 0xb2, 0x39, 0xec, 0x46, 0x93, 0xc7, 0x12, 0xb8, 0x6d, + 0x10, 0xc5, 0x6f, 0xba, 0xee, 0x3b, 0x91, 0x44, 0x6b, 0xbe, 0x14, 0xc1, 0x95, + 0x40, 0xea, 0x3f, 0x42, 0x97, 0x3d, 0xe8, 0xbc, 0x69, 0xc3, 0x16, 0xef, 0x3a, + 0x90, 0x45, 0x11, 0xc4, 0x6e, 0xbb, 0xc6, 0x13, 0xb9, 0x6c, 0x38, 0xed, 0x47, + 0x92, 0xbd, 0x68, 0xc2, 0x17, 0x43, 0x96, 0x3c, 0xe9, 0x94, 0x41, 0xeb, 0x3e, + 0x6a, 0xbf, 0x15, 0xc0, 0x4b, 0x9e, 0x34, 0xe1, 0xb5, 0x60, 0xca, 0x1f, 0x62, + 0xb7, 0x1d, 0xc8, 0x9c, 0x49, 0xe3, 0x36, 0x19, 0xcc, 0x66, 0xb3, 0xe7, 0x32, + 0x98, 0x4d, 0x30, 0xe5, 0x4f, 0x9a, 0xce, 0x1b, 0xb1, 0x64, 0x72, 0xa7, 0x0d, + 0xd8, 0x8c, 0x59, 0xf3, 0x26, 0x5b, 0x8e, 0x24, 0xf1, 0xa5, 0x70, 0xda, 0x0f, + 0x20, 0xf5, 0x5f, 0x8a, 0xde, 0x0b, 0xa1, 0x74, 0x09, 0xdc, 0x76, 0xa3, 0xf7, + 0x22, 0x88, 0x5d, 0xd6, 0x03, 0xa9, 0x7c, 0x28, 0xfd, 0x57, 0x82, 0xff, 0x2a, + 0x80, 0x55, 0x01, 0xd4, 0x7e, 0xab, 0x84, 0x51, 0xfb, 0x2e, 0x7a, 0xaf, 0x05, + 0xd0, 0xad, 0x78, 0xd2, 0x07, 0x53, 0x86, 0x2c, 0xf9 }; /** @@ -115,12 +118,12 @@ static const crc_t crc_dvb_s2_table[256] = { * * \return The initial crc value. */ -static inline crc_t crc_init(void) +static inline crc_t +crc_init(void) { - return 0x00; + return 0x00; } - /** * Update the crc value with new data. * @@ -131,23 +134,23 @@ static inline crc_t crc_init(void) */ crc_t crc_update(crc_t crc, const void *data, size_t data_len); - /** * Calculate the final crc value. * * \param[in] crc The current crc value. * \return The final crc value. */ -static inline crc_t crc_finalize(crc_t crc) +static inline crc_t +crc_finalize(crc_t crc) { - return crc; + return crc; } uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a); uint8_t crc8_dvb_s2_table(uint8_t crc, unsigned char a); #ifdef __cplusplus -} /* closing brace for extern "C" */ +} /* closing brace for extern "C" */ #endif -#endif /* CRC_H */ +#endif /* CRC_H */ diff --git a/main/db_esp32_control.c b/main/db_esp32_control.c index 759137b..43e9f30 100644 --- a/main/db_esp32_control.c +++ b/main/db_esp32_control.c @@ -1,5 +1,4 @@ -#include -/* +/****************************************************************************** * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2018 Wolfgang Christl @@ -16,857 +15,1144 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ + ******************************************************************************/ + +/****************************************************************************** + * Standard C Library Headers + ******************************************************************************/ +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include #include -#include + +/****************************************************************************** + * ESP-IDF APIs + ******************************************************************************/ #include "esp_log.h" +#include "esp_task_wdt.h" +#include "esp_timer.h" +#include "esp_vfs_dev.h" +#include "esp_wifi.h" + +/****************************************************************************** + * ESP-IDF Networking (LWIP) APIs + ******************************************************************************/ +#include "lwip/inet.h" +#include "lwip/netdb.h" #include "lwip/sockets.h" + +/****************************************************************************** + * ESP-IDF Driver APIs + ******************************************************************************/ #include "driver/uart.h" + +/****************************************************************************** + * Project Headers + ******************************************************************************/ +#include "db_esp32_control.h" +#include "db_esp_now.h" +#include "db_parameters.h" +#include "db_protocol.h" +#include "db_serial.h" #include "globals.h" +#include "main.h" #include "msp_ltm_serial.h" -#include "db_protocol.h" #include "tcp_server.h" -#include "db_esp32_control.h" + #ifdef CONFIG_BT_ENABLED #include "db_ble.h" #endif -#include -#include "main.h" -#include "db_serial.h" -#include "db_esp_now.h" +/****************************************************************************** + * MACROS + ******************************************************************************/ #define TAG "DB_CONTROL" -uint16_t app_port_proxy = APP_PORT_PROXY; +/****************************************************************************** + * Private Variables + ******************************************************************************/ +static uint16_t app_port_proxy = APP_PORT_PROXY; +/****************************************************************************** + * Public Variables + ******************************************************************************/ int8_t num_connected_tcp_clients = 0; -/** - * Opens non-blocking UDP socket used for WiFi to UART communication. Does also accept broadcast packets just in case. - * @return returns socket file descriptor - */ -int db_open_serial_udp_socket() { - struct sockaddr_in server_addr; - server_addr.sin_addr.s_addr = htonl(INADDR_ANY); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(APP_PORT_PROXY_UDP); - int addr_family = AF_INET; - int ip_protocol = IPPROTO_IP; - char addr_str[128]; - inet_ntoa_r(server_addr.sin_addr, addr_str, sizeof(addr_str) - 1); - - int udp_socket = socket(addr_family, SOCK_DGRAM, ip_protocol); - if (udp_socket < 0) { - ESP_LOGE(TAG, "Unable to create socket for serial communication: errno %d", errno); - return -1; - } - int broadcastEnable = 1; - int err = setsockopt(udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcastEnable, sizeof(broadcastEnable)); - if (err < 0) { - ESP_LOGE(TAG, "Socket unable to set udp socket to accept broadcast messages: errno %d", errno); - } - err = bind(udp_socket, (struct sockaddr *) &server_addr, sizeof(server_addr)); - if (err < 0) { - ESP_LOGE(TAG, "Socket unable to bind to %i errno %d", APP_PORT_PROXY_UDP, errno); - } - fcntl(udp_socket, F_SETFL, O_NONBLOCK); - ESP_LOGI(TAG, "Opened UDP socket on port %i", APP_PORT_PROXY_UDP); - return udp_socket; -} - -#ifdef CONFIG_DB_SKYBRUSH_SUPPORT +/****************************************************************************** + * Private Function Declaration + ******************************************************************************/ -/** - * Opens non-blocking UDP socket used for WiFi to UART communication using WiFi UDP broadcast packets e.g. by Skybrush. +/****************************************************************************** + * Opens non-blocking UDP socket used for WiFi to UART communication. Does also + * accept broadcast packets just in case. * @return returns socket file descriptor - */ -int db_open_serial_udp_broadcast_socket() { - struct sockaddr_in server_addr; - server_addr.sin_addr.s_addr = htonl(INADDR_ANY); - server_addr.sin_family = AF_INET; - server_addr.sin_port = htons(UDP_BROADCAST_PORT_SKYBRUSH); - int addr_family = AF_INET; - int ip_protocol = IPPROTO_IP; - char addr_str[128]; - inet_ntoa_r(server_addr.sin_addr, addr_str, sizeof(addr_str) - 1); - - int udp_socket = socket(addr_family, SOCK_DGRAM, ip_protocol); - if (udp_socket < 0) { - ESP_LOGE(TAG, "Unable to create socket for Skybrush communication: errno %d", errno); - return -1; - } - int broadcastEnable = 1; - int err = setsockopt(udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcastEnable, sizeof(broadcastEnable)); - if (err < 0) { - ESP_LOGE(TAG, "Socket unable to set Skybrush udp socket to accept broadcast messages: errno %d", errno); - } - err = bind(udp_socket, (struct sockaddr *) &server_addr, sizeof(server_addr)); - if (err < 0) { - ESP_LOGE(TAG, "Socket unable to bind Skybrush socket to %i errno %d", UDP_BROADCAST_PORT_SKYBRUSH, errno); - } - fcntl(udp_socket, F_SETFL, O_NONBLOCK); - ESP_LOGI(TAG, "Opened UDP broadcast enabled socket on port %i", UDP_BROADCAST_PORT_SKYBRUSH); - return udp_socket; -} - -#endif + ******************************************************************************/ +static int open_serial_udp_socket(); -/** - * Opens non-blocking UDP socket used for internal DroneBridge telemetry. Socket shall only be opened when local ESP32 - * is in client mode. Socket is used to receive internal Wifi telemetry from the ESP32 access point we are connected to. +/****************************************************************************** + * Opens non-blocking UDP socket used for WiFi to UART communication using WiFi + * UDP broadcast packets e.g. by Skybrush. * @return returns socket file descriptor - */ -int db_open_int_telemetry_udp_socket() { - // Create a socket for sending to the multicast address - int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); - if (sock < 0) { - ESP_LOGE(TAG, "Failed to create socket. Error %d", errno); - return -1; - } - struct sockaddr_in saddr = {0}; - // Bind the socket to any address - saddr.sin_family = PF_INET; - saddr.sin_port = htons(DB_ESP32_INTERNAL_TELEMETRY_PORT); - saddr.sin_addr.s_addr = htonl(INADDR_ANY); - esp_err_t err = bind(sock, (struct sockaddr *) &saddr, sizeof(struct sockaddr_in)); - if (err < 0) { - ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno); - return -1; - } - - // Set the time-to-live of messages to 1 so they do not go past the local network - int ttl = MULTICAST_TTL; - if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0) { - ESP_LOGE(TAG, "Failed to set IP_MULTICAST_TTL. Error %d", errno); - close(sock); - return -1; - } - - fcntl(sock, F_SETFL, O_NONBLOCK); - - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - // Configure for listening when in station mode - struct ip_mreq imreq = {0}; - struct in_addr iaddr = {0}; - // Configure source interface - imreq.imr_interface.s_addr = IPADDR_ANY; - // Configure multicast address to listen to - err = inet_aton(MULTICAST_IPV4_ADDR, &imreq.imr_multiaddr.s_addr); - if (err != 1) { - ESP_LOGE(TAG, "Configured IPV4 multicast address '%s' is invalid.", MULTICAST_IPV4_ADDR); - // Errors in the return value have to be negative - close(sock); - return -1; - } - ESP_LOGI(TAG, "Configured internal Multicast address %s", inet_ntoa(imreq.imr_multiaddr.s_addr)); - if (!IP_MULTICAST(ntohl(imreq.imr_multiaddr.s_addr))) { - ESP_LOGW(TAG, - "Configured IPV4 multicast address '%s' is not a valid multicast address. This will probably not work.", - MULTICAST_IPV4_ADDR); - } - - err = setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF, &iaddr, sizeof(struct in_addr)); - if (err < 0) { - ESP_LOGE(TAG, "Failed to set IP_MULTICAST_IF. Error %d", errno); - close(sock); - return -1; - } + ******************************************************************************/ +static int open_serial_udp_broadcast_socket(); - err = setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(struct ip_mreq)); - if (err < 0) { - ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno); - close(sock); - return -1; - } - } - ESP_LOGI(TAG, "Opened internal telemetry socket on port: %i", DB_ESP32_INTERNAL_TELEMETRY_PORT); - return sock; -} +/****************************************************************************** + * Opens non-blocking UDP socket used for internal DroneBridge telemetry. + * Socket shall only be opened when local ESP32 is in client mode. Socket is + * used to receive internal Wifi telemetry from the ESP32 access point we are + * connected to. + * @return returns socket file descriptor + ******************************************************************************/ +static int open_int_telemetry_udp_socket(); -/** - * Sends data to all clients that are part of the udp connection list. No resending of packets in case of failure. +/****************************************************************************** + * Sends data to all clients that are part of the udp connection list. No + * resending of packets in case of failure. * @param n_udp_conn_list List of known UDP clients/connections * @param data Buffer with the data to send * @param data_length Length of the data in the buffer - */ -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, - sizeof(n_udp_conn_list->db_udp_clients[i].udp_client)); - if (sent != data_length) { - int err = errno; - ESP_LOGE(TAG, "UDP - Error sending (%i/%i) to because of %d", sent, data_length, err); - } - } -} + ******************************************************************************/ +static void send_to_all_udp_clients(udp_conn_list_t *n_udp_conn_list, + const uint8_t *data, uint data_length); -/** - * 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) +/****************************************************************************** + * 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 - } -} + * @param data_length Length of the payload data. Must not be bigger than + * DB_ESPNOW_PAYLOAD_MAXSIZE - fails otherwise + ******************************************************************************/ +static void send_to_all_espnow(uint8_t data[], const uint16_t *data_length); -/** - * 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. +/****************************************************************************** + * Check for incoming connections on TCP server * - * When in ESP-NOW mode the packets will be split if they are bigger than DB_ESPNOW_PAYLOAD_MAXSIZE. + * @param tcp_master_socket Main open TCP socket to accept TCP + * connections/clients + * @param tcp_clients List of active TCP client connections (socket IDs) + ******************************************************************************/ +static void handle_tcp_master(const int tcp_master_socket, int tcp_clients[]); + +/****************************************************************************** + * Reads serial (UART/USB/JTAG) transparently or parsing MAVLink/MSP/LTM + * protocol. Then sends read data to all connected clients via TCP/UDP or + * ESP-NOW. Non-Blocking function * - * @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 db_send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, uint8_t data[], uint16_t data_length) { - db_ble_queue_event_t bleData; - switch (DB_PARAM_RADIO_MODE) { - case DB_WIFI_MODE_ESPNOW_AIR: - case DB_WIFI_MODE_ESPNOW_GND: - // ESP-NOW mode - if (data_length > DB_ESPNOW_PAYLOAD_MAXSIZE) { - // Data not properly sized, 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; - } - db_send_to_all_espnow(&data[sent_bytes], &next_chunk_len); - sent_bytes += next_chunk_len; - } while (sent_bytes < data_length); - } else { - // Packet is properly sized - send to ESP-NOW outbound queue - db_send_to_all_espnow(data, &data_length); - } - break; + * @param tcp_clients Array of connected TCP client sockets + * @param transparent_buff_pos Counter variable for total read UART bytes + * @param msp_ltm_buff_pos Pointer position/data length of destination-buffer + * for read MSP messages + * @param msp_message_buffer Destination-buffer for read MSP messages + * @param serial_buffer Destination-buffer for the serial data + * @param db_msp_ltm_port Pointer to structure containing MSP/LTM parser + * information + ******************************************************************************/ +static void read_process_serial_link(int *tcp_clients, + uint *transparent_buff_pos, + uint *msp_ltm_buff_pos, + uint8_t *msp_message_buffer, + uint8_t *serial_buffer, + msp_ltm_port_t *db_msp_ltm_port); + +/****************************************************************************** + * Thread that manages all incoming and outgoing ESP-NOW and serial (UART) + * connections. Called only when ESP-NOW mode is selected + ******************************************************************************/ +_Noreturn static void control_module_esp_now(); + +/****************************************************************************** + * Sends DroneBridge internal telemetry to tell every connected WiFi station + * how well we receive their data (rssi). Uses UDP multicast message. Format: + * [NUM_Entries - (MAC + RSSI) - (MAC + RSSI) - ...] Internal telemetry uses + * DB_ESP32_INTERNAL_TELEMETRY_PORT port + * + * @param sta_list + ******************************************************************************/ +static void send_internal_telemetry_to_stations(int tel_sock, + wifi_sta_list_t *sta_list, + udp_conn_list_t *udp_conns); + +/****************************************************************************** + * Receive and process internal telemetry (ESP32 AP to ESP32 Station) sent by + * ESP32 LR access point. Matches with send_internal_telemetry_to_stations() + * Sets station_rssi_ap based on the received value + * Packet format: [NUM_Entries, (MAC + RSSI), (MAC + RSSI), (MAC + RSSI), ...] + * + * @param tel_sock Socket listening for internal telemetry + ******************************************************************************/ +static void handle_internal_telemetry(int tel_sock, uint8_t *udp_buffer, + socklen_t *sock_len, + struct sockaddr_in *udp_client); + +/****************************************************************************** + * Thread that manages all incoming and outgoing TCP, UDP and serial (UART) + * connections. Executed when Wi-Fi modes are set - ESP-NOW has its own thread + ******************************************************************************/ +_Noreturn static void control_module_udp_tcp(); - case DB_BLUETOOTH_MODE: #ifdef CONFIG_BT_ENABLED - bleData.data = malloc(data_length); - bleData.data_len = data_length; - memcpy(bleData.data, data, bleData.data_len); - if (xQueueSend(db_uart_read_queue_ble, &bleData, portMAX_DELAY) != pdPASS) { - ESP_LOGE(TAG, "Failed to send BLE data to queue"); - free(bleData.data); - } +/****************************************************************************** + * Bluetooth Threads + ******************************************************************************/ +_Noreturn static void control_module_ble(); #endif - break; - default: - // Other modes (WiFi Modes using TCP/UDP) - db_send_to_all_tcp_clients(tcp_clients, data, data_length); - db_send_to_all_udp_clients(n_udp_conn_list, data, data_length); - break; - } -} +/****************************************************************************** + * Private Function Definition + ******************************************************************************/ -/** - * Check for incoming connections on TCP server - * - * @param tcp_master_socket Main open TCP socket to accept TCP connections/clients - * @param tcp_clients List of active TCP client connections (socket IDs) - */ -void handle_tcp_master(const int tcp_master_socket, int tcp_clients[]) { - struct sockaddr_in6 source_addr; // Large enough for both IPv4 or IPv6 - uint32_t addr_len = sizeof(source_addr); - int new_tcp_client = accept(tcp_master_socket, (struct sockaddr *) &source_addr, &addr_len); - if (new_tcp_client > 0) { - for (int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { - if (tcp_clients[i] <= 0) { - tcp_clients[i] = new_tcp_client; - fcntl(tcp_clients[i], F_SETFL, O_NONBLOCK); - char addr_str[128]; - inet_ntoa_r(((struct sockaddr_in *) &source_addr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1); - ESP_LOGI(TAG, "TCP: New client connected: %s", addr_str); - num_connected_tcp_clients++; - return; - } - } - ESP_LOGI(TAG, "TCP: Could not accept connection. Too many clients connected."); - } +static int +open_serial_udp_socket() +{ + struct sockaddr_in server_addr; + server_addr.sin_addr.s_addr = htonl(INADDR_ANY); + server_addr.sin_family = AF_INET; + server_addr.sin_port = htons(APP_PORT_PROXY_UDP); + int addr_family = AF_INET; + int ip_protocol = IPPROTO_IP; + char addr_str[128]; + inet_ntoa_r(server_addr.sin_addr, addr_str, sizeof(addr_str) - 1); + + int udp_socket = socket(addr_family, SOCK_DGRAM, ip_protocol); + if(udp_socket < 0) { + ESP_LOGE(TAG, + "Unable to create socket for serial communication: errno %d", + errno); + return -1; + } + int broadcastEnable = 1; + int err = setsockopt(udp_socket, + SOL_SOCKET, + SO_BROADCAST, + &broadcastEnable, + sizeof(broadcastEnable)); + if(err < 0) { + ESP_LOGE( + TAG, + "Socket unable to set udp socket to accept broadcast messages: errno %d", + errno); + } + err = bind(udp_socket, (struct sockaddr *)&server_addr, sizeof(server_addr)); + if(err < 0) { + ESP_LOGE( + TAG, "Socket unable to bind to %i errno %d", APP_PORT_PROXY_UDP, errno); + } + fcntl(udp_socket, F_SETFL, O_NONBLOCK); + ESP_LOGI(TAG, "Opened UDP socket on port %i", APP_PORT_PROXY_UDP); + return udp_socket; } -/** - * Init/Create structure containing all UDP connection information - * @return Structure containing all UDP connection information - */ -udp_conn_list_t *udp_client_list_create() { - udp_conn_list_t *n_udp_conn_list = malloc(sizeof(udp_conn_list_t)); // Allocate memory for the list - if (n_udp_conn_list == NULL) { // Check if the allocation failed - return NULL; // Return NULL to indicate an error - } - n_udp_conn_list->size = 0; // Initialize the size to 0 - return n_udp_conn_list; // Return the pointer to the list +#ifdef CONFIG_DB_SKYBRUSH_SUPPORT + +static int +open_serial_udp_broadcast_socket() +{ + struct sockaddr_in server_addr; + server_addr.sin_addr.s_addr = htonl(INADDR_ANY); + server_addr.sin_family = AF_INET; + server_addr.sin_port = htons(UDP_BROADCAST_PORT_SKYBRUSH); + int addr_family = AF_INET; + int ip_protocol = IPPROTO_IP; + char addr_str[128]; + inet_ntoa_r(server_addr.sin_addr, addr_str, sizeof(addr_str) - 1); + + int udp_socket = socket(addr_family, SOCK_DGRAM, ip_protocol); + if(udp_socket < 0) { + ESP_LOGE(TAG, + "Unable to create socket for Skybrush communication: errno %d", + errno); + return -1; + } + int broadcastEnable = 1; + int err = setsockopt(udp_socket, + SOL_SOCKET, + SO_BROADCAST, + &broadcastEnable, + sizeof(broadcastEnable)); + if(err < 0) { + ESP_LOGE(TAG, + "Socket unable to set Skybrush udp socket to accept broadcast " + "messages: errno %d", + errno); + } + err = bind(udp_socket, (struct sockaddr *)&server_addr, sizeof(server_addr)); + if(err < 0) { + ESP_LOGE(TAG, + "Socket unable to bind Skybrush socket to %i errno %d", + UDP_BROADCAST_PORT_SKYBRUSH, + errno); + } + fcntl(udp_socket, F_SETFL, O_NONBLOCK); + ESP_LOGI(TAG, + "Opened UDP broadcast enabled socket on port %i", + UDP_BROADCAST_PORT_SKYBRUSH); + return udp_socket; } -/** - * Destroy structure containing all UDP connection information - * @param n_udp_conn_list Structure containing all UDP connection information - */ -void udp_client_list_destroy(udp_conn_list_t *n_udp_conn_list) { - if (n_udp_conn_list == NULL) { // Check if the list is NULL - return; // Do nothing +#endif + +static int +open_int_telemetry_udp_socket() +{ + // Create a socket for sending to the multicast address + int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + if(sock < 0) { + ESP_LOGE(TAG, "Failed to create socket. Error %d", errno); + return -1; + } + struct sockaddr_in saddr = { 0 }; + // Bind the socket to any address + saddr.sin_family = PF_INET; + saddr.sin_port = htons(DB_ESP32_INTERNAL_TELEMETRY_PORT); + saddr.sin_addr.s_addr = htonl(INADDR_ANY); + esp_err_t err = + bind(sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in)); + if(err < 0) { + ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno); + return -1; + } + + // Set the time-to-live of messages to 1 so they do not go past the local + // network + int ttl = MULTICAST_TTL; + if(setsockopt(sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0) { + ESP_LOGE(TAG, "Failed to set IP_MULTICAST_TTL. Error %d", errno); + close(sock); + return -1; + } + + fcntl(sock, F_SETFL, O_NONBLOCK); + + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { + // Configure for listening when in station mode + struct ip_mreq imreq = { 0 }; + struct in_addr iaddr = { 0 }; + // Configure source interface + imreq.imr_interface.s_addr = IPADDR_ANY; + // Configure multicast address to listen to + err = inet_aton(MULTICAST_IPV4_ADDR, &imreq.imr_multiaddr.s_addr); + if(err != 1) { + ESP_LOGE(TAG, + "Configured IPV4 multicast address '%s' is invalid.", + MULTICAST_IPV4_ADDR); + // Errors in the return value have to be negative + close(sock); + return -1; + } + ESP_LOGI(TAG, + "Configured internal Multicast address %s", + inet_ntoa(imreq.imr_multiaddr.s_addr)); + if(!IP_MULTICAST(ntohl(imreq.imr_multiaddr.s_addr))) { + ESP_LOGW(TAG, + "Configured IPV4 multicast address '%s' is not a valid " + "multicast address. This will probably not work.", + MULTICAST_IPV4_ADDR); } - free(n_udp_conn_list); // Free the list -} -/** - * Add a new UDP client to the list of known UDP clients. Checks if client is already known based on IP and port. - * Added client will receive UDP packets with serial info and will be able to send UDP packets to the serial interface - * of the ESP32. - * PORT, MAC & IP should be set inside new_db_udp_client. If MAC is not set then the device cannot be removed later on. - * - * @param n_udp_conn_list Structure containing all UDP connection information - * @param new_db_udp_client New client to add to the UDP list. PORT, MAC & IP must be set. If MAC is not set then the - * device cannot be automatically removed later on. To remove it, the user must clear the entire list. - * @param save_to_nvm Set to 1 (true) in case you want the UDP client to survive the reboot. Set to 0 (false) if client is temporary for this session. - * It will then be saved to NVM and added to the udp_conn_list_t on startup. Only one client can be saved to NVM. - * @return 1 if added - 0 if not - */ -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) { - if (n_udp_conn_list == NULL) { // Check if the list is NULL - return false; // Do nothing - } - if (n_udp_conn_list->size == MAX_UDP_CLIENTS) { // Check if the list is full - return false; // Do nothing - } - for (int i = 0; i < n_udp_conn_list->size; i++) { - if ((n_udp_conn_list->db_udp_clients[i].udp_client.sin_port == new_db_udp_client.udp_client.sin_port) && - (n_udp_conn_list->db_udp_clients[i].udp_client.sin_addr.s_addr == - new_db_udp_client.udp_client.sin_addr.s_addr)) { - return false; // client existing - do not add - } + err = setsockopt( + sock, IPPROTO_IP, IP_MULTICAST_IF, &iaddr, sizeof(struct in_addr)); + if(err < 0) { + ESP_LOGE(TAG, "Failed to set IP_MULTICAST_IF. Error %d", errno); + close(sock); + return -1; + } + + err = setsockopt( + sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(struct ip_mreq)); + if(err < 0) { + ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno); + close(sock); + return -1; } - n_udp_conn_list->db_udp_clients[n_udp_conn_list->size] = new_db_udp_client; // Copy the element data to the end of the array - n_udp_conn_list->size++; // Increment the size of the list - // some logging - char ip_port_string[INET_ADDRSTRLEN + 10]; - char ip_string[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &(new_db_udp_client.udp_client.sin_addr), ip_string, INET_ADDRSTRLEN); - sprintf(ip_port_string, "%s:%d", ip_string, htons (new_db_udp_client.udp_client.sin_port)); - ESP_LOGI(TAG, "Added %s to udp client distribution list - save to NVM: %i", ip_port_string, save_to_nvm); - // save to memory - if (save_to_nvm) { - save_udp_client_to_nvm(&new_db_udp_client, false); - } else { - // do not save to NVM - } - return true; + } + ESP_LOGI(TAG, + "Opened internal telemetry socket on port: %i", + DB_ESP32_INTERNAL_TELEMETRY_PORT); + return sock; } -/** - * Remove a client from the sending list. Client will no longer receive UDP packets. MAC address must be given. - * Usually called in AP-Mode when a station disconnects. In any other case we will not know since UDP is a connection-less - * protocol - * - * @param n_udp_conn_list Structure containing all UDP connection information - * @param new_db_udp_client The UDP client to remove based on its MAC address - * @return true if removed - false if nothing was removed - */ -bool remove_from_known_udp_clients(udp_conn_list_t *n_udp_conn_list, struct db_udp_client_t new_db_udp_client) { - if (n_udp_conn_list == NULL) { // Check if the list is NULL - return false; // Do nothing - } - for (int i = 0; i < n_udp_conn_list->size; i++) { // Loop through the array - if (memcmp(n_udp_conn_list->db_udp_clients[i].mac, new_db_udp_client.mac, - sizeof(n_udp_conn_list->db_udp_clients[i].mac)) == - 0) { // Compare the current array element with the element - // Found a match - for (int j = i; j < n_udp_conn_list->size - 1; j++) { // Loop from the current index to the end of the array - n_udp_conn_list->db_udp_clients[j] = n_udp_conn_list->db_udp_clients[j + - 1]; // Shift the array elements to the left - } - n_udp_conn_list->size--; // Decrement the size of the list - return true; // Exit the function - } +static void +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, + sizeof(n_udp_conn_list->db_udp_clients[i].udp_client)); + if(sent != data_length) { + int err = errno; + ESP_LOGE(TAG, + "UDP - Error sending (%i/%i) to because of %d", + sent, + data_length, + err); } - // No match found - return false; + } } -/** - * Reads serial (UART/USB/JTAG) transparently or parsing MAVLink/MSP/LTM protocol. - * Then sends read data to all connected clients via TCP/UDP or ESP-NOW. - * Non-Blocking function - * - * @param tcp_clients Array of connected TCP client sockets - * @param transparent_buff_pos Counter variable for total read UART bytes - * @param msp_ltm_buff_pos Pointer position/data length of destination-buffer for read MSP messages - * @param msp_message_buffer Destination-buffer for read MSP messages - * @param serial_buffer Destination-buffer for the serial data - * @param db_msp_ltm_port Pointer to structure containing MSP/LTM parser information - */ -void read_process_serial_link(int *tcp_clients, uint *transparent_buff_pos, uint *msp_ltm_buff_pos, - uint8_t *msp_message_buffer, - uint8_t *serial_buffer, msp_ltm_port_t *db_msp_ltm_port) { - switch (DB_PARAM_SERIAL_PROTO) { - case 0: - case 2: - case DB_SERIAL_PROTOCOL_MSPLTM: - db_parse_msp_ltm(tcp_clients, udp_conn_list, msp_message_buffer, msp_ltm_buff_pos, db_msp_ltm_port); - break; - case 3: - case DB_SERIAL_PROTOCOL_MAVLINK: - db_read_serial_parse_mavlink(tcp_clients, udp_conn_list, msp_message_buffer, transparent_buff_pos); - break; - case DB_SERIAL_PROTOCOL_TRANSPARENT: - default: - db_read_serial_parse_transparent(tcp_clients, udp_conn_list, serial_buffer, transparent_buff_pos); - break; +static void +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 + } +} + +static void +handle_tcp_master(const int tcp_master_socket, int tcp_clients[]) +{ + struct sockaddr_in6 source_addr; // Large enough for both IPv4 or IPv6 + uint32_t addr_len = sizeof(source_addr); + int new_tcp_client = + accept(tcp_master_socket, (struct sockaddr *)&source_addr, &addr_len); + if(new_tcp_client > 0) { + for(int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { + if(tcp_clients[i] <= 0) { + tcp_clients[i] = new_tcp_client; + fcntl(tcp_clients[i], F_SETFL, O_NONBLOCK); + char addr_str[128]; + inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr.s_addr, + addr_str, + sizeof(addr_str) - 1); + ESP_LOGI(TAG, "TCP: New client connected: %s", addr_str); + num_connected_tcp_clients++; + return; + } } + ESP_LOGI(TAG, + "TCP: Could not accept connection. Too many clients connected."); + } +} + +static void +read_process_serial_link(int *tcp_clients, uint *transparent_buff_pos, + uint *msp_ltm_buff_pos, uint8_t *msp_message_buffer, + uint8_t *serial_buffer, + msp_ltm_port_t *db_msp_ltm_port) +{ + switch(DB_PARAM_SERIAL_PROTO) { + case 0: + case 2: + case DB_SERIAL_PROTOCOL_MSPLTM: + db_parse_msp_ltm(tcp_clients, + udp_conn_list, + msp_message_buffer, + msp_ltm_buff_pos, + db_msp_ltm_port); + break; + case 3: + case DB_SERIAL_PROTOCOL_MAVLINK: + db_read_serial_parse_mavlink( + tcp_clients, udp_conn_list, msp_message_buffer, transparent_buff_pos); + break; + case DB_SERIAL_PROTOCOL_TRANSPARENT: + default: + db_read_serial_parse_transparent( + tcp_clients, udp_conn_list, serial_buffer, transparent_buff_pos); + break; + } } -/** - * Thread that manages all incoming and outgoing ESP-NOW and serial (UART) connections. - * Called only when ESP-NOW mode is selected - */ -_Noreturn void control_module_esp_now() { - ESP_LOGI(TAG, "Starting control module (ESP-NOW)"); - esp_err_t serial_socket = ESP_FAIL; - // open serial socket for comms with FC or GCS - serial_socket = open_serial_socket(); - if (serial_socket == ESP_FAIL) { - ESP_LOGE(TAG, "UART socket not opened. Aborting start of control module."); - vTaskDelete(NULL); - } else { +_Noreturn static void +control_module_esp_now() +{ + ESP_LOGI(TAG, "Starting control module (ESP-NOW)"); + esp_err_t serial_socket = ESP_FAIL; + // open serial socket for comms with FC or GCS + serial_socket = open_serial_socket(); + if(serial_socket == ESP_FAIL) { + ESP_LOGE(TAG, "UART socket not opened. Aborting start of control module."); + vTaskDelete(NULL); + } + else { #ifdef CONFIG_DB_SERIAL_OPTION_JTAG - db_jtag_serial_info_print(); + db_jtag_serial_info_print(); #endif + } + + uint transparent_buff_pos = 0; + uint msp_ltm_buff_pos = 0; + uint8_t msp_message_buffer[UART_BUF_SIZE]; + uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; + msp_ltm_port_t db_msp_ltm_port; + db_espnow_queue_event_t db_espnow_uart_evt; + uint delay_timer_cnt = 0; + + ESP_LOGI(TAG, "Started control module (ESP-NOW)"); + while(1) { + // read UART (and split into packets & process MAVLink if desired); send to + // ESP-NOW queue to be processed by esp-now task + read_process_serial_link(NULL, + &transparent_buff_pos, + &msp_ltm_buff_pos, + msp_message_buffer, + serial_buffer, + &db_msp_ltm_port); + // read queue that was filled by esp-now task to check for data that needs + // to be sent via serial link + if(db_uart_write_queue != NULL && + xQueueReceive(db_uart_write_queue, &db_espnow_uart_evt, 0) == pdTRUE) { + switch(DB_PARAM_SERIAL_PROTO) { + case DB_SERIAL_PROTOCOL_MAVLINK: + // Parse, so we can listen in and react to certain messages - function + // will send parsed messages to serial link. 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 + db_parse_mavlink_from_radio( + NULL, NULL, db_espnow_uart_evt.data, db_espnow_uart_evt.data_len); + break; + + default: + // No parsing with any other protocol - transparent here - just pass + // through + db_write_to_serial(db_espnow_uart_evt.data, + db_espnow_uart_evt.data_len); + break; + } + + free(db_espnow_uart_evt.data); + } + else { + if(db_uart_write_queue == NULL) + ESP_LOGE(TAG, "db_uart_write_queue is NULL!"); + // no new data available to be sent via serial link do nothing } + if(delay_timer_cnt == 5000) { + /* all actions are non-blocking so allow some delay so that the IDLE task + of FreeRTOS and the watchdog can run read: + https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines + for reference */ + vTaskDelay(10 / portTICK_PERIOD_MS); + delay_timer_cnt = 0; + } + else { + delay_timer_cnt++; + } + } + vTaskDelete(NULL); +} - uint transparent_buff_pos = 0; - uint msp_ltm_buff_pos = 0; - uint8_t msp_message_buffer[UART_BUF_SIZE]; - uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; - msp_ltm_port_t db_msp_ltm_port; - db_espnow_queue_event_t db_espnow_uart_evt; - uint delay_timer_cnt = 0; - - ESP_LOGI(TAG, "Started control module (ESP-NOW)"); - while (1) { - // read UART (and split into packets & process MAVLink if desired); send to ESP-NOW queue to be processed by esp-now task - read_process_serial_link(NULL, &transparent_buff_pos, &msp_ltm_buff_pos, msp_message_buffer, serial_buffer, - &db_msp_ltm_port); - // read queue that was filled by esp-now task to check for data that needs to be sent via serial link - if (db_uart_write_queue != NULL && xQueueReceive(db_uart_write_queue, &db_espnow_uart_evt, 0) == pdTRUE) { - if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) { - // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link. - // 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 - db_parse_mavlink_from_radio(NULL, NULL, db_espnow_uart_evt.data, db_espnow_uart_evt.data_len); - } else { - // no parsing with any other protocol - transparent here - just pass through - write_to_serial(db_espnow_uart_evt.data, db_espnow_uart_evt.data_len); - } - free(db_espnow_uart_evt.data); - } else { - if (db_uart_write_queue == NULL) ESP_LOGE(TAG, "db_uart_write_queue is NULL!"); - // no new data available to be sent via serial link do nothing - } - if (delay_timer_cnt == 5000) { - /* all actions are non-blocking so allow some delay so that the IDLE task of FreeRTOS and the watchdog can run - read: https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines for reference */ - vTaskDelay(10 / portTICK_PERIOD_MS); - delay_timer_cnt = 0; - } else { - delay_timer_cnt++; - } +static void +send_internal_telemetry_to_stations(int tel_sock, wifi_sta_list_t *sta_list, + udp_conn_list_t *udp_conns) +{ + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR && udp_conns->size > 0 && + sta_list->num > 0) { + char addr_buf[32] = { 0 }; + struct addrinfo hints = { + .ai_flags = AI_PASSIVE, + .ai_socktype = SOCK_DGRAM, + }; + struct addrinfo *res; + + // Send an IPv4 multicast packet + hints.ai_family = AF_INET; // For an IPv4 socket + int err = getaddrinfo(MULTICAST_IPV4_ADDR, NULL, &hints, &res); + if(err < 0) { + ESP_LOGE(TAG, + "getaddrinfo() failed for IPV4 destination address. error: %d", + err); + return; } - vTaskDelete(NULL); + if(res == 0) { + ESP_LOGE(TAG, "getaddrinfo() did not return any addresses"); + return; + } + ((struct sockaddr_in *)res->ai_addr)->sin_port = + htons(DB_ESP32_INTERNAL_TELEMETRY_PORT); + inet_ntoa_r(((struct sockaddr_in *)res->ai_addr)->sin_addr, + addr_buf, + sizeof(addr_buf) - 1); + static uint8_t buffer[1280] = { 0 }; + uint16_t buffer_pos = + 1; // we start at 1 since we want to put the count in position 0 + uint8_t already_sent = 0; + for(int i = 0; i < sta_list->num; i++) { + if((buffer_pos + 7) < 1280) { + memcpy(&buffer[buffer_pos], sta_list->sta[i].mac, 6); + buffer_pos += 6; + buffer[buffer_pos] = sta_list->sta[i].rssi; + buffer_pos++; + } + else { + // packet would get too long. Sent this chunk already + buffer[0] = (uint8_t) + i; // first byte shall be the number of entries in the packet + sendto(tel_sock, buffer, buffer_pos, 0, res->ai_addr, res->ai_addrlen); + already_sent += i; + buffer_pos = 1; + } + } + buffer[0] = (uint8_t)sta_list->num - already_sent; + err = sendto( + tel_sock, buffer, buffer_pos + 1, 0, res->ai_addr, res->ai_addrlen); + freeaddrinfo(res); + if(err < 0) { + ESP_LOGE(TAG, "Internal telemetry sendto failed. errno: %d", errno); + return; + } + } + else { + // in other modes we cannot do that. ESP-NOW uses a different function and + // way of telling + } } -/** - * Sends DroneBridge internal telemetry to tell every connected WiFi station how well we receive their data (rssi). - * Uses UDP multicast message. Format: [NUM_Entries - (MAC + RSSI) - (MAC + RSSI) - ...] - * Internal telemetry uses DB_ESP32_INTERNAL_TELEMETRY_PORT port - * - * @param sta_list - */ -void db_send_internal_telemetry_to_stations(int tel_sock, wifi_sta_list_t *sta_list, udp_conn_list_t *udp_conns) { - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR && udp_conns->size > 0 && sta_list->num > 0) { - char addr_buf[32] = {0}; - struct addrinfo hints = { - .ai_flags = AI_PASSIVE, - .ai_socktype = SOCK_DGRAM, - }; - struct addrinfo *res; - - // Send an IPv4 multicast packet - hints.ai_family = AF_INET; // For an IPv4 socket - int err = getaddrinfo(MULTICAST_IPV4_ADDR, NULL, &hints, &res); - if (err < 0) { - ESP_LOGE(TAG, "getaddrinfo() failed for IPV4 destination address. error: %d", err); - return; - } - if (res == 0) { - ESP_LOGE(TAG, "getaddrinfo() did not return any addresses"); - return; - } - ((struct sockaddr_in *) res->ai_addr)->sin_port = htons(DB_ESP32_INTERNAL_TELEMETRY_PORT); - inet_ntoa_r(((struct sockaddr_in *) res->ai_addr)->sin_addr, addr_buf, sizeof(addr_buf) - 1); - static uint8_t buffer[1280] = {0}; - uint16_t buffer_pos = 1; // we start at 1 since we want to put the count in position 0 - uint8_t already_sent = 0; - for (int i = 0; i < sta_list->num; i++) { - if ((buffer_pos + 7) < 1280) { - memcpy(&buffer[buffer_pos], sta_list->sta[i].mac, 6); - buffer_pos += 6; - buffer[buffer_pos] = sta_list->sta[i].rssi; - buffer_pos++; - } else { - // packet would get too long. Sent this chunk already - buffer[0] = (uint8_t) i; // first byte shall be the number of entries in the packet - sendto(tel_sock, buffer, buffer_pos, 0, res->ai_addr, res->ai_addrlen); - already_sent += i; - buffer_pos = 1; - } +static void +handle_internal_telemetry(int tel_sock, uint8_t *udp_buffer, + socklen_t *sock_len, struct sockaddr_in *udp_client) +{ + if(tel_sock > 0) { + ssize_t recv_length = recvfrom(tel_sock, + udp_buffer, + UDP_BUF_SIZE, + 0, + (struct sockaddr *)udp_client, + sock_len); + if(recv_length > 0) { + ESP_LOGD( + TAG, "Got internal telem. frame containing %i entries", udp_buffer[0]); + for(int i = 1; i < (udp_buffer[0] * 7); i += 7) { + if(memcmp(LOCAL_MAC_ADDRESS, &udp_buffer[i], ESP_NOW_ETH_ALEN) == 0) { + // found us in the list (this local ESP32 AIR unit) -> update + // internal telemetry buffer, so it gets sent with next Mavlink RADIO + // STATUS in case MAVLink radio status is enabled + db_esp_signal_quality.gnd_rssi = (int8_t)udp_buffer[i + 6]; + ESP_LOGD(TAG, + "AP receives our packets with RSSI: %i", + db_esp_signal_quality.gnd_rssi); + break; } - buffer[0] = (uint8_t) sta_list->num - already_sent; - err = sendto(tel_sock, buffer, buffer_pos + 1, 0, res->ai_addr, res->ai_addrlen); - freeaddrinfo(res); - if (err < 0) { - ESP_LOGE(TAG, "Internal telemetry sendto failed. errno: %d", errno); - return; + else { + // keep on looking for our MAC } - } else { - // in other modes we cannot do that. ESP-NOW uses a different function and way of telling + } } + else { /* received nothing - socket is non-blocking */ + } + } + else { /* socket failed to init or was never inited */ + } } -/** - * Receive and process internal telemetry (ESP32 AP to ESP32 Station) sent by ESP32 LR access point. - * Matches with db_send_internal_telemetry_to_stations() - * Sets station_rssi_ap based on the received value - * Packet format: [NUM_Entries, (MAC + RSSI), (MAC + RSSI), (MAC + RSSI), ...] - * - * @param tel_sock Socket listening for internal telemetry - */ -void handle_internal_telemetry(int tel_sock, uint8_t *udp_buffer, socklen_t *sock_len, struct sockaddr_in *udp_client) { - if (tel_sock > 0) { - ssize_t recv_length = recvfrom(tel_sock, udp_buffer, UDP_BUF_SIZE, 0, - (struct sockaddr *) udp_client, sock_len); - if (recv_length > 0) { - ESP_LOGD(TAG, "Got internal telem. frame containing %i entries", udp_buffer[0]); - for (int i = 1; i < (udp_buffer[0] * 7); i += 7) { - if (memcmp(LOCAL_MAC_ADDRESS, &udp_buffer[i], ESP_NOW_ETH_ALEN) == 0) { - // found us in the list (this local ESP32 AIR unit) -> update internal telemetry buffer, - // so it gets sent with next Mavlink RADIO STATUS in case MAVLink radio status is enabled - db_esp_signal_quality.gnd_rssi = (int8_t) udp_buffer[i + 6]; - ESP_LOGD(TAG, "AP receives our packets with RSSI: %i", db_esp_signal_quality.gnd_rssi); - break; - } else { - // keep on looking for our MAC - } - } - } else {/* received nothing - socket is non-blocking */} - } else {/* socket failed to init or was never inited */} -} - -/** - * Thread that manages all incoming and outgoing TCP, UDP and serial (UART) connections. - * Executed when Wi-Fi modes are set - ESP-NOW has its own thread - */ -_Noreturn void control_module_udp_tcp() { - ESP_LOGI(TAG, "Starting control module (Wi-Fi)"); - esp_err_t serial_socket_status = open_serial_socket(); - if (serial_socket_status == ESP_FAIL) { - ESP_LOGE(TAG, "JTAG serial socket not opened. Aborting start of control module."); - vTaskDelete(NULL); - } else { +_Noreturn static void +control_module_udp_tcp() +{ + ESP_LOGI(TAG, "Starting control module (Wi-Fi)"); + esp_err_t serial_socket_status = open_serial_socket(); + if(serial_socket_status == ESP_FAIL) { + ESP_LOGE( + TAG, "JTAG serial socket not opened. Aborting start of control module."); + vTaskDelete(NULL); + } + else { #ifdef CONFIG_DB_SERIAL_OPTION_JTAG - db_jtag_serial_info_print(); + db_jtag_serial_info_print(); #endif - } + } - int tcp_master_socket = open_tcp_server(app_port_proxy); - if (tcp_master_socket == ESP_FAIL) { - ESP_LOGE(TAG, "TCP master socket failed to open. Aborting start of control module."); - vTaskDelete(NULL); - } - fcntl(tcp_master_socket, F_SETFL, O_NONBLOCK); - int tcp_clients[CONFIG_LWIP_MAX_ACTIVE_TCP]; - for (int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { - tcp_clients[i] = -1; - } + int tcp_master_socket = open_tcp_server(app_port_proxy); + if(tcp_master_socket == ESP_FAIL) { + ESP_LOGE( + TAG, + "TCP master socket failed to open. Aborting start of control module."); + vTaskDelete(NULL); + } + fcntl(tcp_master_socket, F_SETFL, O_NONBLOCK); + int tcp_clients[CONFIG_LWIP_MAX_ACTIVE_TCP]; + for(int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { + tcp_clients[i] = -1; + } - udp_conn_list->udp_socket = db_open_serial_udp_socket(); + udp_conn_list->udp_socket = open_serial_udp_socket(); #ifdef CONFIG_DB_SKYBRUSH_SUPPORT - int udp_broadcast_skybrush_socket = -1; - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - udp_broadcast_skybrush_socket = db_open_serial_udp_broadcast_socket(); - } else { - // we do only support Skybrush Wi-Fi and the broadcast port when in Wi-Fi client mode - } + int udp_broadcast_skybrush_socket = -1; + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { + udp_broadcast_skybrush_socket = open_serial_udp_broadcast_socket(); + } + else { + // we do only support Skybrush Wi-Fi and the broadcast port when in Wi-Fi + // client mode + } #endif - int db_internal_telem_udp_sock = -1; - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - db_internal_telem_udp_sock = db_open_int_telemetry_udp_socket(); - } else { - // other Wi-Fi modes do not need this. Only Wi-Fi stations will receive if connected to LR access point. - // ESP-NOW uses different sockets/systems - } - uint8_t udp_buffer[UDP_BUF_SIZE]; - struct db_udp_client_t new_db_udp_client = {0}; - socklen_t udp_socklen = sizeof(new_db_udp_client.udp_client); - - uint transparent_buff_pos = 0; - uint msp_ltm_buff_pos = 0; - uint8_t tcp_client_buffer[TCP_BUFF_SIZ]; - memset(tcp_client_buffer, 0, TCP_BUFF_SIZ); - uint8_t msp_message_buffer[UART_BUF_SIZE]; - uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; - msp_ltm_port_t db_msp_ltm_port; - int delay_timer_cnt = 0; - - ESP_LOGI(TAG, "Started control module (Wi-Fi)"); - while (1) { - // Read incoming wireless data (Wi-Fi) - // Wi-Fi based modes that use TCP and UDP communication - handle_tcp_master(tcp_master_socket, tcp_clients); - for (int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { // handle TCP clients - if (tcp_clients[i] > 0) { - ssize_t recv_length = recv(tcp_clients[i], tcp_client_buffer, TCP_BUFF_SIZ, 0); - if (recv_length > 0) { - if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) { - // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link. - // 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 - db_parse_mavlink_from_radio(tcp_clients, udp_conn_list, tcp_client_buffer, recv_length); - } else { - // no parsing with any other protocol - transparent here - write_to_serial(tcp_client_buffer, recv_length); - } - } else if (recv_length == 0) { - shutdown(tcp_clients[i], 0); - close(tcp_clients[i]); - tcp_clients[i] = -1; - ESP_LOGI(TAG, "TCP client disconnected"); - num_connected_tcp_clients--; - } else if (errno != EAGAIN && errno != EWOULDBLOCK) { - ESP_LOGE(TAG, "Error receiving from TCP client %i (fd: %i): %d", i, tcp_clients[i], errno); - shutdown(tcp_clients[i], 0); - close(tcp_clients[i]); - num_connected_tcp_clients--; - tcp_clients[i] = -1; - } - } + int db_internal_telem_udp_sock = -1; + switch(DB_PARAM_RADIO_MODE) { + case DB_WIFI_MODE_AP_LR: + case DB_WIFI_MODE_STA: + db_internal_telem_udp_sock = open_int_telemetry_udp_socket(); + break; + + default: + // Other Wi-Fi modes do not need this. Only Wi-Fi stations will receive if + // connected to LR access point. ESP-NOW uses different sockets/systems + break; + } + + uint8_t udp_buffer[UDP_BUF_SIZE]; + struct db_udp_client_t new_db_udp_client = { 0 }; + socklen_t udp_socklen = sizeof(new_db_udp_client.udp_client); + + uint transparent_buff_pos = 0; + uint msp_ltm_buff_pos = 0; + uint8_t tcp_client_buffer[TCP_BUFF_SIZ]; + memset(tcp_client_buffer, 0, TCP_BUFF_SIZ); + uint8_t msp_message_buffer[UART_BUF_SIZE]; + uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; + msp_ltm_port_t db_msp_ltm_port; + int delay_timer_cnt = 0; + + ESP_LOGI(TAG, "Started control module (Wi-Fi)"); + while(1) { + // Read incoming wireless data (Wi-Fi) + // Wi-Fi based modes that use TCP and UDP communication + handle_tcp_master(tcp_master_socket, tcp_clients); + for(int i = 0; i < CONFIG_LWIP_MAX_ACTIVE_TCP; i++) { // handle TCP clients + if(tcp_clients[i] > 0) { + ssize_t recv_length = + recv(tcp_clients[i], tcp_client_buffer, TCP_BUFF_SIZ, 0); + if(recv_length > 0) { + switch(DB_PARAM_SERIAL_PROTO) { + case DB_SERIAL_PROTOCOL_MAVLINK: + // Parse, so we can listen in and react to certain messages - + // function will send parsed messages to serial link. 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 + db_parse_mavlink_from_radio( + tcp_clients, udp_conn_list, tcp_client_buffer, recv_length); + break; + + default: + // No parsing with any other protocol - transparent here + db_write_to_serial(tcp_client_buffer, recv_length); + break; + } } - // handle incoming UDP data on main port 14550 - Read UDP and forward to UART - ssize_t recv_length = recvfrom(udp_conn_list->udp_socket, udp_buffer, UDP_BUF_SIZE, 0, - (struct sockaddr *) &new_db_udp_client.udp_client, &udp_socklen); - if (recv_length > 0) { - if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) { - // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link. - // 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 - db_parse_mavlink_from_radio(tcp_clients, udp_conn_list, udp_buffer, recv_length); - } else { - // no parsing with any other protocol - transparent here - write_to_serial(udp_buffer, recv_length); - } - // all devices that send us UDP data will be added to the list of UDP receivers - // Allows to register new app on different port. Used e.g. for UDP conn setup in sta-mode. - // Devices/Ports added this way cannot be removed in sta-mode since UDP is connectionless, and we cannot - // determine if the client is still existing. This will blow up the list connected devices. - // In AP-Mode the devices can be removed based on the IP/MAC address - add_to_known_udp_clients(udp_conn_list, new_db_udp_client, false); - } else { - // received nothing, keep on going + else if(recv_length == 0) { + shutdown(tcp_clients[i], 0); + close(tcp_clients[i]); + tcp_clients[i] = -1; + ESP_LOGI(TAG, "TCP client disconnected"); + num_connected_tcp_clients--; } -#ifdef CONFIG_DB_SKYBRUSH_SUPPORT - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA && udp_broadcast_skybrush_socket != -1) { - // This is special support for Skybrush. Skybrush sends some UDP broadcast msgs to 14555 in addition to regular msgs to 14550 - // We only read and directly forward here. No parsing and no adding to known UDP clients - recv_length = recvfrom(udp_broadcast_skybrush_socket, udp_buffer, UDP_BUF_SIZE, 0, - (struct sockaddr *) &new_db_udp_client.udp_client, &udp_socklen); - if (recv_length > 0) { - // no parsing - transparent here - write_to_serial(udp_buffer, recv_length); - // add Skybrush server to known UDP target/distribution list - add_to_known_udp_clients(udp_conn_list, new_db_udp_client, false); - } + else if(errno != EAGAIN && errno != EWOULDBLOCK) { + ESP_LOGE(TAG, + "Error receiving from TCP client %i (fd: %i): %d", + i, + tcp_clients[i], + errno); + shutdown(tcp_clients[i], 0); + close(tcp_clients[i]); + num_connected_tcp_clients--; + tcp_clients[i] = -1; } + } + } + // handle incoming UDP data on main port 14550 - Read UDP and forward to + // UART + ssize_t recv_length = + recvfrom(udp_conn_list->udp_socket, + udp_buffer, + UDP_BUF_SIZE, + 0, + (struct sockaddr *)&new_db_udp_client.udp_client, + &udp_socklen); + if(recv_length > 0) { + switch(DB_PARAM_SERIAL_PROTO) { + case DB_SERIAL_PROTOCOL_MAVLINK: + // Parse, so we can listen in and react to certain messages - function + // will send parsed messages to serial link. 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 + db_parse_mavlink_from_radio( + tcp_clients, udp_conn_list, udp_buffer, recv_length); + break; + + default: + // No parsing with any other protocol - transparent here + db_write_to_serial(udp_buffer, recv_length); + break; + } + + // All devices that send us UDP data will be added to the list of UDP + // receivers Allows to register new app on different port. Used e.g. for + // UDP conn setup in sta-mode. Devices/Ports added this way cannot be + // removed in sta-mode since UDP is connectionless, and we cannot + // determine if the client is still existing. This will blow up the list + // of connected devices. In AP-Mode the devices can be removed based on + // the IP/MAC address + db_add_to_known_udp_clients(udp_conn_list, new_db_udp_client, false); + } + +#ifdef CONFIG_DB_SKYBRUSH_SUPPORT + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA && + udp_broadcast_skybrush_socket != -1) { + // This is special support for Skybrush. Skybrush sends some UDP + // broadcast msgs to 14555 in addition to regular msgs to 14550 We only + // read and directly forward here. No parsing and no adding to known UDP + // clients + recv_length = recvfrom(udp_broadcast_skybrush_socket, + udp_buffer, + UDP_BUF_SIZE, + 0, + (struct sockaddr *)&new_db_udp_client.udp_client, + &udp_socklen); + if(recv_length > 0) { + // no parsing - transparent here + db_write_to_serial(udp_buffer, recv_length); + // add Skybrush server to known UDP target/distribution list + db_add_to_known_udp_clients(udp_conn_list, new_db_udp_client, false); + } + } #endif - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - handle_internal_telemetry(db_internal_telem_udp_sock, udp_buffer, &udp_socklen, - &new_db_udp_client.udp_client); - } else { - // internal telemetry only received when in STA mode. Coming from the ESP32 AP. Nothing to do here - } + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { + handle_internal_telemetry(db_internal_telem_udp_sock, + udp_buffer, + &udp_socklen, + &new_db_udp_client.udp_client); + } + else { + // internal telemetry only received when in STA mode. Coming from the + // ESP32 AP. Nothing to do here + } + + // Second check for incoming UART data and send it to TCP/UDP + read_process_serial_link(tcp_clients, + &transparent_buff_pos, + &msp_ltm_buff_pos, + msp_message_buffer, + serial_buffer, + &db_msp_ltm_port); - // Second check for incoming UART data and send it to TCP/UDP - read_process_serial_link(tcp_clients, &transparent_buff_pos, &msp_ltm_buff_pos, msp_message_buffer, - serial_buffer, - &db_msp_ltm_port); - - if (delay_timer_cnt == 6000) { - // all actions are non-blocking so allow some delay so that the IDLE task of FreeRTOS and the watchdog can run - // read: https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines for reference - vTaskDelay(10 / portTICK_PERIOD_MS); - delay_timer_cnt = 0; - // Use the opportunity to get some regular status information like rssi and send them via internal telemetry - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - // update rssi variable - set to -127 when not available - if (esp_wifi_sta_get_rssi((int *) &db_esp_signal_quality.air_rssi) != ESP_OK) { - db_esp_signal_quality.air_rssi = -127; - } else {/* all good */} - } else if (!DB_RADIO_IS_OFF && - (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR)) { - ESP_ERROR_CHECK_WITHOUT_ABORT( - esp_wifi_ap_get_sta_list(&wifi_sta_list)); // update list of connected stations - db_send_internal_telemetry_to_stations(db_internal_telem_udp_sock, &wifi_sta_list, udp_conn_list); - } else { - // no way of getting RSSI here. Do nothing - } -// size_t free_dram = heap_caps_get_free_size(MALLOC_CAP_8BIT); -// size_t lowmark_dram = heap_caps_get_minimum_free_size(MALLOC_CAP_8BIT); -// ESP_LOGI(TAG, "Free heap: %i, low mark: %i", free_dram, lowmark_dram); - } else { - delay_timer_cnt++; + if(delay_timer_cnt == 6000) { + // all actions are non-blocking so allow some delay so that the IDLE task + // of FreeRTOS and the watchdog can run read: + // https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines + // for reference + vTaskDelay(10 / portTICK_PERIOD_MS); + delay_timer_cnt = 0; + // Use the opportunity to get some regular status information like rssi + // and send them via internal telemetry + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { + // update rssi variable - set to -127 when not available + if(esp_wifi_sta_get_rssi((int *)&db_esp_signal_quality.air_rssi) != + ESP_OK) { + db_esp_signal_quality.air_rssi = -127; + } + else { /* all good */ } + } + else if(!DB_RADIO_IS_OFF && + (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR)) { + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_ap_get_sta_list( + &wifi_sta_list)); // update list of connected stations + send_internal_telemetry_to_stations( + db_internal_telem_udp_sock, &wifi_sta_list, udp_conn_list); + } + else { + // no way of getting RSSI here. Do nothing + } + // size_t free_dram = + // heap_caps_get_free_size(MALLOC_CAP_8BIT); size_t + // lowmark_dram = + // heap_caps_get_minimum_free_size(MALLOC_CAP_8BIT); + // ESP_LOGI(TAG, "Free heap: %i, low mark: %i", free_dram, + // lowmark_dram); } - vTaskDelete(NULL); + else { + delay_timer_cnt++; + } + } + vTaskDelete(NULL); } #ifdef CONFIG_BT_ENABLED -static _Noreturn void control_module_ble() { - ESP_LOGI(TAG, "Starting control module (Bluetooth)"); +_Noreturn static void +control_module_ble() +{ + ESP_LOGI(TAG, "Starting control module (Bluetooth)"); - /* Initialize error code as failed, because UART is not initialized*/ - esp_err_t serial_socket = ESP_FAIL; + /* Initialize error code as failed, because UART is not initialized*/ + esp_err_t serial_socket = ESP_FAIL; - /* open serial socket for comms with FC or GCS */ - serial_socket = open_serial_socket(); - if (serial_socket == ESP_FAIL) { - ESP_LOGE(TAG, "UART socket not opened. Aborting start of control module."); - vTaskDelete(NULL); - } + /* open serial socket for comms with FC or GCS */ + serial_socket = open_serial_socket(); + if(serial_socket == ESP_FAIL) { + ESP_LOGE(TAG, "UART socket not opened. Aborting start of control module."); + vTaskDelete(NULL); + } #ifdef CONFIG_DB_SERIAL_OPTION_JTAG + else { + db_jtag_serial_info_print(); + } +#endif + + uint8_t msp_message_buffer[UART_BUF_SIZE]; + uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; + msp_ltm_port_t db_msp_ltm_port; + db_ble_queue_event_t bleData; + uint transparent_buff_pos = 0; + uint msp_ltm_buff_pos = 0; + uint delay_timer_cnt = 0; + + /* Event Loop */ + while(1) { + /* Read UART and send data to BLE */ + read_process_serial_link( + NULL, // NULL, not using TCP + &transparent_buff_pos, // transparent buffer position + &msp_ltm_buff_pos, // msp buffer position + msp_message_buffer, // msp buffer + serial_buffer, // serial buffer + &db_msp_ltm_port // msp port + ); + + if(db_uart_write_queue_ble != NULL && + xQueueReceive(db_uart_write_queue_ble, &bleData, 0) == pdTRUE) { + if(DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) { + // Parse, so we can listen in and react to certain messages - function + // will send parsed messages to serial link. 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 + db_parse_mavlink_from_radio( + NULL, NULL, bleData.data, bleData.data_len); + } + else { + // no parsing with any other protocol - transparent here - just pass + // through + db_write_to_serial(bleData.data, bleData.data_len); + } + free(bleData.data); + } + else { + if(db_uart_write_queue_ble == NULL) + ESP_LOGE(TAG, "db_uart_write_queue is NULL!"); + // no new data available to be sent via serial link do nothing + } + + /**Yield to the scheduler if delay_timer_cnt reaches 5000,allowing other + * tasks to execute and preventing starvation. + */ + if(delay_timer_cnt == 5000) { + vTaskDelay(10 / portTICK_PERIOD_MS); + delay_timer_cnt = 0; + } else { - db_jtag_serial_info_print(); + delay_timer_cnt++; } + } + vTaskDelete(NULL); +} + #endif - uint8_t msp_message_buffer[UART_BUF_SIZE]; - uint8_t serial_buffer[DB_PARAM_SERIAL_PACK_SIZE]; - msp_ltm_port_t db_msp_ltm_port; - db_ble_queue_event_t bleData; - uint transparent_buff_pos = 0; - uint msp_ltm_buff_pos = 0; - uint delay_timer_cnt = 0; - - /* Event Loop */ - while (1) { - /* Read UART and send data to BLE */ - read_process_serial_link(NULL, // NULL, not using TCP - &transparent_buff_pos, // transparent buffer position - &msp_ltm_buff_pos, // msp buffer position - msp_message_buffer, // msp buffer - serial_buffer, // serial buffer - &db_msp_ltm_port // msp port - ); - - if (db_uart_write_queue_ble != NULL && xQueueReceive(db_uart_write_queue_ble, &bleData, 0) == pdTRUE) { - if (DB_PARAM_SERIAL_PROTO == DB_SERIAL_PROTOCOL_MAVLINK) { - // Parse, so we can listen in and react to certain messages - function will send parsed messages to serial link. - // 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 - db_parse_mavlink_from_radio(NULL, NULL, bleData.data, bleData.data_len); - } else { - // no parsing with any other protocol - transparent here - just pass through - write_to_serial(bleData.data, bleData.data_len); - } - free(bleData.data); - } else { - if (db_uart_write_queue_ble == NULL) - ESP_LOGE(TAG, "db_uart_write_queue is NULL!"); - // no new data available to be sent via serial link do nothing - } +/****************************************************************************** + * Public Function Definition + ******************************************************************************/ - /**Yield to the scheduler if delay_timer_cnt reaches 5000,allowing other - * tasks to execute and preventing starvation. - */ - if (delay_timer_cnt == 5000) { - vTaskDelay(10 / portTICK_PERIOD_MS); - delay_timer_cnt = 0; - } else { - delay_timer_cnt++; - } +bool +db_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) +{ + if(n_udp_conn_list == NULL) { // Check if the list is NULL + return false; // Do nothing + } + if(n_udp_conn_list->size == MAX_UDP_CLIENTS) { // Check if the list is full + return false; // Do nothing + } + for(int i = 0; i < n_udp_conn_list->size; i++) { + if((n_udp_conn_list->db_udp_clients[i].udp_client.sin_port == + new_db_udp_client.udp_client.sin_port) && + (n_udp_conn_list->db_udp_clients[i].udp_client.sin_addr.s_addr == + new_db_udp_client.udp_client.sin_addr.s_addr)) { + return false; // client existing - do not add } - vTaskDelete(NULL); + } + n_udp_conn_list->db_udp_clients[n_udp_conn_list->size] = + new_db_udp_client; // Copy the element data to the end of the array + n_udp_conn_list->size++; // Increment the size of the list + // some logging + char ip_port_string[INET_ADDRSTRLEN + 10]; + char ip_string[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, + &(new_db_udp_client.udp_client.sin_addr), + ip_string, + INET_ADDRSTRLEN); + sprintf(ip_port_string, + "%s:%d", + ip_string, + htons(new_db_udp_client.udp_client.sin_port)); + ESP_LOGI(TAG, + "Added %s to udp client distribution list - save to NVM: %i", + ip_port_string, + save_to_nvm); + // save to memory + if(save_to_nvm) { + db_save_udp_client_to_nvm(&new_db_udp_client, false); + } + else { + // do not save to NVM + } + return true; } -#endif +udp_conn_list_t * +db_udp_client_list_create() +{ + udp_conn_list_t *n_udp_conn_list = + malloc(sizeof(udp_conn_list_t)); // Allocate memory for the list + if(n_udp_conn_list == NULL) { // Check if the allocation failed + return NULL; // Return NULL to indicate an error + } + n_udp_conn_list->size = 0; // Initialize the size to 0 + return n_udp_conn_list; // Return the pointer to the list +} -/** - * @brief DroneBridge control module implementation for a ESP32 device. Bidirectional link between FC and ground. Can - * handle MSPv1, MSPv2, LTM and MAVLink. - * MSP & LTM is parsed and sent packet/frame by frame to ground - * MAVLink is passed through (fully transparent). Can be used with any protocol. - */ -void db_start_control_module() { - switch (DB_PARAM_RADIO_MODE) { - case DB_WIFI_MODE_ESPNOW_GND: - case DB_WIFI_MODE_ESPNOW_AIR: - xTaskCreate(&control_module_esp_now, /**< Task function for ESP-NOW communication */ - "control_espnow", /**< Task name (for debugging) */ - 40960, /**< Stack size (in bytes) */ - NULL, /**< Task parameters (unused) */ - 5, /**< Task priority */ - NULL /**< Task handle (unused) */ - ); - break; +void +db_udp_client_list_destroy(udp_conn_list_t *n_udp_conn_list) +{ + if(n_udp_conn_list == NULL) { // Check if the list is NULL + return; // Do nothing + } + free(n_udp_conn_list); // Free the list +} - case DB_BLUETOOTH_MODE: +void +db_send_to_all_clients(int tcp_clients[], udp_conn_list_t *n_udp_conn_list, + uint8_t data[], uint16_t data_length) +{ + db_ble_queue_event_t bleData; + switch(DB_PARAM_RADIO_MODE) { + case DB_WIFI_MODE_ESPNOW_AIR: + case DB_WIFI_MODE_ESPNOW_GND: + // ESP-NOW mode + if(data_length > DB_ESPNOW_PAYLOAD_MAXSIZE) { + // Data not properly sized, 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; + } + send_to_all_espnow(&data[sent_bytes], &next_chunk_len); + sent_bytes += next_chunk_len; + } while(sent_bytes < data_length); + } + else { + // Packet is properly sized - send to ESP-NOW outbound queue + send_to_all_espnow(data, &data_length); + } + break; + + case DB_BLUETOOTH_MODE: #ifdef CONFIG_BT_ENABLED - xTaskCreate(&control_module_ble, /**< Task function for Bluetooth BLE communication */ - "control_bluetooth", /**< Task name (for debugging) */ - 40960, /**< Stack size (in bytes) */ - NULL, /**< Task parameters (unused) */ - 5, /**< Task priority */ - NULL /**< Task handle (unused) */ - ); -#else - ESP_LOGE(TAG, "Bluetooth is not enabled. Aborting start of control module."); + bleData.data = malloc(data_length); + bleData.data_len = data_length; + memcpy(bleData.data, data, bleData.data_len); + if(xQueueSend(db_uart_read_queue_ble, &bleData, portMAX_DELAY) != pdPASS) { + ESP_LOGE(TAG, "Failed to send BLE data to queue"); + free(bleData.data); + } #endif - break; + break; - default: - xTaskCreate(&control_module_udp_tcp, /**< Task function for UDP/TCP communication */ - "control_wifi", /**< Task name (for debugging) */ - 46080, /**< Stack size (in bytes) */ - NULL, /**< Task parameters (unused) */ - 5, /**< Task priority */ - NULL /**< Task handle (unused) */ - ); - break; + default: + // Other modes (WiFi Modes using TCP/UDP) + db_send_to_all_tcp_clients(tcp_clients, data, data_length); + send_to_all_udp_clients(n_udp_conn_list, data, data_length); + break; + } +} + +bool +db_remove_from_known_udp_clients(udp_conn_list_t *n_udp_conn_list, + struct db_udp_client_t new_db_udp_client) +{ + if(n_udp_conn_list == NULL) { // Check if the list is NULL + return false; // Do nothing + } + for(int i = 0; i < n_udp_conn_list->size; i++) { // Loop through the array + if(memcmp(n_udp_conn_list->db_udp_clients[i].mac, + new_db_udp_client.mac, + sizeof(n_udp_conn_list->db_udp_clients[i].mac)) == + 0) { // Compare the current array element with the element + // Found a match + for(int j = i; j < n_udp_conn_list->size - 1; + j++) { // Loop from the current index to the end of the array + n_udp_conn_list->db_udp_clients[j] = + n_udp_conn_list + ->db_udp_clients[j + 1]; // Shift the array elements to the left + } + n_udp_conn_list->size--; // Decrement the size of the list + return true; // Exit the function } + } + // No match found + return false; } + +void +db_start_control_module() +{ + switch(DB_PARAM_RADIO_MODE) { + case DB_WIFI_MODE_ESPNOW_GND: + case DB_WIFI_MODE_ESPNOW_AIR: + xTaskCreate( + &control_module_esp_now, /**< Task function for ESP-NOW communication */ + "control_espnow", /**< Task name (for debugging) */ + 40960, /**< Stack size (in bytes) */ + NULL, /**< Task parameters (unused) */ + 5, /**< Task priority */ + NULL /**< Task handle (unused) */ + ); + break; + + case DB_BLUETOOTH_MODE: +#ifdef CONFIG_BT_ENABLED + xTaskCreate(&control_module_ble, /**< Task function for Bluetooth BLE + communication */ + "control_bluetooth", /**< Task name (for debugging) */ + 40960, /**< Stack size (in bytes) */ + NULL, /**< Task parameters (unused) */ + 5, /**< Task priority */ + NULL /**< Task handle (unused) */ + ); +#else + ESP_LOGE(TAG, + "Bluetooth is not enabled. Aborting start of control module."); +#endif + break; + + default: + xTaskCreate( + &control_module_udp_tcp, /**< Task function for UDP/TCP communication */ + "control_wifi", /**< Task name (for debugging) */ + 46080, /**< Stack size (in bytes) */ + NULL, /**< Task parameters (unused) */ + 5, /**< Task priority */ + NULL /**< Task handle (unused) */ + ); + break; + } +} \ No newline at end of file diff --git a/main/db_esp32_control.h b/main/db_esp32_control.h index 1aa44c1..628c4ff 100644 --- a/main/db_esp32_control.h +++ b/main/db_esp32_control.h @@ -1,4 +1,4 @@ -/* +/****************************************************************************** * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2018 Wolfgang Christl @@ -14,47 +14,146 @@ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. - * - */ + ******************************************************************************/ #ifndef DB_ESP32_DB_ESP32_CONTROL_H #define DB_ESP32_DB_ESP32_CONTROL_H -#include +/****************************************************************************** + * ESP-IDF Networking (LWIP) APIs + ******************************************************************************/ +#include "lwip/sockets.h" -#define MULTICAST_IPV4_ADDR "232.10.11.12" // used for internal telemetry messages between ESP32s +/****************************************************************************** + * MACROS + ******************************************************************************/ +#define MULTICAST_IPV4_ADDR \ + "232.10.11.12" // used for internal telemetry messages between ESP32s #define MULTICAST_TTL 1 #define MAX_UDP_CLIENTS 8 -#define TRANS_RD_BYTES_NUM 8 // amount of bytes read form serial port at once when transparent/MSP/LTM is selected -#define UDP_BUF_SIZE 2048 -#define UART_BUF_SIZE (1024) +#define TRANS_RD_BYTES_NUM \ + 8 // amount of bytes read form serial port at once when transparent/MSP/LTM + // is selected +#define UDP_BUF_SIZE 2048 +#define UART_BUF_SIZE (1024) // per client structure of connected devices in softAP mode -struct db_udp_client_t { - uint8_t mac[6]; // MAC address of connected client - struct sockaddr_in udp_client; // socket address (IP & PORT) of connected client +struct db_udp_client_t +{ + uint8_t mac[6]; // MAC address of connected client + struct sockaddr_in + udp_client; // socket address (IP & PORT) of connected client }; -typedef struct udp_conn_list_s { - struct db_udp_client_t db_udp_clients[MAX_UDP_CLIENTS]; // The array of list items - int size; // The number of items in the list - int udp_socket; // ID of UDP socket +typedef struct udp_conn_list_s +{ + struct db_udp_client_t + db_udp_clients[MAX_UDP_CLIENTS]; // The array of list items + int size; // The number of items in the list + int udp_socket; // ID of UDP socket } udp_conn_list_t; -// Used on the ESP AIR side to keep track and used to fill MAVlink RADIO STATUS msg -typedef struct { - int8_t air_rssi; // [dBm] RSSI of received data from AP. Updated when ESP32 is in station mode and connected to an access point - int8_t gnd_rssi; // [dBm] AP/GND told the rssi he is seeing when receiving our packets. Updated on every DroneBridge internal telemetry frame from GND - int8_t air_noise_floor; // [dBm] Noise floor on air side. Updated when ESP32 is in ESP-NOW mode and receives packet - Not supported by all ESP32 variants - int8_t gnd_noise_floor; // [dBm] AP/GND told the noise floor he is seeing when receiving our packets. Updated on every DroneBridge internal telemetry frame from GND - Not supported by all ESP32 variants - uint16_t gnd_rx_packets_lost; // Number of ESP-NOW packets the GND station lost coming from this AIR peer (based on seq. number) +// Used on the ESP AIR side to keep track and used to fill MAVlink RADIO STATUS +// msg +typedef struct +{ + int8_t air_rssi; // [dBm] RSSI of received data from AP. Updated when ESP32 + // is in station mode and connected to an access point + int8_t gnd_rssi; // [dBm] AP/GND told the rssi he is seeing when receiving + // our packets. Updated on every DroneBridge internal + // telemetry frame from GND + int8_t air_noise_floor; // [dBm] Noise floor on air side. Updated when ESP32 + // is in ESP-NOW mode and receives packet - Not + // supported by all ESP32 variants + int8_t gnd_noise_floor; // [dBm] AP/GND told the noise floor he is seeing + // when receiving our packets. Updated on every + // DroneBridge internal telemetry frame from GND - + // Not supported by all ESP32 variants + uint16_t + gnd_rx_packets_lost; // Number of ESP-NOW packets the GND station lost + // coming from this AIR peer (based on seq. number) } db_esp_signal_quality_t; +/****************************************************************************** + * Public Function Declaration + ******************************************************************************/ + +/****************************************************************************** + * @brief DroneBridge control module implementation for a ESP32 device. + * Bidirectional link between FC and ground. Can handle MSPv1, MSPv2, LTM and + * MAVLink. MSP & LTM is parsed and sent packet/frame by frame to ground + * MAVLink is passed through (fully transparent). Can be used with any + * protocol. + ******************************************************************************/ void db_start_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 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 +/****************************************************************************** + * Init/Create structure containing all UDP connection information + * @return Structure containing all UDP connection information + ******************************************************************************/ +udp_conn_list_t *db_udp_client_list_create(); + +/****************************************************************************** + * Destroy structure containing all UDP connection information + * @param n_udp_conn_list Structure containing all UDP connection information + ******************************************************************************/ +/** + * Witty-Wizard: Does this function needs to be a public function?? + */ +void db_udp_client_list_destroy(udp_conn_list_t *n_udp_conn_list); + +/****************************************************************************** + * Add a new UDP client to the list of known UDP clients. Checks if client is + * already known based on IP and port. Added client will receive UDP packets + * with serial info and will be able to send UDP packets to the serial + * interface of the ESP32. PORT, MAC & IP should be set inside + * new_db_udp_client. If MAC is not set then the device cannot be removed later + * on. + * + * @param n_udp_conn_list Structure containing all UDP connection information + * @param new_db_udp_client New client to add to the UDP list. PORT, MAC & IP + * must be set. If MAC is not set then the device cannot be automatically + * removed later on. To remove it, the user must clear the entire list. + * @param save_to_nvm Set to 1 (true) in case you want the UDP client to + * survive the reboot. Set to 0 (false) if client is temporary for this + * session. It will then be saved to NVM and added to the udp_conn_list_t on + * startup. Only one client can be saved to NVM. + * @return 1 if added - 0 if not + ******************************************************************************/ +bool db_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); + +/****************************************************************************** + * 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 db_send_to_all_clients(int tcp_clients[], + udp_conn_list_t *n_udp_conn_list, uint8_t data[], + uint16_t data_length); + +/****************************************************************************** + * Remove a client from the sending list. Client will no longer receive UDP + * packets. MAC address must be given. Usually called in AP-Mode when a station + * disconnects. In any other case we will not know since UDP is a + * connection-less protocol + * + * @param n_udp_conn_list Structure containing all UDP connection information + * @param new_db_udp_client The UDP client to remove based on its MAC address + * @return true if removed - false if nothing was removed + ******************************************************************************/ +bool +db_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_esp_now.c b/main/db_esp_now.c index 820abbc..bb13721 100644 --- a/main/db_esp_now.c +++ b/main/db_esp_now.c @@ -1,4 +1,4 @@ -/* +/****************************************************************************** * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2024 Wolfgang Christl @@ -15,669 +15,1012 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ + ******************************************************************************/ +/****************************************************************************** + * System & Standard Library Headers + ******************************************************************************/ +#include #include -#include + +/****************************************************************************** + * ESP-IDF Core APIs + ******************************************************************************/ #include +#include #include +#include + +/****************************************************************************** + * FreeRTOS + ******************************************************************************/ #include #include #include -#include -#include + +/****************************************************************************** + * mbedTLS + ******************************************************************************/ #include #include #include -#include "db_esp_now.h" -#include - -#include "globals.h" +/****************************************************************************** + * Project Headers + ******************************************************************************/ #include "main.h" +#include "globals.h" #include "espnow.h" +#include "db_esp_now.h" +#include +/****************************************************************************** + * Macros + ******************************************************************************/ #define TAG "DB_ESPNOW" -const uint8_t BROADCAST_MAC[ESP_NOW_ETH_ALEN] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; -static QueueHandle_t db_espnow_send_recv_callback_queue; // Queue that contains ESP-NOW callback results -QueueHandle_t db_espnow_send_queue; // Queue that contains data to be sent via ESP-NOW (filled by control task) -QueueHandle_t db_uart_write_queue; // Queue that contains data to be written to UART (filled by ESP-NOW task) -db_esp_now_clients_list_t *db_esp_now_clients_list; // Local list of known ESP-NOW peers with the last RSSI value and noise floor and sequence number +const uint8_t BROADCAST_MAC[ESP_NOW_ETH_ALEN] = { 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF }; + +/****************************************************************************** + * Public Variables + ******************************************************************************/ +static QueueHandle_t + db_espnow_send_recv_callback_queue; // Queue that contains ESP-NOW callback + // results + +static db_esp_now_clients_list_t + *db_esp_now_clients_list; // Local list of known ESP-NOW peers with the last + // RSSI value and noise floor and sequence number -// packet that is filled with payload and sent. Updates the sequ. number with every send. +// packet that is filled with payload and sent. Updates the sequ. number with +// every send. static db_esp_now_packet_t db_esp_now_packet_global = { - .db_esp_now_packet_header.seq_num = 0, - .db_esp_now_packet_header.packet_type = DB_ESP_NOW_PACKET_TYPE_DATA -}; // make static so it is gets instanced only once + .db_esp_now_packet_header.seq_num = 0, + .db_esp_now_packet_header.packet_type = DB_ESP_NOW_PACKET_TYPE_DATA +}; // make static so it is gets instanced only once + +static mbedtls_gcm_context aes; +static const uint8_t db_esp_now_packet_header_len = + sizeof(db_esp_now_packet_header_t); + +/****************************************************************************** + * Public Variables + ******************************************************************************/ +QueueHandle_t db_espnow_send_queue; /**< Queue that contains data to be sent + * via ESP-NOW (filled by control task) */ +QueueHandle_t db_uart_write_queue; /**< Queue that contains data to be written + * to UART (filled by ESP-NOW task) */ -mbedtls_gcm_context aes; -uint8_t const db_esp_now_packet_header_len = sizeof(db_esp_now_packet_header_t); +/****************************************************************************** + * Private Function Declarations + ******************************************************************************/ + +/****************************************************************************** + * Checks whether the given MAC address is a broadcast address + * + * @param addr Pointer to the MAC address to check + * @return true if the address is a broadcast address, false otherwise + ******************************************************************************/ +static inline bool +IS_BROADCAST_ADDR(const uint8_t *addr) +{ + return memcmp(addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN) == 0; +} /** * Generates a AES key from a password using pkcs5 - pbkdf2 and mbedTLS * - * @param password The password that gets transformed to PKCS5 key used for encryption + * @param password The password that gets transformed to PKCS5 key used for + * encryption * @param key Output buffer for the key * @param keylen Length of the aes key to be generated */ -void generate_pkcs5_key(const char* password, unsigned char* key, size_t keylen) { - mbedtls_md_context_t mdctx; - const mbedtls_md_info_t* md_info = mbedtls_md_info_from_type(MBEDTLS_MD_SHA256); // Use SHA-256 for key derivation - - // Initialize the context - mbedtls_md_init(&mdctx); - int ret = mbedtls_md_setup(&mdctx, md_info, 0); - switch (ret) { - case 0: - // success - break; - case MBEDTLS_ERR_MD_BAD_INPUT_DATA: - ESP_LOGW(TAG, "mbedtls_md_setup returned MBEDTLS_ERR_MD_BAD_INPUT_DATA"); - break; - case MBEDTLS_ERR_MD_ALLOC_FAILED: - ESP_LOGW(TAG, "mbedtls_md_setup returned MBEDTLS_ERR_MD_ALLOC_FAILED"); - break; - default: - ESP_LOGW(TAG, "mbedtls_md_setup returned unknown error: %i", ret); - break; - } - static unsigned char generic_salt[] = "GenSalt147894562"; // Generic on purpose. No need for salt in this application - ret = mbedtls_pkcs5_pbkdf2_hmac_ext(MBEDTLS_MD_SHA256, - (const unsigned char *)password, strlen(password), - generic_salt, strlen((char*) generic_salt), 10000, keylen, key); - switch (ret) { - case 0: - // success - break; - case MBEDTLS_ERR_PKCS5_BAD_INPUT_DATA: - ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_BAD_INPUT_DATA"); - break; - case MBEDTLS_ERR_PKCS5_INVALID_FORMAT: - ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_INVALID_FORMAT"); - break; - case MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE: - ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE"); - break; - case MBEDTLS_ERR_PKCS5_PASSWORD_MISMATCH: - ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_PASSWORD_MISMATCH"); - break; - default: - ESP_LOGW(TAG, "mbedtls_pkcs5_pbkdf2_hmac returned unknown error: %02x", ret); - break; - } - // Clean up - mbedtls_md_free(&mdctx); -} +static void generate_pkcs5_key(const char *password, unsigned char *key, + size_t keylen); /** - * Encrypts and authenticates a DroneBridge for ESP32 ESP-NOW packet with its payload using AES-GCM 256 - * Beware we are using the same key for both communication directions. - * Calls mbedtls_gcm_crypt_and_tag() + * Encrypts and authenticates a DroneBridge for ESP32 ESP-NOW packet with its + * payload using AES-GCM 256 Beware we are using the same key for both + * communication directions. Calls mbedtls_gcm_crypt_and_tag() * - * @param db_esp_now_packet Packet containing payload data and header info. AES IV & TAG will be filled - * @param encrypt_payload_len Length of the to be encrypted data (db_esp_now_packet_protected_data) - * @return 0 on success, -1 in case of unknown error of mbedtls_gcm_crypt_and_tag or return value of mbedtls_gcm_crypt_and_tag + * @param db_esp_now_packet Packet containing payload data and header info. AES + * IV & TAG will be filled + * @param encrypt_payload_len Length of the to be encrypted data + * (db_esp_now_packet_protected_data) + * @return 0 on success, -1 in case of unknown error of + * mbedtls_gcm_crypt_and_tag or return value of mbedtls_gcm_crypt_and_tag * payload is encrypted and part of the db_esp_now_packet */ -int db_encrypt_payload(db_esp_now_packet_t* db_esp_now_packet, uint8_t encrypt_payload_len) { - // Generate random IV - This is risky since (password+IV) shall never be reused! - // Counter would secure for a single session but super unsecure with multiple sessions where the password is not changed - esp_fill_random(db_esp_now_packet->db_esp_now_packet_header.aes_iv, DB_ESPNOW_AES_IV_LEN); - int ret = mbedtls_gcm_crypt_and_tag( - &aes, - MBEDTLS_GCM_ENCRYPT, - encrypt_payload_len, - db_esp_now_packet->db_esp_now_packet_header.aes_iv, DB_ESPNOW_AES_IV_LEN, - (uint8_t*) &db_esp_now_packet->db_esp_now_packet_header, db_esp_now_packet_header_len, - (uint8_t*) &db_esp_now_packet->db_esp_now_packet_protected_data, - (uint8_t*) &db_esp_now_packet->db_esp_now_packet_protected_data, - DB_ESPNOW_AES_TAG_LEN, db_esp_now_packet->tag - ); - - if (ret == 0) { - return 0; - } else { - if (ret == MBEDTLS_ERR_GCM_BAD_INPUT) { - ESP_LOGW(TAG, "mbedtls_gcm_crypt_and_tag returned MBEDTLS_ERR_GCM_BAD_INPUT"); - } else { - ESP_LOGW(TAG, "mbedtls_gcm_crypt_and_tag returned unknown error"); - } - return -1; - } -} +static int encrypt_payload(db_esp_now_packet_t *db_esp_now_packet, + uint8_t encrypt_payload_len); /** - * Decrypt the DroneBridge ESP-NOW packet payload and authenticate its content using AES-GCM 256 + * Decrypt the DroneBridge ESP-NOW packet payload and authenticate its content + * using AES-GCM 256 * - * @param db_esp_now_packet db_esp_now_packet_protected_data_t structure wth the data de decrypt inside + * @param db_esp_now_packet db_esp_now_packet_protected_data_t structure wth + * the data de decrypt inside * @param decrypt_out_buffer Pointer to the out buffer for the decrypted data - * @param len_encrypted_data length of the buffer (db_esp_now_packet_protected_data) - * @return returns result of mbedtls_gcm_auth_decrypt e.g. 0 on success and valid data + * @param len_encrypted_data length of the buffer + * (db_esp_now_packet_protected_data) + * @return returns result of mbedtls_gcm_auth_decrypt e.g. 0 on success and + * valid data */ -int db_decrypt_payload(db_esp_now_packet_t* db_esp_now_packet, uint8_t* decrypt_out_buffer, - uint8_t len_encrypted_data) { - int ret_decrypt = mbedtls_gcm_auth_decrypt( - &aes, - len_encrypted_data, - db_esp_now_packet->db_esp_now_packet_header.aes_iv, DB_ESPNOW_AES_IV_LEN, - (uint8_t*) &db_esp_now_packet->db_esp_now_packet_header, db_esp_now_packet_header_len, - db_esp_now_packet->tag, DB_ESPNOW_AES_TAG_LEN, - (uint8_t*) &db_esp_now_packet->db_esp_now_packet_protected_data, - decrypt_out_buffer - ); - switch (ret_decrypt) { - case 0: - // Successfully decrypted and authenticated - break; - case MBEDTLS_ERR_GCM_AUTH_FAILED: - ESP_LOGW(TAG, "MBEDTLS_ERR_GCM_AUTH_FAILED: Authenticated decryption failed."); break; - case MBEDTLS_ERR_GCM_BAD_INPUT: - ESP_LOGW(TAG, "MBEDTLS_ERR_GCM_BAD_INPUT: Bad input parameters to function."); break; - default: - ESP_LOGW(TAG, "Unknown mbedtls_gcm_auth_decrypt error %i", ret_decrypt); break; - } - return ret_decrypt; -} - +static int decrypt_payload(db_esp_now_packet_t *db_esp_now_packet, + uint8_t *decrypt_out_buffer, + uint8_t len_encrypted_data); /** * Encrypts, authenticates and sends packet via ESP-NOW * @param db_esp_now_packet Packet to send - * @param payload_length Value of payload_length_decrypted of db_esp_now_packet - saves us from creating a var inside the function - * @return false if no packet was sent, true if packet was sent (actual sending will be confirmed by the send-callback) + * @param payload_length Value of payload_length_decrypted of db_esp_now_packet + * - saves us from creating a var inside the function + * @return false if no packet was sent, true if packet was sent (actual sending + * will be confirmed by the send-callback) */ -bool db_espnow_encrypt_auth_send(db_esp_now_packet_t *db_esp_now_packet, const uint8_t payload_length) { - static int ret; - static esp_err_t err; - // after encryption the payload_length_decrypted will be unreadable -> keep a copy (payload_length) for sending the packet - // const uint8_t payload_length = db_esp_now_packet->db_esp_now_packet_protected_data.payload_length_decrypted; - ret = db_encrypt_payload(db_esp_now_packet, (*db_esp_now_packet).db_esp_now_packet_protected_data.payload_length_decrypted + 1); - if (ret == 0) { - err = esp_now_send(BROADCAST_MAC, (const uint8_t *) db_esp_now_packet, (db_esp_now_packet_header_len + DB_ESPNOW_AES_TAG_LEN + payload_length + 1)); - if (err != ESP_OK) { - ESP_LOGE(TAG, "Error (%s) sending ESP-NOW data - size %i !", esp_err_to_name(err), (db_esp_now_packet_header_len + DB_ESPNOW_AES_TAG_LEN + payload_length + 1)); - return false; - } else { - return true; - } - } else { - ESP_LOGE(TAG, "db_encrypt_payload returned error code: %i, not sending packet", ret); - return false; - } -} +static bool espnow_encrypt_auth_send(db_esp_now_packet_t *db_esp_now_packet, + const uint8_t payload_length); /** - * Tries to read one entry from the ESP-NOW send queue (mainly filled by the UART task) and sends the data via broadcast. - * Only call this function when the last ESP-NOW send callback has returned! Otherwise order of packets is not guaranteed + * Tries to read one entry from the ESP-NOW send queue (mainly filled by the + * UART task) and sends the data via broadcast. Only call this function when + * the last ESP-NOW send callback has returned! Otherwise order of packets is + * not guaranteed * - * @return false if no packet was sent, true if packet was sent (actual sending will be confirmed by the send-callback) + * @return false if no packet was sent, true if packet was sent (actual sending + * will be confirmed by the send-callback) */ -bool db_read_uart_queue_and_send() { - static db_espnow_queue_event_t evt; - // Receive data from Queue that was put there by other tasks to be sent via ESP-NOW - if (db_espnow_send_queue != NULL && xQueueReceive(db_espnow_send_queue, &evt, 0) == pdTRUE) { - db_esp_now_packet_global.db_esp_now_packet_header.packet_type = evt.packet_type; - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { - db_esp_now_packet_global.db_esp_now_packet_header.origin = DB_ESPNOW_ORIGIN_GND; - } else { - db_esp_now_packet_global.db_esp_now_packet_header.origin = DB_ESPNOW_ORIGIN_AIR; - } - // Create an DroneBridge for ESP32 ESP-NOW packet - db_esp_now_packet_global.db_esp_now_packet_protected_data.payload_length_decrypted = evt.data_len; - // ToDo: Potential to optimize: esp_now_send() does not need an instance of buffer after sending, however using evt.data as pointer resulted in errors - memcpy(db_esp_now_packet_global.db_esp_now_packet_protected_data.payload, evt.data, evt.data_len); - free(evt.data); - // Encrypt, authenticate and send packet - if(db_espnow_encrypt_auth_send(&db_esp_now_packet_global, evt.data_len)) { - // update sequence number for next packet - if (db_esp_now_packet_global.db_esp_now_packet_header.seq_num < UINT32_MAX) { - db_esp_now_packet_global.db_esp_now_packet_header.seq_num++; // packet is static so just increase number once we sent it - } else { - db_esp_now_packet_global.db_esp_now_packet_header.seq_num = 0; // catch overflow in case someone got crazy - } - return true; - } else { - return false; - } - } else { - if (db_espnow_send_queue == NULL) { - ESP_LOGE(TAG, "db_espnow_send_queue is NULL!"); - } else { - // nothing to do - Queue is empty - } - - } - return false; -} +static bool read_uart_queue_and_send(); /** - * Add a new ESP-NOW broadcast peer to the list of known ESP-NOW peers. Checks if peer is already known based on MAC. - * List is then used to keep track of the RSSI, packet loss and signal quality on GND side. + * Add a new ESP-NOW broadcast peer to the list of known ESP-NOW peers. Checks + * if peer is already known based on MAC. List is then used to keep track of + * the RSSI, packet loss and signal quality on GND side. * * @param esp_now_clients List of broadcast peers (ESP-NOW) with their RSSI * @param broadcast_peer_mac MAC address of a potentially new broadcast peer - * @return index of the peer inside the list or -1 in case of error e.g. if list is full or not initialized + * @return index of the peer inside the list or -1 in case of error e.g. if + * list is full or not initialized */ -int16_t update_peer_list(db_esp_now_clients_list_t *esp_now_clients, uint8_t broadcast_peer_mac[6]) { - if (esp_now_clients == NULL) { // Check if the list is NULL - return -1; // Do nothing - } - for (uint8_t i = 0; i < esp_now_clients->size; i++) { - if (memcmp(esp_now_clients->db_esp_now_bpeer_info[i].broadcast_peer_mac, broadcast_peer_mac, ESP_NOW_ETH_ALEN) == 0) { - // found the client - he is already part of the known clients list - return i; - } - } - if (esp_now_clients->size == DB_ESPNOW_MAX_BROADCAST_PEERS) { // Check if the list is full - return -1; // Do nothing - list is full already - } else { - // Add new peer to list - Copy the element data to the end of the array - db_esp_now_bpeer_info_t db_esp_now_bpeer_info = {.gnd_rssi = -127, .last_seq_num = 0, .gnd_rx_lost_packets = 0}; - memcpy(db_esp_now_bpeer_info.broadcast_peer_mac, broadcast_peer_mac, ESP_NOW_ETH_ALEN); - esp_now_clients->db_esp_now_bpeer_info[esp_now_clients->size] = db_esp_now_bpeer_info; - esp_now_clients->size++; // Increment the size of the list - return (esp_now_clients->size-1) ; - } -} +static int16_t update_peer_list(db_esp_now_clients_list_t *esp_now_clients, + uint8_t broadcast_peer_mac[6]); /** - * Steps to process received ESPNOW data. ESP firmware ensures that only correct packets are forwarded to us + * Steps to process received ESPNOW data. ESP firmware ensures that only + * correct packets are forwarded to us * 1. Decrypt & authenticate - * 2. Check if we know the client based on MAC - update rssi in case we are GND - AIR side only expects to only ever have on GND peer anyways - * 3. Check sequence number and if we are GND station then update lost packet count based on seq. numbers - * 4. Write payload to uart-queue so it can be processed by the control_espnow task + * 2. Check if we know the client based on MAC - update rssi in case we are GND + * - AIR side only expects to only ever have on GND peer anyways + * 3. Check sequence number and if we are GND station then update lost packet + * count based on seq. numbers + * 4. Write payload to uart-queue so it can be processed by the control_espnow + * task * * @param data Received raw data via ESP-NOW * @param data_len Length of received data * @param src_addr Source MAC address of the data * @param rssi RSSI in dBm of this packet */ -void db_espnow_process_rcv_data(uint8_t *data, uint16_t data_len, uint8_t *src_addr, int8_t rssi) { - db_esp_now_packet_t *db_esp_now_packet = (db_esp_now_packet_t *) data; - uint8_t len_payload = data_len - DB_ESPNOW_AES_TAG_LEN - db_esp_now_packet_header_len; - uint8_t db_decrypted_data[len_payload]; - - /* Decrypt and authenticate packet - only then process its contents */ - if (db_decrypt_payload(db_esp_now_packet, db_decrypted_data, len_payload) == 0) { - /* Check if we know that peer already */ - int16_t peer_index = update_peer_list(db_esp_now_clients_list, src_addr); - if (peer_index != -1) { - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { - // update the list with the rssi only if we are GND - db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].gnd_rssi = rssi; - } else { - // AIR unit keeps track of RSSI using db_esp_signal_quality variable - // no need to do here anything - // TODO: Clean up RSSI variables - a bit confusing that AIR and GND use different structures - } - // check packet sequence number - if (db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].last_seq_num < db_esp_now_packet->db_esp_now_packet_header.seq_num) { - // Count the lost packets per peer since the last received packet - db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].gnd_rx_lost_packets += - (db_esp_now_packet->db_esp_now_packet_header.seq_num - - db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].last_seq_num - 1); - } else { - ESP_LOGW(TAG, "Sequence number lower than expected. Sender did reset or packet may be part of a replay attack."); - // accept packet anyway for now to make for a more robust link - db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].gnd_rx_lost_packets = 0; - } - - db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].last_seq_num = db_esp_now_packet->db_esp_now_packet_header.seq_num; - } else { - /* Do nothing since we only count packet loss and rssi on the GND side not on the air side - * and only if there is still a free spot in the list */ - } - - /* Process packet depending on packet type */ - if (db_esp_now_packet->db_esp_now_packet_header.packet_type == DB_ESP_NOW_PACKET_TYPE_DATA) { - // Pass data to UART queue - db_espnow_queue_event_t db_uart_evt; - db_uart_evt.data_len = db_decrypted_data[0]; // should be equal to len_payload-1 if everything worked out - db_uart_evt.data = malloc(db_uart_evt.data_len); - // For some reason it seems we cannot directly decrypt to db_espnow_uart_event_t -> Queues get set to NULL - memcpy(db_uart_evt.data, &db_decrypted_data[1], db_uart_evt.data_len); - if (xQueueSend(db_uart_write_queue, &db_uart_evt, ESPNOW_MAXDELAY) != pdTRUE) { - ESP_LOGW(TAG, "Send to db_uart_write_queue failed"); - free(db_uart_evt.data); - } else { - // all good - } - } else if (db_esp_now_packet->db_esp_now_packet_header.packet_type == DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY) { - /* This is only called on the AIR side since GND sends telemetry to AIR only */ - db_esp_now_clients_list_t *clients = (db_esp_now_clients_list_t*) &db_decrypted_data[1]; - ESP_LOGD(TAG, "Received internal telemetry frame containing %i entries", clients->size); - for (int i = 0; i < clients->size; i++) { - if (memcmp(LOCAL_MAC_ADDRESS, clients->db_esp_now_bpeer_info[i].broadcast_peer_mac, ESP_NOW_ETH_ALEN) == 0) { - // found us (this local ESP32 AIR unit) -> update internal telemetry buffer, - // so it gets sent with next Mavlink RADIO STATUS in case MAVLink radio status is enabled - db_esp_signal_quality.gnd_noise_floor = clients->gnd_noise_floor; - db_esp_signal_quality.gnd_rssi = clients->db_esp_now_bpeer_info[i].gnd_rssi; - db_esp_signal_quality.gnd_rx_packets_lost = clients->db_esp_now_bpeer_info[i].gnd_rx_lost_packets; - break; - } else { - // keep on looking for our MAC - } - } - } else { - ESP_LOGW(TAG, "Received unknown DroneBridge for ESP32 ESP-NOW packet type"); - } - } else { - ESP_LOGE(TAG, "Failed to Decrypt & Authenticate. Ignoring"); - return; - } -} +static void espnow_process_rcv_data(uint8_t *data, uint16_t data_len, + uint8_t *src_addr, int8_t rssi); /** * ESP-NOW sending callback function is called in WiFi task. - * Do not do lengthy operations from this task. Instead, post necessary data to a queue and handle it from the control - * module task. - * Note that too short interval between sending two ESP-NOW data may lead to disorder of sending callback function. - * So, it is recommended that sending the next ESP-NOW data after the sending callback function of the previous sending - * has returned. + * Do not do lengthy operations from this task. Instead, post necessary data to + * a queue and handle it from the control module task. Note that too short + * interval between sending two ESP-NOW data may lead to disorder of sending + * callback function. So, it is recommended that sending the next ESP-NOW data + * after the sending callback function of the previous sending has returned. */ -static void db_espnow_send_callback(const uint8_t *mac_addr, esp_now_send_status_t status) { - if (mac_addr == NULL) { - ESP_LOGE(TAG, "Send callback arg error"); - return; - } - - if (status == ESP_NOW_SEND_FAIL) { - ESP_LOGE(TAG, "Failed to send ESP-NOW packet - MAC: %02xh:%02xh:%02xh:%02xh:%02xh:%02xh", - mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); - } - - db_espnow_event_t evt; - db_espnow_event_send_cb_t *send_cb = &evt.info.send_cb; - - evt.id = DB_ESPNOW_SEND_CB; - memcpy(send_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN); - send_cb->status = status; - if (xQueueSend(db_espnow_send_recv_callback_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) { - ESP_LOGW(TAG, "Send to send queue fail"); - } -} +static void espnow_send_callback(const uint8_t *mac_addr, + esp_now_send_status_t status); /** - * ESP-NOW receiving callback function is called in WiFi task. This is the entry point when new data is incoming. - * Do not do lengthy operations from this task. Instead, post necessary data to a queue and handle it from the control - * module task + * ESP-NOW receiving callback function is called in WiFi task. This is the + * entry point when new data is incoming. Do not do lengthy operations from + * this task. Instead, post necessary data to a queue and handle it from the + * control module task */ -static void db_espnow_receive_callback(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len) { - db_espnow_event_t evt; - db_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb; - uint8_t *src_addr = recv_info->src_addr; - uint8_t *des_addr = recv_info->des_addr; - - if (src_addr == NULL || data == NULL || len <= 0) { - ESP_LOGE(TAG, "Receive callback arg error"); - return; - } - - if (!IS_BROADCAST_ADDR(des_addr)) { - ESP_LOGD(TAG, "Receive uni-cast ESP-NOW data. Ignoring"); - return; - } +static void espnow_receive_callback(const esp_now_recv_info_t *recv_info, + const uint8_t *data, int len); - if (data[0] == DB_ESPNOW_ORIGIN_GND && DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { - // Ignoring packet - Came from another GND station - return; - } else { - // we are GND and packet is for us from AIR -#if defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C2) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C6) - db_esp_now_clients_list->gnd_noise_floor = (int8_t ) recv_info->rx_ctrl->noise_floor; - // rest RSSI will be processed in process_espnow_data() -#endif - } +/** + * Init/Create structure containing all ESP-NOW broadcast connection quality + * information List is used to keep track of the RSSI and signal quality on GND + * side. + * @return Structure containing all ESP-NOW broadcast connection quality + * information + */ +static db_esp_now_clients_list_t *espnow_broadcast_peers_list_create(); - if (data[0] == DB_ESPNOW_ORIGIN_AIR && DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_AIR) { - // Ignoring packet - Came from another AIR station - return; - } else { - // we are AIR a packet is for us from GND - we only expect one GND station to be talking to us - db_esp_signal_quality.air_rssi = recv_info->rx_ctrl->rssi; -#if defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C2) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C6) - db_esp_signal_quality.air_noise_floor = recv_info->rx_ctrl->noise_floor; -#endif - } +/** + * Destroy structure containing all ESP-NOW broadcast connection quality + * information + * @param db_esp_now_clients Structure containing all ESP-NOW broadcast + * connection quality information + */ +static void espnow_broadcast_peers_list_destroy( + db_esp_now_clients_list_t *esp_now_clients); - evt.id = DB_ESPNOW_RECV_CB; - memcpy(recv_cb->mac_addr, src_addr, ESP_NOW_ETH_ALEN); - recv_cb->data = malloc(len); - if (recv_cb->data == NULL) { - ESP_LOGE(TAG, "Malloc receive data fail"); - return; - } - memcpy(recv_cb->data, data, len); - recv_cb->data_len = len; - recv_cb->rssi = recv_info->rx_ctrl->rssi; - if (db_espnow_send_recv_callback_queue != NULL && xQueueSend(db_espnow_send_recv_callback_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) { - ESP_LOGW(TAG, "Send to receive queue fail"); - free(recv_cb->data); - } -} +/** + * Init mbedtls aes gcm mode and set encryption key based on the WiFi password + * specified by the user + * @param aes_key buffer for saving the generated aes key of len + * AES_256_KEY_BYTES + * @return result of mbedtls_gcm_setkey + */ +static int init_gcm_encryption_module(uint8_t *aes_key); +/** + * @brief Deinitialise all espnow + */ +static void deinit_espnow_all(); /** - * Init/Create structure containing all ESP-NOW broadcast connection quality information - * List is used to keep track of the RSSI and signal quality on GND side. - * @return Structure containing all ESP-NOW broadcast connection quality information + * Init all relevant structures and Queues for ESP-NOW communication + * @return ESP_FAIL on failure or ESP_OK on success */ -db_esp_now_clients_list_t *db_espnow_broadcast_peers_list_create() { - db_esp_now_clients_list_t *client_list = malloc(sizeof(db_esp_now_clients_list_t)); // Allocate memory for the list - if (client_list == NULL) { // Check if the allocation failed - return NULL; // Return NULL to indicate an error - } - client_list->size = 0; // Initialize the size to 0 - return client_list; // Return the pointer to the list -} +static esp_err_t db_espnow_init(); /** - * Destroy structure containing all ESP-NOW broadcast connection quality information - * @param db_esp_now_clients Structure containing all ESP-NOW broadcast connection quality information + * Send ESP-NOW DroneBridge for ESP32 internal telemetry packet containing RSSI + * and noise floor info from GND to AIR. Used by AIR side to create a + * RADIO_STATUS MAVLink message sent to GCS. + * @return true when packet was scheduled for sending, false if it will not be + * sent */ -void db_espnow_broadcast_peers_list_destroy(db_esp_now_clients_list_t *esp_now_clients) { - if (esp_now_clients == NULL) { // Check if the list is NULL - return; // Do nothing - } - free(esp_now_clients); // Free the list -} +static bool espnow_schedule_internal_telemetry_packet(); /** - * Init mbedtls aes gcm mode and set encryption key based on the WiFi password specified by the user - * @param aes_key buffer for saving the generated aes key of len AES_256_KEY_BYTES - * @return result of mbedtls_gcm_setkey + * Task that handles all ESP-NOW related data processing. Reads ESP-NOW + * Callback-Queue, Reads ESP-NOW send queue and writes to UART-WRITE Queue. */ -int init_gcm_encryption_module(uint8_t *aes_key) { - mbedtls_gcm_init(&aes); - generate_pkcs5_key(DB_PARAM_PASS, aes_key, AES_256_KEY_BYTES); - ESP_LOGI(TAG, "Derived AES Key:"); - for (int i = 0; i < AES_256_KEY_BYTES; ++i) { - printf("%02x", aes_key[i]); +_Noreturn static void process_espnow_data(); + +/****************************************************************************** + * Private Function Definitions + ******************************************************************************/ + +static void +generate_pkcs5_key(const char *password, unsigned char *key, size_t keylen) +{ + mbedtls_md_context_t mdctx; + const mbedtls_md_info_t *md_info = mbedtls_md_info_from_type( + MBEDTLS_MD_SHA256); // Use SHA-256 for key derivation + + // Initialize the context + mbedtls_md_init(&mdctx); + int ret = mbedtls_md_setup(&mdctx, md_info, 0); + switch(ret) { + case 0: + // success + break; + case MBEDTLS_ERR_MD_BAD_INPUT_DATA: + ESP_LOGW(TAG, "mbedtls_md_setup returned MBEDTLS_ERR_MD_BAD_INPUT_DATA"); + break; + case MBEDTLS_ERR_MD_ALLOC_FAILED: + ESP_LOGW(TAG, "mbedtls_md_setup returned MBEDTLS_ERR_MD_ALLOC_FAILED"); + break; + default: + ESP_LOGW(TAG, "mbedtls_md_setup returned unknown error: %i", ret); + break; + } + static unsigned char generic_salt[] = + "GenSalt147894562"; // Generic on purpose. No need for salt in this + // application + ret = mbedtls_pkcs5_pbkdf2_hmac_ext(MBEDTLS_MD_SHA256, + (const unsigned char *)password, + strlen(password), + generic_salt, + strlen((char *)generic_salt), + 10000, + keylen, + key); + switch(ret) { + case 0: + // success + break; + case MBEDTLS_ERR_PKCS5_BAD_INPUT_DATA: + ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_BAD_INPUT_DATA"); + break; + case MBEDTLS_ERR_PKCS5_INVALID_FORMAT: + ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_INVALID_FORMAT"); + break; + case MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE: + ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE"); + break; + case MBEDTLS_ERR_PKCS5_PASSWORD_MISMATCH: + ESP_LOGW(TAG, "MBEDTLS_ERR_PKCS5_PASSWORD_MISMATCH"); + break; + default: + ESP_LOGW( + TAG, "mbedtls_pkcs5_pbkdf2_hmac returned unknown error: %02x", ret); + break; + } + // Clean up + mbedtls_md_free(&mdctx); +} + +static int +encrypt_payload(db_esp_now_packet_t *db_esp_now_packet, + uint8_t encrypt_payload_len) +{ + // Generate random IV - This is risky since (password+IV) shall never be + // reused! Counter would secure for a single session but super unsecure with + // multiple sessions where the password is not changed + esp_fill_random(db_esp_now_packet->db_esp_now_packet_header.aes_iv, + DB_ESPNOW_AES_IV_LEN); + int ret = mbedtls_gcm_crypt_and_tag( + &aes, + MBEDTLS_GCM_ENCRYPT, + encrypt_payload_len, + db_esp_now_packet->db_esp_now_packet_header.aes_iv, + DB_ESPNOW_AES_IV_LEN, + (uint8_t *)&db_esp_now_packet->db_esp_now_packet_header, + db_esp_now_packet_header_len, + (uint8_t *)&db_esp_now_packet->db_esp_now_packet_protected_data, + (uint8_t *)&db_esp_now_packet->db_esp_now_packet_protected_data, + DB_ESPNOW_AES_TAG_LEN, + db_esp_now_packet->tag); + + if(ret == 0) { + return 0; + } + else { + if(ret == MBEDTLS_ERR_GCM_BAD_INPUT) { + ESP_LOGW(TAG, + "mbedtls_gcm_crypt_and_tag returned MBEDTLS_ERR_GCM_BAD_INPUT"); + } + else { + ESP_LOGW(TAG, "mbedtls_gcm_crypt_and_tag returned unknown error"); } - printf("\n"); - int ret = mbedtls_gcm_setkey(&aes, MBEDTLS_CIPHER_ID_AES, aes_key, DB_ESPNOW_AES_KEY_LEN); - return ret; + return -1; + } } -void deinit_espnow_all(){ - ESP_LOGW(TAG, "De init ESPNOW incl. Queues & AES"); - mbedtls_gcm_free(&aes); - vSemaphoreDelete(db_espnow_send_recv_callback_queue); - vSemaphoreDelete(db_espnow_send_queue); // ToDo: Check if that is a good idea since control task might be using it - vSemaphoreDelete(db_uart_write_queue); // ToDo: Check if that is a good idea since control task might be using it - db_espnow_broadcast_peers_list_destroy(db_esp_now_clients_list); - esp_now_deinit(); +static int +decrypt_payload(db_esp_now_packet_t *db_esp_now_packet, + uint8_t *decrypt_out_buffer, uint8_t len_encrypted_data) +{ + int ret_decrypt = mbedtls_gcm_auth_decrypt( + &aes, + len_encrypted_data, + db_esp_now_packet->db_esp_now_packet_header.aes_iv, + DB_ESPNOW_AES_IV_LEN, + (uint8_t *)&db_esp_now_packet->db_esp_now_packet_header, + db_esp_now_packet_header_len, + db_esp_now_packet->tag, + DB_ESPNOW_AES_TAG_LEN, + (uint8_t *)&db_esp_now_packet->db_esp_now_packet_protected_data, + decrypt_out_buffer); + switch(ret_decrypt) { + case 0: + // Successfully decrypted and authenticated + break; + case MBEDTLS_ERR_GCM_AUTH_FAILED: + ESP_LOGW(TAG, + "MBEDTLS_ERR_GCM_AUTH_FAILED: Authenticated decryption failed."); + break; + case MBEDTLS_ERR_GCM_BAD_INPUT: + ESP_LOGW(TAG, + "MBEDTLS_ERR_GCM_BAD_INPUT: Bad input parameters to function."); + break; + default: + ESP_LOGW(TAG, "Unknown mbedtls_gcm_auth_decrypt error %i", ret_decrypt); + break; + } + return ret_decrypt; } -/** - * Init all relevant structures and Queues for ESP-NOW communication - * @return ESP_FAIL on failure or ESP_OK on success - */ -esp_err_t db_espnow_init() { - ESP_LOGI(TAG, "Initializing ESP-NOW parameters"); - db_esp_now_clients_list = db_espnow_broadcast_peers_list_create(); - /* Init Queue for ESP-NOW internal callbacks when packet is finally sent or received */ - db_espnow_send_recv_callback_queue = xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_event_t)); - if (db_espnow_send_recv_callback_queue == NULL) { - ESP_LOGE(TAG, "Create db_espnow_send_recv_callback_queue mutex fail"); - return ESP_FAIL; +static bool +espnow_encrypt_auth_send(db_esp_now_packet_t *db_esp_now_packet, + const uint8_t payload_length) +{ + static int ret; + static esp_err_t err; + // after encryption the payload_length_decrypted will be unreadable -> keep a + // copy (payload_length) for sending the packet const uint8_t payload_length + // = + // db_esp_now_packet->db_esp_now_packet_protected_data.payload_length_decrypted; + ret = encrypt_payload( + db_esp_now_packet, + (*db_esp_now_packet) + .db_esp_now_packet_protected_data.payload_length_decrypted + + 1); + if(ret == 0) { + err = esp_now_send(BROADCAST_MAC, + (const uint8_t *)db_esp_now_packet, + (db_esp_now_packet_header_len + DB_ESPNOW_AES_TAG_LEN + + payload_length + 1)); + if(err != ESP_OK) { + ESP_LOGE(TAG, + "Error (%s) sending ESP-NOW data - size %i !", + esp_err_to_name(err), + (db_esp_now_packet_header_len + DB_ESPNOW_AES_TAG_LEN + + payload_length + 1)); + return false; + } + else { + return true; } + } + else { + ESP_LOGE( + TAG, "encrypt_payload returned error code: %i, not sending packet", ret); + return false; + } +} - /* Init Queues for communication with control task */ - db_espnow_send_queue = xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_queue_event_t)); - db_uart_write_queue = xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_queue_event_t)); - if (db_espnow_send_queue == NULL) { - ESP_LOGE(TAG, "Create db_espnow_send_queue mutex fail"); - return ESP_FAIL; +static bool +read_uart_queue_and_send() +{ + static db_espnow_queue_event_t evt; + // Receive data from Queue that was put there by other tasks to be sent via + // ESP-NOW + if(db_espnow_send_queue != NULL && + xQueueReceive(db_espnow_send_queue, &evt, 0) == pdTRUE) { + db_esp_now_packet_global.db_esp_now_packet_header.packet_type = + evt.packet_type; + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { + db_esp_now_packet_global.db_esp_now_packet_header.origin = + DB_ESPNOW_ORIGIN_GND; } - if (db_uart_write_queue == NULL) { - ESP_LOGE(TAG, "Create db_uart_write_queue mutex fail"); - return ESP_FAIL; + else { + db_esp_now_packet_global.db_esp_now_packet_header.origin = + DB_ESPNOW_ORIGIN_AIR; } - - /* Initialize ESP-NOW and register sending and receiving callback function. */ - ESP_ERROR_CHECK(esp_now_init()); - ESP_ERROR_CHECK(esp_now_register_send_cb(db_espnow_send_callback)); - ESP_ERROR_CHECK(esp_now_register_recv_cb(db_espnow_receive_callback)); - - /* Add broadcast peer information to peer list. */ - esp_now_peer_info_t peer; - memset(&peer, 0, sizeof(esp_now_peer_info_t)); - memcpy(peer.peer_addr, BROADCAST_MAC, 6); - if (!esp_now_is_peer_exist(BROADCAST_MAC)) ESP_ERROR_CHECK(esp_now_add_peer(&peer)); - - /* Limit payload size to the max we can do */ - if (DB_PARAM_SERIAL_PACK_SIZE > DB_ESPNOW_PAYLOAD_MAXSIZE || DB_PARAM_SERIAL_PACK_SIZE < 1) { - DB_PARAM_SERIAL_PACK_SIZE = DB_ESPNOW_PAYLOAD_MAXSIZE; - } else { - // all good + // Create an DroneBridge for ESP32 ESP-NOW packet + db_esp_now_packet_global.db_esp_now_packet_protected_data + .payload_length_decrypted = evt.data_len; + // ToDo: Potential to optimize: esp_now_send() does not need an instance of + // buffer after sending, however using evt.data as pointer resulted in + // errors + memcpy(db_esp_now_packet_global.db_esp_now_packet_protected_data.payload, + evt.data, + evt.data_len); + free(evt.data); + // Encrypt, authenticate and send packet + if(espnow_encrypt_auth_send(&db_esp_now_packet_global, evt.data_len)) { + // update sequence number for next packet + if(db_esp_now_packet_global.db_esp_now_packet_header.seq_num < + UINT32_MAX) { + db_esp_now_packet_global.db_esp_now_packet_header + .seq_num++; // packet is static so just increase number once we sent + // it + } + else { + db_esp_now_packet_global.db_esp_now_packet_header.seq_num = + 0; // catch overflow in case someone got crazy + } + return true; } - - /* Init AES GCM encryption module */ - uint8_t aes_key[AES_256_KEY_BYTES]; - int ret = init_gcm_encryption_module(aes_key); - if (ret != 0) { - ESP_LOGE(TAG, "mbedtls_gcm_setkey returned an error: %i", ret); - deinit_espnow_all(); - return ESP_FAIL; + else { + return false; } - ESP_ERROR_CHECK(esp_now_set_pmk(aes_key)); // only first 16 bytes will be used + } + else { + if(db_espnow_send_queue == NULL) { + ESP_LOGE(TAG, "db_espnow_send_queue is NULL!"); + } + else { + // nothing to do - Queue is empty + } + } + return false; +} - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { - ESP_LOGI(TAG, "ESP-NOW for DroneBridge init done - acting as ESP-NOW GND station"); - } else { - ESP_LOGI(TAG, "ESP-NOW for DroneBridge init done - acting as ESP-NOW AIR station"); +static int16_t +update_peer_list(db_esp_now_clients_list_t *esp_now_clients, + uint8_t broadcast_peer_mac[6]) +{ + if(esp_now_clients == NULL) { // Check if the list is NULL + return -1; // Do nothing + } + for(uint8_t i = 0; i < esp_now_clients->size; i++) { + if(memcmp(esp_now_clients->db_esp_now_bpeer_info[i].broadcast_peer_mac, + broadcast_peer_mac, + ESP_NOW_ETH_ALEN) == 0) { + // found the client - he is already part of the known clients list + return i; } - return ESP_OK; + } + if(esp_now_clients->size == + DB_ESPNOW_MAX_BROADCAST_PEERS) { // Check if the list is full + return -1; // Do nothing - list is full already + } + else { + // Add new peer to list - Copy the element data to the end of the array + db_esp_now_bpeer_info_t db_esp_now_bpeer_info = { + .gnd_rssi = -127, .last_seq_num = 0, .gnd_rx_lost_packets = 0 + }; + memcpy(db_esp_now_bpeer_info.broadcast_peer_mac, + broadcast_peer_mac, + ESP_NOW_ETH_ALEN); + esp_now_clients->db_esp_now_bpeer_info[esp_now_clients->size] = + db_esp_now_bpeer_info; + esp_now_clients->size++; // Increment the size of the list + return (esp_now_clients->size - 1); + } } -/** - * Send ESP-NOW DroneBridge for ESP32 internal telemetry packet containing RSSI and noise floor info from GND to AIR. - * Used by AIR side to create a RADIO_STATUS MAVLink message sent to GCS. - * @return true when packet was scheduled for sending, false if it will not be sent - */ -bool db_espnow_schedule_internal_telemetry_packet() { - if (DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND) { - // Only GND sends internal telemetry. This function was called wrongly - return false; - } else { - // continue +static void +espnow_process_rcv_data(uint8_t *data, uint16_t data_len, uint8_t *src_addr, + int8_t rssi) +{ + db_esp_now_packet_t *db_esp_now_packet = (db_esp_now_packet_t *)data; + uint8_t len_payload = + data_len - DB_ESPNOW_AES_TAG_LEN - db_esp_now_packet_header_len; + uint8_t db_decrypted_data[len_payload]; + + /* Decrypt and authenticate packet - only then process its contents */ + if(decrypt_payload(db_esp_now_packet, db_decrypted_data, len_payload) == 0) { + /* Check if we know that peer already */ + int16_t peer_index = update_peer_list(db_esp_now_clients_list, src_addr); + if(peer_index != -1) { + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { + // update the list with the rssi only if we are GND + db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].gnd_rssi = + rssi; + } + else { + // AIR unit keeps track of RSSI using db_esp_signal_quality variable + // no need to do here anything + // TODO: Clean up RSSI variables - a bit confusing that AIR and GND use + // different structures + } + // check packet sequence number + if(db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index] + .last_seq_num < + db_esp_now_packet->db_esp_now_packet_header.seq_num) { + // Count the lost packets per peer since the last received packet + db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index] + .gnd_rx_lost_packets += + (db_esp_now_packet->db_esp_now_packet_header.seq_num - + db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index] + .last_seq_num - + 1); + } + else { + ESP_LOGW(TAG, + "Sequence number lower than expected. Sender did reset or " + "packet may be part of a replay attack."); + // accept packet anyway for now to make for a more robust link + db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index] + .gnd_rx_lost_packets = 0; + } + + db_esp_now_clients_list->db_esp_now_bpeer_info[peer_index].last_seq_num = + db_esp_now_packet->db_esp_now_packet_header.seq_num; } - // ToDo: Split list if longer than max payload size - if (sizeof(db_esp_now_clients_list_t) > DB_ESPNOW_PAYLOAD_MAXSIZE) { - ESP_LOGE(TAG, "Size of db_esp_now_clients_list_t is > %i", DB_ESPNOW_PAYLOAD_MAXSIZE); - return false; - } else { - /* continue */ + else { + /* Do nothing since we only count packet loss and rssi on the GND side + * not on the air side and only if there is still a free spot in the list + */ } - ESP_LOGD(TAG, "Scheduling int. telem. packet"); - uint8_t payload_size = sizeof(db_esp_now_clients_list_t); - - db_espnow_queue_event_t evt; - // Set payload of DroneBridge for ESP32 ESP-NOW packet - evt.data = malloc(payload_size); - memcpy(evt.data, db_esp_now_clients_list, payload_size); - evt.data_len = payload_size; - evt.packet_type = DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY; - if (xQueueSend(db_espnow_send_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) { - ESP_LOGW(TAG, "Send to db_espnow_send_queue queue fail"); - free(evt.data); - return false; - } else { + + /* Process packet depending on packet type */ + if(db_esp_now_packet->db_esp_now_packet_header.packet_type == + DB_ESP_NOW_PACKET_TYPE_DATA) { + // Pass data to UART queue + db_espnow_queue_event_t db_uart_evt; + db_uart_evt.data_len = + db_decrypted_data[0]; // should be equal to len_payload-1 if everything + // worked out + db_uart_evt.data = malloc(db_uart_evt.data_len); + // For some reason it seems we cannot directly decrypt to + // db_espnow_uart_event_t -> Queues get set to NULL + memcpy(db_uart_evt.data, &db_decrypted_data[1], db_uart_evt.data_len); + if(xQueueSend(db_uart_write_queue, &db_uart_evt, ESPNOW_MAXDELAY) != + pdTRUE) { + ESP_LOGW(TAG, "Send to db_uart_write_queue failed"); + free(db_uart_evt.data); + } + else { // all good - return true; + } + } + else if(db_esp_now_packet->db_esp_now_packet_header.packet_type == + DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY) { + /* This is only called on the AIR side since GND sends telemetry to AIR + * only */ + db_esp_now_clients_list_t *clients = + (db_esp_now_clients_list_t *)&db_decrypted_data[1]; + ESP_LOGD(TAG, + "Received internal telemetry frame containing %i entries", + clients->size); + for(int i = 0; i < clients->size; i++) { + if(memcmp(LOCAL_MAC_ADDRESS, + clients->db_esp_now_bpeer_info[i].broadcast_peer_mac, + ESP_NOW_ETH_ALEN) == 0) { + // found us (this local ESP32 AIR unit) -> update internal telemetry + // buffer, so it gets sent with next Mavlink RADIO STATUS in case + // MAVLink radio status is enabled + db_esp_signal_quality.gnd_noise_floor = clients->gnd_noise_floor; + db_esp_signal_quality.gnd_rssi = + clients->db_esp_now_bpeer_info[i].gnd_rssi; + db_esp_signal_quality.gnd_rx_packets_lost = + clients->db_esp_now_bpeer_info[i].gnd_rx_lost_packets; + break; + } + else { + // keep on looking for our MAC + } + } } + else { + ESP_LOGW(TAG, + "Received unknown DroneBridge for ESP32 ESP-NOW packet type"); + } + } + else { + ESP_LOGE(TAG, "Failed to Decrypt & Authenticate. Ignoring"); + return; + } } -/** - * Task that handles all ESP-NOW related data processing. Reads ESP-NOW Callback-Queue, Reads ESP-NOW send queue and - * writes to UART-WRITE Queue. - */ -_Noreturn void process_espnow_data() { - esp_err_t err = db_espnow_init(); - if (err == ESP_OK) { - // all good. continue - } else { - ESP_LOGE(TAG, "Failed to init espnow (db_espnow_init()) aborting start of db_espnow task"); - vTaskDelete(NULL); - } - db_espnow_event_t evt; - // indicator that the last ESP-NOW send callback returned -> we can send the next packet - bool ready_to_send = true; // initially true - bool send_internal_telemetry_frame = false; // flag that indicates a new internal telemetry frame shall be sent - int delay_timer_cnt = 0; - while(1) { - if (db_espnow_send_recv_callback_queue == NULL) ESP_LOGE(TAG, "db_espnow_send_recv_callback_queue is NULL!"); - if(db_espnow_send_recv_callback_queue != NULL && xQueueReceive(db_espnow_send_recv_callback_queue, &evt, 0) == pdTRUE) { - switch (evt.id) { - case DB_ESPNOW_SEND_CB: { - db_espnow_event_send_cb_t *send_cb = &evt.info.send_cb; - // indicate that we can send next packet - the last sending callback has returned, so we keep the order - // of packets. ESP-NOW by default does not guarantee the order when many packets are sent in quick succession - ready_to_send = true; - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND && send_internal_telemetry_frame) { - send_internal_telemetry_frame = !db_espnow_schedule_internal_telemetry_packet(); - } else { - // try to immediately send the next packet if available and set ready_to_send accordingly - ready_to_send = !db_read_uart_queue_and_send(); - } - break; - } - case DB_ESPNOW_RECV_CB: { - db_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb; - db_espnow_process_rcv_data(recv_cb->data, recv_cb->data_len, recv_cb->mac_addr, recv_cb->rssi); - free(recv_cb->data); - break; - } - default: - ESP_LOGE(TAG, "Callback type error: %d", evt.id); - break; - } +static void +espnow_send_callback(const uint8_t *mac_addr, esp_now_send_status_t status) +{ + if(mac_addr == NULL) { + ESP_LOGE(TAG, "Send callback arg error"); + return; + } + + if(status == ESP_NOW_SEND_FAIL) { + ESP_LOGE(TAG, + "Failed to send ESP-NOW packet - MAC: " + "%02xh:%02xh:%02xh:%02xh:%02xh:%02xh", + mac_addr[0], + mac_addr[1], + mac_addr[2], + mac_addr[3], + mac_addr[4], + mac_addr[5]); + } + + db_espnow_event_t evt; + db_espnow_event_send_cb_t *send_cb = &evt.info.send_cb; + + evt.id = DB_ESPNOW_SEND_CB; + memcpy(send_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN); + send_cb->status = status; + if(xQueueSend(db_espnow_send_recv_callback_queue, &evt, ESPNOW_MAXDELAY) != + pdTRUE) { + ESP_LOGW(TAG, "Send to send queue fail"); + } +} + +static void +espnow_receive_callback(const esp_now_recv_info_t *recv_info, + const uint8_t *data, int len) +{ + db_espnow_event_t evt; + db_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb; + uint8_t *src_addr = recv_info->src_addr; + uint8_t *des_addr = recv_info->des_addr; + + if(src_addr == NULL || data == NULL || len <= 0) { + ESP_LOGE(TAG, "Receive callback arg error"); + return; + } + + if(!IS_BROADCAST_ADDR(des_addr)) { + ESP_LOGD(TAG, "Receive uni-cast ESP-NOW data. Ignoring"); + return; + } + + if(data[0] == DB_ESPNOW_ORIGIN_GND && + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { + // Ignoring packet - Came from another GND station + return; + } + else { + // we are GND and packet is for us from AIR +#if defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S3) || \ + defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C2) || \ + defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C6) + db_esp_now_clients_list->gnd_noise_floor = + (int8_t)recv_info->rx_ctrl->noise_floor; + // rest RSSI will be processed in process_espnow_data() +#endif + } + + if(data[0] == DB_ESPNOW_ORIGIN_AIR && + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_AIR) { + // Ignoring packet - Came from another AIR station + return; + } + else { + // we are AIR a packet is for us from GND - we only expect one GND station + // to be talking to us + db_esp_signal_quality.air_rssi = recv_info->rx_ctrl->rssi; +#if defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S3) || \ + defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C2) || \ + defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32C6) + db_esp_signal_quality.air_noise_floor = recv_info->rx_ctrl->noise_floor; +#endif + } + + evt.id = DB_ESPNOW_RECV_CB; + memcpy(recv_cb->mac_addr, src_addr, ESP_NOW_ETH_ALEN); + recv_cb->data = malloc(len); + if(recv_cb->data == NULL) { + ESP_LOGE(TAG, "Malloc receive data fail"); + return; + } + memcpy(recv_cb->data, data, len); + recv_cb->data_len = len; + recv_cb->rssi = recv_info->rx_ctrl->rssi; + if(db_espnow_send_recv_callback_queue != NULL && + xQueueSend(db_espnow_send_recv_callback_queue, &evt, ESPNOW_MAXDELAY) != + pdTRUE) { + ESP_LOGW(TAG, "Send to receive queue fail"); + free(recv_cb->data); + } +} + +static db_esp_now_clients_list_t * +espnow_broadcast_peers_list_create() +{ + db_esp_now_clients_list_t *client_list = + malloc(sizeof(db_esp_now_clients_list_t)); // Allocate memory for the list + if(client_list == NULL) { // Check if the allocation failed + return NULL; // Return NULL to indicate an error + } + client_list->size = 0; // Initialize the size to 0 + return client_list; // Return the pointer to the list +} + +static void +espnow_broadcast_peers_list_destroy(db_esp_now_clients_list_t *esp_now_clients) +{ + if(esp_now_clients == NULL) { // Check if the list is NULL + return; // Do nothing + } + free(esp_now_clients); // Free the list +} + +static int +init_gcm_encryption_module(uint8_t *aes_key) +{ + mbedtls_gcm_init(&aes); + generate_pkcs5_key(DB_PARAM_PASS, aes_key, AES_256_KEY_BYTES); + ESP_LOGI(TAG, "Derived AES Key:"); + for(int i = 0; i < AES_256_KEY_BYTES; ++i) { + printf("%02x", aes_key[i]); + } + printf("\n"); + int ret = mbedtls_gcm_setkey( + &aes, MBEDTLS_CIPHER_ID_AES, aes_key, DB_ESPNOW_AES_KEY_LEN); + return ret; +} + +static void +deinit_espnow_all() +{ + ESP_LOGW(TAG, "De init ESPNOW incl. Queues & AES"); + mbedtls_gcm_free(&aes); + vSemaphoreDelete(db_espnow_send_recv_callback_queue); + vSemaphoreDelete( + db_espnow_send_queue); // ToDo: Check if that is a good idea since control + // task might be using it + vSemaphoreDelete( + db_uart_write_queue); // ToDo: Check if that is a good idea since control + // task might be using it + espnow_broadcast_peers_list_destroy(db_esp_now_clients_list); + esp_now_deinit(); +} + +static esp_err_t +db_espnow_init() +{ + ESP_LOGI(TAG, "Initializing ESP-NOW parameters"); + db_esp_now_clients_list = espnow_broadcast_peers_list_create(); + /* Init Queue for ESP-NOW internal callbacks when packet is finally sent or + * received */ + db_espnow_send_recv_callback_queue = + xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_event_t)); + if(db_espnow_send_recv_callback_queue == NULL) { + ESP_LOGE(TAG, "Create db_espnow_send_recv_callback_queue mutex fail"); + return ESP_FAIL; + } + + /* Init Queues for communication with control task */ + db_espnow_send_queue = + xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_queue_event_t)); + db_uart_write_queue = + xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(db_espnow_queue_event_t)); + if(db_espnow_send_queue == NULL) { + ESP_LOGE(TAG, "Create db_espnow_send_queue mutex fail"); + return ESP_FAIL; + } + if(db_uart_write_queue == NULL) { + ESP_LOGE(TAG, "Create db_uart_write_queue mutex fail"); + return ESP_FAIL; + } + + /* Initialize ESP-NOW and register sending and receiving callback function. + */ + ESP_ERROR_CHECK(esp_now_init()); + ESP_ERROR_CHECK(esp_now_register_send_cb(espnow_send_callback)); + ESP_ERROR_CHECK(esp_now_register_recv_cb(espnow_receive_callback)); + + /* Add broadcast peer information to peer list. */ + esp_now_peer_info_t peer; + memset(&peer, 0, sizeof(esp_now_peer_info_t)); + memcpy(peer.peer_addr, BROADCAST_MAC, 6); + if(!esp_now_is_peer_exist(BROADCAST_MAC)) + ESP_ERROR_CHECK(esp_now_add_peer(&peer)); + + /* Limit payload size to the max we can do */ + if(DB_PARAM_SERIAL_PACK_SIZE > DB_ESPNOW_PAYLOAD_MAXSIZE || + DB_PARAM_SERIAL_PACK_SIZE < 1) { + DB_PARAM_SERIAL_PACK_SIZE = DB_ESPNOW_PAYLOAD_MAXSIZE; + } + else { + // all good + } + + /* Init AES GCM encryption module */ + uint8_t aes_key[AES_256_KEY_BYTES]; + int ret = init_gcm_encryption_module(aes_key); + if(ret != 0) { + ESP_LOGE(TAG, "mbedtls_gcm_setkey returned an error: %i", ret); + deinit_espnow_all(); + return ESP_FAIL; + } + ESP_ERROR_CHECK( + esp_now_set_pmk(aes_key)); // only first 16 bytes will be used + + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND) { + ESP_LOGI( + TAG, + "ESP-NOW for DroneBridge init done - acting as ESP-NOW GND station"); + } + else { + ESP_LOGI( + TAG, + "ESP-NOW for DroneBridge init done - acting as ESP-NOW AIR station"); + } + return ESP_OK; +} + +static bool +espnow_schedule_internal_telemetry_packet() +{ + if(DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND) { + // Only GND sends internal telemetry. This function was called wrongly + return false; + } + else { + // continue + } + // ToDo: Split list if longer than max payload size + if(sizeof(db_esp_now_clients_list_t) > DB_ESPNOW_PAYLOAD_MAXSIZE) { + ESP_LOGE(TAG, + "Size of db_esp_now_clients_list_t is > %i", + DB_ESPNOW_PAYLOAD_MAXSIZE); + return false; + } + else { + /* continue */ + } + ESP_LOGD(TAG, "Scheduling int. telem. packet"); + uint8_t payload_size = sizeof(db_esp_now_clients_list_t); + + db_espnow_queue_event_t evt; + // Set payload of DroneBridge for ESP32 ESP-NOW packet + evt.data = malloc(payload_size); + memcpy(evt.data, db_esp_now_clients_list, payload_size); + evt.data_len = payload_size; + evt.packet_type = DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY; + if(xQueueSend(db_espnow_send_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) { + ESP_LOGW(TAG, "Send to db_espnow_send_queue queue fail"); + free(evt.data); + return false; + } + else { + // all good + return true; + } +} + +_Noreturn static void +process_espnow_data() +{ + esp_err_t err = db_espnow_init(); + if(err == ESP_OK) { + // all good. continue + } + else { + ESP_LOGE(TAG, + "Failed to init espnow (db_espnow_init()) aborting start of " + "db_espnow task"); + vTaskDelete(NULL); + } + db_espnow_event_t evt; + // indicator that the last ESP-NOW send callback returned -> we can send the + // next packet + bool ready_to_send = true; // initially true + bool send_internal_telemetry_frame = + false; // flag that indicates a new internal telemetry frame shall be sent + int delay_timer_cnt = 0; + while(1) { + if(db_espnow_send_recv_callback_queue == NULL) + ESP_LOGE(TAG, "db_espnow_send_recv_callback_queue is NULL!"); + if(db_espnow_send_recv_callback_queue != NULL && + xQueueReceive(db_espnow_send_recv_callback_queue, &evt, 0) == pdTRUE) { + switch(evt.id) { + case DB_ESPNOW_SEND_CB: { + db_espnow_event_send_cb_t *send_cb = &evt.info.send_cb; + // indicate that we can send next packet - the last sending callback + // has returned, so we keep the order of packets. ESP-NOW by default + // does not guarantee the order when many packets are sent in quick + // succession + ready_to_send = true; + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND && + send_internal_telemetry_frame) { + send_internal_telemetry_frame = + !espnow_schedule_internal_telemetry_packet(); } - /* process UART -> ESP-NOW */ - if (ready_to_send) { - // send the next packet if available and set ready_to_send accordingly - ready_to_send = !db_read_uart_queue_and_send(); - } else { - // do nothing - we are not ready for sending another packet + else { + // try to immediately send the next packet if available and set + // ready_to_send accordingly + ready_to_send = !read_uart_queue_and_send(); } + break; + } + case DB_ESPNOW_RECV_CB: { + db_espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb; + espnow_process_rcv_data( + recv_cb->data, recv_cb->data_len, recv_cb->mac_addr, recv_cb->rssi); + free(recv_cb->data); + break; + } + default: + ESP_LOGE(TAG, "Callback type error: %d", evt.id); + break; + } + } + /* process UART -> ESP-NOW */ + if(ready_to_send) { + // send the next packet if available and set ready_to_send accordingly + ready_to_send = !read_uart_queue_and_send(); + } + else { + // do nothing - we are not ready for sending another packet + } - if (delay_timer_cnt == 5000) { - /* all actions are non-blocking so allow some delay so that the IDLE task of FreeRTOS and the watchdog can run - read: https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines for reference */ - vTaskDelay(10 / portTICK_PERIOD_MS); - send_internal_telemetry_frame = true; // use this timer trigger to schedule a new internal telemetry frame - delay_timer_cnt = 0; - } else { - delay_timer_cnt++; - } + if(delay_timer_cnt == 5000) { + /* all actions are non-blocking so allow some delay so that the IDLE task + of FreeRTOS and the watchdog can run read: + https://esp32developer.com/programming-in-c-c/tasks/tasks-vs-co-routines + for reference */ + vTaskDelay(10 / portTICK_PERIOD_MS); + send_internal_telemetry_frame = + true; // use this timer trigger to schedule a new internal telemetry + // frame + delay_timer_cnt = 0; } - vTaskDelete(NULL); + else { + delay_timer_cnt++; + } + } + vTaskDelete(NULL); } -/** - * Start task that handles ESP-NOW data - */ -void db_start_espnow_module() { - xTaskCreate(&process_espnow_data, "db_espnow", 40960, NULL, 5, NULL); +/****************************************************************************** + * Public Function Definitions + ******************************************************************************/ + +void +db_start_espnow_module() +{ + xTaskCreate(&process_espnow_data, /**< Task function reference */ + "db_espnow", /**< Task name */ + 40960, /**< Stack Size */ + NULL, /**< Task Parameters (unused) */ + 5, /**< Task priority */ + NULL /**< Task handle */ + ); } \ No newline at end of file diff --git a/main/db_esp_now.h b/main/db_esp_now.h index bced721..471f1c0 100644 --- a/main/db_esp_now.h +++ b/main/db_esp_now.h @@ -1,6 +1,5 @@ -#include -/* - * This file is part of DroneBridge:https://github.com/DroneBridge/ESP32 +/****************************************************************************** + * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2024 Wolfgang Christl * @@ -16,107 +15,172 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ + ******************************************************************************/ #ifndef DB_ESP32_DB_ESP_NOW_H #define DB_ESP32_DB_ESP_NOW_H +/****************************************************************************** + * System & Standard Library Headers + ******************************************************************************/ #include +#include + +/****************************************************************************** + * ESP-IDF Headers + ******************************************************************************/ #include -#define DB_ESPNOW_MAX_BROADCAST_PEERS 19 // Number of max. broadcast peers. that we support with internal telemetry. Limit is 255, but this is the max we can fit into one packet 250bytes -#define ESPNOW_QUEUE_SIZE 6 -#define ESPNOW_MAXDELAY 512 -#define DB_ESPNOW_AES_IV_LEN 12 // 96 bit -#define DB_ESPNOW_AES_TAG_LEN 16 -#define DB_ESPNOW_AES_KEY_LEN 256 // in bits 128 & 192 are supported by ESP32 -#define DB_ESPNOW_PAYLOAD_MAXSIZE (ESP_NOW_MAX_DATA_LEN-DB_ESPNOW_AES_IV_LEN-DB_ESPNOW_AES_TAG_LEN-6-1) // (origin, packet_type, seq_num) = 6 bytes, payload_length_decrypted = 1 byte +/****************************************************************************** + * Macros + ******************************************************************************/ +#define DB_ESPNOW_MAX_BROADCAST_PEERS 19 +#define ESPNOW_QUEUE_SIZE 6 +#define ESPNOW_MAXDELAY 512 +#define DB_ESPNOW_AES_IV_LEN 12 /**< 96-bit IV */ +#define DB_ESPNOW_AES_TAG_LEN 16 +#define DB_ESPNOW_AES_KEY_LEN 256 /**< Key length in bits */ +#define DB_ESPNOW_PAYLOAD_MAXSIZE \ + (ESP_NOW_MAX_DATA_LEN - DB_ESPNOW_AES_IV_LEN - DB_ESPNOW_AES_TAG_LEN - 6 - 1) + +/****************************************************************************** + * Enums + ******************************************************************************/ +typedef enum +{ + DB_ESPNOW_SEND_CB, + DB_ESPNOW_RECV_CB, +} db_espnow_event_id_t; -#define IS_BROADCAST_ADDR(addr) (memcmp(addr, BROADCAST_MAC, ESP_NOW_ETH_ALEN) == 0) +typedef enum +{ + DB_ESP_NOW_PACKET_TYPE_DATA = 0, + DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY = 1, +} db_espnow_queue_event_type_t; -extern QueueHandle_t db_espnow_send_queue; // Queue that contains data to be sent via ESP-NOW -extern QueueHandle_t db_uart_write_queue; // Queue that contains data to be written to UART (filled by ESP-NOW task) +enum +{ + DB_ESPNOW_ORIGIN_GND = 0, + DB_ESPNOW_ORIGIN_AIR = 1, +}; -typedef struct { - int8_t gnd_rssi; // RSSI in dBm of that peer when we received the last message - uint16_t last_seq_num; // last sequ. number received by the GND station ToDO: Not something we need to send via internal telemetry - uint16_t gnd_rx_lost_packets; // number of lost packets on the GND side - uint8_t broadcast_peer_mac[ESP_NOW_ETH_ALEN]; // mac address of a peer that sent us broadcast messages +/****************************************************************************** + * Struct Definitions + ******************************************************************************/ + +/****************************************************************************** + * Stores peer RSSI and sequence number tracking information + ******************************************************************************/ +typedef struct +{ + int8_t gnd_rssi; + uint16_t last_seq_num; + uint16_t gnd_rx_lost_packets; + uint8_t broadcast_peer_mac[ESP_NOW_ETH_ALEN]; } __attribute__((__packed__)) db_esp_now_bpeer_info_t; -// structure used by ESP-NOW (manly GND) to keep infos about RSSI from every device that sent us something -typedef struct { - uint8_t size; // number of peers within db_esp_now_bpeer_info - int8_t gnd_noise_floor; // noise floor as measured on GND side (not all ESP32s support that feature) - db_esp_now_bpeer_info_t db_esp_now_bpeer_info[DB_ESPNOW_MAX_BROADCAST_PEERS]; -} __attribute__((__packed__)) db_esp_now_clients_list_t; // structure is sent as ESP-NOW internal telemetry frame to AIR ESP32 - -typedef enum { - DB_ESPNOW_SEND_CB, - DB_ESPNOW_RECV_CB, -} db_espnow_event_id_t; - -typedef struct { - uint8_t mac_addr[ESP_NOW_ETH_ALEN]; - esp_now_send_status_t status; +/****************************************************************************** + * ESP-NOW internal telemetry message format (sent to AIR) + ******************************************************************************/ +typedef struct +{ + uint8_t size; + int8_t gnd_noise_floor; + db_esp_now_bpeer_info_t db_esp_now_bpeer_info[DB_ESPNOW_MAX_BROADCAST_PEERS]; +} __attribute__((__packed__)) db_esp_now_clients_list_t; + +/****************************************************************************** + * ESP-NOW send callback event + ******************************************************************************/ +typedef struct +{ + uint8_t mac_addr[ESP_NOW_ETH_ALEN]; + esp_now_send_status_t status; } db_espnow_event_send_cb_t; -typedef struct { - uint8_t mac_addr[ESP_NOW_ETH_ALEN]; - int8_t rssi; - uint8_t data_len; - uint8_t *data; +/****************************************************************************** + * ESP-NOW receive callback event + ******************************************************************************/ +typedef struct +{ + uint8_t mac_addr[ESP_NOW_ETH_ALEN]; + int8_t rssi; + uint8_t data_len; + uint8_t *data; } db_espnow_event_recv_cb_t; -typedef union { - db_espnow_event_send_cb_t send_cb; - db_espnow_event_recv_cb_t recv_cb; +/****************************************************************************** + * Union of ESP-NOW send/receive event data + ******************************************************************************/ +typedef union +{ + db_espnow_event_send_cb_t send_cb; + db_espnow_event_recv_cb_t recv_cb; } db_espnow_event_info_t; -/* When ESPNOW sending or receiving callback function is called, post event to ESPNOW task. */ -typedef struct { - db_espnow_event_id_t id; - db_espnow_event_info_t info; +/****************************************************************************** + * Event object passed from callback to ESP-NOW processing task + ******************************************************************************/ +typedef struct +{ + db_espnow_event_id_t id; + db_espnow_event_info_t info; } db_espnow_event_t; -typedef enum { - DB_ESP_NOW_PACKET_TYPE_DATA = 0, - DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY = 1, -} db_espnow_queue_event_type_t; - -/* Element of db_espnow_send_queue and db_uart_write_queue */ -typedef struct { - uint8_t data_len; - uint8_t *data; - db_espnow_queue_event_type_t packet_type; // only relevant when reading queue that sends via ESP-NOW +/****************************************************************************** + * Queue element for both ESP-NOW and UART transmission queues + ******************************************************************************/ +typedef struct +{ + uint8_t data_len; + uint8_t *data; + db_espnow_queue_event_type_t packet_type; } __attribute__((__packed__)) db_espnow_queue_event_t; -enum { - DB_ESPNOW_ORIGIN_GND = 0, - DB_ESPNOW_ORIGIN_AIR = 1, -}; - -typedef struct { - uint8_t origin; // type: db_espnow_data_origin_t: 0=packet sent by GCS - 1=packet sent by drone - uint8_t packet_type; // DB_ESP_NOW_PACKET_TYPE_DATA or DB_ESP_NOW_PACKET_TYPE_INTERNAL_TELEMETRY or FEC in the future - uint32_t seq_num; // Sequence number of the packet - uint8_t aes_iv[DB_ESPNOW_AES_IV_LEN]; // Initialization vector for AES -} __attribute__((__packed__)) db_esp_now_packet_header_t; // authenticated but NOT encrypted by AES-GCM - -typedef struct { - uint8_t payload_length_decrypted; // length of unencrypted payload - uint8_t payload[DB_ESPNOW_PAYLOAD_MAXSIZE]; // actual payload data -} __attribute__((__packed__)) db_esp_now_packet_protected_data_t; // encrypted & authenticated by AES-GCM - -/* DroneBridge for ESP32 ESP-NOW packet */ -typedef struct { - db_esp_now_packet_header_t db_esp_now_packet_header; - uint8_t tag[DB_ESPNOW_AES_TAG_LEN]; // AES-GCM Tag - db_esp_now_packet_protected_data_t db_esp_now_packet_protected_data; // shall be last in struct, so we can cut off when payload is smaller -} __attribute__((__packed__)) db_esp_now_packet_t; // total size must be <=250bytes (ESP-NOW requirement) - +/****************************************************************************** + * ESP-NOW packet header (unencrypted, authenticated) + ******************************************************************************/ +typedef struct +{ + uint8_t origin; + uint8_t packet_type; + uint32_t seq_num; + uint8_t aes_iv[DB_ESPNOW_AES_IV_LEN]; +} __attribute__((__packed__)) db_esp_now_packet_header_t; + +/****************************************************************************** + * ESP-NOW encrypted payload block (AES-GCM) + ******************************************************************************/ +typedef struct +{ + uint8_t payload_length_decrypted; + uint8_t payload[DB_ESPNOW_PAYLOAD_MAXSIZE]; +} __attribute__((__packed__)) db_esp_now_packet_protected_data_t; + +/****************************************************************************** + * Final ESP-NOW packet structure + * Contains authenticated header, AES-GCM tag, and encrypted payload + ******************************************************************************/ +typedef struct +{ + db_esp_now_packet_header_t db_esp_now_packet_header; + uint8_t tag[DB_ESPNOW_AES_TAG_LEN]; + db_esp_now_packet_protected_data_t db_esp_now_packet_protected_data; +} __attribute__((__packed__)) db_esp_now_packet_t; + +/****************************************************************************** + * Public Variables + ******************************************************************************/ +extern QueueHandle_t db_espnow_send_queue; +extern QueueHandle_t db_uart_write_queue; + +/****************************************************************************** + * Public Function Declarations + ******************************************************************************/ + +/****************************************************************************** + * @brief Start task that handles ESP-NOW data + ******************************************************************************/ void db_start_espnow_module(); -_Noreturn void process_espnow_data(); - -#endif //DB_ESP32_DB_ESP_NOW_H +#endif // DB_ESP32_DB_ESP_NOW_H diff --git a/main/db_mavlink_msgs.c b/main/db_mavlink_msgs.c index 21ac97e..694a261 100644 --- a/main/db_mavlink_msgs.c +++ b/main/db_mavlink_msgs.c @@ -26,49 +26,62 @@ #include "main.h" #ifndef MIN -#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define MIN(a, b) ((a) < (b) ? (a) : (b)) #endif #ifndef MAX -#define MAX(a,b) ((a) > (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) #endif -#define FASTMAVLINK_ROUTER_LINKS_MAX 3 -#define FASTMAVLINK_ROUTER_COMPONENTS_MAX 5 +#define FASTMAVLINK_ROUTER_LINKS_MAX 3 +#define FASTMAVLINK_ROUTER_COMPONENTS_MAX 5 #define TAG "DB_MAV_MSGS" /** - * Based on the system architecture and configured wifi mode the ESP32 may have a different role and system id. - * Returns the best fitting component ID for the specific role. + * Based on the system architecture and configured wifi mode the ESP32 may have + * a different role and system id. Returns the best fitting component ID for + * the specific role. * @return component ID for ESP32 */ -uint8_t db_get_mav_comp_id() { - return MAV_COMP_ID_TELEMETRY_RADIO; +uint8_t +db_get_mav_comp_id() +{ + return MAV_COMP_ID_TELEMETRY_RADIO; } /** * Return the Mavlink system ID. Set by handle_mavlink_message() * @return system ID for ESP32 */ -uint8_t db_get_mav_sys_id() { - return DB_MAV_SYS_ID; +uint8_t +db_get_mav_sys_id() +{ + return DB_MAV_SYS_ID; } /** - * Converts the measured (negative dBm) signal strength to a format the MAVLink RADIO STATUS packet accepts and the GCS likes. - * If QGroundControl is desired output format it will not convert but send the value as int8. For Mission Planner it converts int8 to uint8. The value represents the absolute(dBm): -54 dBm -> 54 + * Converts the measured (negative dBm) signal strength to a format the MAVLink + * RADIO STATUS packet accepts and the GCS likes. If QGroundControl is desired + * output format it will not convert but send the value as int8. For Mission + * Planner it converts int8 to uint8. The value represents the absolute(dBm): + * -54 dBm -> 54 * @param signal_strength Signal strength in dBm as reported by the ESP32 * @param noise_floor Spectrum noise floor - not used for now - * @return Signal strength formatted for QGroundControl (0 to -127 [dBm]) or Mission Planner (0 to 100) + * @return Signal strength formatted for QGroundControl (0 to -127 [dBm]) or + * Mission Planner (0 to 100) */ -int8_t db_format_rssi(int8_t signal_strength, int8_t noise_floor) { - if (db_param_rssi_dbm.value.db_param_u8.value) { - // report in [dBm] - no conversion since in Wi-Fi, BLE & ESP-NOW APIs natively report dBm - return signal_strength; - } else { - // dBm from [-50 to -100] scaled to 100 to 0 - return MIN(100, 2 * (MAX(-100, MIN(-50, signal_strength)) + 100)); - } +int8_t +db_format_rssi(int8_t signal_strength, int8_t noise_floor) +{ + if(db_param_rssi_dbm.value.db_param_u8.value) { + // report in [dBm] - no conversion since in Wi-Fi, BLE & ESP-NOW APIs + // natively report dBm + return signal_strength; + } + else { + // dBm from [-50 to -100] scaled to 100 to 0 + return MIN(100, 2 * (MAX(-100, MIN(-50, signal_strength)) + 100)); + } } /** @@ -76,121 +89,158 @@ int8_t db_format_rssi(int8_t signal_strength, int8_t noise_floor) { * @param buff Buffer to write heartbeat to (>280 bytes) * @return Length of the message in the buffer */ -uint16_t db_create_heartbeat(uint8_t *buff, fmav_status_t *fmav_status) { - return fmav_msg_heartbeat_pack_to_frame_buf( - buff, db_get_mav_sys_id(), db_get_mav_comp_id(), - MAV_TYPE_ONBOARD_CONTROLLER, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_ACTIVE, - fmav_status); +uint16_t +db_create_heartbeat(uint8_t *buff, fmav_status_t *fmav_status) +{ + return fmav_msg_heartbeat_pack_to_frame_buf( + buff, + db_get_mav_sys_id(), + db_get_mav_comp_id(), + MAV_TYPE_ONBOARD_CONTROLLER, + MAV_AUTOPILOT_INVALID, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 0, + MAV_STATE_ACTIVE, + fmav_status); } /** - * Creates a mavlink PARAM_VALUE message inside the provided buffer using the provided parameter + * Creates a mavlink PARAM_VALUE message inside the provided buffer using the + * provided parameter * * @param buff Buffer to write the mavlink message to * @param fmav_status fastmavlink status stucture * @param param_index Index of the parameter to send - * @param value The value of the parameter to be sent -> will be converted to IEEE 745 + * @param value The value of the parameter to be sent -> will be converted to + * IEEE 745 * @param type The MAV_PARAM_TYPE * @param param_id The name of the parameter (ID) * @return Length of the mavlink message inside the buffer */ -uint16_t db_get_mavmsg_param_value(uint8_t *buff, fmav_status_t *fmav_status, uint16_t param_index, float_int_union *value, uint8_t type, char *param_id) { - fmav_param_value_t fmav_param_value = { - .param_value = value->f, - .param_type = type, - .param_count = DB_PARAM_MAV_CNT, - .param_index = param_index}; - if (strlen(param_id)>15) { // max size of param_id is 16 bytes - memcpy(fmav_param_value.param_id, param_id, 16); - } else { - strcpy(fmav_param_value.param_id, param_id); - } - return fmav_msg_param_value_encode_to_frame_buf(buff, db_get_mav_sys_id(), db_get_mav_comp_id(), - &fmav_param_value, fmav_status); +uint16_t +db_get_mavmsg_param_value(uint8_t *buff, fmav_status_t *fmav_status, + uint16_t param_index, float_int_union *value, + uint8_t type, char *param_id) +{ + fmav_param_value_t fmav_param_value = { .param_value = value->f, + .param_type = type, + .param_count = DB_PARAM_MAV_CNT, + .param_index = param_index }; + if(strlen(param_id) > 15) { // max size of param_id is 16 bytes + memcpy(fmav_param_value.param_id, param_id, 16); + } + else { + strcpy(fmav_param_value.param_id, param_id); + } + return fmav_msg_param_value_encode_to_frame_buf(buff, + db_get_mav_sys_id(), + db_get_mav_comp_id(), + &fmav_param_value, + fmav_status); } /** - * Gets the mavlink parameter value as float_int_union based on the parameter ID from the internal variable. - * Maps MAVLink parameter names to the internal variable names. + * Gets the mavlink parameter value as float_int_union based on the parameter + * ID from the internal variable. Maps MAVLink parameter names to the internal + * variable names. * * @param float_int IEEE 754 storage for the retrieved value * @param param_id Parameter name you want the value of - * @param param_index Index of the parameter. May be -1 if requested parameter shall be found based on param_id - * @return MAV_PARAM_TYPE of the parameter. Returns 0 if the parameter was not found + * @param param_index Index of the parameter. May be -1 if requested parameter + * shall be found based on param_id + * @return MAV_PARAM_TYPE of the parameter. Returns 0 if the parameter was not + * found */ -MAV_PARAM_TYPE db_mav_get_parameter_value(float_int_union *float_int, const char *param_id, const int16_t param_index) { - MAV_PARAM_TYPE type = 0; - if (param_index >= DB_PARAM_MAV_CNT) { - ESP_LOGE(TAG, "Requested mavlink parameter index %i is out of range (0-%i)", param_index, DB_PARAM_MAV_CNT-1); - return 0; +MAV_PARAM_TYPE +db_mav_get_parameter_value(float_int_union *float_int, const char *param_id, + const int16_t param_index) +{ + MAV_PARAM_TYPE type = 0; + if(param_index >= DB_PARAM_MAV_CNT) { + ESP_LOGE(TAG, + "Requested mavlink parameter index %i is out of range (0-%i)", + param_index, + DB_PARAM_MAV_CNT - 1); + return 0; + } + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + if(strncmp(param_id, (char *)db_params[i]->mav_t.param_name, 16) == 0 || + param_index == db_params[i]->mav_t.param_index) { + // found the parameter to return its value + type = db_params[i]->mav_t.param_type; + switch(db_params[i]->type) { + case STRING: + ESP_LOGE( + TAG, + "db_mav_get_parameter_value(): String parameter not supported."); + break; + case UINT8: + float_int->uint8 = db_params[i]->value.db_param_u8.value; + break; + case UINT16: + float_int->uint16 = db_params[i]->value.db_param_u16.value; + break; + case INT32: + float_int->int32 = db_params[i]->value.db_param_i32.value; + break; + default: + ESP_LOGE(TAG, + "db_mav_get_parameter_value() -> db_parameter.type unknown!"); + break; + } } - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - if (strncmp(param_id, (char *) db_params[i]->mav_t.param_name, 16) == 0 || param_index == db_params[i]->mav_t.param_index) { - // found the parameter to return its value - type = db_params[i]->mav_t.param_type; - switch (db_params[i]->type) { - case STRING: - ESP_LOGE(TAG, "db_mav_get_parameter_value(): String parameter not supported."); - break; - case UINT8: - float_int->uint8 = db_params[i]->value.db_param_u8.value; - break; - case UINT16: - float_int->uint16 = db_params[i]->value.db_param_u16.value; - break; - case INT32: - float_int->int32 = db_params[i]->value.db_param_i32.value; - break; - default: - ESP_LOGE(TAG, "db_mav_get_parameter_value() -> db_parameter.type unknown!"); - break; - } - } else { - // do nothing - no match - } + else { + // do nothing - no match } - return type; + } + return type; } /** - * Writes the parameter received via mavlink PARAM_SET to the internal variable and triggers write to NVS. - * For some parameters to become effective the ESP32 still needs to be rebooted! - * No string/blob parameters supported for now. + * Writes the parameter received via mavlink PARAM_SET to the internal variable + * and triggers write to NVS. For some parameters to become effective the ESP32 + * still needs to be rebooted! No string/blob parameters supported for now. * * @param param_set_payload * @return 1 in case of success and 0 in case of failure */ -bool db_write_mavlink_parameter(const fmav_param_set_t *param_set_payload) { - // BEWARE: ONLY WORKS WITH NUMBERS FOR NOW! - NO SUPPORT FOR STRINGS - float_int_union float_int; // used to convert from IEEE 754 - float_int.f = param_set_payload->param_value; // read parameter value into helper structure - bool success = false; - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - if (strncmp(param_set_payload->param_id, (char *) db_params[i]->mav_t.param_name, 16) == 0) { - switch (db_params[i]->type) { - case STRING: - ESP_LOGE(TAG, "db_write_mavlink_parameter(): String not supported"); - success = false; - break; - case UINT8: - success = db_param_is_valid_assign_u8(float_int.uint8, db_params[i]); - break; - case UINT16: - success = db_param_is_valid_assign_u16(float_int.uint16, db_params[i]); - break; - case INT32: - success = db_param_is_valid_assign_i32(float_int.int32, db_params[i]); - break; - default: - success = false; - ESP_LOGE(TAG, "db_write_mavlink_parameter(): Unknown type"); - break; - } - } else { - // this is not the parameter we are looking for - } +bool +db_write_mavlink_parameter(const fmav_param_set_t *param_set_payload) +{ + // BEWARE: ONLY WORKS WITH NUMBERS FOR NOW! - NO SUPPORT FOR STRINGS + float_int_union float_int; // used to convert from IEEE 754 + float_int.f = param_set_payload + ->param_value; // read parameter value into helper structure + bool success = false; + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + if(strncmp(param_set_payload->param_id, + (char *)db_params[i]->mav_t.param_name, + 16) == 0) { + switch(db_params[i]->type) { + case STRING: + ESP_LOGE(TAG, "db_write_mavlink_parameter(): String not supported"); + success = false; + break; + case UINT8: + success = db_param_is_valid_assign_u8(float_int.uint8, db_params[i]); + break; + case UINT16: + success = db_param_is_valid_assign_u16(float_int.uint16, db_params[i]); + break; + case INT32: + success = db_param_is_valid_assign_i32(float_int.int32, db_params[i]); + break; + default: + success = false; + ESP_LOGE(TAG, "db_write_mavlink_parameter(): Unknown type"); + break; + } + } + else { + // this is not the parameter we are looking for } - return success; + } + return success; } /** @@ -203,40 +253,49 @@ bool db_write_mavlink_parameter(const fmav_param_set_t *param_set_payload) { * @param the_msg The mavlink message that was parsed (containing the_command) * @param status fastmavlink parser status structure */ -void db_answer_mavlink_cmd_request_message(uint16_t requested_msg_id, - uint8_t *buff, enum DB_MAVLINK_DATA_ORIGIN origin, - int *tcp_clients, udp_conn_list_t *udp_conns, fmav_message_t *the_msg, - fmav_status_t *status) { - switch (requested_msg_id) { - case FASTMAVLINK_MSG_ID_AUTOPILOT_VERSION: { - fmav_command_ack_t a = {.command = MAV_CMD_REQUEST_MESSAGE, - .result = MAV_RESULT_ACCEPTED, - .target_system = the_msg->sysid, - .target_component = the_msg->compid}; - uint16_t len = fmav_msg_command_ack_encode_to_frame_buf(buff, db_get_mav_sys_id(), db_get_mav_comp_id(), &a, - status); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); +void +db_answer_mavlink_cmd_request_message(uint16_t requested_msg_id, uint8_t *buff, + enum DB_MAVLINK_DATA_ORIGIN origin, + int *tcp_clients, + udp_conn_list_t *udp_conns, + fmav_message_t *the_msg, + fmav_status_t *status) +{ + switch(requested_msg_id) { + case FASTMAVLINK_MSG_ID_AUTOPILOT_VERSION: { + fmav_command_ack_t a = { .command = MAV_CMD_REQUEST_MESSAGE, + .result = MAV_RESULT_ACCEPTED, + .target_system = the_msg->sysid, + .target_component = the_msg->compid }; + uint16_t len = fmav_msg_command_ack_encode_to_frame_buf( + buff, db_get_mav_sys_id(), db_get_mav_comp_id(), &a, status); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - fmav_autopilot_version_t autopilot_version = { - .board_version = 0, - .capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_BYTEWISE, - .flight_sw_version = DB_MAJOR_VERSION, - .middleware_sw_version = DB_MINOR_VERSION - }; - len = fmav_msg_autopilot_version_encode_to_frame_buf(buff, db_get_mav_sys_id(), - db_get_mav_comp_id(), &autopilot_version, status); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - } - break; - default: { - ESP_LOGW(TAG, "Unsupported MavLink requested message: %i - ignoring", requested_msg_id); - } - break; - } + fmav_autopilot_version_t autopilot_version = { + .board_version = 0, + .capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | + MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_BYTEWISE, + .flight_sw_version = DB_MAJOR_VERSION, + .middleware_sw_version = DB_MINOR_VERSION + }; + len = fmav_msg_autopilot_version_encode_to_frame_buf(buff, + db_get_mav_sys_id(), + db_get_mav_comp_id(), + &autopilot_version, + status); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + } break; + default: { + ESP_LOGW(TAG, + "Unsupported MavLink requested message: %i - ignoring", + requested_msg_id); + } break; + } } /** - * Called when a MSG_ID_COMMAND_LONG was received. Handles the command processing. + * Called when a MSG_ID_COMMAND_LONG was received. Handles the command + * processing. * @param the_command The structure of the received command * @param the_msg The mavlink message that was parsed (containing the_command) * @param status fastmavlink parser status structure @@ -245,228 +304,313 @@ void db_answer_mavlink_cmd_request_message(uint16_t requested_msg_id, * @param tcp_clients List of connected tcp clients as sockets * @param udp_conns List of connected UDP clients */ -void db_process_mavlink_command(fmav_command_long_t *the_command, - fmav_message_t *the_msg, - fmav_status_t *status, - uint8_t *buff, - enum DB_MAVLINK_DATA_ORIGIN origin, int *tcp_clients, udp_conn_list_t *udp_conns) { - switch (the_command->command) { - case MAV_CMD_REQUEST_MESSAGE: { - uint16_t req_msg_id = the_command->param1; - ESP_LOGI(TAG, "\trequest for msg with ID: %i", req_msg_id); - db_answer_mavlink_cmd_request_message(req_msg_id, buff, origin, tcp_clients, udp_conns, - the_msg, status); - } - break; - default: { - fmav_command_ack_t b = {.command = the_command->command, - .result = MAV_RESULT_UNSUPPORTED, - .target_system = the_msg->sysid, - .target_component = the_msg->compid}; - uint16_t len = fmav_msg_command_ack_encode_to_frame_buf(buff, db_get_mav_sys_id(), - db_get_mav_comp_id(), &b, status); - ESP_LOGW(TAG, "Unsupported MavLink command request: %i - ignoring", the_command->command); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - } - break; - } +void +db_process_mavlink_command(fmav_command_long_t *the_command, + fmav_message_t *the_msg, fmav_status_t *status, + uint8_t *buff, enum DB_MAVLINK_DATA_ORIGIN origin, + int *tcp_clients, udp_conn_list_t *udp_conns) +{ + switch(the_command->command) { + case MAV_CMD_REQUEST_MESSAGE: { + uint16_t req_msg_id = the_command->param1; + ESP_LOGI(TAG, "\trequest for msg with ID: %i", req_msg_id); + db_answer_mavlink_cmd_request_message( + req_msg_id, buff, origin, tcp_clients, udp_conns, the_msg, status); + } break; + default: { + fmav_command_ack_t b = { .command = the_command->command, + .result = MAV_RESULT_UNSUPPORTED, + .target_system = the_msg->sysid, + .target_component = the_msg->compid }; + uint16_t len = fmav_msg_command_ack_encode_to_frame_buf( + buff, db_get_mav_sys_id(), db_get_mav_comp_id(), &b, status); + ESP_LOGW(TAG, + "Unsupported MavLink command request: %i - ignoring", + the_command->command); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + } break; + } } /** * Expects GCS to have system ID 255. - * Processes Mavlink messages and sends the radio status message and heartbeat to the GCS on every heartbeat received via UART + * Processes Mavlink messages and sends the radio status message and heartbeat + * to the GCS on every heartbeat received via UART * - * We expect the FC to be connected to serial port when in WiFi-AP or in WiFi-Client Mode. - * We expect the GCS to be connected to serial port when in AP-LR or ESP-NOW GND mode. + * We expect the FC to be connected to serial port when in WiFi-AP or in + * WiFi-Client Mode. We expect the GCS to be connected to serial port when in + * AP-LR or ESP-NOW GND mode. * * @param new_msg Message to process * @param tcp_clients List of connected tcp clients * @param udp_conns List of connected UDP clients - * @param fmav_status fastmavlink library parser status - setup once by the parser for a specific link/interface - * @param origin Indicates from what kind of input/link we received the new message. + * @param fmav_status fastmavlink library parser status - setup once by the + * parser for a specific link/interface + * @param origin Indicates from what kind of input/link we received the new + * message. */ -void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_list_t *udp_conns, - fmav_status_t *fmav_status, - enum DB_MAVLINK_DATA_ORIGIN origin) { - static uint8_t buff[296]; // buffer to handle the response messages - no need to init every time - switch (new_msg->msgid) { - case FASTMAVLINK_MSG_ID_HEARTBEAT: - if (origin == DB_MAVLINK_DATA_ORIGIN_SERIAL) { - // we only process heartbeats coming from the UART (local device) since we also use it as a trigger to send our heartbeat - fmav_heartbeat_t payload; - fmav_msg_heartbeat_decode(&payload, new_msg); - if (payload.autopilot == MAV_AUTOPILOT_INVALID && payload.type == MAV_TYPE_GCS) { - ESP_LOGD(TAG, "Got heartbeat from GCS (sysID: %i)", new_msg->sysid); - DB_MAV_SYS_ID = new_msg->sysid; - // We must be in either one of these modes: AP LR or ESP-NOW GND - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR) { - // Send heartbeat to GCS: Every ESP32 no matter its role or mode is emitting a heartbeat - uint16_t length = db_create_heartbeat(buff, fmav_status); - ESP_LOGD(TAG, "Sending back heartbeat via serial link to GCS"); - // In AP LR mode and in ESP-NOW GND mode the heartbeat has to be emitted via serial directly to the GCS - write_to_serial(buff, length); - } else { - ESP_LOGW(TAG, "We received a heartbeat from GCS while not being in DB_WIFI_MODE_ESPNOW_GND or " - "DB_WIFI_MODE_AP_LR mode. Check your configuration! AIR-Side ESP32 seems to be " - "connected to GCS via UART"); - } - } else if (payload.autopilot != MAV_AUTOPILOT_INVALID && new_msg->compid == MAV_COMP_ID_AUTOPILOT1) { - ESP_LOGD(TAG, "Got heartbeat from flight controller (sysID: %i)", new_msg->sysid); - // This means we are connected to the FC since we only parse mavlink on UART and thus only see the - // device we are connected to via UART - DB_MAV_SYS_ID = new_msg->sysid; - // Check if FC is armed and the Wi-Fi switch based on armed status is configured by the user - if (DB_PARAM_DIS_RADIO_ON_ARM && - (payload.base_mode & MAV_MODE_FLAG_SAFETY_ARMED || - (payload.system_status > MAV_STATE_STANDBY && payload.system_status != MAV_STATE_POWEROFF))) { - // autopilot indicates it is armed - db_set_radio_status(false); - } else { - // autopilot indicates it is <> armed - db_set_radio_status(true); - } - // ESP32s that are connected to a flight controller via UART will send RADIO_STATUS messages to the GND - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_AIR || DB_PARAM_RADIO_MODE == DB_BLUETOOTH_MODE) { - // ToDo: For BLE only the last connected client is considered. - fmav_radio_status_t payload_r = {.fixed = 0, .txbuf=0, - .noise = db_esp_signal_quality.gnd_noise_floor, - .remnoise = db_esp_signal_quality.air_noise_floor, - .remrssi = db_format_rssi(db_esp_signal_quality.air_rssi, db_esp_signal_quality.air_noise_floor), - .rssi = db_format_rssi(db_esp_signal_quality.gnd_rssi, db_esp_signal_quality.gnd_noise_floor), - .rxerrors = db_esp_signal_quality.gnd_rx_packets_lost}; - 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); - db_send_to_all_clients(tcp_clients, udp_conns, buff, len); - } else if (DB_PARAM_RADIO_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 - // -> We are in WiFi AP mode and connected to the drone - // Send each connected client a radio status packet. - // ToDo: Only the RSSI of the first (Wi-Fi) is considered. - // Easier for UDP since we have a nice list with mac addresses to use for mapping. - // Harder for TCP -> no MAC addresses available of connected clients - fmav_radio_status_t payload_r = {.fixed = UINT8_MAX, .noise = UINT8_MAX, .remnoise = UINT8_MAX, .remrssi=db_format_rssi(wifi_sta_list.sta[0].rssi, -88), .rssi=UINT8_MAX, .rxerrors=0, .txbuf=0}; - 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); - 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 - } - } else { - // We do not react to any other heartbeat! - } - // 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 - 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: { - ESP_LOGI(TAG, "Received PARAM_REQUEST_LIST msg. Responding with parameters"); - float_int_union float_int; - uint16_t len = 0; - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - switch (db_params[i]->type) { - case STRING: - // ignoring strings. Not supported with this request - continue; - break; - case UINT8: - float_int.uint8 = db_params[i]->value.db_param_u8.value; - break; - case UINT16: - float_int.uint16 = db_params[i]->value.db_param_u16.value; - break; - case INT32: - float_int.int32 = db_params[i]->value.db_param_i32.value; - break; - default: - ESP_LOGE(TAG, "db_param_write_all_params_json() -> db_parameter.type unknown!"); - break; - } - len = db_get_mavmsg_param_value(buff, fmav_status, db_params[i]->mav_t.param_index, &float_int, - db_params[i]->mav_t.param_type, (char *) db_params[i]->mav_t.param_name); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - } - } - break; - case FASTMAVLINK_MSG_ID_PARAM_REQUEST_READ: { - fmav_param_request_read_t payload; - fmav_msg_param_request_read_decode(&payload, new_msg); - float_int_union float_int; - ESP_LOGI(TAG, "GCS request reading parameter ID: %s with index %i", payload.param_id, payload.param_index); - MAV_PARAM_TYPE type = db_mav_get_parameter_value(&float_int, payload.param_id, payload.param_index); - if (type != 0) { - uint16_t len = db_get_mavmsg_param_value(buff, fmav_status, payload.param_index, &float_int, type, - payload.param_id); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - } else { - // send nothing, unknown parameter - ESP_LOGW(TAG, "\tParameter is unknown. Not responding!"); - } +void +handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, + udp_conn_list_t *udp_conns, fmav_status_t *fmav_status, + enum DB_MAVLINK_DATA_ORIGIN origin) +{ + static uint8_t buff[296]; // buffer to handle the response messages - no need + // to init every time + switch(new_msg->msgid) { + case FASTMAVLINK_MSG_ID_HEARTBEAT: + if(origin == DB_MAVLINK_DATA_ORIGIN_SERIAL) { + // we only process heartbeats coming from the UART (local device) since + // we also use it as a trigger to send our heartbeat + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, new_msg); + if(payload.autopilot == MAV_AUTOPILOT_INVALID && + payload.type == MAV_TYPE_GCS) { + ESP_LOGD(TAG, "Got heartbeat from GCS (sysID: %i)", new_msg->sysid); + DB_MAV_SYS_ID = new_msg->sysid; + // We must be in either one of these modes: AP LR or ESP-NOW GND + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_GND || + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR) { + // Send heartbeat to GCS: Every ESP32 no matter its role or mode is + // emitting a heartbeat + uint16_t length = db_create_heartbeat(buff, fmav_status); + ESP_LOGD(TAG, "Sending back heartbeat via serial link to GCS"); + // In AP LR mode and in ESP-NOW GND mode the heartbeat has to be + // emitted via serial directly to the GCS + db_write_to_serial(buff, length); } - break; - case FASTMAVLINK_MSG_ID_PARAM_SET: { - fmav_param_set_t parame_set_payload; - fmav_msg_param_set_decode(¶me_set_payload, new_msg); - ESP_LOGI(TAG, "GCS requested setting parameter %s", parame_set_payload.param_id); - if (db_write_mavlink_parameter(¶me_set_payload)) { - // Respond with parameter - float_int_union float_int; - MAV_PARAM_TYPE type = db_mav_get_parameter_value(&float_int, parame_set_payload.param_id, -1); - if (type != 0) { - uint16_t len = db_get_mavmsg_param_value(buff, fmav_status, 0, &float_int, type, - parame_set_payload.param_id); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); - db_write_settings_to_nvs(); - } else { - ESP_LOGE(TAG, "Failed to get parameter %s - could not respond with new param", parame_set_payload.param_id); - } - } else { - ESP_LOGE(TAG, "db_write_mavlink_parameter() failed to set new parameter %s ", parame_set_payload.param_id); - } + else { + ESP_LOGW(TAG, + "We received a heartbeat from GCS while not being in " + "DB_WIFI_MODE_ESPNOW_GND or " + "DB_WIFI_MODE_AP_LR mode. Check your configuration! " + "AIR-Side ESP32 seems to be " + "connected to GCS via UART"); } - break; - case FASTMAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST: { - ESP_LOGI(TAG, "GCS requested reading ext parameters list - not supported for now!"); - // ToDo Respond to PARAM_EXT_REQUEST_LIST + } + else if(payload.autopilot != MAV_AUTOPILOT_INVALID && + new_msg->compid == MAV_COMP_ID_AUTOPILOT1) { + ESP_LOGD(TAG, + "Got heartbeat from flight controller (sysID: %i)", + new_msg->sysid); + // This means we are connected to the FC since we only parse mavlink on + // UART and thus only see the device we are connected to via UART + DB_MAV_SYS_ID = new_msg->sysid; + // Check if FC is armed and the Wi-Fi switch based on armed status is + // configured by the user + if(DB_PARAM_DIS_RADIO_ON_ARM && + (payload.base_mode & MAV_MODE_FLAG_SAFETY_ARMED || + (payload.system_status > MAV_STATE_STANDBY && + payload.system_status != MAV_STATE_POWEROFF))) { + // autopilot indicates it is armed + db_set_radio_status(false); } - break; - case FASTMAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ: { - ESP_LOGI(TAG, "GCS requested reading ext parameter - not supported for now!"); - // ToDo Respond to EXT_REQUEST_READ + else { + // autopilot indicates it is <> armed + db_set_radio_status(true); } - break; - case FASTMAVLINK_MSG_ID_COMMAND_LONG: { - fmav_command_long_t payload; - fmav_msg_command_long_decode(&payload, new_msg); - ESP_LOGI(TAG, "Received command long with ID: %hu", payload.command); - db_process_mavlink_command(&payload, new_msg, fmav_status, buff, origin, tcp_clients, udp_conns); + // ESP32s that are connected to a flight controller via UART will send + // RADIO_STATUS messages to the GND + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA || + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_ESPNOW_AIR || + DB_PARAM_RADIO_MODE == DB_BLUETOOTH_MODE) { + // ToDo: For BLE only the last connected client is considered. + fmav_radio_status_t payload_r = { + .fixed = 0, + .txbuf = 0, + .noise = db_esp_signal_quality.gnd_noise_floor, + .remnoise = db_esp_signal_quality.air_noise_floor, + .remrssi = db_format_rssi(db_esp_signal_quality.air_rssi, + db_esp_signal_quality.air_noise_floor), + .rssi = db_format_rssi(db_esp_signal_quality.gnd_rssi, + db_esp_signal_quality.gnd_noise_floor), + .rxerrors = db_esp_signal_quality.gnd_rx_packets_lost + }; + 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); + db_send_to_all_clients(tcp_clients, udp_conns, buff, len); } - break; - case FASTMAVLINK_MSG_ID_PING: { - fmav_ping_t payload; - fmav_msg_ping_decode(&payload, new_msg); - payload.target_system = new_msg->sysid; - payload.target_component = new_msg->compid; - uint16_t len = fmav_msg_ping_encode_to_frame_buf(buff, db_get_mav_sys_id(), db_get_mav_comp_id(), &payload, fmav_status); - ESP_LOGD(TAG, "Answering MAVLink ping from System %i, Component %i", new_msg->sysid, new_msg->compid); - db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + else if(DB_PARAM_RADIO_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 + // -> We are in WiFi AP mode and connected to the drone + // Send each connected client a radio status packet. + // ToDo: Only the RSSI of the first (Wi-Fi) is considered. + // Easier for UDP since we have a nice list with mac addresses to + // use for mapping. Harder for TCP -> no MAC addresses available of + // connected clients + fmav_radio_status_t payload_r = { .fixed = UINT8_MAX, + .noise = UINT8_MAX, + .remnoise = UINT8_MAX, + .remrssi = db_format_rssi( + wifi_sta_list.sta[0].rssi, -88), + .rssi = UINT8_MAX, + .rxerrors = 0, + .txbuf = 0 }; + 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); + db_send_to_all_clients(tcp_clients, udp_conns, buff, len); } - break; - case FASTMAVLINK_MSG_ID_REQUEST_DATA_STREAM: { - fmav_request_data_stream_t data_stream_pay; - fmav_msg_request_data_stream_decode(&data_stream_pay, new_msg); - ESP_LOGW(TAG, "GCS requested data stream with ID: %i and rate: %i and start_stop: %i - ignoring!", - data_stream_pay.req_stream_id, data_stream_pay.req_message_rate, data_stream_pay.start_stop); - } - break; - default: { - if (new_msg->target_sysid == db_get_mav_sys_id() && new_msg->target_compid == db_get_mav_comp_id()) { - ESP_LOGW(TAG, "Received unknown MAVLink message ID: %u - ignoring", new_msg->msgid); - } - break; + else { + // In AP LR mode the clients will send the info to the GCS } + } + else { + // We do not react to any other heartbeat! + } + // 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 + 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: { + ESP_LOGI(TAG, + "Received PARAM_REQUEST_LIST msg. Responding with parameters"); + float_int_union float_int; + uint16_t len = 0; + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + switch(db_params[i]->type) { + case STRING: + // ignoring strings. Not supported with this request + continue; + break; + case UINT8: + float_int.uint8 = db_params[i]->value.db_param_u8.value; + break; + case UINT16: + float_int.uint16 = db_params[i]->value.db_param_u16.value; + break; + case INT32: + float_int.int32 = db_params[i]->value.db_param_i32.value; + break; + default: + ESP_LOGE( + TAG, + "db_param_write_all_params_json() -> db_parameter.type unknown!"); + break; + } + len = db_get_mavmsg_param_value(buff, + fmav_status, + db_params[i]->mav_t.param_index, + &float_int, + db_params[i]->mav_t.param_type, + (char *)db_params[i]->mav_t.param_name); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + } + } break; + case FASTMAVLINK_MSG_ID_PARAM_REQUEST_READ: { + fmav_param_request_read_t payload; + fmav_msg_param_request_read_decode(&payload, new_msg); + float_int_union float_int; + ESP_LOGI(TAG, + "GCS request reading parameter ID: %s with index %i", + payload.param_id, + payload.param_index); + MAV_PARAM_TYPE type = db_mav_get_parameter_value( + &float_int, payload.param_id, payload.param_index); + if(type != 0) { + uint16_t len = db_get_mavmsg_param_value(buff, + fmav_status, + payload.param_index, + &float_int, + type, + payload.param_id); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + } + else { + // send nothing, unknown parameter + ESP_LOGW(TAG, "\tParameter is unknown. Not responding!"); + } + } break; + case FASTMAVLINK_MSG_ID_PARAM_SET: { + fmav_param_set_t parame_set_payload; + fmav_msg_param_set_decode(¶me_set_payload, new_msg); + ESP_LOGI( + TAG, "GCS requested setting parameter %s", parame_set_payload.param_id); + if(db_write_mavlink_parameter(¶me_set_payload)) { + // Respond with parameter + float_int_union float_int; + MAV_PARAM_TYPE type = db_mav_get_parameter_value( + &float_int, parame_set_payload.param_id, -1); + if(type != 0) { + uint16_t len = db_get_mavmsg_param_value( + buff, fmav_status, 0, &float_int, type, parame_set_payload.param_id); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + db_write_settings_to_nvs(); + } + else { + ESP_LOGE( + TAG, + "Failed to get parameter %s - could not respond with new param", + parame_set_payload.param_id); + } + } + else { + ESP_LOGE(TAG, + "db_write_mavlink_parameter() failed to set new parameter %s ", + parame_set_payload.param_id); + } + } break; + case FASTMAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST: { + ESP_LOGI( + TAG, + "GCS requested reading ext parameters list - not supported for now!"); + // ToDo Respond to PARAM_EXT_REQUEST_LIST + } break; + case FASTMAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ: { + ESP_LOGI(TAG, + "GCS requested reading ext parameter - not supported for now!"); + // ToDo Respond to EXT_REQUEST_READ + } break; + case FASTMAVLINK_MSG_ID_COMMAND_LONG: { + fmav_command_long_t payload; + fmav_msg_command_long_decode(&payload, new_msg); + ESP_LOGI(TAG, "Received command long with ID: %hu", payload.command); + db_process_mavlink_command( + &payload, new_msg, fmav_status, buff, origin, tcp_clients, udp_conns); + } break; + case FASTMAVLINK_MSG_ID_PING: { + fmav_ping_t payload; + fmav_msg_ping_decode(&payload, new_msg); + payload.target_system = new_msg->sysid; + payload.target_component = new_msg->compid; + uint16_t len = fmav_msg_ping_encode_to_frame_buf( + buff, db_get_mav_sys_id(), db_get_mav_comp_id(), &payload, fmav_status); + ESP_LOGD(TAG, + "Answering MAVLink ping from System %i, Component %i", + new_msg->sysid, + new_msg->compid); + db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns); + } break; + case FASTMAVLINK_MSG_ID_REQUEST_DATA_STREAM: { + fmav_request_data_stream_t data_stream_pay; + fmav_msg_request_data_stream_decode(&data_stream_pay, new_msg); + ESP_LOGW(TAG, + "GCS requested data stream with ID: %i and rate: %i and " + "start_stop: %i - ignoring!", + data_stream_pay.req_stream_id, + data_stream_pay.req_message_rate, + data_stream_pay.start_stop); + } break; + default: { + if(new_msg->target_sysid == db_get_mav_sys_id() && + new_msg->target_compid == db_get_mav_comp_id()) { + ESP_LOGW(TAG, + "Received unknown MAVLink message ID: %u - ignoring", + new_msg->msgid); } + break; + } + } } \ No newline at end of file diff --git a/main/db_mavlink_msgs.h b/main/db_mavlink_msgs.h index de469b8..666c283 100644 --- a/main/db_mavlink_msgs.h +++ b/main/db_mavlink_msgs.h @@ -27,16 +27,22 @@ uint8_t db_get_mav_comp_id(); uint8_t db_get_mav_sys_id(); uint16_t db_create_heartbeat(uint8_t *buff, fmav_status_t *fmav_status); -uint16_t db_get_mavmsg_param_value(uint8_t *buff, fmav_status_t *fmav_status, uint16_t param_index, float_int_union *value, uint8_t type, char *param_id); -MAV_PARAM_TYPE db_mav_get_parameter_value(float_int_union *float_int, const char *param_id, const int16_t param_index); +uint16_t db_get_mavmsg_param_value(uint8_t *buff, fmav_status_t *fmav_status, + uint16_t param_index, + float_int_union *value, uint8_t type, + char *param_id); +MAV_PARAM_TYPE db_mav_get_parameter_value(float_int_union *float_int, + const char *param_id, + const int16_t param_index); bool db_write_mavlink_parameter(const fmav_param_set_t *param_set_payload); void db_process_mavlink_command(fmav_command_long_t *the_command, - fmav_message_t *the_msg, - fmav_status_t *status, + fmav_message_t *the_msg, fmav_status_t *status, uint8_t *buff, - enum DB_MAVLINK_DATA_ORIGIN origin, int *tcp_clients, udp_conn_list_t *udp_conns); -void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_list_t *udp_conns, + enum DB_MAVLINK_DATA_ORIGIN origin, + int *tcp_clients, udp_conn_list_t *udp_conns); +void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, + udp_conn_list_t *udp_conns, fmav_status_t *fmav_status, enum DB_MAVLINK_DATA_ORIGIN origin); -#endif //DB_ESP32_DB_MAVLINK_MSGS_H +#endif // DB_ESP32_DB_MAVLINK_MSGS_H diff --git a/main/db_parameters.c b/main/db_parameters.c index e508fe0..ef1cc9f 100644 --- a/main/db_parameters.c +++ b/main/db_parameters.c @@ -1,5 +1,5 @@ /* -* This file is part of DroneBridge:https://github.com/DroneBridge/ESP32 + * This file is part of DroneBridge:https://github.com/DroneBridge/ESP32 * * Copyright 2025 Wolfgang Christl * @@ -31,28 +31,36 @@ * 1. Add new parameter as db_parameter_t below * 2. Add new parameter to db_params array * 3. Increase DB_PARAM_TOTAL_NUM - * 4. Increase DB_MAV_PARAM_CNT if parameter is available via MAVLink (non-string parameter) - * 5. Make parameter available globally by adding extern definition into db_parameters.h - * 6. Create an optional macro in db_parameters.h to make the parameter value easy accessible + * 4. Increase DB_MAV_PARAM_CNT if parameter is available via MAVLink + * (non-string parameter) + * 5. Make parameter available globally by adding extern definition into + * db_parameters.h + * 6. Create an optional macro in db_parameters.h to make the parameter value + * easy accessible */ /** - * Introduced and used for settings change only since changing DB_WIFI_MODE directly while system is live will - * eventually lead to crashes. Stores the new Wi-Fi mode and will be written to the settings. This means it will + * Introduced and used for settings change only since changing DB_WIFI_MODE + * directly while system is live will eventually lead to crashes. Stores the + * new Wi-Fi mode and will be written to the settings. This means it will * become active after reboot. */ -uint8_t DB_RADIO_MODE_DESIGNATED = DB_WIFI_MODE_AP; // initially assign the same value as DB_RADIO_MODE +uint8_t DB_RADIO_MODE_DESIGNATED = + DB_WIFI_MODE_AP; // initially assign the same value as DB_RADIO_MODE /* ---------- String based parameters - not available via MAVLink ---------- */ -db_parameter_t db_param_ssid, db_param_pass, db_param_wifi_ap_ip, db_param_wifi_sta_ip, db_param_wifi_sta_gw, - db_param_wifi_sta_netmask, db_param_udp_client_ip, db_param_wifi_hostname = {0}; +db_parameter_t db_param_ssid, db_param_pass, db_param_wifi_ap_ip, + db_param_wifi_sta_ip, db_param_wifi_sta_gw, db_param_wifi_sta_netmask, + db_param_udp_client_ip, db_param_wifi_hostname = { 0 }; -/* ---------- From here with increasing param_index all parameters that are also available via MAVLink ---------- */ +/* ---------- From here with increasing param_index all parameters that are + * also available via MAVLink ---------- */ /** - * never change this value while the ESP32 is running, will likely lead to a crash. - * Assign it during startup when received from storage. Can therefore only be changed via reboot and DB_WIFI_MODE_DESIGNATED + * never change this value while the ESP32 is running, will likely lead to a + * crash. Assign it during startup when received from storage. Can therefore + * only be changed via reboot and DB_WIFI_MODE_DESIGNATED */ db_parameter_t db_param_radio_mode = { .db_name = "esp32_mode", @@ -95,8 +103,8 @@ db_parameter_t db_param_channel = { /** * Allow the usage of 802.11bgn mode - * disabled: only 802.11b support for client mode - set to true: 802.11b/g/n/ax mode support. - * ax only when chip supports it. + * disabled: only 802.11b support for client mode - set to true: + * 802.11b/g/n/ax mode support. ax only when chip supports it. */ db_parameter_t db_param_wifi_en_gn = { .db_name = "wifi_en_gn", @@ -117,8 +125,8 @@ db_parameter_t db_param_wifi_en_gn = { }; /** - * Set to 1 to use external antenna. Set to 0 to enable onboard antenna - board must have antenna switch - * Official ESP32C6 board supports this option. + * Set to 1 to use external antenna. Set to 0 to enable onboard antenna - board + * must have antenna switch Official ESP32C6 board supports this option. */ db_parameter_t db_param_radio_ant_ext = { .db_name = "ant_use_ext", @@ -202,7 +210,8 @@ db_parameter_t db_param_gpio_rx = { }; /** - * RTS GPIO number of the UART. Set to same value as CTS GPIO to disable flow control. + * RTS GPIO number of the UART. Set to same value as CTS GPIO to disable flow + * control. */ db_parameter_t db_param_gpio_rts = { .db_name = "gpio_rts", @@ -223,7 +232,8 @@ db_parameter_t db_param_gpio_rts = { }; /** - * CTS GPIO number of the UART. Set to same value as RTS GPIO to disable flow control. + * CTS GPIO number of the UART. Set to same value as RTS GPIO to disable flow + * control. */ db_parameter_t db_param_gpio_cts = { .db_name = "gpio_cts", @@ -244,7 +254,8 @@ db_parameter_t db_param_gpio_cts = { }; /** - * I really don't know. Maybe some sort of timeout. Just leave it at the defaults. + * I really don't know. Maybe some sort of timeout. Just leave it at the + * defaults. */ db_parameter_t db_param_gpio_rts_thresh = { .db_name = "rts_thresh", @@ -286,7 +297,8 @@ db_parameter_t db_param_proto = { }; /** - * Maximum packet size via ESP-NOW or WiFi in transparent or mavlink mode. Caped to 250 bytes-HEADER in ESP-NOW mode. + * Maximum packet size via ESP-NOW or WiFi in transparent or mavlink mode. + * Caped to 250 bytes-HEADER in ESP-NOW mode. */ db_parameter_t db_param_serial_pack_size = { .db_name = "trans_pack_size", @@ -307,8 +319,9 @@ db_parameter_t db_param_serial_pack_size = { }; /** - * Serial read timeout [ms] for transparent and MAVLink mode, after that the packet will be sent over the air even when - * the max. packet size was not reached. + * Serial read timeout [ms] for transparent and MAVLink mode, after that the + * packet will be sent over the air even when the max. packet size was not + * reached. */ db_parameter_t db_param_serial_read_timeout = { .db_name = "serial_timeout", @@ -350,7 +363,8 @@ db_parameter_t db_param_ltm_per_packet = { }; /** - * Detects armed state from MAVLink & LTM stream and disables radio when autopilot reports armed state. + * Detects armed state from MAVLink & LTM stream and disables radio when + * autopilot reports armed state. */ db_parameter_t db_param_dis_radio_armed = { .db_name = "radio_dis_onarm", @@ -394,7 +408,8 @@ db_parameter_t db_param_udp_client_port = { /** * Format/Unit of the reported RSSI as part of MAVLink RADIO STATUS message. * If set to true (1) the RSSI will be reported as dBm (QGC) - * If set to false (0) the RSSI will be calculated as a value from 0 to 100 (MissionPlanner) + * If set to false (0) the RSSI will be calculated as a value from 0 to 100 + * (MissionPlanner) */ db_parameter_t db_param_rssi_dbm = { .db_name = "rep_rssi_dbm", @@ -415,9 +430,10 @@ db_parameter_t db_param_rssi_dbm = { }; /** - * Array containing all references to the DB parameters assigned with db_param_init_parameters() + * Array containing all references to the DB parameters assigned with + * db_param_init_parameters() */ -db_parameter_t *db_params[DB_PARAM_TOTAL_NUM] = {NULL}; +db_parameter_t *db_params[DB_PARAM_TOTAL_NUM] = { NULL }; /** * Leaks memory. Only call during init of the parameters. @@ -426,12 +442,16 @@ db_parameter_t *db_params[DB_PARAM_TOTAL_NUM] = {NULL}; * @param mav_param_name MAVLink parameter name * @param default_value Default value * @param min_val_len minimum length of the string (used for validation) - * @param max_val_len maximum length of the string (used for memory allocation and validation) + * @param max_val_len maximum length of the string (used for memory allocation + * and validation) * @return The inited string parameter */ -db_parameter_t db_param_init_str_param(char *db_name, char *mav_param_name, const char *default_value, - uint8_t min_val_len, uint8_t max_val_len) { - db_parameter_t db_str_param = { +db_parameter_t +db_param_init_str_param(char *db_name, char *mav_param_name, + const char *default_value, uint8_t min_val_len, + uint8_t max_val_len) +{ + db_parameter_t db_str_param = { .db_name = "", .type = STRING, .mav_t = { @@ -446,369 +466,510 @@ db_parameter_t db_param_init_str_param(char *db_name, char *mav_param_name, cons } } }; - strncpy((char *) db_str_param.db_name, db_name, DB_PARAM_NAME_MAXLEN-1); - uint mav_param_name_str_len = strlen(mav_param_name); - if (mav_param_name_str_len < DB_PARAM_MAX_MAV_PARAM_NAME_LEN) { - // normal case - param name and string terminator fit into allocated memory - strncpy((char *) db_str_param.mav_t.param_name, mav_param_name, DB_PARAM_MAX_MAV_PARAM_NAME_LEN - 1); - } else { - // mav param name is allowed to be 16 chars long - it is valid without a string terminator if len = 16 - // copy the first 16 chars of string without a string terminator - memcpy((char *) db_str_param.mav_t.param_name, mav_param_name, DB_PARAM_MAX_MAV_PARAM_NAME_LEN); - } - db_str_param.value.db_param_str.default_value = (uint8_t *) strdup(default_value); - db_str_param.value.db_param_str.value = malloc(max_val_len); - if (db_str_param.value.db_param_str.value == NULL) { - ESP_LOGE(TAG, "Error allocating %i bytes for string parameter %s", max_val_len, db_name); - } else { - // all good - init value to default value - strncpy((char *) db_str_param.value.db_param_str.value, (char *) db_str_param.value.db_param_str.default_value, max_val_len); - // Ensure null termination, especially if default_value is >= max_val_len. For password this might not be necessary - db_str_param.value.db_param_str.value[max_val_len - 1] = '\0'; - ESP_LOGD(TAG, "Initialized value for %s with default '%s'", db_name, (char*)db_str_param.value.db_param_str.value); - } - return db_str_param; + strncpy((char *)db_str_param.db_name, db_name, DB_PARAM_NAME_MAXLEN - 1); + uint mav_param_name_str_len = strlen(mav_param_name); + if(mav_param_name_str_len < DB_PARAM_MAX_MAV_PARAM_NAME_LEN) { + // normal case - param name and string terminator fit into allocated memory + strncpy((char *)db_str_param.mav_t.param_name, + mav_param_name, + DB_PARAM_MAX_MAV_PARAM_NAME_LEN - 1); + } + else { + // mav param name is allowed to be 16 chars long - it is valid without a + // string terminator if len = 16 copy the first 16 chars of string without + // a string terminator + memcpy((char *)db_str_param.mav_t.param_name, + mav_param_name, + DB_PARAM_MAX_MAV_PARAM_NAME_LEN); + } + db_str_param.value.db_param_str.default_value = + (uint8_t *)strdup(default_value); + db_str_param.value.db_param_str.value = malloc(max_val_len); + if(db_str_param.value.db_param_str.value == NULL) { + ESP_LOGE(TAG, + "Error allocating %i bytes for string parameter %s", + max_val_len, + db_name); + } + else { + // all good - init value to default value + strncpy((char *)db_str_param.value.db_param_str.value, + (char *)db_str_param.value.db_param_str.default_value, + max_val_len); + // Ensure null termination, especially if default_value is >= max_val_len. + // For password this might not be necessary + db_str_param.value.db_param_str.value[max_val_len - 1] = '\0'; + ESP_LOGD(TAG, + "Initialized value for %s with default '%s'", + db_name, + (char *)db_str_param.value.db_param_str.value); + } + return db_str_param; } /** - * Initializes the parameters used for storing user settings. All parameters are collected in an array. - * Leaks memory. Only call once during startup! - * Add new parameters here! - */ -void db_param_init_parameters() { - // Wi-Fi AP SSID name OR Wi-Fi AP SSID name to connect to in Wi-Fi client mode - db_param_ssid = db_param_init_str_param("ssid", "SYS_SSID", "DroneBridge for ESP32", 1, MAX_SSID_LEN); - // Password for Wi-Fi connections & ESP-NOW encryption. - db_param_pass = db_param_init_str_param("wifi_pass", "SYS_PASS", "dronebridge", 7, 64); - // IPv4 of the Wi-Fi access point when in Wi-Fi AP mode - db_param_wifi_ap_ip = db_param_init_str_param("ap_ip", "WIFI_AP_IP", "192.168.2.1", 8, IP4ADDR_STRLEN_MAX); - // User can specify static IP when in Wi-Fi client mode. If this is empty use auto IP. - db_param_wifi_sta_ip = db_param_init_str_param("ip_sta", "WIFI_STA_IP", "", 0, IP4ADDR_STRLEN_MAX); - // If db_param_wifi_sta_ip is set then this must be set to the gateway IP - db_param_wifi_sta_gw = db_param_init_str_param("ip_sta_gw", "WIFI_STA_GW", "", 0, IP4ADDR_STRLEN_MAX); - // If db_param_wifi_sta_ip is set: Netmask when settings static IP in Wi-Fi client mode - db_param_wifi_sta_netmask = db_param_init_str_param("ip_sta_netmsk", "WIFI_STA_NETM", "", 0, IP4ADDR_STRLEN_MAX); - // Users can add custom UDP client targets. This is the IP of the first target added. Only the first one is saved to NVM. - db_param_udp_client_ip = db_param_init_str_param("udp_client_ip", "WIFI_UDP_IP", "", 0, IP4ADDR_STRLEN_MAX); - // Specifies the hostname. Used in Wi-Fi ap & client mode. - db_param_wifi_hostname = db_param_init_str_param("wifi_hostname", "WIFI_HOSTNAME", CONFIG_LWIP_LOCAL_HOSTNAME, 1, 32); - - db_parameter_t *db_params_l[] = { - &db_param_ssid, - &db_param_pass, - &db_param_wifi_ap_ip, - &db_param_wifi_sta_ip, - &db_param_wifi_sta_gw, - &db_param_wifi_sta_netmask, - &db_param_udp_client_ip, - &db_param_wifi_hostname, - &db_param_radio_mode, - &db_param_channel, - &db_param_wifi_en_gn, - &db_param_radio_ant_ext, - &db_param_baud, - &db_param_gpio_tx, - &db_param_gpio_rx, - &db_param_gpio_rts, - &db_param_gpio_cts, - &db_param_gpio_rts_thresh, - &db_param_proto, - &db_param_serial_pack_size, - &db_param_serial_read_timeout, - &db_param_ltm_per_packet, - &db_param_dis_radio_armed, - &db_param_udp_client_port, - &db_param_rssi_dbm - }; - memcpy(db_params, db_params_l, sizeof(db_params_l)); + * Initializes the parameters used for storing user settings. All parameters + * are collected in an array. Leaks memory. Only call once during startup! Add + * new parameters here! + */ +void +db_param_init_parameters() +{ + // Wi-Fi AP SSID name OR Wi-Fi AP SSID name to connect to in Wi-Fi client + // mode + db_param_ssid = db_param_init_str_param( + "ssid", "SYS_SSID", "DroneBridge for ESP32", 1, MAX_SSID_LEN); + // Password for Wi-Fi connections & ESP-NOW encryption. + db_param_pass = + db_param_init_str_param("wifi_pass", "SYS_PASS", "dronebridge", 7, 64); + // IPv4 of the Wi-Fi access point when in Wi-Fi AP mode + db_param_wifi_ap_ip = db_param_init_str_param( + "ap_ip", "WIFI_AP_IP", "192.168.2.1", 8, IP4ADDR_STRLEN_MAX); + // User can specify static IP when in Wi-Fi client mode. If this is empty use + // auto IP. + db_param_wifi_sta_ip = db_param_init_str_param( + "ip_sta", "WIFI_STA_IP", "", 0, IP4ADDR_STRLEN_MAX); + // If db_param_wifi_sta_ip is set then this must be set to the gateway IP + db_param_wifi_sta_gw = db_param_init_str_param( + "ip_sta_gw", "WIFI_STA_GW", "", 0, IP4ADDR_STRLEN_MAX); + // If db_param_wifi_sta_ip is set: Netmask when settings static IP in Wi-Fi + // client mode + db_param_wifi_sta_netmask = db_param_init_str_param( + "ip_sta_netmsk", "WIFI_STA_NETM", "", 0, IP4ADDR_STRLEN_MAX); + // Users can add custom UDP client targets. This is the IP of the first + // target added. Only the first one is saved to NVM. + db_param_udp_client_ip = db_param_init_str_param( + "udp_client_ip", "WIFI_UDP_IP", "", 0, IP4ADDR_STRLEN_MAX); + // Specifies the hostname. Used in Wi-Fi ap & client mode. + db_param_wifi_hostname = db_param_init_str_param( + "wifi_hostname", "WIFI_HOSTNAME", CONFIG_LWIP_LOCAL_HOSTNAME, 1, 32); + + db_parameter_t *db_params_l[] = { &db_param_ssid, + &db_param_pass, + &db_param_wifi_ap_ip, + &db_param_wifi_sta_ip, + &db_param_wifi_sta_gw, + &db_param_wifi_sta_netmask, + &db_param_udp_client_ip, + &db_param_wifi_hostname, + &db_param_radio_mode, + &db_param_channel, + &db_param_wifi_en_gn, + &db_param_radio_ant_ext, + &db_param_baud, + &db_param_gpio_tx, + &db_param_gpio_rx, + &db_param_gpio_rts, + &db_param_gpio_cts, + &db_param_gpio_rts_thresh, + &db_param_proto, + &db_param_serial_pack_size, + &db_param_serial_read_timeout, + &db_param_ltm_per_packet, + &db_param_dis_radio_armed, + &db_param_udp_client_port, + &db_param_rssi_dbm }; + memcpy(db_params, db_params_l, sizeof(db_params_l)); } /** * Sets the value of the supplied the parameter to its default value * @param db_parameter The parameter to reset to default */ -void db_param_set_to_default(db_parameter_t *db_parameter) { - switch (db_parameter->type) { - case STRING: - strncpy((char *) db_parameter->value.db_param_str.value, - (char *) db_parameter->value.db_param_str.default_value, - db_parameter->value.db_param_str.max_len); - db_parameter->value.db_param_str.value[db_parameter->value.db_param_str.max_len - 1] = '\0'; - break; - case UINT8: - db_parameter->value.db_param_u8.value = db_parameter->value.db_param_u8.default_value; - break; - case UINT16: - db_parameter->value.db_param_u16.value = db_parameter->value.db_param_u16.default_value; - break; - case INT32: - db_parameter->value.db_param_i32.value = db_parameter->value.db_param_i32.default_value; - break; - default: - ESP_LOGE(TAG, "db_param_set_to_default() -> db_parameter.type unknown!"); - break; - } +void +db_param_set_to_default(db_parameter_t *db_parameter) +{ + switch(db_parameter->type) { + case STRING: + strncpy((char *)db_parameter->value.db_param_str.value, + (char *)db_parameter->value.db_param_str.default_value, + db_parameter->value.db_param_str.max_len); + db_parameter->value.db_param_str + .value[db_parameter->value.db_param_str.max_len - 1] = '\0'; + break; + case UINT8: + db_parameter->value.db_param_u8.value = + db_parameter->value.db_param_u8.default_value; + break; + case UINT16: + db_parameter->value.db_param_u16.value = + db_parameter->value.db_param_u16.default_value; + break; + case INT32: + db_parameter->value.db_param_i32.value = + db_parameter->value.db_param_i32.default_value; + break; + default: + ESP_LOGE(TAG, "db_param_set_to_default() -> db_parameter.type unknown!"); + break; + } } /** - * Helper function to reset all known parameters to their defaults. Parameter must be part of db_params array + * Helper function to reset all known parameters to their defaults. Parameter + * must be part of db_params array */ -void db_param_reset_all() { - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - db_param_set_to_default(db_params[i]); - } +void +db_param_reset_all() +{ + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + db_param_set_to_default(db_params[i]); + } } /** - * Helper function to convert all parameters with their values to a string buffer for logging etc. - * @param str_buffer Buffer to write the parameter string - must be long enough ~512 bytes - */ -int db_param_print_values_to_buffer(uint8_t *str_buffer) { - int str_len = 0; // overall length of the string in the str_buffer - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - uint8_t param_str_buf[128]; // buffer for the string of a single value - switch (db_params[i]->type) { - case STRING: - str_len += sprintf((char *) param_str_buf, "\t%s: %s\n", (char *) db_params[i]->db_name, - (char *) db_params[i]->value.db_param_str.value); - break; - case UINT8: - str_len += sprintf((char *) param_str_buf, "\t%s: %i\n", (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u8.value); - break; - case UINT16: - str_len += sprintf((char *) param_str_buf, "\t%s: %i\n", (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u16.value); - break; - case INT32: - str_len += sprintf((char *) param_str_buf, "\t%s: %li\n", (char *) db_params[i]->db_name, - db_params[i]->value.db_param_i32.value); - break; - default: - ESP_LOGE(TAG, "db_param_print_values_to_buffer() -> db_parameter.type unknown!"); - break; - } - strcat((char *) str_buffer, - (char *) param_str_buf); // add the string of the individual printed param to the big buffer + * Helper function to convert all parameters with their values to a string + * buffer for logging etc. + * @param str_buffer Buffer to write the parameter string - must be long enough + * ~512 bytes + */ +int +db_param_print_values_to_buffer(uint8_t *str_buffer) +{ + int str_len = 0; // overall length of the string in the str_buffer + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + uint8_t param_str_buf[128]; // buffer for the string of a single value + switch(db_params[i]->type) { + case STRING: + str_len += sprintf((char *)param_str_buf, + "\t%s: %s\n", + (char *)db_params[i]->db_name, + (char *)db_params[i]->value.db_param_str.value); + break; + case UINT8: + str_len += sprintf((char *)param_str_buf, + "\t%s: %i\n", + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u8.value); + break; + case UINT16: + str_len += sprintf((char *)param_str_buf, + "\t%s: %i\n", + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u16.value); + break; + case INT32: + str_len += sprintf((char *)param_str_buf, + "\t%s: %li\n", + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_i32.value); + break; + default: + ESP_LOGE( + TAG, + "db_param_print_values_to_buffer() -> db_parameter.type unknown!"); + break; } - return str_len; + strcat((char *)str_buffer, + (char *)param_str_buf); // add the string of the individual printed + // param to the big buffer + } + return str_len; } /** - * Helper function to read a string from the NVS based on a key. Handles errors accordingly and print result to console + * Helper function to read a string from the NVS based on a key. Handles errors + * accordingly and print result to console * @param my_handle nvs_handle to use * @param key NVS key as string with max length NVS_KEY_NAME_MAX_SIZE-1 * @param dst Destination for the read value */ -void db_read_str_nvs(nvs_handle_t my_handle, db_parameter_t *db_param) { - size_t required_size = 0; - // First call to get size - esp_err_t err = nvs_get_str(my_handle, (char *) db_param->db_name, NULL, &required_size); - if (err == ESP_OK) { - if (required_size > db_param->value.db_param_str.max_len) { - ESP_LOGW(TAG, "NVS string for %s too long (%d > %d), using default.", - (char *) db_param->db_name, required_size, db_param->value.db_param_str.max_len); - db_param_set_to_default(db_param); // Use default if stored value too big - } else if (required_size == 0) { - // Handle case where key exists but string is empty (e.g., saved empty password) - ESP_LOGI(TAG, "NVS string for %s is empty, using default.", (char *) db_param->db_name); - db_param_set_to_default(db_param); - } else { - // Read the actual value - err = nvs_get_str(my_handle, (char *) db_param->db_name, (char *)db_param->value.db_param_str.value, &required_size); - if (err != ESP_OK) { - ESP_LOGE(TAG, "Error (%s) reading string %s from NVS! Using default.", esp_err_to_name(err), (char *) db_param->db_name); - db_param_set_to_default(db_param); - } else { - ESP_LOGD(TAG, "Read %s from NVS: '%s'", (char *) db_param->db_name, (char *)db_param->value.db_param_str.value); - } - } - } else if (err == ESP_ERR_NVS_NOT_FOUND) { - // *** THIS IS THE CRITICAL PART *** - ESP_LOGI(TAG, "Parameter %s not found in NVS, using default.", (char *) db_param->db_name); - db_param_set_to_default(db_param); // Apply the default value - } else { - ESP_LOGE(TAG, "Error (%s) checking NVS for %s! Using default.", esp_err_to_name(err), (char *) db_param->db_name); - db_param_set_to_default(db_param); // Also use default on other errors +void +db_read_str_nvs(nvs_handle_t my_handle, db_parameter_t *db_param) +{ + size_t required_size = 0; + // First call to get size + esp_err_t err = + nvs_get_str(my_handle, (char *)db_param->db_name, NULL, &required_size); + if(err == ESP_OK) { + if(required_size > db_param->value.db_param_str.max_len) { + ESP_LOGW(TAG, + "NVS string for %s too long (%d > %d), using default.", + (char *)db_param->db_name, + required_size, + db_param->value.db_param_str.max_len); + db_param_set_to_default(db_param); // Use default if stored value too big } + else if(required_size == 0) { + // Handle case where key exists but string is empty (e.g., saved empty + // password) + ESP_LOGI(TAG, + "NVS string for %s is empty, using default.", + (char *)db_param->db_name); + db_param_set_to_default(db_param); + } + else { + // Read the actual value + err = nvs_get_str(my_handle, + (char *)db_param->db_name, + (char *)db_param->value.db_param_str.value, + &required_size); + if(err != ESP_OK) { + ESP_LOGE(TAG, + "Error (%s) reading string %s from NVS! Using default.", + esp_err_to_name(err), + (char *)db_param->db_name); + db_param_set_to_default(db_param); + } + else { + ESP_LOGD(TAG, + "Read %s from NVS: '%s'", + (char *)db_param->db_name, + (char *)db_param->value.db_param_str.value); + } + } + } + else if(err == ESP_ERR_NVS_NOT_FOUND) { + // *** THIS IS THE CRITICAL PART *** + ESP_LOGI(TAG, + "Parameter %s not found in NVS, using default.", + (char *)db_param->db_name); + db_param_set_to_default(db_param); // Apply the default value + } + else { + ESP_LOGE(TAG, + "Error (%s) checking NVS for %s! Using default.", + esp_err_to_name(err), + (char *)db_param->db_name); + db_param_set_to_default(db_param); // Also use default on other errors + } } /** * Updates all parameters with their values from the NVM. * Parameters must be part of db_params array - * Checks validity of read parameter and if not valid assigns default value to it. + * Checks validity of read parameter and if not valid assigns default value to + * it. * @param nvs_handle Opened Namespace of the NVM partition. This is the handle. */ -void db_param_read_all_params_nvs(const nvs_handle_t *nvs_handle) { - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - switch (db_params[i]->type) { - case STRING: - db_read_str_nvs(*nvs_handle, db_params[i]); - if (!db_param_is_valid_str((char *) db_params[i]->value.db_param_str.value, db_params[i])) { - // read parameter is not valid - overwrite NVS value with default value - strncpy((char *) db_params[i]->value.db_param_str.value, - (char *) db_params[i]->value.db_param_str.default_value, - db_params[i]->value.db_param_str.max_len); - ESP_LOGW(TAG, "Read invalid parameter %s from NVS, setting to default value %s", - db_params[i]->db_name, db_params[i]->value.db_param_str.default_value); - } - break; - case UINT8: - ESP_ERROR_CHECK_WITHOUT_ABORT( - nvs_get_u8(*nvs_handle, (char *) db_params[i]->db_name, - &db_params[i]->value.db_param_u8.value)); - if (!db_param_is_valid_u8(db_params[i]->value.db_param_u8.value, db_params[i])) { - // read parameter is not valid - overwrite NVS value with default value - db_params[i]->value.db_param_u8.value = db_params[i]->value.db_param_u8.default_value; - ESP_LOGW(TAG, "Read invalid parameter %s from NVS, setting to default value %i", - db_params[i]->db_name, db_params[i]->value.db_param_u8.default_value); - } - break; - case UINT16: - ESP_ERROR_CHECK_WITHOUT_ABORT( - nvs_get_u16(*nvs_handle, (char *) db_params[i]->db_name, - &db_params[i]->value.db_param_u16.value)); - if (!db_param_is_valid_u16(db_params[i]->value.db_param_u16.value, db_params[i])) { - // read parameter is not valid - overwrite NVS value with default value - db_params[i]->value.db_param_u16.value = db_params[i]->value.db_param_u16.default_value; - ESP_LOGW(TAG, "Read invalid parameter %s from NVS, setting to default value %i", - db_params[i]->db_name, db_params[i]->value.db_param_u16.default_value); - } - break; - case INT32: - ESP_ERROR_CHECK_WITHOUT_ABORT( - nvs_get_i32(*nvs_handle, (char *) db_params[i]->db_name, - &db_params[i]->value.db_param_i32.value)); - if (!db_param_is_valid_i32(db_params[i]->value.db_param_i32.value, db_params[i])) { - // read parameter is not valid - overwrite NVS value with default value - db_params[i]->value.db_param_i32.value = db_params[i]->value.db_param_i32.default_value; - ESP_LOGW(TAG, "Read invalid parameter %s from NVS, setting to default value %li", - db_params[i]->db_name, db_params[i]->value.db_param_i32.default_value); - } - break; - default: - ESP_LOGE(TAG, "db_param_read_all_params_to_nvs() -> db_parameter.type unknown!"); - break; - } +void +db_param_read_all_params_nvs(const nvs_handle_t *nvs_handle) +{ + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + switch(db_params[i]->type) { + case STRING: + db_read_str_nvs(*nvs_handle, db_params[i]); + if(!db_param_is_valid_str((char *)db_params[i]->value.db_param_str.value, + db_params[i])) { + // read parameter is not valid - overwrite NVS value with default value + strncpy((char *)db_params[i]->value.db_param_str.value, + (char *)db_params[i]->value.db_param_str.default_value, + db_params[i]->value.db_param_str.max_len); + ESP_LOGW( + TAG, + "Read invalid parameter %s from NVS, setting to default value %s", + db_params[i]->db_name, + db_params[i]->value.db_param_str.default_value); + } + break; + case UINT8: + ESP_ERROR_CHECK_WITHOUT_ABORT( + nvs_get_u8(*nvs_handle, + (char *)db_params[i]->db_name, + &db_params[i]->value.db_param_u8.value)); + if(!db_param_is_valid_u8(db_params[i]->value.db_param_u8.value, + db_params[i])) { + // read parameter is not valid - overwrite NVS value with default value + db_params[i]->value.db_param_u8.value = + db_params[i]->value.db_param_u8.default_value; + ESP_LOGW( + TAG, + "Read invalid parameter %s from NVS, setting to default value %i", + db_params[i]->db_name, + db_params[i]->value.db_param_u8.default_value); + } + break; + case UINT16: + ESP_ERROR_CHECK_WITHOUT_ABORT( + nvs_get_u16(*nvs_handle, + (char *)db_params[i]->db_name, + &db_params[i]->value.db_param_u16.value)); + if(!db_param_is_valid_u16(db_params[i]->value.db_param_u16.value, + db_params[i])) { + // read parameter is not valid - overwrite NVS value with default value + db_params[i]->value.db_param_u16.value = + db_params[i]->value.db_param_u16.default_value; + ESP_LOGW( + TAG, + "Read invalid parameter %s from NVS, setting to default value %i", + db_params[i]->db_name, + db_params[i]->value.db_param_u16.default_value); + } + break; + case INT32: + ESP_ERROR_CHECK_WITHOUT_ABORT( + nvs_get_i32(*nvs_handle, + (char *)db_params[i]->db_name, + &db_params[i]->value.db_param_i32.value)); + if(!db_param_is_valid_i32(db_params[i]->value.db_param_i32.value, + db_params[i])) { + // read parameter is not valid - overwrite NVS value with default value + db_params[i]->value.db_param_i32.value = + db_params[i]->value.db_param_i32.default_value; + ESP_LOGW( + TAG, + "Read invalid parameter %s from NVS, setting to default value %li", + db_params[i]->db_name, + db_params[i]->value.db_param_i32.default_value); + } + break; + default: + ESP_LOGE( + TAG, + "db_param_read_all_params_to_nvs() -> db_parameter.type unknown!"); + break; } + } } /** - * Writes all parameters with their values to the NVM. Makes an exception for the mode. - * Here DB_RADIO_MODE_DESIGNATED is saved instead. - * Parameters must be part of db_params array + * Writes all parameters with their values to the NVM. Makes an exception for + * the mode. Here DB_RADIO_MODE_DESIGNATED is saved instead. Parameters must be + * part of db_params array * @param nvs_handle Opened Namespace of the NVM partition. This is the handle. */ -void db_param_write_all_params_nvs(const nvs_handle_t *nvs_handle) { - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - switch (db_params[i]->type) { - case STRING: - ESP_ERROR_CHECK(nvs_set_str(*nvs_handle, (char *) db_params[i]->db_name, - (char *) db_params[i]->value.db_param_str.value)); - break; - case UINT8: - if (strcmp((char *) db_params[i]->db_name, (char *) db_param_radio_mode.db_name) == 0) { - // This is different. User writes the desired mode into DB_RADIO_MODE_DESIGNATED and does not overwrite the value - ESP_ERROR_CHECK(nvs_set_u8(*nvs_handle, (char *) db_params[i]->db_name, DB_RADIO_MODE_DESIGNATED)); - } else { - ESP_ERROR_CHECK( - nvs_set_u8(*nvs_handle, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u8.value)); - } - break; - case UINT16: - ESP_ERROR_CHECK(nvs_set_u16(*nvs_handle, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u16.value)); - break; - case INT32: - ESP_ERROR_CHECK(nvs_set_i32(*nvs_handle, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_i32.value)); - break; - default: - ESP_LOGE(TAG, "db_param_write_all_params_to_nvs() -> db_parameter.type unknown!"); - break; - } +void +db_param_write_all_params_nvs(const nvs_handle_t *nvs_handle) +{ + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + switch(db_params[i]->type) { + case STRING: + ESP_ERROR_CHECK( + nvs_set_str(*nvs_handle, + (char *)db_params[i]->db_name, + (char *)db_params[i]->value.db_param_str.value)); + break; + case UINT8: + if(strcmp((char *)db_params[i]->db_name, + (char *)db_param_radio_mode.db_name) == 0) { + // This is different. User writes the desired mode into + // DB_RADIO_MODE_DESIGNATED and does not overwrite the value + ESP_ERROR_CHECK(nvs_set_u8(*nvs_handle, + (char *)db_params[i]->db_name, + DB_RADIO_MODE_DESIGNATED)); + } + else { + ESP_ERROR_CHECK(nvs_set_u8(*nvs_handle, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u8.value)); + } + break; + case UINT16: + ESP_ERROR_CHECK(nvs_set_u16(*nvs_handle, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u16.value)); + break; + case INT32: + ESP_ERROR_CHECK(nvs_set_i32(*nvs_handle, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_i32.value)); + break; + default: + ESP_LOGE( + TAG, + "db_param_write_all_params_to_nvs() -> db_parameter.type unknown!"); + break; } + } } /** - * Parses the provided json for all known parameters. Applies all recognized parameter values to local storage. - * Checks if parameter values are within the valid range. Otherwise, reject by not applying the new value. - * Does not save them to NVM! - * @param root_obj JSON that contains a single layer with the parameters to change. - */ -void db_param_read_all_params_json(const cJSON *root_obj) { - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - cJSON *jobject = cJSON_GetObjectItem(root_obj, (char *) db_params[i]->db_name); - switch (db_params[i]->type) { - case STRING: - if (jobject) { - if (!cJSON_IsNull(jobject)) { - db_param_is_valid_assign_str(jobject->valuestring, db_params[i]); - } else { - // received empty string - db_params[i]->value.db_param_str.value[0] = '\0'; - } - } else { - // do nothing - param was not found in the json - } - break; - case UINT8: - if (jobject) { - db_param_is_valid_assign_u8(jobject->valueint, db_params[i]); - } else { - // do nothing - param was not found in the json - } - break; - case UINT16: - if (jobject) { - db_param_is_valid_assign_u16(jobject->valueint, db_params[i]); - } else { - // do nothing - param was not found in the json - } - break; - case INT32: - if (jobject) { - db_param_is_valid_assign_i32(jobject->valueint, db_params[i]); - } else { - // do nothing - param was not found in the json - } - break; - default: - ESP_LOGE(TAG, "db_param_write_all_params_to_nvs() -> db_parameter.type unknown!"); - break; + * Parses the provided json for all known parameters. Applies all recognized + * parameter values to local storage. Checks if parameter values are within the + * valid range. Otherwise, reject by not applying the new value. Does not save + * them to NVM! + * @param root_obj JSON that contains a single layer with the parameters to + * change. + */ +void +db_param_read_all_params_json(const cJSON *root_obj) +{ + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + cJSON *jobject = + cJSON_GetObjectItem(root_obj, (char *)db_params[i]->db_name); + switch(db_params[i]->type) { + case STRING: + if(jobject) { + if(!cJSON_IsNull(jobject)) { + db_param_is_valid_assign_str(jobject->valuestring, db_params[i]); } + else { + // received empty string + db_params[i]->value.db_param_str.value[0] = '\0'; + } + } + else { + // do nothing - param was not found in the json + } + break; + case UINT8: + if(jobject) { + db_param_is_valid_assign_u8(jobject->valueint, db_params[i]); + } + else { + // do nothing - param was not found in the json + } + break; + case UINT16: + if(jobject) { + db_param_is_valid_assign_u16(jobject->valueint, db_params[i]); + } + else { + // do nothing - param was not found in the json + } + break; + case INT32: + if(jobject) { + db_param_is_valid_assign_i32(jobject->valueint, db_params[i]); + } + else { + // do nothing - param was not found in the json + } + break; + default: + ESP_LOGE( + TAG, + "db_param_write_all_params_to_nvs() -> db_parameter.type unknown!"); + break; } + } } /** * Writes all known parameters to a supplied cJSON. * @param root_obj A cJSON object that will be filled with all known parameters */ -void db_param_write_all_params_json(cJSON *root_obj) { - for (int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { - switch (db_params[i]->type) { - case STRING: - cJSON_AddStringToObject(root_obj, (char *) db_params[i]->db_name, - (char *) db_params[i]->value.db_param_str.value); - break; - case UINT8: - cJSON_AddNumberToObject(root_obj, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u8.value); - break; - case UINT16: - cJSON_AddNumberToObject(root_obj, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_u16.value); - break; - case INT32: - cJSON_AddNumberToObject(root_obj, (char *) db_params[i]->db_name, - db_params[i]->value.db_param_i32.value); - break; - default: - ESP_LOGE(TAG, "db_param_write_all_params_json() -> db_parameter.type unknown!"); - break; - } +void +db_param_write_all_params_json(cJSON *root_obj) +{ + for(int i = 0; i < sizeof(db_params) / sizeof(db_params[0]); i++) { + switch(db_params[i]->type) { + case STRING: + cJSON_AddStringToObject(root_obj, + (char *)db_params[i]->db_name, + (char *)db_params[i]->value.db_param_str.value); + break; + case UINT8: + cJSON_AddNumberToObject(root_obj, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u8.value); + break; + case UINT16: + cJSON_AddNumberToObject(root_obj, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_u16.value); + break; + case INT32: + cJSON_AddNumberToObject(root_obj, + (char *)db_params[i]->db_name, + db_params[i]->value.db_param_i32.value); + break; + default: + ESP_LOGE( + TAG, "db_param_write_all_params_json() -> db_parameter.type unknown!"); + break; } + } } /** @@ -816,10 +977,12 @@ void db_param_write_all_params_json(cJSON *root_obj) { * @param ipaddress IPv4 string * @return true if valid IPv4 string was supplied */ -bool is_valid_ip4(const char *ipaddress) { - struct sockaddr_in sa; - const int result = inet_pton(AF_INET, ipaddress, &(sa.sin_addr)); - return result != 0; +bool +is_valid_ip4(const char *ipaddress) +{ + struct sockaddr_in sa; + const int result = inet_pton(AF_INET, ipaddress, &(sa.sin_addr)); + return result != 0; } /** @@ -828,13 +991,19 @@ bool is_valid_ip4(const char *ipaddress) { * @param target_param The target parameter * @return true if valid or false if not */ -bool db_param_is_valid_str(char *new_string_value, db_parameter_t *target_param) { - // ToDo: Add IPv4 check for strings via custom validation function in db_parameter_t - if (new_string_value != NULL && - strlen(new_string_value) <= target_param->value.db_param_str.max_len && - strlen(new_string_value) >= target_param->value.db_param_str.min_len) { - return true; - } else { return false; } +bool +db_param_is_valid_str(char *new_string_value, db_parameter_t *target_param) +{ + // ToDo: Add IPv4 check for strings via custom validation function in + // db_parameter_t + if(new_string_value != NULL && + strlen(new_string_value) <= target_param->value.db_param_str.max_len && + strlen(new_string_value) >= target_param->value.db_param_str.min_len) { + return true; + } + else { + return false; + } }; /** @@ -843,10 +1012,16 @@ bool db_param_is_valid_str(char *new_string_value, db_parameter_t *target_param) * @param target_param The target parameter * @return true if valid or false if not */ -bool db_param_is_valid_u8(const uint8_t new_u8_value, db_parameter_t *target_param) { - if (new_u8_value <= target_param->value.db_param_u8.max && new_u8_value >= target_param->value.db_param_u8.min) { - return true; - } else { return false; } +bool +db_param_is_valid_u8(const uint8_t new_u8_value, db_parameter_t *target_param) +{ + if(new_u8_value <= target_param->value.db_param_u8.max && + new_u8_value >= target_param->value.db_param_u8.min) { + return true; + } + else { + return false; + } } /** @@ -855,10 +1030,17 @@ bool db_param_is_valid_u8(const uint8_t new_u8_value, db_parameter_t *target_par * @param target_param The target parameter * @return true if valid or false if not */ -bool db_param_is_valid_u16(const uint16_t new_u16_value, db_parameter_t *target_param) { - if (new_u16_value <= target_param->value.db_param_u16.max && new_u16_value >= target_param->value.db_param_u16.min) { - return true; - } else { return false; } +bool +db_param_is_valid_u16(const uint16_t new_u16_value, + db_parameter_t *target_param) +{ + if(new_u16_value <= target_param->value.db_param_u16.max && + new_u16_value >= target_param->value.db_param_u16.min) { + return true; + } + else { + return false; + } } /** @@ -867,90 +1049,138 @@ bool db_param_is_valid_u16(const uint16_t new_u16_value, db_parameter_t *target_ * @param target_param The target parameter * @return true if valid or false if not */ -bool db_param_is_valid_i32(const int32_t new_i32_value, db_parameter_t *target_param) { - if (new_i32_value <= target_param->value.db_param_i32.max && new_i32_value >= target_param->value.db_param_i32.min) { - return true; - } else { return false; } +bool +db_param_is_valid_i32(const int32_t new_i32_value, + db_parameter_t *target_param) +{ + if(new_i32_value <= target_param->value.db_param_i32.max && + new_i32_value >= target_param->value.db_param_i32.min) { + return true; + } + else { + return false; + } } /** - * Checks if string is valid for assignment to the target_param. If valid assigns the new value to the parameter + * Checks if string is valid for assignment to the target_param. If valid + * assigns the new value to the parameter * @param new_string_value The new string to be checked and assigned - * @param target_param The internal db parameter to be assigned with the new value + * @param target_param The internal db parameter to be assigned with the new + * value * @return true when valid and assigned - else false */ -bool db_param_is_valid_assign_str(char *new_string_value, db_parameter_t *target_param) { - if (db_param_is_valid_str(new_string_value, target_param)) { - strncpy((char *) target_param->value.db_param_str.value, new_string_value, DB_PARAM_VALUE_MAXLEN); - return true; - } else { - // new value is not valid - do not assign - ESP_LOGE(TAG, "db_param_is_valid_assign_str(): Invalid string length (%i-%i) or NULL for param %s", - target_param->value.db_param_str.min_len, target_param->value.db_param_str.max_len, - (char *) target_param->db_name); - return false; - } +bool +db_param_is_valid_assign_str(char *new_string_value, + db_parameter_t *target_param) +{ + if(db_param_is_valid_str(new_string_value, target_param)) { + strncpy((char *)target_param->value.db_param_str.value, + new_string_value, + DB_PARAM_VALUE_MAXLEN); + return true; + } + else { + // new value is not valid - do not assign + ESP_LOGE(TAG, + "db_param_is_valid_assign_str(): Invalid string length (%i-%i) " + "or NULL for param %s", + target_param->value.db_param_str.min_len, + target_param->value.db_param_str.max_len, + (char *)target_param->db_name); + return false; + } } /** - * Checks if u8 is valid for assignment to the target_param. If valid assigns the new value to the parameter + * Checks if u8 is valid for assignment to the target_param. If valid assigns + * the new value to the parameter * @param new_u8_value The new u8 to be checked and assigned - * @param target_param The internal db parameter to be assigned with the new value + * @param target_param The internal db parameter to be assigned with the new + * value * @return true when valid and assigned - else false */ -bool db_param_is_valid_assign_u8(const uint8_t new_u8_value, db_parameter_t *target_param) { - if (db_param_is_valid_u8(new_u8_value, target_param)) { - if (strcmp((char *) target_param->db_name, (char *) db_param_radio_mode.db_name) == 0) { - // Special case check: Do not directly change DB_WIFI_MODE since it is not safe and constantly - // processed by other tasks. Save settings and reboot will assign DB_RADIO_MODE_DESIGNATED to DB_WIFI_MODE - DB_RADIO_MODE_DESIGNATED = new_u8_value; - } else { - target_param->value.db_param_u8.value = new_u8_value; // accept value and assign - } - return true; +bool +db_param_is_valid_assign_u8(const uint8_t new_u8_value, + db_parameter_t *target_param) +{ + if(db_param_is_valid_u8(new_u8_value, target_param)) { + if(strcmp((char *)target_param->db_name, + (char *)db_param_radio_mode.db_name) == 0) { + // Special case check: Do not directly change DB_WIFI_MODE since it is + // not safe and constantly processed by other tasks. Save settings and + // reboot will assign DB_RADIO_MODE_DESIGNATED to DB_WIFI_MODE + DB_RADIO_MODE_DESIGNATED = new_u8_value; } - // new value is not valid - ESP_LOGE( - TAG, "db_param_is_valid_assign_u8(): Value %i is out of valid range (%i-%i) for param %s", - new_u8_value, target_param->value.db_param_u8.max, target_param->value.db_param_u8.min, - (char *) target_param->db_name); - return false; + else { + target_param->value.db_param_u8.value = + new_u8_value; // accept value and assign + } + return true; + } + // new value is not valid + ESP_LOGE(TAG, + "db_param_is_valid_assign_u8(): Value %i is out of valid range " + "(%i-%i) for param %s", + new_u8_value, + target_param->value.db_param_u8.max, + target_param->value.db_param_u8.min, + (char *)target_param->db_name); + return false; } /** - * Checks if u16 is valid for assignment to the target_param. If valid assigns the new value to the parameter + * Checks if u16 is valid for assignment to the target_param. If valid assigns + * the new value to the parameter * @param new_u16_value The new u16 to be checked and assigned - * @param target_param The internal db parameter to be assigned with the new value + * @param target_param The internal db parameter to be assigned with the new + * value * @return true when valid and assigned - else false */ -bool db_param_is_valid_assign_u16(const uint16_t new_u16_value, db_parameter_t *target_param) { - if (db_param_is_valid_u16(new_u16_value, target_param)) { - target_param->value.db_param_u16.value = new_u16_value; // accept value and assign - return true; - } - // new value is not valid - ESP_LOGE( - TAG, "db_param_is_valid_assign_u16(): Value %i is out of valid range (%i-%i) for param %s", - new_u16_value, target_param->value.db_param_u16.max, target_param->value.db_param_u16.min, - (char *) target_param->db_name); - return false; +bool +db_param_is_valid_assign_u16(const uint16_t new_u16_value, + db_parameter_t *target_param) +{ + if(db_param_is_valid_u16(new_u16_value, target_param)) { + target_param->value.db_param_u16.value = + new_u16_value; // accept value and assign + return true; + } + // new value is not valid + ESP_LOGE(TAG, + "db_param_is_valid_assign_u16(): Value %i is out of valid range " + "(%i-%i) for param %s", + new_u16_value, + target_param->value.db_param_u16.max, + target_param->value.db_param_u16.min, + (char *)target_param->db_name); + return false; } /** - * Checks if i32 is valid for assignment to the target_param. If valid assigns the new value to the parameter + * Checks if i32 is valid for assignment to the target_param. If valid assigns + * the new value to the parameter * @param new_i32_value The new i32 to be checked and assigned - * @param target_param The internal db parameter to be assigned with the new value + * @param target_param The internal db parameter to be assigned with the new + * value * @return true when valid and assigned - else false */ -bool db_param_is_valid_assign_i32(const int32_t new_i32_value, db_parameter_t *target_param) { - if (db_param_is_valid_i32(new_i32_value, target_param)) { - target_param->value.db_param_i32.value = new_i32_value; // accept value and assign - return true; - } - // new value is not valid - ESP_LOGE( - TAG, "db_param_is_valid_assign_i32(): Value %li is out of valid range (%li-%li) for param %s", - new_i32_value, target_param->value.db_param_i32.max, target_param->value.db_param_i32.min, - (char *) target_param->db_name); - return false; +bool +db_param_is_valid_assign_i32(const int32_t new_i32_value, + db_parameter_t *target_param) +{ + if(db_param_is_valid_i32(new_i32_value, target_param)) { + target_param->value.db_param_i32.value = + new_i32_value; // accept value and assign + return true; + } + // new value is not valid + ESP_LOGE(TAG, + "db_param_is_valid_assign_i32(): Value %li is out of valid range " + "(%li-%li) for param %s", + new_i32_value, + target_param->value.db_param_i32.max, + target_param->value.db_param_i32.min, + (char *)target_param->db_name); + return false; } diff --git a/main/db_parameters.h b/main/db_parameters.h index 4972a55..18a0f3d 100644 --- a/main/db_parameters.h +++ b/main/db_parameters.h @@ -1,5 +1,5 @@ /* -* This file is part of DroneBridge:https://github.com/DroneBridge/ESP32 + * This file is part of DroneBridge:https://github.com/DroneBridge/ESP32 * * Copyright 2025 Wolfgang Christl * @@ -33,14 +33,19 @@ #define DB_PATCH_VERSION 0 #define DB_MATURITY_VERSION "RC1" -#define DB_PARAM_TOTAL_NUM 25 // total number of db parameters -#define DB_PARAM_MAV_CNT 17 // Number of MAVLink parameters returned by ESP32 in the PARAM message. Needed by GCS. - -#define DB_PARAM_NAME_MAXLEN 16 // max len of a parameter/key stored in the ESP32 NVM -#define DB_PARAM_MAX_MAV_PARAM_NAME_LEN 16 // max len of the field used to store the mav param name (max len 16 by def.) -#define DB_PARAM_VALUE_MAXLEN 64 // max len of a value stored for a key in the ESP32 NVM (string len) -#define MAX_LTM_FRAMES_IN_BUFFER 5 +#define DB_PARAM_TOTAL_NUM 25 // total number of db parameters +#define DB_PARAM_MAV_CNT \ + 17 // Number of MAVLink parameters returned by ESP32 in the PARAM message. + // Needed by GCS. +#define DB_PARAM_NAME_MAXLEN \ + 16 // max len of a parameter/key stored in the ESP32 NVM +#define DB_PARAM_MAX_MAV_PARAM_NAME_LEN \ + 16 // max len of the field used to store the mav param name (max len 16 by + // def.) +#define DB_PARAM_VALUE_MAXLEN \ + 64 // max len of a value stored for a key in the ESP32 NVM (string len) +#define MAX_LTM_FRAMES_IN_BUFFER 5 #ifdef CONFIG_DB_OFFICIAL_BOARD_1_X #define DB_DEFAULT_UART_TX_PIN GPIO_NUM_5 @@ -55,7 +60,8 @@ #define DB_DEFAULT_UART_CTS_PIN GPIO_NUM_23 #define DB_DEFAULT_UART_BAUD_RATE 115200 #elif CONFIG_DB_GENERIC_BOARD -// initially set pins to 0 to allow the start of the system on all boards. User has to set the correct pins +// initially set pins to 0 to allow the start of the system on all boards. User +// has to set the correct pins #define DB_DEFAULT_UART_TX_PIN GPIO_NUM_0 #define DB_DEFAULT_UART_RX_PIN GPIO_NUM_0 #define DB_DEFAULT_UART_RTS_PIN GPIO_NUM_0 @@ -71,91 +77,107 @@ #endif /* ---------- Optional Macros for quick access to param values ---------- */ -#define DB_PARAM_WIFI_SSID (char *) db_param_ssid.value.db_param_str.value -#define DB_PARAM_PASS (char *) db_param_pass.value.db_param_str.value +#define DB_PARAM_WIFI_SSID (char *)db_param_ssid.value.db_param_str.value +#define DB_PARAM_PASS (char *)db_param_pass.value.db_param_str.value #define DB_PARAM_CHANNEL db_param_channel.value.db_param_u8.value #define DB_PARAM_RADIO_MODE db_param_radio_mode.value.db_param_u8.value #define DB_PARAM_STA_IP db_param_wifi_sta_ip.value.db_param_str.value #define DB_PARAM_STA_GW db_param_wifi_sta_gw.value.db_param_str.value -#define DB_PARAM_STA_IP_NETMASK db_param_wifi_sta_netmask.value.db_param_str.value -#define DB_PARAM_AP_IP (char *) db_param_wifi_ap_ip.value.db_param_str.value +#define DB_PARAM_STA_IP_NETMASK \ + db_param_wifi_sta_netmask.value.db_param_str.value +#define DB_PARAM_AP_IP (char *)db_param_wifi_ap_ip.value.db_param_str.value #define DB_PARAM_WIFI_EN_GN db_param_wifi_en_gn.value.db_param_u8.value -#define DB_PARAM_DIS_RADIO_ON_ARM db_param_dis_radio_armed.value.db_param_u8.value +#define DB_PARAM_DIS_RADIO_ON_ARM \ + db_param_dis_radio_armed.value.db_param_u8.value #define DB_PARAM_SERIAL_BAUD db_param_baud.value.db_param_i32.value #define DB_PARAM_SERIAL_PROTO db_param_proto.value.db_param_u8.value -#define DB_PARAM_SERIAL_PACK_SIZE db_param_serial_pack_size.value.db_param_u16.value +#define DB_PARAM_SERIAL_PACK_SIZE \ + db_param_serial_pack_size.value.db_param_u16.value #define DB_PARAM_GPIO_TX db_param_gpio_tx.value.db_param_u8.value #define DB_PARAM_GPIO_RX db_param_gpio_rx.value.db_param_u8.value #define DB_PARAM_GPIO_RTS db_param_gpio_rts.value.db_param_u8.value #define DB_PARAM_GPIO_CTS db_param_gpio_cts.value.db_param_u8.value -#define DB_PARAM_SERIAL_RTS_THRESH db_param_gpio_rts_thresh.value.db_param_u8.value +#define DB_PARAM_SERIAL_RTS_THRESH \ + db_param_gpio_rts_thresh.value.db_param_u8.value #define DB_PARAM_EN_EXT_ANT db_param_radio_ant_ext.value.db_param_u8.value -enum E_DB_WIFI_MODE { - DB_WIFI_MODE_AP = 1, // Wi-Fi access point mode with 802.11b mode enabled - DB_WIFI_MODE_STA = 2, // Wi-Fi client mode with 802.11b and LR mode enabled - DB_WIFI_MODE_AP_LR = 3, // ESP32 WiFi LR Mode 802.11b +enum E_DB_WIFI_MODE +{ + DB_WIFI_MODE_AP = 1, // Wi-Fi access point mode with 802.11b mode enabled + DB_WIFI_MODE_STA = 2, // Wi-Fi client mode with 802.11b and LR mode enabled + DB_WIFI_MODE_AP_LR = 3, // ESP32 WiFi LR Mode 802.11b DB_WIFI_MODE_ESPNOW_AIR = 4, // ESP-NOW Mode for broadcasting device DB_WIFI_MODE_ESPNOW_GND = 5, // ESP-NOW Mode for GND station - DB_BLUETOOTH_MODE = 6, // Bluetooth BLE mode - DB_WIFI_MODE_END = 7, // End of enum + DB_BLUETOOTH_MODE = 6, // Bluetooth BLE mode + DB_WIFI_MODE_END = 7, // End of enum }; -enum E_DB_SERIAL_PROTOCOL { - DB_SERIAL_PROTOCOL_MSPLTM = 1, - DB_SERIAL_PROTOCOL_MAVLINK = 4, - DB_SERIAL_PROTOCOL_TRANSPARENT = 5 +enum E_DB_SERIAL_PROTOCOL +{ + DB_SERIAL_PROTOCOL_MSPLTM = 1, + DB_SERIAL_PROTOCOL_MAVLINK = 4, + DB_SERIAL_PROTOCOL_TRANSPARENT = 5 }; -typedef struct db_parameter_str_s { - uint8_t *value; - uint8_t *default_value; - uint8_t min_len; // valid if >= min_len - uint8_t max_len; // valid if <= max_len +typedef struct db_parameter_str_s +{ + uint8_t *value; + uint8_t *default_value; + uint8_t min_len; // valid if >= min_len + uint8_t max_len; // valid if <= max_len } db_param_str_t; -typedef struct db_parameter_u8_s { - uint8_t value; - uint8_t default_value; - uint8_t min; - uint8_t max; +typedef struct db_parameter_u8_s +{ + uint8_t value; + uint8_t default_value; + uint8_t min; + uint8_t max; } db_param_u8_t; -typedef struct db_parameter_u16_s { - uint16_t value; - uint16_t default_value; - uint16_t min; - uint16_t max; +typedef struct db_parameter_u16_s +{ + uint16_t value; + uint16_t default_value; + uint16_t min; + uint16_t max; } db_param_u16_t; -typedef struct db_parameter_i32_s { - int32_t value; - int32_t default_value; - int32_t min; - int32_t max; +typedef struct db_parameter_i32_s +{ + int32_t value; + int32_t default_value; + int32_t min; + int32_t max; } db_param_i32_t; -enum db_param_type_t { +enum db_param_type_t +{ STRING, UINT8, UINT16, INT32 }; -typedef struct db_parameter_s { - uint8_t db_name[DB_PARAM_NAME_MAXLEN]; // equals the NVS key, JSON object name and web interface input ID - enum db_param_type_t type; - struct { // no string support for mavlink - uint8_t param_name[DB_PARAM_MAX_MAV_PARAM_NAME_LEN]; - uint16_t param_index; // strings will be ignored for mavlink set to -1 for strings - MAV_PARAM_TYPE param_type; // set to MAV_PARAM_TYPE_ENUM_END for strings - } mav_t; - union { - db_param_str_t db_param_str; - db_param_u8_t db_param_u8; - db_param_u16_t db_param_u16; - db_param_i32_t db_param_i32; - } value; +typedef struct db_parameter_s +{ + uint8_t db_name[DB_PARAM_NAME_MAXLEN]; // equals the NVS key, JSON object + // name and web interface input ID + enum db_param_type_t type; + struct + { // no string support for mavlink + uint8_t param_name[DB_PARAM_MAX_MAV_PARAM_NAME_LEN]; + uint16_t + param_index; // strings will be ignored for mavlink set to -1 for strings + MAV_PARAM_TYPE param_type; // set to MAV_PARAM_TYPE_ENUM_END for strings + } mav_t; + union + { + db_param_str_t db_param_str; + db_param_u8_t db_param_u8; + db_param_u16_t db_param_u16; + db_param_i32_t db_param_i32; + } value; } db_parameter_t; extern uint8_t DB_RADIO_MODE_DESIGNATED; @@ -196,14 +218,21 @@ void db_param_write_all_params_nvs(const nvs_handle_t *nvs_handle); void db_param_read_all_params_json(const cJSON *root_obj); void db_param_write_all_params_json(cJSON *root_obj); -bool db_param_is_valid_str(char *new_string_value, db_parameter_t *target_param); +bool db_param_is_valid_str(char *new_string_value, + db_parameter_t *target_param); bool db_param_is_valid_u8(uint8_t new_u8_value, db_parameter_t *target_param); -bool db_param_is_valid_u16(uint16_t new_u16_value, db_parameter_t *target_param); -bool db_param_is_valid_i32(int32_t new_i32_value, db_parameter_t *target_param); -bool db_param_is_valid_assign_str(char *new_string_value, db_parameter_t *target_param); -bool db_param_is_valid_assign_u8(uint8_t new_u8_value, db_parameter_t *target_param); -bool db_param_is_valid_assign_u16(uint16_t new_u16_value, db_parameter_t *target_param); -bool db_param_is_valid_assign_i32(int32_t new_i32_value, db_parameter_t *target_param); +bool db_param_is_valid_u16(uint16_t new_u16_value, + db_parameter_t *target_param); +bool db_param_is_valid_i32(int32_t new_i32_value, + db_parameter_t *target_param); +bool db_param_is_valid_assign_str(char *new_string_value, + db_parameter_t *target_param); +bool db_param_is_valid_assign_u8(uint8_t new_u8_value, + db_parameter_t *target_param); +bool db_param_is_valid_assign_u16(uint16_t new_u16_value, + db_parameter_t *target_param); +bool db_param_is_valid_assign_i32(int32_t new_i32_value, + db_parameter_t *target_param); bool is_valid_ip4(const char *ipaddress); -#endif //DB_PARAMETERS_H +#endif // DB_PARAMETERS_H diff --git a/main/db_protocol.h b/main/db_protocol.h index 42dfe52..479075d 100644 --- a/main/db_protocol.h +++ b/main/db_protocol.h @@ -24,56 +24,71 @@ #ifndef DB_PROTOCOL_H_INCLUDED #define DB_PROTOCOL_H_INCLUDED -#define RADIOTAP_LENGTH 13 +#define RADIOTAP_LENGTH 13 #define DB_RAW_V2_HEADER_LENGTH 10 #define DB_MAX_ADAPTERS 4 -#define MSP_DATA_LENTH 34 // size of MSP v1 -#define MSP_V2_DATA_LENGTH 37 // size of MSP v2 frame -#define DB_RC_DATA_LENGTH 16 // size of DB_RC frame -#define DATA_UNI_LENGTH 2048 // max payload length for raw protocol -#define DB_RAW_OFFSET 14 // when adhering the 802.11 header the payload is offset to not be overwritten by SQN -#define MAX_DB_DATA_LENGTH (RADIOTAP_LENGTH + DB_RAW_V2_HEADER_LENGTH + DATA_UNI_LENGTH) // max length of a db raw packet -#define ETHER_TYPE 0x88ab +#define MSP_DATA_LENTH 34 // size of MSP v1 +#define MSP_V2_DATA_LENGTH 37 // size of MSP v2 frame +#define DB_RC_DATA_LENGTH 16 // size of DB_RC frame +#define DATA_UNI_LENGTH 2048 // max payload length for raw protocol +#define DB_RAW_OFFSET \ + 14 // when adhering the 802.11 header the payload is offset to not be + // overwritten by SQN +#define MAX_DB_DATA_LENGTH \ + (RADIOTAP_LENGTH + DB_RAW_V2_HEADER_LENGTH + \ + DATA_UNI_LENGTH) // max length of a db raw packet +#define ETHER_TYPE 0x88ab -#define DEFAULT_DB_MODE 'm' -#define DEFAULT_DB_IF "18a6f716a511" -#define DEFAULT_V2_COMMID 0xc8 +#define DEFAULT_DB_MODE 'm' +#define DEFAULT_DB_IF "18a6f716a511" +#define DEFAULT_V2_COMMID 0xc8 -#define DB_FRAMETYPE_RTS 1 -#define DB_FRAMETYPE_DATA 2 -#define DB_FRAMETYPE_BEACON 3 -#define DB_FRAMETYPE_DEFAULT DB_FRAMETYPE_DATA +#define DB_FRAMETYPE_RTS 1 +#define DB_FRAMETYPE_DATA 2 +#define DB_FRAMETYPE_BEACON 3 +#define DB_FRAMETYPE_DEFAULT DB_FRAMETYPE_DATA -#define NUM_CHANNELS 14 // max number of channels sent over DroneBridge control module (ground) -#define DB_RC_NUM_CHANNELS 12 // number of channels supported by DroneBridge RC protocol +#define NUM_CHANNELS \ + 14 // max number of channels sent over DroneBridge control module (ground) +#define DB_RC_NUM_CHANNELS \ + 12 // number of channels supported by DroneBridge RC protocol -#define DB_PORT_CONTROLLER 0x01 -#define DB_PORT_TELEMETRY 0x02 // deprecated. Use proxy port for bidirectional telemetry -#define DB_PORT_VIDEO 0x03 -#define DB_PORT_COMM 0x04 -#define DB_PORT_STATUS 0x05 -#define DB_PORT_PROXY 0x06 -#define DB_PORT_RC 0x07 +#define DB_PORT_CONTROLLER 0x01 +#define DB_PORT_TELEMETRY \ + 0x02 // deprecated. Use proxy port for bidirectional telemetry +#define DB_PORT_VIDEO 0x03 +#define DB_PORT_COMM 0x04 +#define DB_PORT_STATUS 0x05 +#define DB_PORT_PROXY 0x06 +#define DB_PORT_RC 0x07 -#define DB_DIREC_DRONE 0x01 // packet to/for drone -#define DB_DIREC_GROUND 0x03 // packet to/for ground station +#define DB_DIREC_DRONE 0x01 // packet to/for drone +#define DB_DIREC_GROUND 0x03 // packet to/for ground station -#define APP_PORT_STATUS 1602 // for all kinds of status protocol messages. Same port on ground station and app -#define APP_PORT_COMM 1603 -#define APP_PORT_TELEMETRY 1604 // accepts MAVLink and LTM telemetry messages. Non MAVLink telemetry messages get rerouted internally to APP_PORT_PROXY +#define APP_PORT_STATUS \ + 1602 // for all kinds of status protocol messages. Same port on ground + // station and app +#define APP_PORT_COMM 1603 +#define APP_PORT_TELEMETRY \ + 1604 // accepts MAVLink and LTM telemetry messages. Non MAVLink telemetry + // messages get rerouted internally to APP_PORT_PROXY #define PORT_TCP_SYSLOG_SERVER 1605 -#define APP_PORT_PROXY 5760 // use this port for all MAVLink messages (TCP) -#define APP_PORT_PROXY_UDP 14550 // use this port for all MAVLink messages (UDP) -#define APP_PORT_VIDEO 5000 // app accepts raw H.264 streams -#define APP_PORT_VIDEO_FEC 5001 // app accepts raw DroneBridge video stream data, performs FEC on Android device -#define DB_ESP32_INTERNAL_TELEMETRY_PORT 1606 +#define APP_PORT_PROXY 5760 // use this port for all MAVLink messages (TCP) +#define APP_PORT_PROXY_UDP \ + 14550 // use this port for all MAVLink messages (UDP) +#define APP_PORT_VIDEO 5000 // app accepts raw H.264 streams +#define APP_PORT_VIDEO_FEC \ + 5001 // app accepts raw DroneBridge video stream data, performs FEC on + // Android device +#define DB_ESP32_INTERNAL_TELEMETRY_PORT 1606 #define UDP_BROADCAST_PORT_SKYBRUSH 14555 -#define MAX_PENUMBRA_INTERFACES 8 +#define MAX_PENUMBRA_INTERFACES 8 -#define DB_UNIX_DOMAIN_VIDEO_PATH "/tmp/db_video_out" -#define DB_AP_CLIENT_IP "192.168.2.1" // default IP address of GCS connected via WiFi AP +#define DB_UNIX_DOMAIN_VIDEO_PATH "/tmp/db_video_out" +#define DB_AP_CLIENT_IP \ + "192.168.2.1" // default IP address of GCS connected via WiFi AP #define DB_SYS_HID_ESP32 1 diff --git a/main/db_serial.c b/main/db_serial.c index 570648c..9203a80 100644 --- a/main/db_serial.c +++ b/main/db_serial.c @@ -1,4 +1,4 @@ -/* +/****************************************************************************** * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2024 Wolfgang Christl @@ -15,32 +15,51 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ + ******************************************************************************/ +/****************************************************************************** + * System & Standard Library Headers + ******************************************************************************/ +#include +#include +#include +#include +#include + +/****************************************************************************** + * LwIP & Networking + ******************************************************************************/ #include "lwip/sockets.h" +#include + +/****************************************************************************** + * ESP-IDF Core APIs + ******************************************************************************/ #include "esp_log.h" #include #include -#include #include #include -#include -#include -#include -#include -#include + +/****************************************************************************** + * ESP-IDF Driver APIs + ******************************************************************************/ #include -#include "db_serial.h" +#include "driver/uart.h" + +/****************************************************************************** + * Project Headers + ******************************************************************************/ #include "main.h" -#include "db_esp32_control.h" +#include "globals.h" +#include "db_serial.h" #include "db_protocol.h" +#include "db_esp32_control.h" #include "msp_ltm_serial.h" -#include "globals.h" -#include "driver/uart.h" #include -#define FASTMAVLINK_ROUTER_LINKS_MAX 3 -#define FASTMAVLINK_ROUTER_COMPONENTS_MAX 5 +#define FASTMAVLINK_ROUTER_LINKS_MAX 3 +#define FASTMAVLINK_ROUTER_COMPONENTS_MAX 5 #include "db_mavlink_msgs.h" @@ -56,390 +75,495 @@ uint ltm_frames_in_buffer_pnt = 0; fmav_message_t msg; -/** +/****************************************************************************** + * Private Function Declaration + ******************************************************************************/ + +/****************************************************************************** * Opens UART socket. * Enables UART flow control if RTS and CTS pins do NOT match. - * Only open serial socket/UART if PINs are not matching - matching PIN nums mean they still need to be defined by - * the user. No pre-defined pins since ESP32 boards have wildly different pin configurations + * Only open serial socket/UART if PINs are not matching - matching PIN nums + * mean they still need to be defined by the user. No pre-defined pins since + * ESP32 boards have wildly different pin configurations * * 8 data bits, no parity, 1 stop bit * @return ESP_ERROR or ESP_OK - */ -esp_err_t open_uart_serial_socket() { - // only open serial socket/UART if PINs are not matching - matching PIN nums mean they still need to be defined by - // the user no pre-defined pins as of this release since ESP32 boards have wildly different pin configurations - if (DB_PARAM_GPIO_RX == DB_PARAM_GPIO_TX) { - ESP_LOGW(TAG, "Init UART socket aborted. TX GPIO == RX GPIO - Configure first!"); - return ESP_FAIL; - } - if (DB_PARAM_GPIO_TX > SOC_GPIO_IN_RANGE_MAX || DB_PARAM_GPIO_RX > SOC_GPIO_IN_RANGE_MAX || DB_PARAM_GPIO_CTS > SOC_GPIO_IN_RANGE_MAX || DB_PARAM_GPIO_RTS > SOC_GPIO_IN_RANGE_MAX) { - ESP_LOGW(TAG, "UART GPIO numbers out of range %i. Configure first!", SOC_GPIO_IN_RANGE_MAX); - return ESP_FAIL; - } - bool flow_control = DB_PARAM_GPIO_CTS != DB_PARAM_GPIO_RTS; - ESP_LOGI(TAG, "Flow control enabled: %s", flow_control ? "true" : "false"); - uart_config_t uart_config = { - .baud_rate = DB_PARAM_SERIAL_BAUD, - .data_bits = UART_DATA_8_BITS, - .parity = UART_PARITY_DISABLE, - .stop_bits = UART_STOP_BITS_1, - .flow_ctrl = flow_control ? UART_HW_FLOWCTRL_CTS_RTS : UART_HW_FLOWCTRL_DISABLE, - .rx_flow_ctrl_thresh = DB_PARAM_SERIAL_RTS_THRESH, - }; - ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config)); - ESP_ERROR_CHECK(uart_set_pin(UART_NUM, DB_PARAM_GPIO_TX, DB_PARAM_GPIO_RX, - flow_control ? DB_PARAM_GPIO_RTS : UART_PIN_NO_CHANGE, - flow_control ? DB_PARAM_GPIO_CTS : UART_PIN_NO_CHANGE)); - return uart_driver_install(UART_NUM, 1024, 0, 10, NULL, 0); -} + ******************************************************************************/ +static esp_err_t open_uart_serial_socket(); -/** - * Configures the onboard USB/JTAG interface for serial communication (instead of an UART). Board must support this interface. +#ifdef CONFIG_DB_SERIAL_OPTION_JTAG +/****************************************************************************** + * Configures the onboard USB/JTAG interface for serial communication (instead + * of an UART). Board must support this interface. * * @return result of usb_serial_jtag_driver_install() - */ -esp_err_t open_jtag_serial_socket() { - // Configure USB SERIAL JTAG - usb_serial_jtag_driver_config_t usb_serial_jtag_config = { - .rx_buffer_size = 256, - .tx_buffer_size = 256, - }; - ESP_LOGI(TAG, "Initializing USB/JTAG serial interface."); - return usb_serial_jtag_driver_install(&usb_serial_jtag_config); + ******************************************************************************/ +static esp_err_t open_jtag_serial_socket(); +#endif + +/****************************************************************************** + * Read data from the open serial interface in non-blocking fashion. + * @param uart_read_buf Pointer to buffer to put the read bytes into + * @param length Max length to read + * @return number of read bytes + ******************************************************************************/ +static int db_read_serial(uint8_t *uart_read_buf, uint length); + +/****************************************************************************** + * Check armed state of LTM packet if feature DB_PARAM_DIS_RADIO_ON_ARM is set + * and we got a status frame. Triggers the enabling or disabling of the Wi-Fi. + * @param db_msp_ltm_port MSP/LTM parser struct + ******************************************************************************/ +static void +ltm_check_arm_state_set_wifi(const msp_ltm_port_t *db_msp_ltm_port); + +/****************************************************************************** + * Private Function Definition + ******************************************************************************/ + +static esp_err_t +open_uart_serial_socket() +{ + // only open serial socket/UART if PINs are not matching - matching PIN nums + // mean they still need to be defined by the user no pre-defined pins as of + // this release since ESP32 boards have wildly different pin configurations + if(DB_PARAM_GPIO_RX == DB_PARAM_GPIO_TX) { + ESP_LOGW( + TAG, "Init UART socket aborted. TX GPIO == RX GPIO - Configure first!"); + return ESP_FAIL; + } + if(DB_PARAM_GPIO_TX > SOC_GPIO_IN_RANGE_MAX || + DB_PARAM_GPIO_RX > SOC_GPIO_IN_RANGE_MAX || + DB_PARAM_GPIO_CTS > SOC_GPIO_IN_RANGE_MAX || + DB_PARAM_GPIO_RTS > SOC_GPIO_IN_RANGE_MAX) { + ESP_LOGW(TAG, + "UART GPIO numbers out of range %i. Configure first!", + SOC_GPIO_IN_RANGE_MAX); + return ESP_FAIL; + } + bool flow_control = DB_PARAM_GPIO_CTS != DB_PARAM_GPIO_RTS; + ESP_LOGI(TAG, "Flow control enabled: %s", flow_control ? "true" : "false"); + uart_config_t uart_config = { + .baud_rate = DB_PARAM_SERIAL_BAUD, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = + flow_control ? UART_HW_FLOWCTRL_CTS_RTS : UART_HW_FLOWCTRL_DISABLE, + .rx_flow_ctrl_thresh = DB_PARAM_SERIAL_RTS_THRESH, + }; + ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config)); + ESP_ERROR_CHECK( + uart_set_pin(UART_NUM, + DB_PARAM_GPIO_TX, + DB_PARAM_GPIO_RX, + flow_control ? DB_PARAM_GPIO_RTS : UART_PIN_NO_CHANGE, + flow_control ? DB_PARAM_GPIO_CTS : UART_PIN_NO_CHANGE)); + return uart_driver_install(UART_NUM, 1024, 0, 10, NULL, 0); } -/** - * Opens a serial socket for communication with a serial source. On the GND this is the GCS and on the air side this is the flight controller. - * Depending on the configuration it may open a native UART socket or a JTAG based serial interface. - * The JTAG serial based interface is a special feature of official DroneBridge for ESP32 boards. Uses the onboard USB for serial I/O with GCS. No FTDI required. - * - * @return ESP_FAIL on failure - */ -esp_err_t open_serial_socket() { #ifdef CONFIG_DB_SERIAL_OPTION_JTAG - // open JTAG based serial socket for comms with FC or GCS via FTDI - special feature of official DB for ESP32 boards. Uses the onboard USB for serial I/O with GCS. - // this is basically the GND-Station mode for the ESP32 - return open_jtag_serial_socket(); +static esp_err_t +open_jtag_serial_socket() +{ + // Configure USB SERIAL JTAG + usb_serial_jtag_driver_config_t usb_serial_jtag_config = { + .rx_buffer_size = 256, + .tx_buffer_size = 256, + }; + ESP_LOGI(TAG, "Initializing USB/JTAG serial interface."); + return usb_serial_jtag_driver_install(&usb_serial_jtag_config); +} +#endif + +static int +db_read_serial(uint8_t *uart_read_buf, uint length) +{ +#ifdef CONFIG_DB_SERIAL_OPTION_JTAG + return usb_serial_jtag_read_bytes(uart_read_buf, length, 0); #else - // open UART based serial socket for comms with FC or GCS via FTDI - configured by pins in the web interface - return open_uart_serial_socket(); + // UART based serial socket for communication with FC or GCS via FTDI - + // configured by pins in the web interface + return uart_read_bytes(UART_NUM, uart_read_buf, length, 0); #endif } -/** - * Writes data from buffer to the opened serial device - * @param data_buffer Payload to write to UART - * @param data_length Size of payload to write to UART - */ -void write_to_serial(const uint8_t data_buffer[], const unsigned int data_length) { -#ifdef CONFIG_DB_SERIAL_OPTION_JTAG - // Writes data from buffer to JTAG based serial interface - int written = usb_serial_jtag_write_bytes(data_buffer, data_length, 20 / portTICK_PERIOD_MS); - if (written != data_length) { - ESP_LOGW(TAG, "Wrote only %i of %i bytes to JTAG", written, data_length); - } else { - // all good. Wrote all bytes +static void +ltm_check_arm_state_set_wifi(const msp_ltm_port_t *db_msp_ltm_port) +{ + if(DB_PARAM_DIS_RADIO_ON_ARM && db_msp_ltm_port->ltm_type == LTM_TYPE_S) { + if(db_msp_ltm_port->ltm_frame_buffer[2 + LTM_TYPE_S_PAYLOAD_SIZE] & + LTM_ARMED_BIT_MASK) { + // autopilot says it is armed + db_set_radio_status(false); // disable Wi-Fi } -#else - // UART based serial socket for comms with FC or GCS via FTDI - configured by pins in the web interface - // Writes data from buffer to native UART interface - int written = uart_write_bytes(UART_NUM, data_buffer, data_length); - if (written != data_length) { - // This is a debug log since it happens very rarely that not all bytes get written. Save some cpu cycles. - ESP_LOGD(TAG, "Wrote only %i of %i bytes to UART", written, data_length); - } else { - // all good + else { + // autopilot says it is <> armed + db_set_radio_status(true); // enable Wi-Fi } -#endif + } + else { + // nothing to do + } } -/** - * Read data from the open serial interface in non-blocking fashion. - * @param uart_read_buf Pointer to buffer to put the read bytes into - * @param length Max length to read - * @return number of read bytes - */ -int db_read_serial(uint8_t *uart_read_buf, uint length) { +/****************************************************************************** + * Public Function Definition + ******************************************************************************/ + +esp_err_t +open_serial_socket() +{ #ifdef CONFIG_DB_SERIAL_OPTION_JTAG - return usb_serial_jtag_read_bytes(uart_read_buf, length, 0); + // open JTAG based serial socket for comms with FC or GCS via FTDI - special + // feature of official DB for ESP32 boards. Uses the onboard USB for serial + // I/O with GCS. this is basically the GND-Station mode for the ESP32 + return open_jtag_serial_socket(); #else - // UART based serial socket for communication with FC or GCS via FTDI - configured by pins in the web interface - return uart_read_bytes(UART_NUM, uart_read_buf, length, 0); + // open UART based serial socket for comms with FC or GCS via FTDI - + // configured by pins in the web interface + return open_uart_serial_socket(); #endif } -/** - * Check armed state of LTM packet if feature DB_PARAM_DIS_RADIO_ON_ARM is set and we got a status frame. - * Triggers the enabling or disabling of the Wi-Fi. - * @param db_msp_ltm_port MSP/LTM parser struct - */ -void db_ltm_check_arm_state_set_wifi(const msp_ltm_port_t *db_msp_ltm_port) { - if (DB_PARAM_DIS_RADIO_ON_ARM && db_msp_ltm_port->ltm_type == LTM_TYPE_S) { - if (db_msp_ltm_port->ltm_frame_buffer[2 + LTM_TYPE_S_PAYLOAD_SIZE] & LTM_ARMED_BIT_MASK) { - // autopilot says it is armed - db_set_radio_status(false); // disable Wi-Fi - } else { - // autopilot says it is <> armed - db_set_radio_status(true); // enable Wi-Fi - } - } else { - // nothing to do - } +void +db_write_to_serial(const uint8_t data_buffer[], const unsigned int data_length) +{ +#ifdef CONFIG_DB_SERIAL_OPTION_JTAG + // Writes data from buffer to JTAG based serial interface + int written = usb_serial_jtag_write_bytes( + data_buffer, data_length, 20 / portTICK_PERIOD_MS); + if(written != data_length) { + ESP_LOGW(TAG, "Wrote only %i of %i bytes to JTAG", written, data_length); + } + else { + // all good. Wrote all bytes + } +#else + // UART based serial socket for comms with FC or GCS via FTDI - configured by + // pins in the web interface Writes data from buffer to native UART interface + int written = uart_write_bytes(UART_NUM, data_buffer, data_length); + if(written != data_length) { + // This is a debug log since it happens very rarely that not all bytes get + // written. Save some cpu cycles. + ESP_LOGD(TAG, "Wrote only %i of %i bytes to UART", written, data_length); + } + else { + // all good + } +#endif } -/** - * @brief Reads serial interface, parses & sends complete MSP & LTM messages over the air. - */ -void db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_t msp_message_buffer[], - unsigned int *serial_read_bytes, - msp_ltm_port_t *db_msp_ltm_port) { - uint8_t serial_bytes[TRANS_RD_BYTES_NUM]; - unsigned int read; - if ((read = db_read_serial(serial_bytes, TRANS_RD_BYTES_NUM)) > 0) { - serial_total_byte_count += read; - for (unsigned int j = 0; j < read; j++) { - (*serial_read_bytes)++; - uint8_t serial_byte = serial_bytes[j]; - 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) { - 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, - (db_msp_ltm_port->ltm_payload_cnt + 4)); - ltm_frames_in_buffer_pnt += (db_msp_ltm_port->ltm_payload_cnt + 4); - ltm_frames_in_buffer++; - db_ltm_check_arm_state_set_wifi(db_msp_ltm_port); - if (ltm_frames_in_buffer == db_param_ltm_per_packet.value.db_param_u8.value && - (db_param_ltm_per_packet.value.db_param_u8.value <= MAX_LTM_FRAMES_IN_BUFFER)) { - 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!", ltm_frames_in_buffer); - ltm_frames_in_buffer = 0; - ltm_frames_in_buffer_pnt = 0; - *serial_read_bytes = 0; - } - } - } else { // Leads to crashes of the ESP32 without it! - *serial_read_bytes = 0; - } +void +db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, + uint8_t msp_message_buffer[], unsigned int *serial_read_bytes, + msp_ltm_port_t *db_msp_ltm_port) +{ + uint8_t serial_bytes[TRANS_RD_BYTES_NUM]; + unsigned int read; + if((read = db_read_serial(serial_bytes, TRANS_RD_BYTES_NUM)) > 0) { + serial_total_byte_count += read; + for(unsigned int j = 0; j < read; j++) { + (*serial_read_bytes)++; + uint8_t serial_byte = serial_bytes[j]; + 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) { + 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, + (db_msp_ltm_port->ltm_payload_cnt + 4)); + ltm_frames_in_buffer_pnt += (db_msp_ltm_port->ltm_payload_cnt + 4); + ltm_frames_in_buffer++; + ltm_check_arm_state_set_wifi(db_msp_ltm_port); + if(ltm_frames_in_buffer == + db_param_ltm_per_packet.value.db_param_u8.value && + (db_param_ltm_per_packet.value.db_param_u8.value <= + MAX_LTM_FRAMES_IN_BUFFER)) { + 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!", + ltm_frames_in_buffer); + ltm_frames_in_buffer = 0; + ltm_frames_in_buffer_pnt = 0; + *serial_read_bytes = 0; + } } + } + else { // Leads to crashes of the ESP32 without it! + *serial_read_bytes = 0; + } } + } } -/** - * We received some MAVLink request via the origin. Decide on which interface to respond with an answer - * @param buffer Data to send - * @param length Data length to send - * @param origin Origin of the MAVLink request - * @param tcp_clients List of connected TCP client - * @param udp_conns List of active UDP connections - */ -void db_route_mavlink_response(uint8_t *buffer, uint16_t length, enum DB_MAVLINK_DATA_ORIGIN origin, int *tcp_clients, - udp_conn_list_t *udp_conns) { - if (origin == DB_MAVLINK_DATA_ORIGIN_SERIAL) { - write_to_serial(buffer, length); - } else if (origin == DB_MAVLINK_DATA_ORIGIN_RADIO) { - 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!"); +void +db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns, + uint8_t *serial_buffer, + unsigned int *serial_buff_pos) +{ + static uint mav_msg_counter = 0; + static uint8_t mav_parser_rx_buf[296]; // at least 280 bytes which is the max + // len for a MAVLink v2 packet + static fmav_status_t + fmav_status_serial; // fmav parser status struct for serial parser + uint8_t uart_read_buf[DB_PARAM_SERIAL_PACK_SIZE]; + // timeout variables + static TickType_t last_tick = 0; // time when we received something from the + // serial interface for the last time + static TickType_t current_tick = 0; + current_tick = xTaskGetTickCount(); // get current time + + // Read bytes from serial link (UART or USB/JTAG interface) + int bytes_read = db_read_serial(uart_read_buf, DB_PARAM_SERIAL_PACK_SIZE); + + if(bytes_read == 0) { + // did not read anything this cycle -> check serial read timeout + if(current_tick - last_tick >= pdMS_TO_TICKS(DB_SERIAL_READ_TIMEOUT_MS)) { + // serial read timeout detected + 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) { + 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 + } } -} + else { + // nothing received but no timeout yet -> do nothing + } + } + else { + // have received something -> reset timeout + last_tick = current_tick; + } -/** - * Parses MAVLink coming from WiFi/ESPNOW - and sends the packet to the serial output. - * - * @param tcp_clients Array of connected TCP clients - * @param up_conns Structure containing all UDP connection data including the sockets - * @param buffer Buffer containing the raw bytes to be parsed - * @param bytes_read Number of bytes in the buffer - * @param origin Origin of the data - serial link or radio link - */ -void db_parse_mavlink_from_radio(int *tcp_clients, udp_conn_list_t *udp_conns, uint8_t *buffer, int bytes_read) { - static uint8_t mav_parser_rx_buf[296]; // at least 280 bytes which is the max len for a MAVLink v2 packet - static fmav_status_t fmav_status_radio; // fmav parser status struct for radio/ESPNOW/WiFi parser - - // Parse each byte received - for (int i = 0; i < bytes_read; ++i) { - fmav_result_t result = {0}; - if (fmav_parse_and_check_to_frame_buf(&result, mav_parser_rx_buf, &fmav_status_radio, buffer[i])) { - // Parser detected a full message, write to serial - write_to_serial(mav_parser_rx_buf, result.frame_len); - // Decode message and react to it if it was for us - fmav_frame_buf_to_msg(&msg, &result, mav_parser_rx_buf); - if (result.res == FASTMAVLINK_PARSE_RESULT_OK) { - if (fmav_msg_is_for_me(db_get_mav_sys_id(), db_get_mav_comp_id(), &msg)) { - handle_mavlink_message(&msg, tcp_clients, udp_conns, &fmav_status_radio, DB_MAVLINK_DATA_ORIGIN_RADIO); - } else { - // message was not for us so ignore it - } - } else { - switch (result.res) { - case FASTMAVLINK_PARSE_RESULT_MSGID_UNKNOWN: - ESP_LOGW(TAG, "fastmavlink parser had an error FASTMAVLINK_PARSE_RESULT_MSGID_UNKNOWN msgID: %lu", result.msgid); - break; - case FASTMAVLINK_PARSE_RESULT_LENGTH_ERROR: - ESP_LOGW(TAG, "fastmavlink parser had an error FASTMAVLINK_PARSE_RESULT_LENGTH_ERROR msgID: %lu", result.msgid); - break; - case FASTMAVLINK_PARSE_RESULT_CRC_ERROR: - ESP_LOGW(TAG, "fastmavlink parser had an error FASTMAVLINK_PARSE_RESULT_CRC_ERROR msgID: %lu", result.msgid); - break; - case FASTMAVLINK_PARSE_RESULT_SIGNATURE_ERROR: - ESP_LOGW(TAG, "fastmavlink parser had an error FASTMAVLINK_PARSE_RESULT_SIGNATURE_ERROR msgID: %lu", result.msgid); - break; - default: - ESP_LOGW(TAG, "fastmavlink parser had an error parsing the message: %i", result.res); - break; - } - - } - } else { - // do nothing since parser did not detect a message + serial_total_byte_count += + bytes_read; // increase total bytes read via serial interface + // Parse each byte received + for(int i = 0; i < bytes_read; ++i) { + fmav_result_t result = { 0 }; + + if(fmav_parse_and_check_to_frame_buf( + &result, mav_parser_rx_buf, &fmav_status_serial, uart_read_buf[i])) { + ESP_LOGD( + TAG, + "Parser detected a full message (%i total): result.frame_len %i", + mav_msg_counter, + result.frame_len); + mav_msg_counter++; + // Check if the new message will fit in the buffer + if(*serial_buff_pos == 0 && + result.frame_len > DB_PARAM_SERIAL_PACK_SIZE) { + // frame_len is bigger than DB_PARAM_SERIAL_PACK_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_chunk_len = 0; + do { + next_chunk_len = result.frame_len - sent_bytes; + if(next_chunk_len > DB_PARAM_SERIAL_PACK_SIZE) { + next_chunk_len = DB_PARAM_SERIAL_PACK_SIZE; + } + else { + } + 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_PARAM_SERIAL_PACK_SIZE) { + // New message won't fit into the buffer, send buffer first + 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); + *serial_buff_pos += result.frame_len; + } + else { + // 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); + *serial_buff_pos += result.frame_len; + } + + // Decode message and react to it if it was for us + fmav_frame_buf_to_msg(&msg, &result, mav_parser_rx_buf); + if(result.res == FASTMAVLINK_PARSE_RESULT_OK) { + if(fmav_msg_is_for_me( + db_get_mav_sys_id(), db_get_mav_comp_id(), &msg)) { + // This will also instantly send a response. That is OK at this + // position since we buffer and send out only full packets and this + // "MAVLink packet injection" into the stream will not mess with the + // main MAVLink packet stream. + handle_mavlink_message(&msg, + tcp_clients, + udp_conns, + &fmav_status_serial, + DB_MAVLINK_DATA_ORIGIN_SERIAL); + } + else { + // message was not for us so ignore it } + } + else { + // message had a parsing error - we cannot decode it so skip + } } - // done parsing all received data via radio link + else { + // do nothing since parser had a now new message, LENGTH_ERROR, CRC_ERROR + // or SIGNATURE_ERROR + } + } + // done parsing all received data via UART } -/** - * Parses MAVLink messages and sends them via the radio link. - * This function reads data from the serial interface, parses it for complete MAVLink messages, and sends those messages in a buffer. - * It ensures that only complete messages are sent and that the buffer does not exceed TRANS_BUFF_SIZE. - * Checks for a serial read timeout. In case timeout is reached all data read from serial so far will be flushed to radio interface - * The parsing is done semi-transparent as in: parser understands the MavLink frame format but performs no further checks - * - * @param tcp_clients Array of connected TCP clients - * @param up_conns Structure containing all UDP connection data including the sockets - * @param serial_buffer Buffer that gets filled with data and then sent via radio, shall be >x2 the max payload - * @param serial_buff_pos Number of bytes already read for the current packet - */ -void db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns, uint8_t *serial_buffer, - unsigned int *serial_buff_pos) { - static uint mav_msg_counter = 0; - static uint8_t mav_parser_rx_buf[296]; // at least 280 bytes which is the max len for a MAVLink v2 packet - static fmav_status_t fmav_status_serial; // fmav parser status struct for serial parser - uint8_t uart_read_buf[DB_PARAM_SERIAL_PACK_SIZE]; - // timeout variables - static TickType_t last_tick = 0; // time when we received something from the serial interface for the last time - static TickType_t current_tick = 0; - current_tick = xTaskGetTickCount(); // get current time - - // Read bytes from serial link (UART or USB/JTAG interface) - int bytes_read = db_read_serial(uart_read_buf, DB_PARAM_SERIAL_PACK_SIZE); - - if (bytes_read == 0) { - // did not read anything this cycle -> check serial read timeout - if (current_tick - last_tick >= pdMS_TO_TICKS(DB_SERIAL_READ_TIMEOUT_MS)) { - // serial read timeout detected - 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) { - 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 - } - } else { - // nothing received but no timeout yet -> do nothing - } - } else { - // have received something -> reset timeout - last_tick = current_tick; +void +db_read_serial_parse_transparent(int tcp_clients[], + udp_conn_list_t *udp_connection, + uint8_t serial_buffer[], + unsigned int *serial_read_bytes) +{ + uint16_t read; + static bool serial_read_timeout_reached = false; + static TickType_t last_tick = 0; // time when we received something from the + // serial interface for the last time + static TickType_t current_tick = 0; + current_tick = xTaskGetTickCount(); // get current time + // read from UART directly into TCP & UDP send buffer + if((read = db_read_serial( + &serial_buffer[*serial_read_bytes], + (DB_PARAM_SERIAL_PACK_SIZE - *serial_read_bytes))) > 0) { + serial_total_byte_count += read; // increase total bytes read via UART + *serial_read_bytes += read; // set new buffer position + serial_read_timeout_reached = false; // reset serial read timeout + last_tick = current_tick; // reset time for serial read timeout + } + else { + /* did not read anything this cycle -> check serial read timeout */ + if(current_tick - last_tick >= pdMS_TO_TICKS(DB_SERIAL_READ_TIMEOUT_MS)) { + serial_read_timeout_reached = true; + last_tick = current_tick; } - - serial_total_byte_count += bytes_read; // increase total bytes read via serial interface - // Parse each byte received - for (int i = 0; i < bytes_read; ++i) { - fmav_result_t result = {0}; - - if (fmav_parse_and_check_to_frame_buf(&result, mav_parser_rx_buf, &fmav_status_serial, uart_read_buf[i])) { - ESP_LOGD(TAG, "Parser detected a full message (%i total): result.frame_len %i", mav_msg_counter, - result.frame_len); - mav_msg_counter++; - // Check if the new message will fit in the buffer - if (*serial_buff_pos == 0 && result.frame_len > DB_PARAM_SERIAL_PACK_SIZE) { - // frame_len is bigger than DB_PARAM_SERIAL_PACK_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_chunk_len = 0; - do { - next_chunk_len = result.frame_len - sent_bytes; - if (next_chunk_len > DB_PARAM_SERIAL_PACK_SIZE) { - next_chunk_len = DB_PARAM_SERIAL_PACK_SIZE; - } else {} - 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_PARAM_SERIAL_PACK_SIZE) { - // New message won't fit into the buffer, send buffer first - 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); - *serial_buff_pos += result.frame_len; - } else { - // 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); - *serial_buff_pos += result.frame_len; - } - - // Decode message and react to it if it was for us - fmav_frame_buf_to_msg(&msg, &result, mav_parser_rx_buf); - if (result.res == FASTMAVLINK_PARSE_RESULT_OK) { - if (fmav_msg_is_for_me(db_get_mav_sys_id(), db_get_mav_comp_id(), &msg)) { - // This will also instantly send a response. That is OK at this position since we buffer and send - // out only full packets and this "MAVLink packet injection" into the stream will not mess with the - // main MAVLink packet stream. - handle_mavlink_message(&msg, tcp_clients, udp_conns, &fmav_status_serial, - DB_MAVLINK_DATA_ORIGIN_SERIAL); - } else { - // message was not for us so ignore it - } - } else { - // message had a parsing error - we cannot decode it so skip - } - } else { - // do nothing since parser had a now new message, LENGTH_ERROR, CRC_ERROR or SIGNATURE_ERROR - } + else { + // no timeout detected } - // done parsing all received data via UART + } + // send serial data over the air interface + if(*serial_read_bytes >= DB_PARAM_SERIAL_PACK_SIZE || + (serial_read_timeout_reached && *serial_read_bytes > 0)) { + 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 + } } -/** - * Reads TRANS_RD_BYTES_NUM bytes from serial interface and checks if we already got enough bytes to send them out. - * Timeout ensures that no data is getting stuck in the buffer. Once serial read timeout is reached, the buffer will be - * flushed to the radio interface (send what we have) - * - * @param tcp_clients Array of connected TCP clients - * @param udp_connection Structure containing all UDP connection data including the sockets - * @param serial_buffer Buffer that gets filled with data and then sent via radio - * @param serial_read_bytes Number of bytes already read for the current packet - */ -void db_read_serial_parse_transparent(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_t serial_buffer[], - unsigned int *serial_read_bytes) { - uint16_t read; - static bool serial_read_timeout_reached = false; - static TickType_t last_tick = 0; // time when we received something from the serial interface for the last time - static TickType_t current_tick = 0; - current_tick = xTaskGetTickCount(); // get current time - // read from UART directly into TCP & UDP send buffer - if ((read = db_read_serial(&serial_buffer[*serial_read_bytes], (DB_PARAM_SERIAL_PACK_SIZE - *serial_read_bytes))) > 0) { - serial_total_byte_count += read; // increase total bytes read via UART - *serial_read_bytes += read; // set new buffer position - serial_read_timeout_reached = false; // reset serial read timeout - last_tick = current_tick; // reset time for serial read timeout - } else { - /* did not read anything this cycle -> check serial read timeout */ - if (current_tick - last_tick >= pdMS_TO_TICKS(DB_SERIAL_READ_TIMEOUT_MS)) { - serial_read_timeout_reached = true; - last_tick = current_tick; - } else { - // no timeout detected +void +db_parse_mavlink_from_radio(int *tcp_clients, udp_conn_list_t *udp_conns, + uint8_t *buffer, int bytes_read) +{ + static uint8_t mav_parser_rx_buf[296]; // at least 280 bytes which is the max + // len for a MAVLink v2 packet + static fmav_status_t fmav_status_radio; // fmav parser status struct for + // radio/ESPNOW/WiFi parser + + // Parse each byte received + for(int i = 0; i < bytes_read; ++i) { + fmav_result_t result = { 0 }; + if(fmav_parse_and_check_to_frame_buf( + &result, mav_parser_rx_buf, &fmav_status_radio, buffer[i])) { + // Parser detected a full message, write to serial + db_write_to_serial(mav_parser_rx_buf, result.frame_len); + // Decode message and react to it if it was for us + fmav_frame_buf_to_msg(&msg, &result, mav_parser_rx_buf); + if(result.res == FASTMAVLINK_PARSE_RESULT_OK) { + if(fmav_msg_is_for_me( + db_get_mav_sys_id(), db_get_mav_comp_id(), &msg)) { + handle_mavlink_message(&msg, + tcp_clients, + udp_conns, + &fmav_status_radio, + DB_MAVLINK_DATA_ORIGIN_RADIO); } + else { + // message was not for us so ignore it + } + } + else { + switch(result.res) { + case FASTMAVLINK_PARSE_RESULT_MSGID_UNKNOWN: + ESP_LOGW(TAG, + "fastmavlink parser had an error " + "FASTMAVLINK_PARSE_RESULT_MSGID_UNKNOWN msgID: %lu", + result.msgid); + break; + case FASTMAVLINK_PARSE_RESULT_LENGTH_ERROR: + ESP_LOGW(TAG, + "fastmavlink parser had an error " + "FASTMAVLINK_PARSE_RESULT_LENGTH_ERROR msgID: %lu", + result.msgid); + break; + case FASTMAVLINK_PARSE_RESULT_CRC_ERROR: + ESP_LOGW(TAG, + "fastmavlink parser had an error " + "FASTMAVLINK_PARSE_RESULT_CRC_ERROR msgID: %lu", + result.msgid); + break; + case FASTMAVLINK_PARSE_RESULT_SIGNATURE_ERROR: + ESP_LOGW(TAG, + "fastmavlink parser had an error " + "FASTMAVLINK_PARSE_RESULT_SIGNATURE_ERROR msgID: %lu", + result.msgid); + break; + default: + ESP_LOGW(TAG, + "fastmavlink parser had an error parsing the message: %i", + result.res); + break; + } + } } - // send serial data over the air interface - if (*serial_read_bytes >= DB_PARAM_SERIAL_PACK_SIZE || (serial_read_timeout_reached && *serial_read_bytes > 0)) { - 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 + else { + // do nothing since parser did not detect a message } + } + // done parsing all received data via radio link +} + +void +db_route_mavlink_response(uint8_t *buffer, uint16_t length, + enum DB_MAVLINK_DATA_ORIGIN origin, int *tcp_clients, + udp_conn_list_t *udp_conns) +{ + if(origin == DB_MAVLINK_DATA_ORIGIN_SERIAL) { + db_write_to_serial(buffer, length); + } + else if(origin == DB_MAVLINK_DATA_ORIGIN_RADIO) { + 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!"); + } } \ No newline at end of file diff --git a/main/db_serial.h b/main/db_serial.h index d74baf6..9ca8bd3 100644 --- a/main/db_serial.h +++ b/main/db_serial.h @@ -1,4 +1,4 @@ -/* +/****************************************************************************** * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * * Copyright 2024 Wolfgang Christl @@ -15,7 +15,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * - */ + ******************************************************************************/ #ifndef DB_ESP32_DB_SERIAL_H #define DB_ESP32_DB_SERIAL_H @@ -23,34 +23,125 @@ #include "msp_ltm_serial.h" #include "db_esp32_control.h" -#define UART_NUM UART_NUM_1 // The UART interface of the ESP32 we use -#define DB_SERIAL_READ_TIMEOUT_MS_DEFAULT 50 // Serial read timeout for transparent and MAVLink mode, after that the packet will be sent over the air even when the max. packet size was not reached. +#define UART_NUM UART_NUM_1 // The UART interface of the ESP32 we use +#define DB_SERIAL_READ_TIMEOUT_MS_DEFAULT \ + 50 // Serial read timeout for transparent and MAVLink mode, after that the + // packet will be sent over the air even when the max. packet size was not + // reached. -enum DB_MAVLINK_DATA_ORIGIN { - DB_MAVLINK_DATA_ORIGIN_SERIAL, - DB_MAVLINK_DATA_ORIGIN_RADIO +enum DB_MAVLINK_DATA_ORIGIN +{ + DB_MAVLINK_DATA_ORIGIN_SERIAL, + DB_MAVLINK_DATA_ORIGIN_RADIO }; -typedef union { - float f; - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; +typedef union +{ + float f; + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; } float_int_union; -int open_serial_socket(); -void write_to_serial(const uint8_t data_buffer[], unsigned int data_length); -void db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_t msp_message_buffer[], +/****************************************************************************** + * Public Function Declaration + ******************************************************************************/ + +/****************************************************************************** + * Opens a serial socket for communication with a serial source. On the GND + * this is the GCS and on the air side this is the flight controller. Depending + * on the configuration it may open a native UART socket or a JTAG based serial + * interface. The JTAG serial based interface is a special feature of official + * DroneBridge for ESP32 boards. Uses the onboard USB for serial I/O with GCS. + * No FTDI required. + * + * @return ESP_FAIL on failure + ******************************************************************************/ +esp_err_t open_serial_socket(); + +/****************************************************************************** + * Writes data from buffer to the opened serial device + * @param data_buffer Payload to write to UART + * @param data_length Size of payload to write to UART + ******************************************************************************/ +void db_write_to_serial(const uint8_t data_buffer[], unsigned int data_length); + +/****************************************************************************** + * @brief Reads serial interface, parses & sends complete MSP & LTM messages + * over the air. + ******************************************************************************/ +void db_parse_msp_ltm(int tcp_clients[], udp_conn_list_t *udp_connection, + uint8_t msp_message_buffer[], unsigned int *serial_read_bytes, msp_ltm_port_t *db_msp_ltm_port); -void db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns, uint8_t *serial_buffer, unsigned int *serial_buff_pos); -void db_read_serial_parse_transparent(int tcp_clients[], udp_conn_list_t *udp_connection, uint8_t serial_buffer[], + +/****************************************************************************** + * Parses MAVLink messages and sends them via the radio link. + * This function reads data from the serial interface, parses it for complete + * MAVLink messages, and sends those messages in a buffer. It ensures that only + * complete messages are sent and that the buffer does not exceed + * TRANS_BUFF_SIZE. Checks for a serial read timeout. In case timeout is + * reached all data read from serial so far will be flushed to radio interface + * The parsing is done semi-transparent as in: parser understands the MavLink + * frame format but performs no further checks + * + * @param tcp_clients Array of connected TCP clients + * @param up_conns Structure containing all UDP connection data including the + * sockets + * @param serial_buffer Buffer that gets filled with data and then sent via + * radio, shall be >x2 the max payload + * @param serial_buff_pos Number of bytes already read for the current packet + ******************************************************************************/ +void db_read_serial_parse_mavlink(int *tcp_clients, udp_conn_list_t *udp_conns, + uint8_t *serial_buffer, + unsigned int *serial_buff_pos); + +/****************************************************************************** + * Reads TRANS_RD_BYTES_NUM bytes from serial interface and checks if we + * already got enough bytes to send them out. Timeout ensures that no data is + * getting stuck in the buffer. Once serial read timeout is reached, the buffer + * will be flushed to the radio interface (send what we have) + * + * @param tcp_clients Array of connected TCP clients + * @param udp_connection Structure containing all UDP connection data including + * the sockets + * @param serial_buffer Buffer that gets filled with data and then sent via + * radio + * @param serial_read_bytes Number of bytes already read for the current packet + ******************************************************************************/ +void db_read_serial_parse_transparent(int tcp_clients[], + udp_conn_list_t *udp_connection, + uint8_t serial_buffer[], unsigned int *serial_read_bytes); -void db_parse_mavlink_from_radio(int *tcp_clients, udp_conn_list_t *udp_conns, uint8_t *buffer, int bytes_read); -void db_route_mavlink_response(uint8_t *buffer, uint16_t length, enum DB_MAVLINK_DATA_ORIGIN origin, int *tcp_clients, - udp_conn_list_t *udp_conns); -#endif //DB_ESP32_DB_SERIAL_H +/****************************************************************************** + * Parses MAVLink coming from WiFi/ESPNOW - and sends the packet to the serial + * output. + * + * @param tcp_clients Array of connected TCP clients + * @param up_conns Structure containing all UDP connection data including the + * sockets + * @param buffer Buffer containing the raw bytes to be parsed + * @param bytes_read Number of bytes in the buffer + * @param origin Origin of the data - serial link or radio link + ******************************************************************************/ +void db_parse_mavlink_from_radio(int *tcp_clients, udp_conn_list_t *udp_conns, + uint8_t *buffer, int bytes_read); + +/****************************************************************************** + * We received some MAVLink request via the origin. Decide on which interface + * to respond with an answer + * @param buffer Data to send + * @param length Data length to send + * @param origin Origin of the MAVLink request + * @param tcp_clients List of connected TCP client + * @param udp_conns List of active UDP connections + ******************************************************************************/ +void db_route_mavlink_response(uint8_t *buffer, uint16_t length, + enum DB_MAVLINK_DATA_ORIGIN origin, + int *tcp_clients, udp_conn_list_t *udp_conns); + +#endif // DB_ESP32_DB_SERIAL_H diff --git a/main/globals.h b/main/globals.h index 1f264b4..100169c 100644 --- a/main/globals.h +++ b/main/globals.h @@ -24,18 +24,28 @@ #include #include "db_esp32_control.h" -extern char CURRENT_CLIENT_IP[IP4ADDR_STRLEN_MAX]; // IP address of the ESP32 when we are in client mode connected +extern char + CURRENT_CLIENT_IP[IP4ADDR_STRLEN_MAX]; // IP address of the ESP32 when we are + // in client mode connected extern uint8_t DB_RADIO_IS_OFF; -extern db_esp_signal_quality_t db_esp_signal_quality; // used on AIR/station side to store RSSI information -extern wifi_sta_list_t wifi_sta_list; // updated when ESP32 is in ap mode. Contains RSSI of every connected station -extern uint8_t LOCAL_MAC_ADDRESS[6]; // filled with the mac address during init of WiFi interface -extern uint8_t DB_MAV_SYS_ID; // stores the local system ID - set by heartbeat that is received via UART (UART is the local connection) +extern db_esp_signal_quality_t + db_esp_signal_quality; // used on AIR/station side to store RSSI information +extern wifi_sta_list_t + wifi_sta_list; // updated when ESP32 is in ap mode. Contains RSSI of every + // connected station +extern uint8_t LOCAL_MAC_ADDRESS[6]; // filled with the mac address during init + // of WiFi interface +extern uint8_t + DB_MAV_SYS_ID; // stores the local system ID - set by heartbeat that is + // received via UART (UART is the local connection) -extern uint32_t serial_total_byte_count; // Total bytes read from serial link (UART or USB/JTAG) +extern uint32_t serial_total_byte_count; // Total bytes read from serial link + // (UART or USB/JTAG) extern int8_t num_connected_tcp_clients; -extern udp_conn_list_t *udp_conn_list; // List of UDP clients that the ESP32 will send to +extern udp_conn_list_t + *udp_conn_list; // List of UDP clients that the ESP32 will send to // extern int WIFI_ESP_MAXIMUM_RETRY; -#endif //DB_ESP32_GLOBALS_H +#endif // DB_ESP32_GLOBALS_H diff --git a/main/http_server.c b/main/http_server.c index 0157507..62fdd81 100644 --- a/main/http_server.c +++ b/main/http_server.c @@ -34,96 +34,109 @@ #include "db_serial.h" #define TAG "DB_HTTP_REST" -#define REST_CHECK(a, str, goto_tag, ...) \ - do \ - { \ - if (!(a)) \ - { \ - ESP_LOGE(TAG, "%s(%d): " str, __FUNCTION__, __LINE__, ##__VA_ARGS__); \ - goto goto_tag; \ - } \ - } while (0) +#define REST_CHECK(a, str, goto_tag, ...) \ + do { \ + if(!(a)) { \ + ESP_LOGE(TAG, "%s(%d): " str, __FUNCTION__, __LINE__, ##__VA_ARGS__); \ + goto goto_tag; \ + } \ + } while(0) #define FILE_PATH_MAX (ESP_VFS_PATH_MAX + 128) #define SCRATCH_BUFSIZE (10240) -typedef struct rest_server_context { - char base_path[ESP_VFS_PATH_MAX + 1]; - char scratch[SCRATCH_BUFSIZE]; +typedef struct rest_server_context +{ + char base_path[ESP_VFS_PATH_MAX + 1]; + char scratch[SCRATCH_BUFSIZE]; } rest_server_context_t; -#define CHECK_FILE_EXTENSION(filename, ext) (strcasecmp(&filename[strlen(filename) - strlen(ext)], ext) == 0) +#define CHECK_FILE_EXTENSION(filename, ext) \ + (strcasecmp(&filename[strlen(filename) - strlen(ext)], ext) == 0) /** * Set HTTP response content type according to file extension */ -static esp_err_t set_content_type_from_file(httpd_req_t *req, const char *filepath) { - const char *type = "text/plain"; - if (CHECK_FILE_EXTENSION(filepath, ".html")) { - type = "text/html"; - } else if (CHECK_FILE_EXTENSION(filepath, ".js")) { - type = "application/javascript"; - } else if (CHECK_FILE_EXTENSION(filepath, ".css")) { - type = "text/css"; - } else if (CHECK_FILE_EXTENSION(filepath, ".png")) { - type = "image/png"; - } else if (CHECK_FILE_EXTENSION(filepath, ".ico")) { - type = "image/x-icon"; - } else if (CHECK_FILE_EXTENSION(filepath, ".svg")) { - type = "text/xml"; - } - return httpd_resp_set_type(req, type); +static esp_err_t +set_content_type_from_file(httpd_req_t *req, const char *filepath) +{ + const char *type = "text/plain"; + if(CHECK_FILE_EXTENSION(filepath, ".html")) { + type = "text/html"; + } + else if(CHECK_FILE_EXTENSION(filepath, ".js")) { + type = "application/javascript"; + } + else if(CHECK_FILE_EXTENSION(filepath, ".css")) { + type = "text/css"; + } + else if(CHECK_FILE_EXTENSION(filepath, ".png")) { + type = "image/png"; + } + else if(CHECK_FILE_EXTENSION(filepath, ".ico")) { + type = "image/x-icon"; + } + else if(CHECK_FILE_EXTENSION(filepath, ".svg")) { + type = "text/xml"; + } + return httpd_resp_set_type(req, type); } /** * Send HTTP response with the contents of the requested file */ -static esp_err_t rest_common_get_handler(httpd_req_t *req) { - char filepath[FILE_PATH_MAX]; - - rest_server_context_t *rest_context = (rest_server_context_t *) req->user_ctx; - strlcpy(filepath, rest_context->base_path, sizeof(filepath)); - if (req->uri[strlen(req->uri) - 1] == '/') { - strlcat(filepath, "/index.html", sizeof(filepath)); - } else { - strlcat(filepath, req->uri, sizeof(filepath)); +static esp_err_t +rest_common_get_handler(httpd_req_t *req) +{ + char filepath[FILE_PATH_MAX]; + + rest_server_context_t *rest_context = (rest_server_context_t *)req->user_ctx; + strlcpy(filepath, rest_context->base_path, sizeof(filepath)); + if(req->uri[strlen(req->uri) - 1] == '/') { + strlcat(filepath, "/index.html", sizeof(filepath)); + } + else { + strlcat(filepath, req->uri, sizeof(filepath)); + } + int fd = open(filepath, O_RDONLY, 0); + if(fd == -1) { + ESP_LOGE(TAG, "Failed to open file : %s", filepath); + /* Respond with 500 Internal Server Error */ + httpd_resp_send_err( + req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to read existing file"); + return ESP_FAIL; + } + + set_content_type_from_file(req, filepath); + + char *chunk = rest_context->scratch; + ssize_t read_bytes; + do { + /* Read file in chunks into the scratch buffer */ + read_bytes = read(fd, chunk, SCRATCH_BUFSIZE); + if(read_bytes == -1) { + ESP_LOGE(TAG, "Failed to read file : %s", filepath); } - int fd = open(filepath, O_RDONLY, 0); - if (fd == -1) { - ESP_LOGE(TAG, "Failed to open file : %s", filepath); + else if(read_bytes > 0) { + /* Send the buffer contents as HTTP response chunk */ + if(httpd_resp_send_chunk(req, chunk, read_bytes) != ESP_OK) { + close(fd); + ESP_LOGE(TAG, "File sending failed!"); + /* Abort sending file */ + httpd_resp_sendstr_chunk(req, NULL); /* Respond with 500 Internal Server Error */ - httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to read existing file"); + httpd_resp_send_err( + req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to send file"); return ESP_FAIL; + } } - - set_content_type_from_file(req, filepath); - - char *chunk = rest_context->scratch; - ssize_t read_bytes; - do { - /* Read file in chunks into the scratch buffer */ - read_bytes = read(fd, chunk, SCRATCH_BUFSIZE); - if (read_bytes == -1) { - ESP_LOGE(TAG, "Failed to read file : %s", filepath); - } else if (read_bytes > 0) { - /* Send the buffer contents as HTTP response chunk */ - if (httpd_resp_send_chunk(req, chunk, read_bytes) != ESP_OK) { - close(fd); - ESP_LOGE(TAG, "File sending failed!"); - /* Abort sending file */ - httpd_resp_sendstr_chunk(req, NULL); - /* Respond with 500 Internal Server Error */ - httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to send file"); - return ESP_FAIL; - } - } - } while (read_bytes > 0); - /* Close file after sending complete */ - close(fd); - ESP_LOGI(TAG, "File sending complete"); - /* Respond with an empty chunk to signal HTTP response completion */ - httpd_resp_send_chunk(req, NULL, 0); - return ESP_OK; + } while(read_bytes > 0); + /* Close file after sending complete */ + close(fd); + ESP_LOGI(TAG, "File sending complete"); + /* Respond with an empty chunk to signal HTTP response completion */ + httpd_resp_send_chunk(req, NULL, 0); + return ESP_OK; } /** @@ -131,56 +144,62 @@ static esp_err_t rest_common_get_handler(httpd_req_t *req) { * @param req * @return ESP error code */ -static esp_err_t settings_post_handler(httpd_req_t *req) { - int total_len = req->content_len; - int cur_len = 0; - char *buf = ((rest_server_context_t *) (req->user_ctx))->scratch; - int received = 0; - if (total_len >= SCRATCH_BUFSIZE) { - // This should be HTTPD_414_PAYLOAD_TOO_LARGE, but that's not - // implemented yet, so use 400 Bad Request instead. - httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "content too long"); - return ESP_FAIL; - } - while (cur_len < total_len) { - received = httpd_req_recv(req, buf + cur_len, total_len); - if (received <= 0) { - /* Respond with 500 Internal Server Error */ - httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to post control value"); - return ESP_FAIL; - } - cur_len += received; +static esp_err_t +settings_post_handler(httpd_req_t *req) +{ + int total_len = req->content_len; + int cur_len = 0; + char *buf = ((rest_server_context_t *)(req->user_ctx))->scratch; + int received = 0; + if(total_len >= SCRATCH_BUFSIZE) { + // This should be HTTPD_414_PAYLOAD_TOO_LARGE, but that's not + // implemented yet, so use 400 Bad Request instead. + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "content too long"); + return ESP_FAIL; + } + while(cur_len < total_len) { + received = httpd_req_recv(req, buf + cur_len, total_len); + if(received <= 0) { + /* Respond with 500 Internal Server Error */ + httpd_resp_send_err( + req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to post control value"); + return ESP_FAIL; } - buf[total_len] = '\0'; - - cJSON *root = cJSON_Parse(buf); - - db_param_read_all_params_json(root); - db_write_settings_to_nvs(); - ESP_LOGI(TAG, "Settings changed!"); - - cJSON_Delete(root); - httpd_resp_sendstr(req, "{\n" - " \"status\": \"success\",\n" - " \"msg\": \"Settings changed! Rebooting ...\"\n" - " }"); - vTaskDelay(1000 / portTICK_PERIOD_MS); // wait to allow the website displaying the success message - // Send reboot message - httpd_resp_set_type(req, "application/json"); - cJSON *reboot_info = cJSON_CreateObject(); - cJSON_AddStringToObject(reboot_info, "msg", "Rebooting!"); - const char *sys_info = cJSON_Print(reboot_info); - httpd_resp_sendstr(req, sys_info); - free((void *) sys_info); - cJSON_Delete(reboot_info); - vTaskDelay(500 / portTICK_PERIOD_MS); // wait 1s to allow the website displaying the success message - esp_restart(); - return ESP_OK; + cur_len += received; + } + buf[total_len] = '\0'; + + cJSON *root = cJSON_Parse(buf); + + db_param_read_all_params_json(root); + db_write_settings_to_nvs(); + ESP_LOGI(TAG, "Settings changed!"); + + cJSON_Delete(root); + httpd_resp_sendstr(req, + "{\n" + " \"status\": \"success\",\n" + " \"msg\": \"Settings changed! Rebooting ...\"\n" + " }"); + vTaskDelay(1000 / portTICK_PERIOD_MS); // wait to allow the website + // displaying the success message + // Send reboot message + httpd_resp_set_type(req, "application/json"); + cJSON *reboot_info = cJSON_CreateObject(); + cJSON_AddStringToObject(reboot_info, "msg", "Rebooting!"); + const char *sys_info = cJSON_Print(reboot_info); + httpd_resp_sendstr(req, sys_info); + free((void *)sys_info); + cJSON_Delete(reboot_info); + vTaskDelay(500 / portTICK_PERIOD_MS); // wait 1s to allow the website + // displaying the success message + esp_restart(); + return ESP_OK; } /** - * Process incoming UDP connection add request. Only one IPv4 connection can be added at a time - * Expecting JSON in the form of: + * Process incoming UDP connection add request. Only one IPv4 connection can be + * added at a time Expecting JSON in the form of: * { * "ip": "XXX.XXX.XXX.XXX", * "port": 452 @@ -188,93 +207,111 @@ static esp_err_t settings_post_handler(httpd_req_t *req) { * @param req * @return */ -static esp_err_t settings_clients_udp_post(httpd_req_t *req) { - int total_len = req->content_len; - int cur_len = 0; - char *buf = ((rest_server_context_t *) (req->user_ctx))->scratch; - int received = 0; - if (total_len >= SCRATCH_BUFSIZE) { - httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "content too long"); - return ESP_FAIL; +static esp_err_t +settings_clients_udp_post(httpd_req_t *req) +{ + int total_len = req->content_len; + int cur_len = 0; + char *buf = ((rest_server_context_t *)(req->user_ctx))->scratch; + int received = 0; + if(total_len >= SCRATCH_BUFSIZE) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "content too long"); + return ESP_FAIL; + } + while(cur_len < total_len) { + received = httpd_req_recv(req, buf + cur_len, total_len); + if(received <= 0) { + /* Respond with 500 Internal Server Error */ + httpd_resp_send_err( + req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to post control value"); + return ESP_FAIL; } - while (cur_len < total_len) { - received = httpd_req_recv(req, buf + cur_len, total_len); - if (received <= 0) { - /* Respond with 500 Internal Server Error */ - httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Failed to post control value"); - return ESP_FAIL; - } - cur_len += received; + cur_len += received; + } + buf[total_len] = '\0'; + + // Obtain & process JSON from request + cJSON *root = cJSON_Parse(buf); + + int new_udp_port = 0; + char new_ip[IP4ADDR_STRLEN_MAX]; + uint8_t save_to_nvm = false; + cJSON *json = + cJSON_GetObjectItem(root, (char *)db_param_udp_client_ip.db_name); + if(json) + strncpy(new_ip, json->valuestring, sizeof(new_ip)); + new_ip[IP4ADDR_STRLEN_MAX - 1] = '\0'; // to remove warning and to be sure + json = cJSON_GetObjectItem(root, (char *)db_param_udp_client_port.db_name); + if(json) + new_udp_port = json->valueint; + json = cJSON_GetObjectItem(root, "save"); + if(json && cJSON_IsBool(json)) { + if(cJSON_IsTrue(json)) { + save_to_nvm = true; } - buf[total_len] = '\0'; - - // Obtain & process JSON from request - cJSON *root = cJSON_Parse(buf); - - int new_udp_port = 0; - char new_ip[IP4ADDR_STRLEN_MAX]; - uint8_t save_to_nvm = false; - cJSON *json = cJSON_GetObjectItem(root, (char *) db_param_udp_client_ip.db_name); - if (json) strncpy(new_ip, json->valuestring, sizeof(new_ip)); - new_ip[IP4ADDR_STRLEN_MAX-1] = '\0'; // to remove warning and to be sure - json = cJSON_GetObjectItem(root, (char *) db_param_udp_client_port.db_name); - if (json) new_udp_port = json->valueint; - json = cJSON_GetObjectItem(root, "save"); - if (json && cJSON_IsBool(json)) { - if(cJSON_IsTrue(json)) { - save_to_nvm = true; - } else { - save_to_nvm = false; - } - } else {} - - // populate the UDP connections list with a new connection - struct sockaddr_in new_sockaddr; - memset(&new_sockaddr, 0, sizeof(new_sockaddr)); - new_sockaddr.sin_family = AF_INET; - inet_pton(AF_INET, new_ip, &new_sockaddr.sin_addr); - new_sockaddr.sin_port = htons(new_udp_port); - struct db_udp_client_t new_udp_client = { - .udp_client = new_sockaddr, - .mac = {0, 0, 0, 0, 0, 0} // dummy MAC - }; - // udp_conn_list is initialized as the very first thing during startup - we expect it to be there - bool success = add_to_known_udp_clients(udp_conn_list, new_udp_client, save_to_nvm); - - // Clean up - cJSON_Delete(root); - if (success) { - httpd_resp_sendstr(req, "{\n" - " \"status\": \"success\",\n" - " \"msg\": \"Added UDP connection!\"\n" - " }"); - } else { - httpd_resp_sendstr(req, "{\n" - " \"status\": \"failed\",\n" - " \"msg\": \"Failed to add UDP connection!\"\n" - " }"); + else { + save_to_nvm = false; } - - return ESP_OK; + } + else { + } + + // populate the UDP connections list with a new connection + struct sockaddr_in new_sockaddr; + memset(&new_sockaddr, 0, sizeof(new_sockaddr)); + new_sockaddr.sin_family = AF_INET; + inet_pton(AF_INET, new_ip, &new_sockaddr.sin_addr); + new_sockaddr.sin_port = htons(new_udp_port); + struct db_udp_client_t new_udp_client = { + .udp_client = new_sockaddr, .mac = { 0, 0, 0, 0, 0, 0 } // dummy MAC + }; + // udp_conn_list is initialized as the very first thing during startup - we + // expect it to be there + bool success = + db_add_to_known_udp_clients(udp_conn_list, new_udp_client, save_to_nvm); + + // Clean up + cJSON_Delete(root); + if(success) { + httpd_resp_sendstr(req, + "{\n" + " \"status\": \"success\",\n" + " \"msg\": \"Added UDP connection!\"\n" + " }"); + } + else { + httpd_resp_sendstr(req, + "{\n" + " \"status\": \"failed\",\n" + " \"msg\": \"Failed to add UDP connection!\"\n" + " }"); + } + + return ESP_OK; } /** * Process a request that shall clear all active UDP connections. - * ESP32 will remove all maintained UDP connections from its internal list and UDP clients will have to register again. - * This also clears the one UDP client connection that gets saved to NVM. + * ESP32 will remove all maintained UDP connections from its internal list and + * UDP clients will have to register again. This also clears the one UDP client + * connection that gets saved to NVM. * * @param req * @return ESP_OK if all was good. Else ESP_FAIL */ -static esp_err_t settings_clients_clear_udp_get(httpd_req_t *req) { - for (int i = 0; i < udp_conn_list->size; ++i) { - memset(&udp_conn_list->db_udp_clients[i], 0, sizeof(struct db_udp_client_t)); - } - udp_conn_list->size = 0; - ESP_LOGI(TAG, "Removed all UDP clients from list!"); - // Clear saved client as well. Pass any client since it will be ignored as long as clear_client is set to true. - save_udp_client_to_nvm(&udp_conn_list->db_udp_clients[0], true); - return ESP_OK; +static esp_err_t +settings_clients_clear_udp_get(httpd_req_t *req) +{ + for(int i = 0; i < udp_conn_list->size; ++i) { + memset( + &udp_conn_list->db_udp_clients[i], 0, sizeof(struct db_udp_client_t)); + } + udp_conn_list->size = 0; + ESP_LOGI(TAG, "Removed all UDP clients from list!"); + // Clear saved client as well. Pass any client since it will be ignored as + // long as clear_client is set to true. + db_save_udp_client_to_nvm(&udp_conn_list->db_udp_clients[0], true); + return ESP_OK; } /** @@ -282,81 +319,107 @@ static esp_err_t settings_clients_clear_udp_get(httpd_req_t *req) { * @param req * @return */ -static esp_err_t system_info_get_handler(httpd_req_t *req) { - httpd_resp_set_type(req, "application/json"); - cJSON *root = cJSON_CreateObject(); - esp_chip_info_t chip_info; - esp_chip_info(&chip_info); - cJSON_AddStringToObject(root, "idf_version", IDF_VER); - cJSON_AddNumberToObject(root, "db_build_version", DB_BUILD_VERSION); - cJSON_AddNumberToObject(root, "major_version", DB_MAJOR_VERSION); - cJSON_AddNumberToObject(root, "minor_version", DB_MINOR_VERSION); - cJSON_AddNumberToObject(root, "patch_version", DB_PATCH_VERSION); - cJSON_AddStringToObject(root, "maturity_version", DB_MATURITY_VERSION); - cJSON_AddNumberToObject(root, "esp_chip_model", chip_info.model); - cJSON_AddNumberToObject(root, "has_rf_switch", DB_HAS_RF_SWITCH); - char mac_str[18]; - sprintf(mac_str, "%02X:%02X:%02X:%02X:%02X:%02X", - LOCAL_MAC_ADDRESS[0], LOCAL_MAC_ADDRESS[1], LOCAL_MAC_ADDRESS[2], LOCAL_MAC_ADDRESS[3], LOCAL_MAC_ADDRESS[4], LOCAL_MAC_ADDRESS[5]); - cJSON_AddStringToObject(root, "esp_mac", mac_str); +static esp_err_t +system_info_get_handler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "application/json"); + cJSON *root = cJSON_CreateObject(); + esp_chip_info_t chip_info; + esp_chip_info(&chip_info); + cJSON_AddStringToObject(root, "idf_version", IDF_VER); + cJSON_AddNumberToObject(root, "db_build_version", DB_BUILD_VERSION); + cJSON_AddNumberToObject(root, "major_version", DB_MAJOR_VERSION); + cJSON_AddNumberToObject(root, "minor_version", DB_MINOR_VERSION); + cJSON_AddNumberToObject(root, "patch_version", DB_PATCH_VERSION); + cJSON_AddStringToObject(root, "maturity_version", DB_MATURITY_VERSION); + cJSON_AddNumberToObject(root, "esp_chip_model", chip_info.model); + cJSON_AddNumberToObject(root, "has_rf_switch", DB_HAS_RF_SWITCH); + char mac_str[18]; + sprintf(mac_str, + "%02X:%02X:%02X:%02X:%02X:%02X", + LOCAL_MAC_ADDRESS[0], + LOCAL_MAC_ADDRESS[1], + LOCAL_MAC_ADDRESS[2], + LOCAL_MAC_ADDRESS[3], + LOCAL_MAC_ADDRESS[4], + LOCAL_MAC_ADDRESS[5]); + cJSON_AddStringToObject(root, "esp_mac", mac_str); #ifdef CONFIG_DB_SERIAL_OPTION_JTAG - cJSON_AddNumberToObject(root, "serial_via_JTAG", 1); + cJSON_AddNumberToObject(root, "serial_via_JTAG", 1); #else - cJSON_AddNumberToObject(root, "serial_via_JTAG", 0); + cJSON_AddNumberToObject(root, "serial_via_JTAG", 0); #endif - const char *sys_info = cJSON_Print(root); - httpd_resp_sendstr(req, sys_info); - free((void *) sys_info); - cJSON_Delete(root); - return ESP_OK; + const char *sys_info = cJSON_Print(root); + httpd_resp_sendstr(req, sys_info); + free((void *)sys_info); + cJSON_Delete(root); + return ESP_OK; } /** - * Returns a JSON containing read bytes from UART, number of connected TCP connections and UDP broadcasts as well as the - * current IP address of the ESP32 + * Returns a JSON containing read bytes from UART, number of connected TCP + * connections and UDP broadcasts as well as the current IP address of the + * ESP32 * @param req * @return ESP_OK on successfully sending the http request */ -static esp_err_t system_stats_get_handler(httpd_req_t *req) { - httpd_resp_set_type(req, "application/json"); - cJSON *root = cJSON_CreateObject(); - cJSON_AddNumberToObject(root, "read_bytes", serial_total_byte_count); - cJSON_AddNumberToObject(root, "tcp_connected", num_connected_tcp_clients); - cJSON_AddNumberToObject(root, "udp_connected", udp_conn_list->size); - // add IP:PORT info on connected UDP clients - cJSON *udp_clients = cJSON_CreateArray(); - for (int i = 0; i < udp_conn_list->size; i++) { - char ip_string[INET_ADDRSTRLEN]; - char ip_port_string[INET_ADDRSTRLEN+10]; - inet_ntop(AF_INET, &(udp_conn_list->db_udp_clients[i].udp_client.sin_addr), ip_string, INET_ADDRSTRLEN); - sprintf(ip_port_string, "%s:%d", ip_string, htons (udp_conn_list->db_udp_clients[i].udp_client.sin_port)); - cJSON_AddItemToArray(udp_clients, cJSON_CreateString(ip_port_string)); +static esp_err_t +system_stats_get_handler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "application/json"); + cJSON *root = cJSON_CreateObject(); + cJSON_AddNumberToObject(root, "read_bytes", serial_total_byte_count); + cJSON_AddNumberToObject(root, "tcp_connected", num_connected_tcp_clients); + cJSON_AddNumberToObject(root, "udp_connected", udp_conn_list->size); + // add IP:PORT info on connected UDP clients + cJSON *udp_clients = cJSON_CreateArray(); + for(int i = 0; i < udp_conn_list->size; i++) { + char ip_string[INET_ADDRSTRLEN]; + char ip_port_string[INET_ADDRSTRLEN + 10]; + inet_ntop(AF_INET, + &(udp_conn_list->db_udp_clients[i].udp_client.sin_addr), + ip_string, + INET_ADDRSTRLEN); + sprintf(ip_port_string, + "%s:%d", + ip_string, + htons(udp_conn_list->db_udp_clients[i].udp_client.sin_port)); + cJSON_AddItemToArray(udp_clients, cJSON_CreateString(ip_port_string)); + } + cJSON_AddItemToObject(root, "udp_clients", udp_clients); + // add RSSI and IP info + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { + cJSON_AddStringToObject(root, "current_client_ip", CURRENT_CLIENT_IP); + cJSON_AddNumberToObject(root, "esp_rssi", db_esp_signal_quality.air_rssi); + } + else if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || + DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR) { + cJSON *sta_array = cJSON_AddArrayToObject(root, "connected_sta"); + for(int i = 0; i < wifi_sta_list.num; i++) { + cJSON *connected_stations_status = cJSON_CreateObject(); + char mac_str[18]; + sprintf(mac_str, + "%02X:%02X:%02X:%02X:%02X:%02X", + wifi_sta_list.sta[i].mac[0], + wifi_sta_list.sta[i].mac[1], + wifi_sta_list.sta[i].mac[2], + wifi_sta_list.sta[i].mac[3], + wifi_sta_list.sta[i].mac[4], + wifi_sta_list.sta[i].mac[5]); + cJSON_AddStringToObject(connected_stations_status, "sta_mac", mac_str); + cJSON_AddNumberToObject( + connected_stations_status, "sta_rssi", wifi_sta_list.sta[i].rssi); + cJSON_AddItemToArray(sta_array, connected_stations_status); } - cJSON_AddItemToObject(root, "udp_clients", udp_clients); - // add RSSI and IP info - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA) { - cJSON_AddStringToObject(root, "current_client_ip", CURRENT_CLIENT_IP); - cJSON_AddNumberToObject(root, "esp_rssi", db_esp_signal_quality.air_rssi); - } else if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP || DB_PARAM_RADIO_MODE == DB_WIFI_MODE_AP_LR) { - cJSON *sta_array = cJSON_AddArrayToObject(root, "connected_sta"); - for (int i = 0; i < wifi_sta_list.num; i++) { - cJSON *connected_stations_status = cJSON_CreateObject(); - char mac_str[18]; - sprintf(mac_str, "%02X:%02X:%02X:%02X:%02X:%02X", - wifi_sta_list.sta[i].mac[0], wifi_sta_list.sta[i].mac[1], wifi_sta_list.sta[i].mac[2], - wifi_sta_list.sta[i].mac[3], wifi_sta_list.sta[i].mac[4], wifi_sta_list.sta[i].mac[5]); - cJSON_AddStringToObject(connected_stations_status, "sta_mac", mac_str); - cJSON_AddNumberToObject(connected_stations_status, "sta_rssi", wifi_sta_list.sta[i].rssi); - cJSON_AddItemToArray(sta_array, connected_stations_status); - } - } else { - // other modes like ESP-NOW do not activate HTTP server so do nothing - } - const char *sys_info = cJSON_Print(root); - httpd_resp_sendstr(req, sys_info); - free((void *) sys_info); - cJSON_Delete(root); - return ESP_OK; + } + else { + // other modes like ESP-NOW do not activate HTTP server so do nothing + } + const char *sys_info = cJSON_Print(root); + httpd_resp_sendstr(req, sys_info); + free((void *)sys_info); + cJSON_Delete(root); + return ESP_OK; } /** @@ -364,30 +427,39 @@ static esp_err_t system_stats_get_handler(httpd_req_t *req) { * @param req * @return ESP_OK on successfully sending the http request */ -static esp_err_t system_clients_get_handler(httpd_req_t *req) { - httpd_resp_set_type(req, "application/json"); - cJSON *root = cJSON_CreateObject(); -// cJSON *tcp_clients = cJSON_CreateArray(); -// for (int i = 0; i < udp_conn_list->size; i++) { -// //TODO: Save the TCP IPs and port during accept and put them here into a JSON -// } -// cJSON_AddItemToObject(root, "tcp_clients", tcp_clients); - - cJSON *udp_clients = cJSON_CreateArray(); - for (int i = 0; i < udp_conn_list->size; i++) { - char ip_string[INET_ADDRSTRLEN]; - char ip_port_string[INET_ADDRSTRLEN+10]; - inet_ntop(AF_INET, &(udp_conn_list->db_udp_clients[i].udp_client.sin_addr), ip_string, INET_ADDRSTRLEN); - sprintf(ip_port_string, "%s:%d", ip_string, htons (udp_conn_list->db_udp_clients[i].udp_client.sin_port)); - cJSON_AddItemToArray(udp_clients, cJSON_CreateString(ip_port_string)); - } - cJSON_AddItemToObject(root, "udp_clients", udp_clients); - - const char *sys_info = cJSON_Print(root); - httpd_resp_sendstr(req, sys_info); - free((void *) sys_info); - cJSON_Delete(root); - return ESP_OK; +static esp_err_t +system_clients_get_handler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "application/json"); + cJSON *root = cJSON_CreateObject(); + // cJSON *tcp_clients = cJSON_CreateArray(); + // for (int i = 0; i < udp_conn_list->size; i++) { + // //TODO: Save the TCP IPs and port during accept and put them here + // into a JSON + // } + // cJSON_AddItemToObject(root, "tcp_clients", tcp_clients); + + cJSON *udp_clients = cJSON_CreateArray(); + for(int i = 0; i < udp_conn_list->size; i++) { + char ip_string[INET_ADDRSTRLEN]; + char ip_port_string[INET_ADDRSTRLEN + 10]; + inet_ntop(AF_INET, + &(udp_conn_list->db_udp_clients[i].udp_client.sin_addr), + ip_string, + INET_ADDRSTRLEN); + sprintf(ip_port_string, + "%s:%d", + ip_string, + htons(udp_conn_list->db_udp_clients[i].udp_client.sin_port)); + cJSON_AddItemToArray(udp_clients, cJSON_CreateString(ip_port_string)); + } + cJSON_AddItemToObject(root, "udp_clients", udp_clients); + + const char *sys_info = cJSON_Print(root); + httpd_resp_sendstr(req, sys_info); + free((void *)sys_info); + cJSON_Delete(root); + return ESP_OK; } /** @@ -395,105 +467,99 @@ static esp_err_t system_clients_get_handler(httpd_req_t *req) { * @param req * @return ESP_OK on successfully sending the http request */ -static esp_err_t settings_get_handler(httpd_req_t *req) { - httpd_resp_set_type(req, "application/json"); - cJSON *root = cJSON_CreateObject(); - db_param_write_all_params_json(root); - const char *sys_info = cJSON_Print(root); - httpd_resp_sendstr(req, sys_info); - free((void *) sys_info); - cJSON_Delete(root); - return ESP_OK; +static esp_err_t +settings_get_handler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "application/json"); + cJSON *root = cJSON_CreateObject(); + db_param_write_all_params_json(root); + const char *sys_info = cJSON_Print(root); + httpd_resp_sendstr(req, sys_info); + free((void *)sys_info); + cJSON_Delete(root); + return ESP_OK; } -esp_err_t start_rest_server(const char *base_path) { - REST_CHECK(base_path, "wrong base path", err); - rest_server_context_t *rest_context = calloc(1, sizeof(rest_server_context_t)); - REST_CHECK(rest_context, "No memory for rest context", err); - strlcpy(rest_context->base_path, base_path, sizeof(rest_context->base_path)); - - httpd_handle_t server = NULL; - httpd_config_t config = HTTPD_DEFAULT_CONFIG(); - config.uri_match_fn = httpd_uri_match_wildcard; - config.max_uri_handlers = 9; - - ESP_LOGI(TAG, "Starting HTTP Server"); - REST_CHECK(httpd_start(&server, &config) == ESP_OK, "Start server failed", err_start); - - /* URI handler for fetching system info */ - httpd_uri_t system_info_get_uri = { - .uri = "/api/system/info", - .method = HTTP_GET, - .handler = system_info_get_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &system_info_get_uri); - - /* URI handler for fetching client connection info */ - httpd_uri_t system_clients_get_uri = { - .uri = "/api/system/clients", - .method = HTTP_GET, - .handler = system_clients_get_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &system_clients_get_uri); - - /* URI handler for fetching system info */ - httpd_uri_t system_stats_get_uri = { - .uri = "/api/system/stats", - .method = HTTP_GET, - .handler = system_stats_get_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &system_stats_get_uri); - - /* URI handler for fetching settings data */ - httpd_uri_t settings_get_uri = { - .uri = "/api/settings", - .method = HTTP_GET, - .handler = settings_get_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &settings_get_uri); - - httpd_uri_t settings_post_uri = { - .uri = "/api/settings", - .method = HTTP_POST, - .handler = settings_post_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &settings_post_uri); - - /* URI handler for adding a new udp client connection */ - httpd_uri_t settings_clients_udp_post_uri = { - .uri = "/api/settings/clients/udp", - .method = HTTP_POST, - .handler = settings_clients_udp_post, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &settings_clients_udp_post_uri); - - /* URI handler for removing all known UDP client connections */ - httpd_uri_t settings_clients_clear_udp_get_uri = { - .uri = "/api/settings/clients/clear_udp", - .method = HTTP_DELETE, - .handler = settings_clients_clear_udp_get, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &settings_clients_clear_udp_get_uri); - - /* URI handler for getting web server files */ - httpd_uri_t common_get_uri = { - .uri = "/*", - .method = HTTP_GET, - .handler = rest_common_get_handler, - .user_ctx = rest_context - }; - httpd_register_uri_handler(server, &common_get_uri); - - return ESP_OK; - err_start: - free(rest_context); - err: - return ESP_FAIL; +esp_err_t +start_rest_server(const char *base_path) +{ + REST_CHECK(base_path, "wrong base path", err); + rest_server_context_t *rest_context = + calloc(1, sizeof(rest_server_context_t)); + REST_CHECK(rest_context, "No memory for rest context", err); + strlcpy(rest_context->base_path, base_path, sizeof(rest_context->base_path)); + + httpd_handle_t server = NULL; + httpd_config_t config = HTTPD_DEFAULT_CONFIG(); + config.uri_match_fn = httpd_uri_match_wildcard; + config.max_uri_handlers = 9; + + ESP_LOGI(TAG, "Starting HTTP Server"); + REST_CHECK( + httpd_start(&server, &config) == ESP_OK, "Start server failed", err_start); + + /* URI handler for fetching system info */ + httpd_uri_t system_info_get_uri = { .uri = "/api/system/info", + .method = HTTP_GET, + .handler = system_info_get_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &system_info_get_uri); + + /* URI handler for fetching client connection info */ + httpd_uri_t system_clients_get_uri = { .uri = "/api/system/clients", + .method = HTTP_GET, + .handler = system_clients_get_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &system_clients_get_uri); + + /* URI handler for fetching system info */ + httpd_uri_t system_stats_get_uri = { .uri = "/api/system/stats", + .method = HTTP_GET, + .handler = system_stats_get_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &system_stats_get_uri); + + /* URI handler for fetching settings data */ + httpd_uri_t settings_get_uri = { .uri = "/api/settings", + .method = HTTP_GET, + .handler = settings_get_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &settings_get_uri); + + httpd_uri_t settings_post_uri = { .uri = "/api/settings", + .method = HTTP_POST, + .handler = settings_post_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &settings_post_uri); + + /* URI handler for adding a new udp client connection */ + httpd_uri_t settings_clients_udp_post_uri = { .uri = + "/api/settings/clients/udp", + .method = HTTP_POST, + .handler = + settings_clients_udp_post, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &settings_clients_udp_post_uri); + + /* URI handler for removing all known UDP client connections */ + httpd_uri_t settings_clients_clear_udp_get_uri = { + .uri = "/api/settings/clients/clear_udp", + .method = HTTP_DELETE, + .handler = settings_clients_clear_udp_get, + .user_ctx = rest_context + }; + httpd_register_uri_handler(server, &settings_clients_clear_udp_get_uri); + + /* URI handler for getting web server files */ + httpd_uri_t common_get_uri = { .uri = "/*", + .method = HTTP_GET, + .handler = rest_common_get_handler, + .user_ctx = rest_context }; + httpd_register_uri_handler(server, &common_get_uri); + + return ESP_OK; +err_start: + free(rest_context); +err: + return ESP_FAIL; } diff --git a/main/http_server.h b/main/http_server.h index e4470ce..79d9593 100644 --- a/main/http_server.h +++ b/main/http_server.h @@ -24,4 +24,4 @@ esp_err_t start_rest_server(const char *base_path); -#endif //DB_ESP32_HTTP_SERVER_NEW_H +#endif // DB_ESP32_HTTP_SERVER_NEW_H diff --git a/main/main.c b/main/main.c index 892e814..a5d3b05 100644 --- a/main/main.c +++ b/main/main.c @@ -17,42 +17,64 @@ * */ +/****************************************************************************** + * Header Inclusion + ******************************************************************************/ + +/****************************************************************************** + * Standard C Libraries + ******************************************************************************/ #include -#include -#include #include -#include -#include -#include "freertos/event_groups.h" +/****************************************************************************** + * ESP-IDF Core Libraries + ******************************************************************************/ +#include "esp_log.h" #include "esp_mac.h" +#include "esp_event.h" #include "esp_wifi.h" #include "esp_wifi_types.h" -#include "esp_log.h" -#include "esp_event.h" -#include "db_esp32_control.h" -#include "http_server.h" -#include "db_protocol.h" +#include "nvs_flash.h" +#include "driver/gpio.h" #include "esp_vfs_semihost.h" #include "esp_spiffs.h" -#include "http_server.h" + +/****************************************************************************** + * FreeRTOS + ******************************************************************************/ +#include "freertos/event_groups.h" + +/****************************************************************************** + * LWIP / Network Services + ******************************************************************************/ +#include "lwip/apps/netbiosns.h" + +/****************************************************************************** + * Project Headers + ******************************************************************************/ #include "main.h" +#include "globals.h" #include "db_parameters.h" -#include "mdns.h" +#include "db_serial.h" +#include "db_protocol.h" +#include "db_esp32_control.h" #include "db_esp_now.h" +#include "http_server.h" +#include "mdns.h" #include "iot_button.h" -#include "db_serial.h" -#include "globals.h" #ifdef CONFIG_BT_ENABLED - #include "db_ble.h" - #endif +/****************************************************************************** + * Macros + ******************************************************************************/ #define NVS_NAMESPACE "settings" -/* used to reset ESP to defaults and force restart or to reset the mode to access point mode */ +/* used to reset ESP to defaults and force restart or to reset the mode to + * access point mode */ #ifdef CONFIG_IDF_TARGET_ESP32C3 #define DB_RESET_PIN GPIO_NUM_9 #elif CONFIG_IDF_TARGET_ESP32C6 @@ -70,9 +92,14 @@ static const char *TAG = "DB_ESP32"; char CURRENT_CLIENT_IP[IP4ADDR_STRLEN_MAX] = "192.168.2.1"; -uint8_t DB_RADIO_IS_OFF = false; // keep track if we switched Wi-Fi/BLE off already - by default a radio is started -db_esp_signal_quality_t db_esp_signal_quality = {.air_rssi = UINT8_MAX, .air_noise_floor = UINT8_MAX, .gnd_rssi= UINT8_MAX, .gnd_noise_floor = UINT8_MAX}; -wifi_sta_list_t wifi_sta_list = {.num = 0}; +uint8_t DB_RADIO_IS_OFF = false; // keep track if we switched Wi-Fi/BLE off + // already - by default a radio is started +db_esp_signal_quality_t db_esp_signal_quality = { .air_rssi = UINT8_MAX, + .air_noise_floor = UINT8_MAX, + .gnd_rssi = UINT8_MAX, + .gnd_noise_floor = + UINT8_MAX }; +wifi_sta_list_t wifi_sta_list = { .num = 0 }; uint8_t LOCAL_MAC_ADDRESS[6]; udp_conn_list_t *udp_conn_list; @@ -80,571 +107,674 @@ udp_conn_list_t *udp_conn_list; static int s_retry_num = 0; static EventGroupHandle_t s_wifi_event_group; #define WIFI_CONNECTED_BIT BIT0 -#define WIFI_FAIL_BIT BIT1 +#define WIFI_FAIL_BIT BIT1 esp_netif_t *esp_default_netif; -static esp_err_t db_set_dns_server(esp_netif_t *netif, uint32_t addr, esp_netif_dns_type_t type) { - if (addr && (addr != IPADDR_NONE)) { - esp_netif_dns_info_t dns; - dns.ip.u_addr.ip4.addr = addr; - dns.ip.type = IPADDR_TYPE_V4; - ESP_ERROR_CHECK(esp_netif_set_dns_info(netif, type, &dns)); - } - return ESP_OK; -} +/****************************************************************************** + * Private Function Declarations + ******************************************************************************/ +static esp_err_t set_dns_server(esp_netif_t *netif, uint32_t addr, + esp_netif_dns_type_t type); /** - * Assigns static IP to ESP32 when in client mode and static IP, GW and netmask are set in config. - * Stops client DHCP server + * Assigns static IP to ESP32 when in client mode and static IP, GW and netmask + * are set in config. Stops client DHCP server */ -static void set_client_static_ip() { - if (DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA && strlen((char *) DB_PARAM_STA_IP) > 0 && - strlen((char *) DB_PARAM_STA_GW) > 0 && - strlen((char *) DB_PARAM_STA_IP_NETMASK) > 0) { - ESP_LOGI(TAG, "Assigning static IP to ESP32: ESP32-IP: %s Gateway: %s Netmask: %s", (char *) DB_PARAM_STA_IP, - (char *) DB_PARAM_STA_GW, (char *) DB_PARAM_STA_IP_NETMASK); - - if (esp_netif_dhcpc_stop(esp_default_netif) != ESP_OK) { - ESP_LOGE(TAG, "Failed to stop dhcp client in order to set static IP"); - return; - } - esp_netif_ip_info_t ip; - memset(&ip, 0, sizeof(esp_netif_ip_info_t)); - ip.ip.addr = ipaddr_addr((char *) DB_PARAM_STA_IP); - ip.netmask.addr = ipaddr_addr((char *) DB_PARAM_STA_IP_NETMASK); - ip.gw.addr = ipaddr_addr((char *) DB_PARAM_STA_GW); - if (esp_netif_set_ip_info(esp_default_netif, &ip) != ESP_OK) { - ESP_LOGE(TAG, "Failed to set static ip info"); - } - ESP_LOGD(TAG, "Success to set static ip: %s, netmask: %s, gw: %s", (char *) DB_PARAM_STA_IP, - (char *) DB_PARAM_STA_IP_NETMASK, - (char *) DB_PARAM_STA_GW); - ESP_ERROR_CHECK( - db_set_dns_server(esp_default_netif, ipaddr_addr((char *) DB_PARAM_STA_GW), ESP_NETIF_DNS_MAIN)); - ESP_ERROR_CHECK(db_set_dns_server(esp_default_netif, ipaddr_addr("0.0.0.0"), ESP_NETIF_DNS_BACKUP)); - } else { - //no static IP specified let the DHCP assign us one - } -} +static void set_client_static_ip(); /** - * Devices get added based on IP (check if IP & PORT are already listed) and removed from the UDP broadcast connection - * based on MAC address + * Devices get added based on IP (check if IP & PORT are already listed) and + * removed from the UDP broadcast connection based on MAC address * * @param arg * @param event_base * @param event_id * @param event_data */ -static void wifi_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data) { - // Wifi access point mode events - if (event_id == WIFI_EVENT_AP_STACONNECTED) { - wifi_event_ap_staconnected_t *event = (wifi_event_ap_staconnected_t *) event_data; - ESP_LOGI(TAG, "WIFI_EVENT - Client connected - station:"MACSTR", AID=%d", MAC2STR(event->mac), event->aid); - ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_ap_get_sta_list(&wifi_sta_list)); // update list of connected stations - } else if (event_id == WIFI_EVENT_AP_STADISCONNECTED) { - wifi_event_ap_stadisconnected_t *event = (wifi_event_ap_stadisconnected_t *) event_data; - ESP_LOGI(TAG, "WIFI_EVENT - Client disconnected - station:"MACSTR", AID=%d", MAC2STR(event->mac), event->aid); - struct db_udp_client_t db_udp_client; - memcpy(db_udp_client.mac, event->mac, sizeof(db_udp_client.mac)); - remove_from_known_udp_clients(udp_conn_list, db_udp_client); - ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_ap_get_sta_list(&wifi_sta_list)); // update list of connected stations - } else if (event_id == WIFI_EVENT_AP_START) { - ESP_LOGI(TAG, "WIFI_EVENT_AP_START (SSID: %s PASS: %s)", DB_PARAM_WIFI_SSID, DB_PARAM_PASS); - } else if (event_id == WIFI_EVENT_AP_STOP) { - ESP_LOGI(TAG, "WIFI_EVENT - AP stopped!"); - } else if (event_base == IP_EVENT && event_id == IP_EVENT_AP_STAIPASSIGNED) { - ip_event_ap_staipassigned_t *event = (ip_event_ap_staipassigned_t *) event_data; - ESP_LOGI(TAG, "IP_EVENT_AP_STAIPASSIGNED - New station IP:" IPSTR, IP2STR(&event->ip)); - ESP_LOGI(TAG, "IP_EVENT_AP_STAIPASSIGNED - MAC: " MACSTR, MAC2STR(event->mac)); - struct db_udp_client_t db_udp_client; - db_udp_client.udp_client.sin_family = PF_INET; - db_udp_client.udp_client.sin_port = htons(APP_PORT_PROXY_UDP); - db_udp_client.udp_client.sin_len = 16; - db_udp_client.udp_client.sin_addr.s_addr = event->ip.addr; - memcpy(db_udp_client.mac, event->mac, sizeof(db_udp_client.mac)); - add_to_known_udp_clients(udp_conn_list, db_udp_client, false); +static void wifi_event_handler(void *arg, esp_event_base_t event_base, + int32_t event_id, void *event_data); + +static void start_mdns_service(); + +/** + * Launches an access point where ground stations can connect to + * + * @param wifi_mode Allows to overwrite an AP mode from traditional WiFi to LR + * Mode + */ +static void init_wifi_apmode(int wifi_mode); + +/** + * Initializes the ESP Wi-fi client/station mode where we connect to a known + * access point. + */ +static int init_wifi_clientmode(); + +/** + * Initialize Wi-Fi for ESP-NOW mode. + * If someone uses ESP-NOW over Wi-Fi it is because he wants range over + * everything else. LR mode makes it very inconvenient to change settings but + * gives the most range. No AP mode since AP will not be visible. + */ +static void init_wifi_espnow(); + +/** + * Read stored settings from internal storage including the saved UDP client. + */ +static void read_settings_nvs(); + +/** + * Callback for a short press (CONFIG_BUTTON_LONG_PRESS_TIME_MS) of the + * reset/boot button. Resets all settings to defaults. + * @param arg + */ +static void long_press_callback(void *arg, void *usr_data); + +/** + * Setup boot button GPIO to reset entire ESP32 settings and to force a reboot + * of the system + */ +static void set_reset_trigger(); + +/** + * 1. Checks if RF Switch is configured. + * 2. Enables RF switch (necessary on some boards). + * 3. Sets RF switch according to user setting. + * Must be called before radio/Wi-Fi gets initialized! + */ +static void configure_antenna(); + +/****************************************************************************** + * Private Function Definitions + ******************************************************************************/ + +static esp_err_t +set_dns_server(esp_netif_t *netif, uint32_t addr, esp_netif_dns_type_t type) +{ + if(addr && (addr != IPADDR_NONE)) { + esp_netif_dns_info_t dns; + dns.ip.u_addr.ip4.addr = addr; + dns.ip.type = IPADDR_TYPE_V4; + ESP_ERROR_CHECK(esp_netif_set_dns_info(netif, type, &dns)); + } + return ESP_OK; +} + +static void +set_client_static_ip() +{ + if(DB_PARAM_RADIO_MODE == DB_WIFI_MODE_STA && + strlen((char *)DB_PARAM_STA_IP) > 0 && + strlen((char *)DB_PARAM_STA_GW) > 0 && + strlen((char *)DB_PARAM_STA_IP_NETMASK) > 0) { + ESP_LOGI( + TAG, + "Assigning static IP to ESP32: ESP32-IP: %s Gateway: %s Netmask: %s", + (char *)DB_PARAM_STA_IP, + (char *)DB_PARAM_STA_GW, + (char *)DB_PARAM_STA_IP_NETMASK); + + if(esp_netif_dhcpc_stop(esp_default_netif) != ESP_OK) { + ESP_LOGE(TAG, "Failed to stop dhcp client in order to set static IP"); + return; } - // Wifi client mode events - if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { - ESP_LOGI(TAG, "WIFI_EVENT_STA_START - Wifi Started"); - if (!DB_RADIO_IS_OFF) { // maybe the other task did set it in the meantime - ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_connect()); - } else { - ESP_LOGW(TAG, "Did not start Wi-Fi since autopilot told us he is armed (WIFI_EVENT_STA_START)"); - } - } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_CONNECTED) { - set_client_static_ip(); - } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) { - ESP_LOGI(TAG, "WIFI_EVENT_STA_DISCONNECTED - Lost connection to access point"); - // Keep on trying - if (!DB_RADIO_IS_OFF) { - ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_connect()); - s_retry_num++; - ESP_LOGI(TAG, "Retry to connect to the AP (%i)", s_retry_num); - } else { - ESP_LOGD(TAG, "WIFI_EVENT_STA_DISCONNECTED - did not try to re-connect since WiFi was commanded off"); - } - } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { - ip_event_got_ip_t *event = (ip_event_got_ip_t *) event_data; - ESP_LOGI(TAG, "IP_EVENT_STA_GOT_IP:" IPSTR, IP2STR(&event->ip_info.ip)); - sprintf(CURRENT_CLIENT_IP, IPSTR, IP2STR(&event->ip_info.ip)); - s_retry_num = 0; - xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + esp_netif_ip_info_t ip; + memset(&ip, 0, sizeof(esp_netif_ip_info_t)); + ip.ip.addr = ipaddr_addr((char *)DB_PARAM_STA_IP); + ip.netmask.addr = ipaddr_addr((char *)DB_PARAM_STA_IP_NETMASK); + ip.gw.addr = ipaddr_addr((char *)DB_PARAM_STA_GW); + if(esp_netif_set_ip_info(esp_default_netif, &ip) != ESP_OK) { + ESP_LOGE(TAG, "Failed to set static ip info"); } + ESP_LOGD(TAG, + "Success to set static ip: %s, netmask: %s, gw: %s", + (char *)DB_PARAM_STA_IP, + (char *)DB_PARAM_STA_IP_NETMASK, + (char *)DB_PARAM_STA_GW); + ESP_ERROR_CHECK(set_dns_server(esp_default_netif, + ipaddr_addr((char *)DB_PARAM_STA_GW), + ESP_NETIF_DNS_MAIN)); + ESP_ERROR_CHECK(set_dns_server( + esp_default_netif, ipaddr_addr("0.0.0.0"), ESP_NETIF_DNS_BACKUP)); + } + else { + // no static IP specified let the DHCP assign us one + } } -void start_mdns_service() { - esp_err_t err = mdns_init(); - if (err) { - printf("MDNS Init failed: %d\n", err); - return; +static void +wifi_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, + void *event_data) +{ + // Wifi access point mode events + if(event_id == WIFI_EVENT_AP_STACONNECTED) { + wifi_event_ap_staconnected_t *event = + (wifi_event_ap_staconnected_t *)event_data; + ESP_LOGI(TAG, + "WIFI_EVENT - Client connected - station:" MACSTR ", AID=%d", + MAC2STR(event->mac), + event->aid); + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_ap_get_sta_list( + &wifi_sta_list)); // update list of connected stations + } + else if(event_id == WIFI_EVENT_AP_STADISCONNECTED) { + wifi_event_ap_stadisconnected_t *event = + (wifi_event_ap_stadisconnected_t *)event_data; + ESP_LOGI(TAG, + "WIFI_EVENT - Client disconnected - station:" MACSTR ", AID=%d", + MAC2STR(event->mac), + event->aid); + struct db_udp_client_t db_udp_client; + memcpy(db_udp_client.mac, event->mac, sizeof(db_udp_client.mac)); + db_remove_from_known_udp_clients(udp_conn_list, db_udp_client); + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_ap_get_sta_list( + &wifi_sta_list)); // update list of connected stations + } + else if(event_id == WIFI_EVENT_AP_START) { + ESP_LOGI(TAG, + "WIFI_EVENT_AP_START (SSID: %s PASS: %s)", + DB_PARAM_WIFI_SSID, + DB_PARAM_PASS); + } + else if(event_id == WIFI_EVENT_AP_STOP) { + ESP_LOGI(TAG, "WIFI_EVENT - AP stopped!"); + } + else if(event_base == IP_EVENT && event_id == IP_EVENT_AP_STAIPASSIGNED) { + ip_event_ap_staipassigned_t *event = + (ip_event_ap_staipassigned_t *)event_data; + ESP_LOGI(TAG, + "IP_EVENT_AP_STAIPASSIGNED - New station IP:" IPSTR, + IP2STR(&event->ip)); + ESP_LOGI( + TAG, "IP_EVENT_AP_STAIPASSIGNED - MAC: " MACSTR, MAC2STR(event->mac)); + struct db_udp_client_t db_udp_client; + db_udp_client.udp_client.sin_family = PF_INET; + db_udp_client.udp_client.sin_port = htons(APP_PORT_PROXY_UDP); + db_udp_client.udp_client.sin_len = 16; + db_udp_client.udp_client.sin_addr.s_addr = event->ip.addr; + memcpy(db_udp_client.mac, event->mac, sizeof(db_udp_client.mac)); + db_add_to_known_udp_clients(udp_conn_list, db_udp_client, false); + } + // Wifi client mode events + if(event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { + ESP_LOGI(TAG, "WIFI_EVENT_STA_START - Wifi Started"); + if(!DB_RADIO_IS_OFF) { // maybe the other task did set it in the meantime + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_connect()); + } + else { + ESP_LOGW(TAG, + "Did not start Wi-Fi since autopilot told us he is armed " + "(WIFI_EVENT_STA_START)"); + } + } + else if(event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_CONNECTED) { + set_client_static_ip(); + } + else if(event_base == WIFI_EVENT && + event_id == WIFI_EVENT_STA_DISCONNECTED) { + ESP_LOGI(TAG, + "WIFI_EVENT_STA_DISCONNECTED - Lost connection to access point"); + // Keep on trying + if(!DB_RADIO_IS_OFF) { + ESP_ERROR_CHECK_WITHOUT_ABORT(esp_wifi_connect()); + s_retry_num++; + ESP_LOGI(TAG, "Retry to connect to the AP (%i)", s_retry_num); } - ESP_ERROR_CHECK(mdns_hostname_set("dronebridge")); - ESP_ERROR_CHECK(mdns_instance_name_set("DroneBridge for ESP32")); - - ESP_ERROR_CHECK(mdns_service_add(NULL, "_http", "_tcp", 80, NULL, 0)); - ESP_ERROR_CHECK(mdns_service_add(NULL, "_db_proxy", "_tcp", APP_PORT_PROXY, NULL, 0)); - ESP_ERROR_CHECK(mdns_service_add(NULL, "_db_comm", "_tcp", APP_PORT_COMM, NULL, 0)); - ESP_ERROR_CHECK(mdns_service_instance_name_set("_http", "_tcp", "DroneBridge for ESP32")); - ESP_LOGI(TAG, "MDNS Service started!"); + else { + ESP_LOGD(TAG, + "WIFI_EVENT_STA_DISCONNECTED - did not try to re-connect since " + "WiFi was commanded off"); + } + } + else if(event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { + ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data; + ESP_LOGI(TAG, "IP_EVENT_STA_GOT_IP:" IPSTR, IP2STR(&event->ip_info.ip)); + sprintf(CURRENT_CLIENT_IP, IPSTR, IP2STR(&event->ip_info.ip)); + s_retry_num = 0; + xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + } +} + +static void +start_mdns_service() +{ + esp_err_t err = mdns_init(); + if(err) { + printf("MDNS Init failed: %d\n", err); + return; + } + ESP_ERROR_CHECK(mdns_hostname_set("dronebridge")); + ESP_ERROR_CHECK(mdns_instance_name_set("DroneBridge for ESP32")); + + ESP_ERROR_CHECK(mdns_service_add(NULL, "_http", "_tcp", 80, NULL, 0)); + ESP_ERROR_CHECK( + mdns_service_add(NULL, "_db_proxy", "_tcp", APP_PORT_PROXY, NULL, 0)); + ESP_ERROR_CHECK( + mdns_service_add(NULL, "_db_comm", "_tcp", APP_PORT_COMM, NULL, 0)); + ESP_ERROR_CHECK( + mdns_service_instance_name_set("_http", "_tcp", "DroneBridge for ESP32")); + ESP_LOGI(TAG, "MDNS Service started!"); } #if CONFIG_WEB_DEPLOY_SEMIHOST -esp_err_t init_fs(void) { - esp_err_t ret = esp_vfs_semihost_register(CONFIG_WEB_MOUNT_POINT, CONFIG_HOST_PATH_TO_MOUNT); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "Failed to register semihost driver (%s)!", esp_err_to_name(ret)); - return ESP_FAIL; - } - return ESP_OK; +esp_err_t +init_fs(void) +{ + esp_err_t ret = esp_vfs_semihost_register(CONFIG_WEB_MOUNT_POINT, + CONFIG_HOST_PATH_TO_MOUNT); + if(ret != ESP_OK) { + ESP_LOGE( + TAG, "Failed to register semihost driver (%s)!", esp_err_to_name(ret)); + return ESP_FAIL; + } + return ESP_OK; } #endif - #if CONFIG_WEB_DEPLOY_SF -esp_err_t init_fs(void) { - esp_vfs_spiffs_conf_t conf = { - .base_path = CONFIG_WEB_MOUNT_POINT, - .partition_label = NULL, - .max_files = 5, - .format_if_mount_failed = false - }; - esp_err_t ret = esp_vfs_spiffs_register(&conf); - - if (ret != ESP_OK) { - if (ret == ESP_FAIL) { - ESP_LOGE(TAG, "Failed to mount or format filesystem"); - } else if (ret == ESP_ERR_NOT_FOUND) { - ESP_LOGE(TAG, "Failed to find SPIFFS partition"); - } else { - ESP_LOGE(TAG, "Failed to initialize SPIFFS (%s)", esp_err_to_name(ret)); - } - return ESP_FAIL; +esp_err_t +init_fs(void) +{ + esp_vfs_spiffs_conf_t conf = { .base_path = CONFIG_WEB_MOUNT_POINT, + .partition_label = NULL, + .max_files = 5, + .format_if_mount_failed = false }; + esp_err_t ret = esp_vfs_spiffs_register(&conf); + + if(ret != ESP_OK) { + if(ret == ESP_FAIL) { + ESP_LOGE(TAG, "Failed to mount or format filesystem"); } - - size_t total = 0, used = 0; - ret = esp_spiffs_info(NULL, &total, &used); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "Failed to get SPIFFS partition information (%s)", esp_err_to_name(ret)); - } else { - ESP_LOGI(TAG, "Filesystem init finished! Partition size: total: %d bytes, used: %d bytes (%i%%)", total, used, - (used * 100) / total); + else if(ret == ESP_ERR_NOT_FOUND) { + ESP_LOGE(TAG, "Failed to find SPIFFS partition"); + } + else { + ESP_LOGE(TAG, "Failed to initialize SPIFFS (%s)", esp_err_to_name(ret)); } - return ret; + return ESP_FAIL; + } + + size_t total = 0, used = 0; + ret = esp_spiffs_info(NULL, &total, &used); + if(ret != ESP_OK) { + ESP_LOGE(TAG, + "Failed to get SPIFFS partition information (%s)", + esp_err_to_name(ret)); + } + else { + ESP_LOGI(TAG, + "Filesystem init finished! Partition size: total: %d bytes, " + "used: %d bytes (%i%%)", + total, + used, + (used * 100) / total); + } + return ret; } #endif -/** - * Launches an access point where ground stations can connect to - * - * @param wifi_mode Allows to overwrite an AP mode from traditional WiFi to LR Mode - */ -void db_init_wifi_apmode(int wifi_mode) { - ESP_ERROR_CHECK(esp_netif_init()); - ESP_ERROR_CHECK(esp_event_loop_create_default()); - esp_default_netif = esp_netif_create_default_wifi_ap(); - wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); - ESP_ERROR_CHECK(esp_wifi_init(&cfg)); - ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, - ESP_EVENT_ANY_ID, - &wifi_event_handler, - NULL, - NULL)); - - esp_event_handler_instance_t ap_staipassigned_ip; - ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, - IP_EVENT_AP_STAIPASSIGNED, - &wifi_event_handler, - NULL, - &ap_staipassigned_ip)); - - wifi_config_t wifi_config = { - .ap = { - .ssid = "DroneBridge ESP32 Init Error", - .password = "dronebridge", - .ssid_len = 0, - .authmode = WIFI_AUTH_WPA2_PSK, - .channel = db_param_channel.value.db_param_u8.value, - .ssid_hidden = 0, - .beacon_interval = 100, - .max_connection = 10 - }, - }; +static void +init_wifi_apmode(int wifi_mode) +{ + ESP_ERROR_CHECK(esp_netif_init()); + ESP_ERROR_CHECK(esp_event_loop_create_default()); + esp_default_netif = esp_netif_create_default_wifi_ap(); + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + ESP_ERROR_CHECK(esp_event_handler_instance_register( + WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL)); + + esp_event_handler_instance_t ap_staipassigned_ip; + ESP_ERROR_CHECK( + esp_event_handler_instance_register(IP_EVENT, + IP_EVENT_AP_STAIPASSIGNED, + &wifi_event_handler, + NULL, + &ap_staipassigned_ip)); + + wifi_config_t wifi_config = { + .ap = { .ssid = "DroneBridge ESP32 Init Error", + .password = "dronebridge", + .ssid_len = 0, + .authmode = WIFI_AUTH_WPA2_PSK, + .channel = db_param_channel.value.db_param_u8.value, + .ssid_hidden = 0, + .beacon_interval = 100, + .max_connection = 10 }, + }; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstringop-truncation" - // Set Wi-Fi SSID and password from the stored parameters - if (strlen(DB_PARAM_WIFI_SSID) >= db_param_ssid.value.db_param_str.min_len) { - strncpy((char *) wifi_config.ap.ssid, DB_PARAM_WIFI_SSID, db_param_ssid.value.db_param_str.max_len); - } else { - // something is wrong - switch to default value - strncpy((char *) wifi_config.ap.ssid, (char *) db_param_ssid.value.db_param_str.default_value, - db_param_ssid.value.db_param_str.max_len); - } - if (strlen(DB_PARAM_PASS) >= db_param_pass.value.db_param_str.min_len) { - strncpy((char *) wifi_config.ap.password, DB_PARAM_PASS, db_param_pass.value.db_param_str.max_len); - } else { - // something is wrong - switch to default value - strncpy((char *) wifi_config.ap.password, (char *) db_param_pass.value.db_param_str.default_value, - db_param_pass.value.db_param_str.max_len); - } + // Set Wi-Fi SSID and password from the stored parameters + if(strlen(DB_PARAM_WIFI_SSID) >= db_param_ssid.value.db_param_str.min_len) { + strncpy((char *)wifi_config.ap.ssid, + DB_PARAM_WIFI_SSID, + db_param_ssid.value.db_param_str.max_len); + } + else { + // something is wrong - switch to default value + strncpy((char *)wifi_config.ap.ssid, + (char *)db_param_ssid.value.db_param_str.default_value, + db_param_ssid.value.db_param_str.max_len); + } + if(strlen(DB_PARAM_PASS) >= db_param_pass.value.db_param_str.min_len) { + strncpy((char *)wifi_config.ap.password, + DB_PARAM_PASS, + db_param_pass.value.db_param_str.max_len); + } + else { + // something is wrong - switch to default value + strncpy((char *)wifi_config.ap.password, + (char *)db_param_pass.value.db_param_str.default_value, + db_param_pass.value.db_param_str.max_len); + } #pragma GCC diagnostic pop - ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); - if (wifi_mode == DB_WIFI_MODE_AP_LR) { - ESP_LOGI(TAG, "Enabling LR Mode on access point. This device will be invisible to non-ESP32 devices!"); - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); - } else { - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_11B)); - } - ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config)); - wifi_country_t wifi_country = {.cc = "US", .schan = 1, .nchan = 13, .policy = WIFI_COUNTRY_POLICY_MANUAL}; - ESP_ERROR_CHECK(esp_wifi_set_country(&wifi_country)); - ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); - ESP_ERROR_CHECK(esp_wifi_start()); - DB_RADIO_IS_OFF = false; // just to be sure, but should not be necessary - - /* Assign IP to ap/gateway */ - esp_netif_ip_info_t ip; - memset(&ip, 0, sizeof(esp_netif_ip_info_t)); - ip.ip.addr = ipaddr_addr(DB_PARAM_AP_IP); - ip.netmask.addr = ipaddr_addr("255.255.255.0"); - ip.gw.addr = ipaddr_addr(DB_PARAM_AP_IP); - ESP_ERROR_CHECK(esp_netif_dhcps_stop(esp_default_netif)); - ESP_ERROR_CHECK(esp_netif_set_ip_info(esp_default_netif, &ip)); - ESP_ERROR_CHECK(esp_netif_dhcps_start(esp_default_netif)); - + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); + if(wifi_mode == DB_WIFI_MODE_AP_LR) { + ESP_LOGI(TAG, + "Enabling LR Mode on access point. This device will be invisible " + "to non-ESP32 devices!"); ESP_ERROR_CHECK( - esp_netif_set_hostname(esp_default_netif, (char *) db_param_wifi_hostname.value.db_param_str.value)); + esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); + } + else { + ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_AP, WIFI_PROTOCOL_11B)); + } + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config)); + wifi_country_t wifi_country = { + .cc = "US", .schan = 1, .nchan = 13, .policy = WIFI_COUNTRY_POLICY_MANUAL + }; + ESP_ERROR_CHECK(esp_wifi_set_country(&wifi_country)); + ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); + ESP_ERROR_CHECK(esp_wifi_start()); + DB_RADIO_IS_OFF = false; // just to be sure, but should not be necessary + + /* Assign IP to ap/gateway */ + esp_netif_ip_info_t ip; + memset(&ip, 0, sizeof(esp_netif_ip_info_t)); + ip.ip.addr = ipaddr_addr(DB_PARAM_AP_IP); + ip.netmask.addr = ipaddr_addr("255.255.255.0"); + ip.gw.addr = ipaddr_addr(DB_PARAM_AP_IP); + ESP_ERROR_CHECK(esp_netif_dhcps_stop(esp_default_netif)); + ESP_ERROR_CHECK(esp_netif_set_ip_info(esp_default_netif, &ip)); + ESP_ERROR_CHECK(esp_netif_dhcps_start(esp_default_netif)); + + ESP_ERROR_CHECK(esp_netif_set_hostname( + esp_default_netif, + (char *)db_param_wifi_hostname.value.db_param_str.value)); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstringop-truncation" - strncpy(CURRENT_CLIENT_IP, DB_PARAM_AP_IP, sizeof(CURRENT_CLIENT_IP)); + strncpy(CURRENT_CLIENT_IP, DB_PARAM_AP_IP, sizeof(CURRENT_CLIENT_IP)); #pragma GCC diagnostic pop - ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_SOFTAP)); + ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_SOFTAP)); } -/** - * Initializes the ESP Wi-fi client/station mode where we connect to a known access point. - */ -int db_init_wifi_clientmode() { - s_wifi_event_group = xEventGroupCreate(); - ESP_ERROR_CHECK(esp_netif_init()); - ESP_ERROR_CHECK(esp_event_loop_create_default()); - esp_default_netif = esp_netif_create_default_wifi_sta(); - assert(esp_default_netif); - ESP_ERROR_CHECK( - esp_netif_set_hostname(esp_default_netif, (char *) db_param_wifi_hostname.value.db_param_str.value)); - - wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); - ESP_ERROR_CHECK(esp_wifi_init(&cfg)); - esp_event_handler_instance_t instance_any_id; - esp_event_handler_instance_t instance_got_ip; - ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, - ESP_EVENT_ANY_ID, - &wifi_event_handler, - NULL, - &instance_any_id)); - ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, - IP_EVENT_STA_GOT_IP, - &wifi_event_handler, - NULL, - &instance_got_ip)); - - - wifi_config_t wifi_config = { - .sta = { - .ssid = "DroneBridge_ESP32_Init", - .password = "dronebridge", - .threshold.authmode = WIFI_AUTH_WEP - }, - }; +static int +init_wifi_clientmode() +{ + s_wifi_event_group = xEventGroupCreate(); + ESP_ERROR_CHECK(esp_netif_init()); + ESP_ERROR_CHECK(esp_event_loop_create_default()); + esp_default_netif = esp_netif_create_default_wifi_sta(); + assert(esp_default_netif); + ESP_ERROR_CHECK(esp_netif_set_hostname( + esp_default_netif, + (char *)db_param_wifi_hostname.value.db_param_str.value)); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + esp_event_handler_instance_t instance_any_id; + esp_event_handler_instance_t instance_got_ip; + ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, + ESP_EVENT_ANY_ID, + &wifi_event_handler, + NULL, + &instance_any_id)); + ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, + IP_EVENT_STA_GOT_IP, + &wifi_event_handler, + NULL, + &instance_got_ip)); + + wifi_config_t wifi_config = { + .sta = { .ssid = "DroneBridge_ESP32_Init", + .password = "dronebridge", + .threshold.authmode = WIFI_AUTH_WEP }, + }; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstringop-truncation" - // Set Wi-Fi SSID and password from the stored parameters - if (strlen(DB_PARAM_WIFI_SSID) >= db_param_ssid.value.db_param_str.min_len) { - strncpy((char *) wifi_config.ap.ssid, DB_PARAM_WIFI_SSID, db_param_ssid.value.db_param_str.max_len); - } else { - // something is wrong - switch to default value - strncpy((char *) wifi_config.ap.ssid, (char *) db_param_ssid.value.db_param_str.default_value, - db_param_ssid.value.db_param_str.max_len); - } - if (strlen(DB_PARAM_PASS) >= db_param_pass.value.db_param_str.min_len) { - strncpy((char *) wifi_config.ap.password, DB_PARAM_PASS, db_param_pass.value.db_param_str.max_len); - } else { - // something is wrong - switch to default value - strncpy((char *) wifi_config.ap.password, (char *) db_param_pass.value.db_param_str.default_value, - db_param_pass.value.db_param_str.max_len); - } + // Set Wi-Fi SSID and password from the stored parameters + if(strlen(DB_PARAM_WIFI_SSID) >= db_param_ssid.value.db_param_str.min_len) { + strncpy((char *)wifi_config.ap.ssid, + DB_PARAM_WIFI_SSID, + db_param_ssid.value.db_param_str.max_len); + } + else { + // something is wrong - switch to default value + strncpy((char *)wifi_config.ap.ssid, + (char *)db_param_ssid.value.db_param_str.default_value, + db_param_ssid.value.db_param_str.max_len); + } + if(strlen(DB_PARAM_PASS) >= db_param_pass.value.db_param_str.min_len) { + strncpy((char *)wifi_config.ap.password, + DB_PARAM_PASS, + db_param_pass.value.db_param_str.max_len); + } + else { + // something is wrong - switch to default value + strncpy((char *)wifi_config.ap.password, + (char *)db_param_pass.value.db_param_str.default_value, + db_param_pass.value.db_param_str.max_len); + } #pragma GCC diagnostic pop - ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); - if (DB_PARAM_WIFI_EN_GN) { - // only makes sense if the AP can not do proper N or you do not need range or want Wi-Fi 6 ax support + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); + if(DB_PARAM_WIFI_EN_GN) { + // only makes sense if the AP can not do proper N or you do not need range + // or want Wi-Fi 6 ax support #ifdef CONFIG_IDF_TARGET_ESP32C6 - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_11AX | WIFI_PROTOCOL_LR)); + ESP_ERROR_CHECK(esp_wifi_set_protocol( + WIFI_IF_STA, + WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | + WIFI_PROTOCOL_11AX | WIFI_PROTOCOL_LR)); #else - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | - WIFI_PROTOCOL_LR)); + ESP_ERROR_CHECK( + esp_wifi_set_protocol(WIFI_IF_STA, + WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | + WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR)); #endif - } else { - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); // range for sure - } - ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config)); - ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); // disable power saving - ESP_ERROR_CHECK(esp_wifi_start()); - DB_RADIO_IS_OFF = false; // just to be sure, but should not be necessary - // consider connection lost after 1s of no beacon - triggers reconnect via WIFI_EVENT_STA_DISCONNECTED event - ESP_ERROR_CHECK(esp_wifi_set_inactive_time(WIFI_IF_STA, 3)); - - ESP_LOGI(TAG, "Init of WiFi Client-Mode finished. (SSID: %s PASS: %s)", DB_PARAM_WIFI_SSID, DB_PARAM_PASS); - - /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum - * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */ - EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group, - WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, - pdFALSE, - pdFALSE, - portMAX_DELAY); - - /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually - * happened. */ - bool enable_temp_ap_mode = false; - if (bits & WIFI_CONNECTED_BIT) { - ESP_LOGI(TAG, "Connected to ap SSID:%s password:%s", DB_PARAM_WIFI_SSID, DB_PARAM_PASS); - } else if (bits & WIFI_FAIL_BIT) { - ESP_LOGW(TAG, "Failed to connect to SSID:%s, password:%s", DB_PARAM_WIFI_SSID, DB_PARAM_PASS); - enable_temp_ap_mode = true; - } else { - ESP_LOGE(TAG, "UNEXPECTED WIFI EVENT"); - } - if (enable_temp_ap_mode) { - ESP_LOGW(TAG, "WiFi client mode was not able to connect to the specified access point"); - return -1; - } - - ESP_LOGI(TAG, "WiFi client mode enabled and connected!"); - ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_STA)); - return 0; + } + else { + ESP_ERROR_CHECK(esp_wifi_set_protocol( + WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); // range for sure + } + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); // disable power saving + ESP_ERROR_CHECK(esp_wifi_start()); + DB_RADIO_IS_OFF = false; // just to be sure, but should not be necessary + // consider connection lost after 1s of no beacon - triggers reconnect via + // WIFI_EVENT_STA_DISCONNECTED event + ESP_ERROR_CHECK(esp_wifi_set_inactive_time(WIFI_IF_STA, 3)); + + ESP_LOGI(TAG, + "Init of WiFi Client-Mode finished. (SSID: %s PASS: %s)", + DB_PARAM_WIFI_SSID, + DB_PARAM_PASS); + + /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or + * connection failed for the maximum number of re-tries (WIFI_FAIL_BIT). The + * bits are set by event_handler() (see above) */ + EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group, + WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, + pdFALSE, + pdFALSE, + portMAX_DELAY); + + /* xEventGroupWaitBits() returns the bits before the call returned, hence we + * can test which event actually happened. */ + bool enable_temp_ap_mode = false; + if(bits & WIFI_CONNECTED_BIT) { + ESP_LOGI(TAG, + "Connected to ap SSID:%s password:%s", + DB_PARAM_WIFI_SSID, + DB_PARAM_PASS); + } + else if(bits & WIFI_FAIL_BIT) { + ESP_LOGW(TAG, + "Failed to connect to SSID:%s, password:%s", + DB_PARAM_WIFI_SSID, + DB_PARAM_PASS); + enable_temp_ap_mode = true; + } + else { + ESP_LOGE(TAG, "UNEXPECTED WIFI EVENT"); + } + if(enable_temp_ap_mode) { + ESP_LOGW(TAG, + "WiFi client mode was not able to connect to the specified " + "access point"); + return -1; + } + + ESP_LOGI(TAG, "WiFi client mode enabled and connected!"); + ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_STA)); + return 0; } -/** - * Initialize Wi-Fi for ESP-NOW mode. - * If someone uses ESP-NOW over Wi-Fi it is because he wants range over everything else. - * LR mode makes it very inconvenient to change settings but gives the most range. No AP mode since AP will not be - * visible. - */ -void db_init_wifi_espnow() { - ESP_LOGI(TAG, "Setting up Wi-Fi for ESP-NOW"); - ESP_ERROR_CHECK(esp_netif_init()); - ESP_ERROR_CHECK(esp_event_loop_create_default()); - wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); - ESP_ERROR_CHECK(esp_wifi_init(&cfg)); - ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); - ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); - ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); - ESP_ERROR_CHECK(esp_wifi_start()); - ESP_ERROR_CHECK(esp_wifi_set_channel(DB_PARAM_CHANNEL, WIFI_SECOND_CHAN_NONE)); - ESP_ERROR_CHECK(esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); - ESP_LOGI(TAG, "Enabled ESP-NOW WiFi Mode! LR Mode is set. This device will be invisible to non-ESP32 devices!"); - ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_STA)); +static void +init_wifi_espnow() +{ + ESP_LOGI(TAG, "Setting up Wi-Fi for ESP-NOW"); + ESP_ERROR_CHECK(esp_netif_init()); + ESP_ERROR_CHECK(esp_event_loop_create_default()); + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); + ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); + ESP_ERROR_CHECK(esp_wifi_start()); + ESP_ERROR_CHECK( + esp_wifi_set_channel(DB_PARAM_CHANNEL, WIFI_SECOND_CHAN_NONE)); + ESP_ERROR_CHECK( + esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_LR)); + ESP_LOGI(TAG, + "Enabled ESP-NOW WiFi Mode! LR Mode is set. This device will be " + "invisible to non-ESP32 devices!"); + ESP_ERROR_CHECK(esp_read_mac(LOCAL_MAC_ADDRESS, ESP_MAC_WIFI_STA)); } -/** - * Enables or disables (via reboot) the Wi-Fi/BLE if the DB_PARAM_DIS_RADIO_ON_ARM parameter is set. - * Not used during boot. - * Usually called when arm state change of the autopilot is detected. - * As internal check if the Wi-Fi is already enabled/disabled Wi-Fi must be inited first (done during boot). - * - * This is handy for modes like BLE & WiFi AP which are used to configure the drone and usually do not act as a long - * range telemetry link. In these cases interference on the drone can be reduced by disabling the radio. - * - * @param enable_wifi True to enable the Wi-Fi and FALSE to disable it - */ -void db_set_radio_status(uint8_t enable_wifi) { - if (DB_PARAM_DIS_RADIO_ON_ARM) { // check if the user enables that feature - if (enable_wifi && DB_RADIO_IS_OFF) { - ESP_LOGI(TAG, "Rebooting ESP32 to re-enable Wi-Fi/BLE"); - esp_restart(); // enable Wi-Fi/BLE by restarting ESP32 - an easy way to make sure all things are set up right - } else if (!enable_wifi && !DB_RADIO_IS_OFF) { - ESP_LOGI(TAG, "Disabling Wi-Fi/BLE"); - if (DB_PARAM_RADIO_MODE == DB_BLUETOOTH_MODE) { - db_ble_deinit(); // disable BLE - DB_RADIO_IS_OFF = true; - } else { - if (esp_wifi_stop() == ESP_OK) { // disable WiFi - DB_RADIO_IS_OFF = true; - } else { - ESP_LOGW(TAG, "db_set_radio_status tried to disable Wi-Fi. FAILED"); - } - } - } - } else { - // nothing to do - } -} - -/** - * Write settings to non-volatile memory so they can be loaded on next startup. The UDP clients are saved using a - * separate function since the "save" operation is triggered by a separate button on the UI. - */ -void db_write_settings_to_nvs() { - // print parameters to console for logging - ESP_LOGI(TAG, "Trying to save parameters:"); - uint8_t param_str_buffer[512]; - db_param_print_values_to_buffer(param_str_buffer); - ESP_LOGI(TAG, "%s", param_str_buffer); - ESP_LOGI(TAG, "Saving to NVS %s", NVS_NAMESPACE); - nvs_handle_t my_handle; - ESP_ERROR_CHECK(nvs_open(NVS_NAMESPACE, NVS_READWRITE, &my_handle)); - db_param_write_all_params_nvs(&my_handle); - ESP_ERROR_CHECK(nvs_commit(my_handle)); +static void +read_settings_nvs() +{ + nvs_handle my_handle; + if(nvs_open(NVS_NAMESPACE, NVS_READONLY, &my_handle) != ESP_OK) { + ESP_LOGI( + TAG, "NVS namespace not found. Using default values, setting up NVS..."); nvs_close(my_handle); - ESP_LOGI(TAG, "Finished saving operation."); -} + ESP_ERROR_CHECK(nvs_flash_erase()); + ESP_ERROR_CHECK(nvs_flash_init()); -/** - * Saves an udp client to the NVM so it can be automatically added on the next boot. No need for the user to manually add it again. - * Only one UDP client can be saved to the NVM. - * @param new_db_udp_client The client to add to NVM. Must have IP and port set. - * @param clear_client Set to true to remove the current client from NVM. In that case the new_db_udp_client param will be ignored. - */ -void save_udp_client_to_nvm(struct db_udp_client_t *new_db_udp_client, bool clear_client) { - char ip[INET_ADDRSTRLEN]; - uint16_t port; - if (!clear_client) { - // convert addr to string - char client_str[INET_ADDRSTRLEN + 6]; - inet_ntop(AF_INET, &(new_db_udp_client->udp_client.sin_addr), ip, INET_ADDRSTRLEN); - port = ntohs(new_db_udp_client->udp_client.sin_port); - snprintf(client_str, sizeof(client_str), "%s:%d", ip, port); - ESP_LOGI(TAG, "Saving UDP client %s to NVS %s", client_str, NVS_NAMESPACE); - } else { - // clear client from NVM by setting string to empty "" and port to 0 - ip[0] = '\0'; - port = 0; - ESP_LOGI(TAG, "Clearing UDP client from NVM"); - } + // Set all parameters to their default values when flash is empty + db_param_reset_all(); - nvs_handle my_handle; - ESP_ERROR_CHECK(nvs_open(NVS_NAMESPACE, NVS_READWRITE, &my_handle)); - ESP_ERROR_CHECK(nvs_set_str(my_handle, (char *) db_param_udp_client_ip.db_name, ip)); - ESP_ERROR_CHECK(nvs_set_u16(my_handle, (char *) db_param_udp_client_port.db_name, port)); + // Now save these defaults to NVS + db_write_settings_to_nvs(); - ESP_ERROR_CHECK(nvs_commit(my_handle)); + // Print parameters to console for logging + uint8_t param_str_buffer[512] = { 0 }; + db_param_print_values_to_buffer(param_str_buffer); + ESP_LOGI( + TAG, "Initialized with default values:\n%s", (char *)param_str_buffer); + } + else { + ESP_LOGI(TAG, "Reading settings from NVS"); + db_param_read_all_params_nvs(&my_handle); nvs_close(my_handle); -} -/** - * Read stored settings from internal storage including the saved UDP client. - */ -void db_read_settings_nvs() { - nvs_handle my_handle; - if (nvs_open(NVS_NAMESPACE, NVS_READONLY, &my_handle) != ESP_OK) { - ESP_LOGI(TAG, "NVS namespace not found. Using default values, setting up NVS..."); - nvs_close(my_handle); - ESP_ERROR_CHECK(nvs_flash_erase()); - ESP_ERROR_CHECK(nvs_flash_init()); - - // Set all parameters to their default values when flash is empty - db_param_reset_all(); - - // Now save these defaults to NVS - db_write_settings_to_nvs(); - - // Print parameters to console for logging - uint8_t param_str_buffer[512] = {0}; - db_param_print_values_to_buffer(param_str_buffer); - ESP_LOGI(TAG, "Initialized with default values:\n%s", (char *)param_str_buffer); - } else { - ESP_LOGI(TAG, "Reading settings from NVS"); - db_param_read_all_params_nvs(&my_handle); - nvs_close(my_handle); - - // print parameters to console for logging - uint8_t param_str_buffer[512] = {0}; - db_param_print_values_to_buffer(param_str_buffer); - ESP_LOGI(TAG, "\n%s", (char *) param_str_buffer); - - // Check if we have a saved UDP client from the last session. Add it to the known udp clients if there is one. - if (strlen((char *) db_param_udp_client_ip.value.db_param_str.value) > 0 && - db_param_udp_client_port.value.db_param_u16.value != 0) { - // there was a saved UDP client in the NVM from last session - add it to the udp clients list - ESP_LOGI(TAG, "Adding %s:%i to known UDP clients.", - (char *) db_param_udp_client_ip.value.db_param_str.value, - db_param_udp_client_port.value.db_param_u8.value); - struct sockaddr_in new_sockaddr; - memset(&new_sockaddr, 0, sizeof(new_sockaddr)); - new_sockaddr.sin_family = AF_INET; - inet_pton(AF_INET, (char *) db_param_udp_client_ip.value.db_param_str.value, &new_sockaddr.sin_addr); - new_sockaddr.sin_port = htons(db_param_udp_client_port.value.db_param_u16.value); - struct db_udp_client_t new_udp_client = { - .udp_client = new_sockaddr, - .mac = {0, 0, 0, 0, 0, 0} // dummy MAC - }; - bool save_to_nvm = false; // no need to save it to NVM again - add_to_known_udp_clients(udp_conn_list, new_udp_client, save_to_nvm); - } else { - // no saved UDP client - do nothing - ESP_LOGI(TAG, "No saved UDP client - skipping"); - } + // print parameters to console for logging + uint8_t param_str_buffer[512] = { 0 }; + db_param_print_values_to_buffer(param_str_buffer); + ESP_LOGI(TAG, "\n%s", (char *)param_str_buffer); + + // Check if we have a saved UDP client from the last session. Add it to the + // known udp clients if there is one. + if(strlen((char *)db_param_udp_client_ip.value.db_param_str.value) > 0 && + db_param_udp_client_port.value.db_param_u16.value != 0) { + // there was a saved UDP client in the NVM from last session - add it to + // the udp clients list + ESP_LOGI(TAG, + "Adding %s:%i to known UDP clients.", + (char *)db_param_udp_client_ip.value.db_param_str.value, + db_param_udp_client_port.value.db_param_u8.value); + struct sockaddr_in new_sockaddr; + memset(&new_sockaddr, 0, sizeof(new_sockaddr)); + new_sockaddr.sin_family = AF_INET; + inet_pton(AF_INET, + (char *)db_param_udp_client_ip.value.db_param_str.value, + &new_sockaddr.sin_addr); + new_sockaddr.sin_port = + htons(db_param_udp_client_port.value.db_param_u16.value); + struct db_udp_client_t new_udp_client = { + .udp_client = new_sockaddr, .mac = { 0, 0, 0, 0, 0, 0 } // dummy MAC + }; + bool save_to_nvm = false; // no need to save it to NVM again + db_add_to_known_udp_clients(udp_conn_list, new_udp_client, save_to_nvm); + } + else { + // no saved UDP client - do nothing + ESP_LOGI(TAG, "No saved UDP client - skipping"); } + } } -/** - * Callback for a short press (CONFIG_BUTTON_LONG_PRESS_TIME_MS) of the reset/boot button. - * Resets all settings to defaults. - * @param arg - */ -void long_press_callback(void *arg, void *usr_data) { - ESP_LOGW(TAG, "Reset triggered via GPIO %i. Resetting settings and rebooting", DB_RESET_PIN); - DB_RADIO_MODE_DESIGNATED = DB_WIFI_MODE_AP; // do not directly change DB_PARAM_RADIO_MODE since it is not safe and constantly processed by other tasks. Save settings and reboot will assign DB_RADIO_MODE_DESIGNATED to DB_PARAM_RADIO_MODE. - db_param_reset_all(); - db_write_settings_to_nvs(); - esp_restart(); +static void +long_press_callback(void *arg, void *usr_data) +{ + ESP_LOGW(TAG, + "Reset triggered via GPIO %i. Resetting settings and rebooting", + DB_RESET_PIN); + DB_RADIO_MODE_DESIGNATED = + DB_WIFI_MODE_AP; // do not directly change DB_PARAM_RADIO_MODE since it is + // not safe and constantly processed by other tasks. Save + // settings and reboot will assign + // DB_RADIO_MODE_DESIGNATED to DB_PARAM_RADIO_MODE. + db_param_reset_all(); + db_write_settings_to_nvs(); + esp_restart(); } -/** - * Setup boot button GPIO to reset entire ESP32 settings and to force a reboot of the system - */ -void set_reset_trigger() { - button_config_t gpio_btn_cfg = { +static void +set_reset_trigger() +{ + button_config_t gpio_btn_cfg = { .type = BUTTON_TYPE_GPIO, .long_press_time = CONFIG_BUTTON_LONG_PRESS_TIME_MS, .short_press_time = CONFIG_BUTTON_SHORT_PRESS_TIME_MS, @@ -653,107 +783,203 @@ void set_reset_trigger() { .active_level = 0, }, }; - button_handle_t gpio_btn = iot_button_create(&gpio_btn_cfg); - if (NULL == gpio_btn) { - ESP_LOGE(TAG, "Creating reset button failed"); - } else { - iot_button_register_cb(gpio_btn, BUTTON_SINGLE_CLICK, short_press_callback, NULL); - iot_button_register_cb(gpio_btn, BUTTON_LONG_PRESS_UP, long_press_callback, NULL); - } -} - -/** - * For simple debugging when serial via JTAG is enabled. Printed once control module configured USB serial socket. - * Write settings to JTAG/USB, so we can debug issues better - */ -void db_jtag_serial_info_print() { - uint8_t buffer[512]; - const int len = db_param_print_values_to_buffer(buffer); - write_to_serial(buffer, len); + button_handle_t gpio_btn = iot_button_create(&gpio_btn_cfg); + if(NULL == gpio_btn) { + ESP_LOGE(TAG, "Creating reset button failed"); + } + else { + iot_button_register_cb( + gpio_btn, BUTTON_SINGLE_CLICK, short_press_callback, NULL); + iot_button_register_cb( + gpio_btn, BUTTON_LONG_PRESS_UP, long_press_callback, NULL); + } } -/** - * 1. Checks if RF Switch is configured. - * 2. Enables RF switch (necessary on some boards). - * 3. Sets RF switch according to user setting. - * Must be called before radio/Wi-Fi gets initialized! - */ -void db_configure_antenna() { -#if defined(CONFIG_DB_HAS_RF_SWITCH) && defined(CONFIG_DB_RF_SWITCH_GPIO) && (CONFIG_DB_RF_SWITCH_GPIO != 0) +static void +configure_antenna() +{ +#if defined(CONFIG_DB_HAS_RF_SWITCH) && defined(CONFIG_DB_RF_SWITCH_GPIO) && \ + (CONFIG_DB_RF_SWITCH_GPIO != 0) #ifdef CONFIG_DB_OFFICIAL_BOARD_1_X_C6 - gpio_set_direction(GPIO_NUM_3, GPIO_MODE_OUTPUT); - gpio_set_level(GPIO_NUM_3, 0); // set to low to enable RF switching + gpio_set_direction(GPIO_NUM_3, GPIO_MODE_OUTPUT); + gpio_set_level(GPIO_NUM_3, 0); // set to low to enable RF switching #endif - gpio_set_direction(CONFIG_DB_RF_SWITCH_GPIO, GPIO_MODE_OUTPUT); - gpio_set_level(CONFIG_DB_RF_SWITCH_GPIO, DB_PARAM_EN_EXT_ANT); // set level to enable external/internal antenna - ESP_LOGI(TAG, "External antenna usage: %i", DB_PARAM_EN_EXT_ANT); + gpio_set_direction(CONFIG_DB_RF_SWITCH_GPIO, GPIO_MODE_OUTPUT); + gpio_set_level( + CONFIG_DB_RF_SWITCH_GPIO, + DB_PARAM_EN_EXT_ANT); // set level to enable external/internal antenna + ESP_LOGI(TAG, "External antenna usage: %i", DB_PARAM_EN_EXT_ANT); #endif } /** * Main entry point. */ -void app_main() { - db_param_init_parameters(); - udp_conn_list = udp_client_list_create(); // http server functions and db_read_settings_nvs expect the list to exist - esp_err_t ret = nvs_flash_init(); - if (ret == ESP_ERR_NVS_NO_FREE_PAGES) { - ESP_ERROR_CHECK(nvs_flash_erase()); - ret = nvs_flash_init(); - } - ESP_ERROR_CHECK(ret); - db_read_settings_nvs(); - DB_RADIO_MODE_DESIGNATED = DB_PARAM_RADIO_MODE; // must always match, mismatch only allowed when changed by user action and not rebooted, yet. - set_reset_trigger(); - db_configure_antenna(); - - switch (DB_PARAM_RADIO_MODE) { - case DB_WIFI_MODE_AP: - case DB_WIFI_MODE_AP_LR: - db_init_wifi_apmode(DB_PARAM_RADIO_MODE); - break; - case DB_WIFI_MODE_ESPNOW_AIR: - case DB_WIFI_MODE_ESPNOW_GND: - db_init_wifi_espnow(); - db_start_espnow_module(); - break; - case DB_BLUETOOTH_MODE: +void +app_main() +{ + db_param_init_parameters(); + udp_conn_list = db_udp_client_list_create(); // http server functions and + // read_settings_nvs expect + // the list to exist + esp_err_t ret = nvs_flash_init(); + if(ret == ESP_ERR_NVS_NO_FREE_PAGES) { + ESP_ERROR_CHECK(nvs_flash_erase()); + ret = nvs_flash_init(); + } + ESP_ERROR_CHECK(ret); + read_settings_nvs(); + DB_RADIO_MODE_DESIGNATED = + DB_PARAM_RADIO_MODE; // must always match, mismatch only allowed when + // changed by user action and not rebooted, yet. + set_reset_trigger(); + configure_antenna(); + + switch(DB_PARAM_RADIO_MODE) { + case DB_WIFI_MODE_AP: + case DB_WIFI_MODE_AP_LR: + init_wifi_apmode(DB_PARAM_RADIO_MODE); + break; + case DB_WIFI_MODE_ESPNOW_AIR: + case DB_WIFI_MODE_ESPNOW_GND: + init_wifi_espnow(); + db_start_espnow_module(); + break; + case DB_BLUETOOTH_MODE: #ifdef CONFIG_BT_ENABLED - db_init_wifi_apmode(DB_WIFI_MODE_AP); // WiFi & BLE co-existence to enable webinterface - db_ble_queue_init(); - db_ble_init(); + init_wifi_apmode( + DB_WIFI_MODE_AP); // WiFi & BLE co-existence to enable webinterface + db_ble_queue_init(); + db_ble_init(); #else - DB_RADIO_MODE_DESIGNATED = DB_WIFI_MODE_AP; - DB_PARAM_RADIO_MODE = DB_WIFI_MODE_AP; - ESP_LOGE(TAG, "Bluetooth is not enabled with this build. Please enable it in menuconfig and re-compile. Switching to AP mode."); - db_init_wifi_apmode(DB_WIFI_MODE_AP); + DB_RADIO_MODE_DESIGNATED = DB_WIFI_MODE_AP; + DB_PARAM_RADIO_MODE = DB_WIFI_MODE_AP; + ESP_LOGE(TAG, + "Bluetooth is not enabled with this build. Please enable it in " + "menuconfig and re-compile. Switching to AP mode."); + init_wifi_apmode(DB_WIFI_MODE_AP); #endif - break; - default: - // Wi-Fi client mode with LR mode enabled - if (db_init_wifi_clientmode() < 0) { - ESP_LOGE(TAG, "Failed to init Wifi Client Mode"); - } - break; + break; + default: + // Wi-Fi client mode with LR mode enabled + if(init_wifi_clientmode() < 0) { + ESP_LOGE(TAG, "Failed to init Wifi Client Mode"); } + break; + } + + if(DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_AIR && + DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND && + DB_PARAM_RADIO_MODE != DB_WIFI_MODE_AP_LR) { + // no need to start these services - won`t be available anyway - safe the + // resources + start_mdns_service(); + netbiosns_init(); + netbiosns_set_name("dronebridge"); + } + ESP_ERROR_CHECK(init_fs()); + db_start_control_module(); + + if(DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_AIR && + DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND && + DB_PARAM_RADIO_MODE != DB_WIFI_MODE_AP_LR) { + // no need to start these services - won`t be available anyway - safe the + // resources + ESP_ERROR_CHECK(start_rest_server(CONFIG_WEB_MOUNT_POINT)); + ESP_LOGI(TAG, "Rest Server started"); + // Disable legacy support for DroneBridge communication module - no use + // case for DroneBridge for ESP32 communication_module(); + } + ESP_LOGI(TAG, "app_main finished initial setup"); +} - if (DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_AIR && DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND && - DB_PARAM_RADIO_MODE != DB_WIFI_MODE_AP_LR) { - // no need to start these services - won`t be available anyway - safe the resources - start_mdns_service(); - netbiosns_init(); - netbiosns_set_name("dronebridge"); +/****************************************************************************** + * Public Function Definitions + ******************************************************************************/ + +void +db_jtag_serial_info_print() +{ + uint8_t buffer[512]; + const int len = db_param_print_values_to_buffer(buffer); + db_write_to_serial(buffer, len); +} + +void +db_write_settings_to_nvs() +{ + // print parameters to console for logging + ESP_LOGI(TAG, "Trying to save parameters:"); + uint8_t param_str_buffer[512]; + db_param_print_values_to_buffer(param_str_buffer); + ESP_LOGI(TAG, "%s", param_str_buffer); + ESP_LOGI(TAG, "Saving to NVS %s", NVS_NAMESPACE); + nvs_handle_t my_handle; + ESP_ERROR_CHECK(nvs_open(NVS_NAMESPACE, NVS_READWRITE, &my_handle)); + db_param_write_all_params_nvs(&my_handle); + ESP_ERROR_CHECK(nvs_commit(my_handle)); + nvs_close(my_handle); + ESP_LOGI(TAG, "Finished saving operation."); +} + +void db_save_udp_client_to_nvm(struct db_udp_client_t *new_db_udp_client, + bool clear_client) +{ + char ip[INET_ADDRSTRLEN]; + uint16_t port; + if(!clear_client) { + // convert addr to string + char client_str[INET_ADDRSTRLEN + 6]; + inet_ntop( + AF_INET, &(new_db_udp_client->udp_client.sin_addr), ip, INET_ADDRSTRLEN); + port = ntohs(new_db_udp_client->udp_client.sin_port); + snprintf(client_str, sizeof(client_str), "%s:%d", ip, port); + ESP_LOGI(TAG, "Saving UDP client %s to NVS %s", client_str, NVS_NAMESPACE); + } + else { + // clear client from NVM by setting string to empty "" and port to 0 + ip[0] = '\0'; + port = 0; + ESP_LOGI(TAG, "Clearing UDP client from NVM"); + } + + nvs_handle my_handle; + ESP_ERROR_CHECK(nvs_open(NVS_NAMESPACE, NVS_READWRITE, &my_handle)); + ESP_ERROR_CHECK( + nvs_set_str(my_handle, (char *)db_param_udp_client_ip.db_name, ip)); + ESP_ERROR_CHECK( + nvs_set_u16(my_handle, (char *)db_param_udp_client_port.db_name, port)); + + ESP_ERROR_CHECK(nvs_commit(my_handle)); + nvs_close(my_handle); +} + +void +db_set_radio_status(uint8_t enable_wifi) +{ + if(DB_PARAM_DIS_RADIO_ON_ARM) { // check if the user enables that feature + if(enable_wifi && DB_RADIO_IS_OFF) { + ESP_LOGI(TAG, "Rebooting ESP32 to re-enable Wi-Fi/BLE"); + esp_restart(); // enable Wi-Fi/BLE by restarting ESP32 - an easy way to + // make sure all things are set up right } - ESP_ERROR_CHECK(init_fs()); - db_start_control_module(); - - if (DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_AIR && DB_PARAM_RADIO_MODE != DB_WIFI_MODE_ESPNOW_GND && - DB_PARAM_RADIO_MODE != DB_WIFI_MODE_AP_LR) { - // no need to start these services - won`t be available anyway - safe the resources - ESP_ERROR_CHECK(start_rest_server(CONFIG_WEB_MOUNT_POINT)); - ESP_LOGI(TAG, "Rest Server started"); - // Disable legacy support for DroneBridge communication module - no use case for DroneBridge for ESP32 - // communication_module(); + else if(!enable_wifi && !DB_RADIO_IS_OFF) { + ESP_LOGI(TAG, "Disabling Wi-Fi/BLE"); + if(DB_PARAM_RADIO_MODE == DB_BLUETOOTH_MODE) { + db_ble_deinit(); // disable BLE + DB_RADIO_IS_OFF = true; + } + else { + if(esp_wifi_stop() == ESP_OK) { // disable WiFi + DB_RADIO_IS_OFF = true; + } + else { + ESP_LOGW(TAG, "db_set_radio_status tried to disable Wi-Fi. FAILED"); + } + } } - ESP_LOGI(TAG, "app_main finished initial setup"); -} + } + else { + // nothing to do + } +} \ No newline at end of file diff --git a/main/main.h b/main/main.h index 21bf604..a1307dd 100644 --- a/main/main.h +++ b/main/main.h @@ -1,35 +1,88 @@ -/* - * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 +/****************************************************************************** + * @file db_esp32_main.h + * @brief DroneBridge ESP32 Main Header File * - * Copyright 2018 Wolfgang Christl + * This file is part of DroneBridge: https://github.com/DroneBridge/ESP32 * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at + * This file declares core utility and system-level functions used in the + * DroneBridge ESP32 firmware, including JTAG info, NVS write operations, + * RF switch configuration, and radio status handling. * - * http://www.apache.org/licenses/LICENSE-2.0 + * @copyright + * Copyright 2018 Wolfgang Christl + * Licensed under the Apache License, Version 2.0 (the "License"); + * You may not use this file except in compliance with the License. + * You may obtain a copy of the License at: + * http://www.apache.org/licenses/LICENSE-2.0 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ - + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + ******************************************************************************/ #ifndef DB_ESP32_MAIN_H #define DB_ESP32_MAIN_H +#include "db_esp32_control.h" + +/****************************************************************************** + * Configuration Macros + ******************************************************************************/ + +/****************************************************************************** + * @brief Enable RF switch support if configured in Kconfig + ******************************************************************************/ #ifdef CONFIG_DB_HAS_RF_SWITCH #define DB_HAS_RF_SWITCH 1 #else #define DB_HAS_RF_SWITCH 0 #endif -void db_jtag_serial_info_print(); -void db_write_settings_to_nvs(); -void save_udp_client_to_nvm(struct db_udp_client_t *new_db_udp_client, bool clear_client); +/****************************************************************************** + * Public Function Declarations + ******************************************************************************/ + +/****************************************************************************** + * For simple debugging when serial via JTAG is enabled. Printed once control + * module configured USB serial socket. Write settings to JTAG/USB, so we can + * debug issues better + ******************************************************************************/ +void db_jtag_serial_info_print(void); + +/****************************************************************************** + * Write settings to non-volatile memory so they can be loaded on next + * startup. The UDP clients are saved using a separate function since the + * "save" operation is triggered by a separate button on the UI. + ******************************************************************************/ +void db_write_settings_to_nvs(void); + +/****************************************************************************** + * Saves an udp client to the NVM so it can be automatically added on the next + * boot. No need for the user to manually add it again. Only one UDP client can + * be saved to the NVM. + * @param new_db_udp_client The client to add to NVM. Must have IP and port + * set. + * @param clear_client Set to true to remove the current client from NVM. In + * that case the new_db_udp_client param will be ignored. + ******************************************************************************/ +void db_save_udp_client_to_nvm(struct db_udp_client_t *new_db_udp_client, + bool clear_client); + +/****************************************************************************** + * Enables or disables (via reboot) the Wi-Fi/BLE if the + * DB_PARAM_DIS_RADIO_ON_ARM parameter is set. Not used during boot. Usually + * called when arm state change of the autopilot is detected. As internal check + * if the Wi-Fi is already enabled/disabled Wi-Fi must be inited first (done + * during boot). + * + * This is handy for modes like BLE & WiFi AP which are used to configure the + * drone and usually do not act as a long range telemetry link. In these cases + * interference on the drone can be reduced by disabling the radio. + * + * @param enable_wifi True to enable the Wi-Fi and FALSE to disable it + ******************************************************************************/ void db_set_radio_status(uint8_t enable_wifi); -#endif //DB_ESP32_MAIN_H +#endif /* DB_ESP32_MAIN_H */ diff --git a/main/msp_ltm_serial.c b/main/msp_ltm_serial.c index 61d99eb..e855938 100644 --- a/main/msp_ltm_serial.c +++ b/main/msp_ltm_serial.c @@ -42,241 +42,270 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ -bool parse_msp_ltm_byte(msp_ltm_port_t *msp_ltm_port, uint8_t new_byte) { - switch (msp_ltm_port->parse_state) { - default: - case IDLE: - if (new_byte == '$') { - msp_ltm_port->mspVersion = MSP_V1; - msp_ltm_port->parse_state = HEADER_START; - msp_ltm_port->ltm_frame_buffer[0] = '$'; - } else { return false; } - msp_ltm_port->ltm_payload_cnt = 0; - msp_ltm_port->checksum1 = 0; - break; +bool +parse_msp_ltm_byte(msp_ltm_port_t *msp_ltm_port, uint8_t new_byte) +{ + switch(msp_ltm_port->parse_state) { + default: + case IDLE: + if(new_byte == '$') { + msp_ltm_port->mspVersion = MSP_V1; + msp_ltm_port->parse_state = HEADER_START; + msp_ltm_port->ltm_frame_buffer[0] = '$'; + } + else { + return false; + } + msp_ltm_port->ltm_payload_cnt = 0; + msp_ltm_port->checksum1 = 0; + break; - case HEADER_START: - switch (new_byte) { - case 'M': - msp_ltm_port->parse_state = MSP_HEADER_M; - break; - case 'X': - msp_ltm_port->parse_state = MSP_HEADER_X; - break; - case 'T': - msp_ltm_port->parse_state = LTM_HEADER; - msp_ltm_port->ltm_frame_buffer[1] = 'T'; - break; - default: - msp_ltm_port->parse_state = IDLE; - return false; - } - break; + case HEADER_START: + switch(new_byte) { + case 'M': + msp_ltm_port->parse_state = MSP_HEADER_M; + break; + case 'X': + msp_ltm_port->parse_state = MSP_HEADER_X; + break; + case 'T': + msp_ltm_port->parse_state = LTM_HEADER; + msp_ltm_port->ltm_frame_buffer[1] = 'T'; + break; + default: + msp_ltm_port->parse_state = IDLE; + return false; + } + break; - case LTM_HEADER: - switch (new_byte) { - case 'A': - msp_ltm_port->ltm_type = LTM_TYPE_A; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - case 'G': - msp_ltm_port->ltm_type = LTM_TYPE_G; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - case 'N': - msp_ltm_port->ltm_type = LTM_TYPE_N; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - case 'O': - msp_ltm_port->ltm_type = LTM_TYPE_O; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - case 'S': - msp_ltm_port->ltm_type = LTM_TYPE_S; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - case 'X': - msp_ltm_port->ltm_type = LTM_TYPE_X; - msp_ltm_port->parse_state = LTM_TYPE_IDENT; - break; - default: - msp_ltm_port->parse_state = IDLE; - break; - } - msp_ltm_port->ltm_frame_buffer[2] = new_byte; - msp_ltm_port->ltm_payload_cnt = 0; - break; + case LTM_HEADER: + switch(new_byte) { + case 'A': + msp_ltm_port->ltm_type = LTM_TYPE_A; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + case 'G': + msp_ltm_port->ltm_type = LTM_TYPE_G; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + case 'N': + msp_ltm_port->ltm_type = LTM_TYPE_N; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + case 'O': + msp_ltm_port->ltm_type = LTM_TYPE_O; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + case 'S': + msp_ltm_port->ltm_type = LTM_TYPE_S; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + case 'X': + msp_ltm_port->ltm_type = LTM_TYPE_X; + msp_ltm_port->parse_state = LTM_TYPE_IDENT; + break; + default: + msp_ltm_port->parse_state = IDLE; + break; + } + msp_ltm_port->ltm_frame_buffer[2] = new_byte; + msp_ltm_port->ltm_payload_cnt = 0; + break; - case LTM_TYPE_IDENT: - msp_ltm_port->ltm_payload_cnt++; - msp_ltm_port->ltm_frame_buffer[2 + msp_ltm_port->ltm_payload_cnt] = new_byte; - msp_ltm_port->checksum1 ^= new_byte; - switch (msp_ltm_port->ltm_type) { - case LTM_TYPE_A: - case LTM_TYPE_N: - case LTM_TYPE_X: - if (msp_ltm_port->ltm_payload_cnt == LTM_TYPE_A_PAYLOAD_SIZE) msp_ltm_port->parse_state = LTM_CRC; - break; - case LTM_TYPE_G: - case LTM_TYPE_O: - if (msp_ltm_port->ltm_payload_cnt == LTM_TYPE_G_PAYLOAD_SIZE) msp_ltm_port->parse_state = LTM_CRC; - break; - case LTM_TYPE_S: - if (msp_ltm_port->ltm_payload_cnt == LTM_TYPE_S_PAYLOAD_SIZE) msp_ltm_port->parse_state = LTM_CRC; - break; - default: - msp_ltm_port->parse_state = IDLE; - return false; - } - break; + case LTM_TYPE_IDENT: + msp_ltm_port->ltm_payload_cnt++; + msp_ltm_port->ltm_frame_buffer[2 + msp_ltm_port->ltm_payload_cnt] = + new_byte; + msp_ltm_port->checksum1 ^= new_byte; + switch(msp_ltm_port->ltm_type) { + case LTM_TYPE_A: + case LTM_TYPE_N: + case LTM_TYPE_X: + if(msp_ltm_port->ltm_payload_cnt == LTM_TYPE_A_PAYLOAD_SIZE) + msp_ltm_port->parse_state = LTM_CRC; + break; + case LTM_TYPE_G: + case LTM_TYPE_O: + if(msp_ltm_port->ltm_payload_cnt == LTM_TYPE_G_PAYLOAD_SIZE) + msp_ltm_port->parse_state = LTM_CRC; + break; + case LTM_TYPE_S: + if(msp_ltm_port->ltm_payload_cnt == LTM_TYPE_S_PAYLOAD_SIZE) + msp_ltm_port->parse_state = LTM_CRC; + break; + default: + msp_ltm_port->parse_state = IDLE; + return false; + } + break; - case LTM_CRC: - msp_ltm_port->ltm_frame_buffer[3 + msp_ltm_port->ltm_payload_cnt] = new_byte; - if (msp_ltm_port->checksum1 == new_byte) { - msp_ltm_port->parse_state = LTM_PACKET_RECEIVED; - } else { - msp_ltm_port->parse_state = IDLE; - } - break; + case LTM_CRC: + msp_ltm_port->ltm_frame_buffer[3 + msp_ltm_port->ltm_payload_cnt] = + new_byte; + if(msp_ltm_port->checksum1 == new_byte) { + msp_ltm_port->parse_state = LTM_PACKET_RECEIVED; + } + else { + msp_ltm_port->parse_state = IDLE; + } + break; - case MSP_HEADER_M: - if (new_byte == '>' || new_byte == '<' || new_byte == '!') { - msp_ltm_port->offset = 0; - msp_ltm_port->checksum1 = 0; - msp_ltm_port->checksum2 = 0; - msp_ltm_port->parse_state = MSP_HEADER_V1; - } else { - msp_ltm_port->parse_state = IDLE; - } - break; + case MSP_HEADER_M: + if(new_byte == '>' || new_byte == '<' || new_byte == '!') { + msp_ltm_port->offset = 0; + msp_ltm_port->checksum1 = 0; + msp_ltm_port->checksum2 = 0; + msp_ltm_port->parse_state = MSP_HEADER_V1; + } + else { + msp_ltm_port->parse_state = IDLE; + } + break; - case MSP_HEADER_X: - if (new_byte == '>' || new_byte == '<' || new_byte == '!') { - msp_ltm_port->offset = 0; - msp_ltm_port->checksum2 = 0; - msp_ltm_port->mspVersion = MSP_V2_NATIVE; - msp_ltm_port->parse_state = MSP_HEADER_V2_NATIVE; - } else { - msp_ltm_port->parse_state = IDLE; - } - break; + case MSP_HEADER_X: + if(new_byte == '>' || new_byte == '<' || new_byte == '!') { + msp_ltm_port->offset = 0; + msp_ltm_port->checksum2 = 0; + msp_ltm_port->mspVersion = MSP_V2_NATIVE; + msp_ltm_port->parse_state = MSP_HEADER_V2_NATIVE; + } + else { + msp_ltm_port->parse_state = IDLE; + } + break; - case MSP_HEADER_V1: - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - msp_ltm_port->checksum1 ^= new_byte; - if (msp_ltm_port->offset == sizeof(mspHeaderV1_t)) { - mspHeaderV1_t *hdr = (mspHeaderV1_t *) &msp_ltm_port->inBuf[0]; - // No need to check incoming buffer size limit for V1 - // because the buffer is 512 which is bigger than the INTMAX for byte - if (hdr->cmd == MSP_V2_FRAME_ID) { - if (hdr->size >= sizeof(mspHeaderV2_t) + 1) { - msp_ltm_port->mspVersion = MSP_V2_OVER_V1; - msp_ltm_port->parse_state = MSP_HEADER_V2_OVER_V1; - } else { - msp_ltm_port->parse_state = IDLE; - return false; - } - } else { - msp_ltm_port->dataSize = hdr->size; - msp_ltm_port->cmdMSP = hdr->cmd; - msp_ltm_port->cmdFlags = 0; - msp_ltm_port->offset = 0; - msp_ltm_port->parse_state = msp_ltm_port->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; - } - } - break; + case MSP_HEADER_V1: + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + msp_ltm_port->checksum1 ^= new_byte; + if(msp_ltm_port->offset == sizeof(mspHeaderV1_t)) { + mspHeaderV1_t *hdr = (mspHeaderV1_t *)&msp_ltm_port->inBuf[0]; + // No need to check incoming buffer size limit for V1 + // because the buffer is 512 which is bigger than the INTMAX for byte + if(hdr->cmd == MSP_V2_FRAME_ID) { + if(hdr->size >= sizeof(mspHeaderV2_t) + 1) { + msp_ltm_port->mspVersion = MSP_V2_OVER_V1; + msp_ltm_port->parse_state = MSP_HEADER_V2_OVER_V1; + } + else { + msp_ltm_port->parse_state = IDLE; + return false; + } + } + else { + msp_ltm_port->dataSize = hdr->size; + msp_ltm_port->cmdMSP = hdr->cmd; + msp_ltm_port->cmdFlags = 0; + msp_ltm_port->offset = 0; + msp_ltm_port->parse_state = + msp_ltm_port->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; + } + } + break; - case MSP_PAYLOAD_V1: - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - msp_ltm_port->checksum1 ^= new_byte; - if (msp_ltm_port->offset == msp_ltm_port->dataSize) { - msp_ltm_port->parse_state = MSP_CHECKSUM_V1; - } - break; + case MSP_PAYLOAD_V1: + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + msp_ltm_port->checksum1 ^= new_byte; + if(msp_ltm_port->offset == msp_ltm_port->dataSize) { + msp_ltm_port->parse_state = MSP_CHECKSUM_V1; + } + break; - case MSP_CHECKSUM_V1: - if (msp_ltm_port->checksum1 == new_byte) { - msp_ltm_port->parse_state = MSP_PACKET_RECEIVED; - } else { - msp_ltm_port->parse_state = IDLE; - } - break; + case MSP_CHECKSUM_V1: + if(msp_ltm_port->checksum1 == new_byte) { + msp_ltm_port->parse_state = MSP_PACKET_RECEIVED; + } + else { + msp_ltm_port->parse_state = IDLE; + } + break; - case MSP_HEADER_V2_OVER_V1: - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - msp_ltm_port->checksum1 ^= new_byte; - msp_ltm_port->checksum2 = crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); - if (msp_ltm_port->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) { - mspHeaderV2_t *hdrv2 = (mspHeaderV2_t *) &msp_ltm_port->inBuf[sizeof(mspHeaderV1_t)]; - msp_ltm_port->dataSize = hdrv2->size; - if (hdrv2->size > MSP_PORT_INBUF_SIZE) { - msp_ltm_port->parse_state = IDLE; - return false; - } else { - msp_ltm_port->cmdMSP = hdrv2->cmd; - msp_ltm_port->cmdFlags = hdrv2->flags; - msp_ltm_port->offset = 0; - msp_ltm_port->parse_state = - msp_ltm_port->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1; - } - } - break; + case MSP_HEADER_V2_OVER_V1: + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + msp_ltm_port->checksum1 ^= new_byte; + msp_ltm_port->checksum2 = + crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); + if(msp_ltm_port->offset == + (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) { + mspHeaderV2_t *hdrv2 = + (mspHeaderV2_t *)&msp_ltm_port->inBuf[sizeof(mspHeaderV1_t)]; + msp_ltm_port->dataSize = hdrv2->size; + if(hdrv2->size > MSP_PORT_INBUF_SIZE) { + msp_ltm_port->parse_state = IDLE; + return false; + } + else { + msp_ltm_port->cmdMSP = hdrv2->cmd; + msp_ltm_port->cmdFlags = hdrv2->flags; + msp_ltm_port->offset = 0; + msp_ltm_port->parse_state = msp_ltm_port->dataSize > 0 + ? MSP_PAYLOAD_V2_OVER_V1 + : MSP_CHECKSUM_V2_OVER_V1; + } + } + break; - case MSP_PAYLOAD_V2_OVER_V1: - msp_ltm_port->checksum2 = crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); - msp_ltm_port->checksum1 ^= new_byte; - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + case MSP_PAYLOAD_V2_OVER_V1: + msp_ltm_port->checksum2 = + crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); + msp_ltm_port->checksum1 ^= new_byte; + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - if (msp_ltm_port->offset == msp_ltm_port->dataSize) { - msp_ltm_port->parse_state = MSP_CHECKSUM_V2_OVER_V1; - } - break; + if(msp_ltm_port->offset == msp_ltm_port->dataSize) { + msp_ltm_port->parse_state = MSP_CHECKSUM_V2_OVER_V1; + } + break; - case MSP_CHECKSUM_V2_OVER_V1: - msp_ltm_port->checksum1 ^= new_byte; - if (msp_ltm_port->checksum2 == new_byte) { - msp_ltm_port->parse_state = MSP_CHECKSUM_V1; - } else { - msp_ltm_port->parse_state = IDLE; - return false; - } - break; + case MSP_CHECKSUM_V2_OVER_V1: + msp_ltm_port->checksum1 ^= new_byte; + if(msp_ltm_port->checksum2 == new_byte) { + msp_ltm_port->parse_state = MSP_CHECKSUM_V1; + } + else { + msp_ltm_port->parse_state = IDLE; + return false; + } + break; - case MSP_HEADER_V2_NATIVE: - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - msp_ltm_port->checksum2 = crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); - if (msp_ltm_port->offset == sizeof(mspHeaderV2_t)) { - mspHeaderV2_t *hdrv2 = (mspHeaderV2_t *) &msp_ltm_port->inBuf[0]; - if (hdrv2->size > MSP_PORT_INBUF_SIZE) { - msp_ltm_port->parse_state = IDLE; - } else { - msp_ltm_port->dataSize = hdrv2->size; - msp_ltm_port->cmdMSP = hdrv2->cmd; - msp_ltm_port->cmdFlags = hdrv2->flags; - msp_ltm_port->offset = 0; - msp_ltm_port->parse_state = - msp_ltm_port->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE; - } - } - break; + case MSP_HEADER_V2_NATIVE: + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + msp_ltm_port->checksum2 = + crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); + if(msp_ltm_port->offset == sizeof(mspHeaderV2_t)) { + mspHeaderV2_t *hdrv2 = (mspHeaderV2_t *)&msp_ltm_port->inBuf[0]; + if(hdrv2->size > MSP_PORT_INBUF_SIZE) { + msp_ltm_port->parse_state = IDLE; + } + else { + msp_ltm_port->dataSize = hdrv2->size; + msp_ltm_port->cmdMSP = hdrv2->cmd; + msp_ltm_port->cmdFlags = hdrv2->flags; + msp_ltm_port->offset = 0; + msp_ltm_port->parse_state = msp_ltm_port->dataSize > 0 + ? MSP_PAYLOAD_V2_NATIVE + : MSP_CHECKSUM_V2_NATIVE; + } + } + break; - case MSP_PAYLOAD_V2_NATIVE: - msp_ltm_port->checksum2 = crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); - msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; + case MSP_PAYLOAD_V2_NATIVE: + msp_ltm_port->checksum2 = + crc8_dvb_s2_table(msp_ltm_port->checksum2, new_byte); + msp_ltm_port->inBuf[msp_ltm_port->offset++] = new_byte; - if (msp_ltm_port->offset == msp_ltm_port->dataSize) { - msp_ltm_port->parse_state = MSP_CHECKSUM_V2_NATIVE; - } - break; + if(msp_ltm_port->offset == msp_ltm_port->dataSize) { + msp_ltm_port->parse_state = MSP_CHECKSUM_V2_NATIVE; + } + break; - case MSP_CHECKSUM_V2_NATIVE: - if (msp_ltm_port->checksum2 == new_byte) { - msp_ltm_port->parse_state = MSP_PACKET_RECEIVED; - } else { - msp_ltm_port->parse_state = IDLE; - return false; - } - break; + case MSP_CHECKSUM_V2_NATIVE: + if(msp_ltm_port->checksum2 == new_byte) { + msp_ltm_port->parse_state = MSP_PACKET_RECEIVED; + } + else { + msp_ltm_port->parse_state = IDLE; + return false; } - return true; + break; + } + return true; } diff --git a/main/msp_ltm_serial.h b/main/msp_ltm_serial.h index 3fcb5b1..7a3dd01 100644 --- a/main/msp_ltm_serial.h +++ b/main/msp_ltm_serial.h @@ -22,11 +22,11 @@ #include #define MSP_MAX_HEADER_SIZE 9 -#define MSP_V2_FRAME_ID 255 -#define MSP_PORT_INBUF_SIZE 512 -#define MAX_MSP_PORT_COUNT 3 +#define MSP_V2_FRAME_ID 255 +#define MSP_PORT_INBUF_SIZE 512 +#define MAX_MSP_PORT_COUNT 3 #define MSP_PORT_DATAFLASH_BUFFER_SIZE 4096 -#define MSP_PORT_OUTBUF_SIZE 512 +#define MSP_PORT_OUTBUF_SIZE 512 #define MSP_VERSION_MAGIC_INITIALIZER { 'M', 'M', 'X' } #define LTM_TYPE_A_PAYLOAD_SIZE 6 @@ -35,106 +35,116 @@ #define LTM_MAX_FRAME_SIZE 18 #define LTM_ARMED_BIT_MASK 0x01 // 0b00000001 -typedef enum { - LTM_TYPE_G, - LTM_TYPE_A, - LTM_TYPE_S, - LTM_TYPE_O, - LTM_TYPE_N, - LTM_TYPE_X, +typedef enum +{ + LTM_TYPE_G, + LTM_TYPE_A, + LTM_TYPE_S, + LTM_TYPE_O, + LTM_TYPE_N, + LTM_TYPE_X, } ltm_type_e; +typedef enum +{ + IDLE, + HEADER_START, -typedef enum { - IDLE, - HEADER_START, + MSP_HEADER_M, + MSP_HEADER_X, + LTM_HEADER, + LTM_TYPE_IDENT, - MSP_HEADER_M, - MSP_HEADER_X, - LTM_HEADER, - LTM_TYPE_IDENT, + MSP_HEADER_V1, + MSP_PAYLOAD_V1, + MSP_CHECKSUM_V1, - MSP_HEADER_V1, - MSP_PAYLOAD_V1, - MSP_CHECKSUM_V1, + MSP_HEADER_V2_OVER_V1, + MSP_PAYLOAD_V2_OVER_V1, + MSP_CHECKSUM_V2_OVER_V1, - MSP_HEADER_V2_OVER_V1, - MSP_PAYLOAD_V2_OVER_V1, - MSP_CHECKSUM_V2_OVER_V1, + MSP_HEADER_V2_NATIVE, + MSP_PAYLOAD_V2_NATIVE, + MSP_CHECKSUM_V2_NATIVE, - MSP_HEADER_V2_NATIVE, - MSP_PAYLOAD_V2_NATIVE, - MSP_CHECKSUM_V2_NATIVE, + MSP_PACKET_RECEIVED, - MSP_PACKET_RECEIVED, - - LTM_DATA, - LTM_CRC, - LTM_PACKET_RECEIVED + LTM_DATA, + LTM_CRC, + LTM_PACKET_RECEIVED } msp_ltm_parse_state_e; -typedef struct __attribute__((packed)) { - uint8_t size; - uint8_t cmd; +typedef struct __attribute__((packed)) +{ + uint8_t size; + uint8_t cmd; } mspHeaderV1_t; -typedef struct __attribute__((packed)) { - uint16_t size; +typedef struct __attribute__((packed)) +{ + uint16_t size; } mspHeaderJUMBO_t; -typedef struct __attribute__((packed)) { - uint8_t flags; - uint16_t cmd; - uint16_t size; +typedef struct __attribute__((packed)) +{ + uint8_t flags; + uint16_t cmd; + uint16_t size; } mspHeaderV2_t; -typedef enum { - MSP_V1 = 0, - MSP_V2_OVER_V1 = 1, - MSP_V2_NATIVE = 2, - MSP_VERSION_COUNT +typedef enum +{ + MSP_V1 = 0, + MSP_V2_OVER_V1 = 1, + MSP_V2_NATIVE = 2, + MSP_VERSION_COUNT } mspVersion_e; struct serialPort_s; -typedef struct mspPort_s { - struct serialPort_s *port; // null when port unused. - msp_ltm_parse_state_e parse_state; - uint8_t inBuf[MSP_PORT_INBUF_SIZE]; - uint_fast16_t offset; - uint_fast16_t dataSize; - ltm_type_e ltm_type; - uint8_t ltm_payload_cnt; - uint8_t ltm_frame_buffer[LTM_MAX_FRAME_SIZE]; - mspVersion_e mspVersion; - uint8_t cmdFlags; - uint16_t cmdMSP; - uint8_t checksum1; - uint8_t checksum2; +typedef struct mspPort_s +{ + struct serialPort_s *port; // null when port unused. + msp_ltm_parse_state_e parse_state; + uint8_t inBuf[MSP_PORT_INBUF_SIZE]; + uint_fast16_t offset; + uint_fast16_t dataSize; + ltm_type_e ltm_type; + uint8_t ltm_payload_cnt; + uint8_t ltm_frame_buffer[LTM_MAX_FRAME_SIZE]; + mspVersion_e mspVersion; + uint8_t cmdFlags; + uint16_t cmdMSP; + uint8_t checksum1; + uint8_t checksum2; } msp_ltm_port_t; // return positive for ACK, negative on error, zero for no reply -typedef enum { - MSP_RESULT_ACK = 1, - MSP_RESULT_ERROR = -1, - MSP_RESULT_NO_REPLY = 0 +typedef enum +{ + MSP_RESULT_ACK = 1, + MSP_RESULT_ERROR = -1, + MSP_RESULT_NO_REPLY = 0 } mspResult_e; // simple buffer-based serializer/deserializer without implicit size check // little-endian encoding implemneted now -typedef struct sbuf_s { - uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) - uint8_t *end; +typedef struct sbuf_s +{ + uint8_t + *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *end; } sbuf_t; -typedef struct mspPacket_s { - sbuf_t buf; - int16_t cmd; - uint8_t flags; - int16_t result; +typedef struct mspPacket_s +{ + sbuf_t buf; + int16_t cmd; + uint8_t flags; + int16_t result; } mspPacket_t; -//static msp_ltm_port_t mspPorts[MAX_MSP_PORT_COUNT]; +// static msp_ltm_port_t mspPorts[MAX_MSP_PORT_COUNT]; bool parse_msp_ltm_byte(msp_ltm_port_t *msp_ltm_port, uint8_t new_byte); -#endif //CONTROL_STATUS_MSP_SERIAL_H +#endif // CONTROL_STATUS_MSP_SERIAL_H diff --git a/main/tcp_server.c b/main/tcp_server.c index 37deebe..450c8cc 100644 --- a/main/tcp_server.c +++ b/main/tcp_server.c @@ -24,47 +24,53 @@ #define TCP_TAG "TCP_SERVER_SETUP" -int open_tcp_server(int port) { - char addr_str[128]; - int addr_family; - int ip_protocol; +int +open_tcp_server(int port) +{ + char addr_str[128]; + int addr_family; + int ip_protocol; - struct sockaddr_in dest_addr; - dest_addr.sin_addr.s_addr = htonl(INADDR_ANY); - dest_addr.sin_family = AF_INET; - dest_addr.sin_port = htons(port); - addr_family = AF_INET; - ip_protocol = IPPROTO_IP; - inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1); + struct sockaddr_in dest_addr; + dest_addr.sin_addr.s_addr = htonl(INADDR_ANY); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(port); + addr_family = AF_INET; + ip_protocol = IPPROTO_IP; + inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1); - int listen_sock = socket(addr_family, SOCK_STREAM, ip_protocol); - if (listen_sock < 0) { - ESP_LOGE(TCP_TAG, "Unable to create socket: %s", esp_err_to_name(errno)); - return ESP_FAIL; - } - int err = bind(listen_sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr)); - if (err != 0) { - ESP_LOGE(TCP_TAG, "Socket unable to bind: %s", esp_err_to_name(errno)); - return ESP_FAIL; - } - err = listen(listen_sock, 1); - if (err != 0) { - ESP_LOGE(TCP_TAG, "Error occurred during listen: %s", esp_err_to_name(errno)); - return ESP_FAIL; - } - ESP_LOGI(TCP_TAG, "Opened TCP server on port %d", port); - return listen_sock; + int listen_sock = socket(addr_family, SOCK_STREAM, ip_protocol); + if(listen_sock < 0) { + ESP_LOGE(TCP_TAG, "Unable to create socket: %s", esp_err_to_name(errno)); + return ESP_FAIL; + } + int err = + bind(listen_sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr)); + if(err != 0) { + ESP_LOGE(TCP_TAG, "Socket unable to bind: %s", esp_err_to_name(errno)); + return ESP_FAIL; + } + err = listen(listen_sock, 1); + if(err != 0) { + ESP_LOGE( + TCP_TAG, "Error occurred during listen: %s", esp_err_to_name(errno)); + return ESP_FAIL; + } + ESP_LOGI(TCP_TAG, "Opened TCP server on port %d", port); + return listen_sock; } -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); - int err = write(tcp_clients[i], data, data_length); - if (err < 0) { - ESP_LOGE(TCP_TAG, "Error occurred during sending: %d", errno); - } - } +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); + int err = write(tcp_clients[i], data, data_length); + if(err < 0) { + ESP_LOGE(TCP_TAG, "Error occurred during sending: %d", errno); + } } - + } } \ No newline at end of file diff --git a/main/tcp_server.h b/main/tcp_server.h index 7d9e834..207643a 100644 --- a/main/tcp_server.h +++ b/main/tcp_server.h @@ -23,6 +23,7 @@ #define TCP_BUFF_SIZ 4096 int open_tcp_server(int port); -void db_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 +#endif // DB_ESP32_TCP_SERVER_H