Skip to content

Commit

Permalink
Made serial read timeout configurable
Browse files Browse the repository at this point in the history
Via MAVLink interface and web interface
  • Loading branch information
seeul8er committed Oct 20, 2024
1 parent 366f4e4 commit a16c8a5
Show file tree
Hide file tree
Showing 9 changed files with 41 additions and 13 deletions.
4 changes: 2 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ A firmware for the popular ESP32 modules from Espressif Systems. Probably the ch
communicate with your drone, UAV, UAS, ground-based vehicle or whatever you may call them.

It also allows for a fully transparent serial to WiFi pass-through link with variable packet size
(Continuous stream of data required).
(As of release v2.0RC4 no continuous stream of data required anymore in MAVLink and transparent mode).

DroneBridge for ESP32 is a telemetry/low data rate-only solution. There is no support for cameras connected to the ESP32
since it does not support video encoding.
Expand All @@ -34,7 +34,7 @@ since it does not support video encoding.
- Fully configurable through an easy-to-use web interface
- Parsing of LTM & MSPv2 for more reliable connection and less packet loss
- Parsing of MAVLink with the injection of Radio Status packets for the display of RSSI in the GCS
- Fully transparent telemetry down-link option for continuous streams
- Fully transparent telemetry down-link option
- Reliable, low latency

<div align="center">
Expand Down
6 changes: 5 additions & 1 deletion frontend/index.html
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ <h3>Serial</h3>
</div>
</div>
<div id="trans_pack_size_div" class="row">
<div class="twelve columns">
<div class="six columns">
<label for="trans_pack_size">Maximum packet size</label>
<select id="trans_pack_size" name="trans_pack_size" form="settings_form">
<option value="16">16</option>
Expand All @@ -177,6 +177,10 @@ <h3>Serial</h3>
<option value="768">768</option>
</select>
</div>
<div class="six columns">
<label for="serial_timeout">Serial read timeout [ms]</label>
<input type="number" id="serial_timeout" name="serial_timeout" min="1" max="65535" value="50">
</div>
</div>
<div class="row">
<div class="six columns" id="ap_ip_div">
Expand Down
2 changes: 1 addition & 1 deletion main/db_esp32_control.c
Original file line number Diff line number Diff line change
Expand Up @@ -449,7 +449,7 @@ _Noreturn void control_module_esp_now(){
if (db_uart_write_queue != NULL && xQueueReceive(db_uart_write_queue, &db_espnow_uart_evt, 0) == pdTRUE) {
if (DB_SERIAL_PROTOCOL == 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
// 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
Expand Down
16 changes: 15 additions & 1 deletion main/db_mavlink_msgs.c
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@

#define TAG "DB_MAV_MSGS"

uint16_t DB_MAV_PARAM_CNT = 11;
uint16_t DB_MAV_PARAM_CNT = 12; // Number of MAVLink parameters returned by ESP32 in the PARAM message. Needed by GCS.

/**
* Based on the system architecture and configured wifi mode the ESP32 may have a different role and system id.
Expand Down Expand Up @@ -129,6 +129,9 @@ MAV_TYPE db_mav_get_parameter_value(float_int_union *float_int, char *param_id,
} else if (strncmp(param_id, "SERIAL_RTS_THRES", 16) == 0 || param_index == 10) {
float_int->uint8 = DB_UART_RTS_THRESH;
type = MAV_PARAM_TYPE_UINT8;
} else if (strncmp(param_id, "SERIAL_T_OUT_MS", 16) == 0 || param_index == 11) {
float_int->uint16 = DB_SERIAL_READ_TIMEOUT_MS;
type = MAV_PARAM_TYPE_UINT16;
} else {
type = 0;
}
Expand Down Expand Up @@ -162,6 +165,13 @@ bool db_write_mavlink_parameter(fmav_param_set_t *param_set_payload) {
} else {
ESP_LOGE(TAG, "SERIAL_PACK_SIZE must be <1024 bytes");
}
} else if (strncmp(param_set_payload->param_id, "SERIAL_T_OUT_MS", 16) == 0) {
if (float_int.uint16 > 0) {
DB_SERIAL_READ_TIMEOUT_MS = float_int.uint16;
success = true;
} else {
ESP_LOGE(TAG, "SERIAL_T_OUT_MS must be >0 MS");
}
} else if (strncmp(param_set_payload->param_id, "SERIAL_BAUD", 16) == 0) {
DB_UART_BAUD_RATE = float_int.int32;
success = true;
Expand Down Expand Up @@ -419,6 +429,10 @@ void handle_mavlink_message(fmav_message_t *new_msg, int *tcp_clients, udp_conn_
float_int.uint8 = DB_UART_RTS_THRESH;
len = db_get_mavmsg_param(buff, fmav_status, 10, &float_int, MAV_PARAM_TYPE_UINT8, "SERIAL_RTS_THRES");
db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns);

float_int.uint16 = DB_SERIAL_READ_TIMEOUT_MS;
len = db_get_mavmsg_param(buff, fmav_status, 11, &float_int, MAV_PARAM_TYPE_UINT16, "SERIAL_T_OUT_MS");
db_route_mavlink_response(buff, len, origin, tcp_clients, udp_conns);
}
break;
case FASTMAVLINK_MSG_ID_PARAM_REQUEST_READ: {
Expand Down
5 changes: 3 additions & 2 deletions main/db_serial.c
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,9 @@
#define TAG "DB_SERIAL"

uint8_t DB_MAV_SYS_ID = 1;

uint32_t serial_total_byte_count = 0;
uint16_t DB_SERIAL_READ_TIMEOUT_MS = DB_SERIAL_READ_TIMEOUT_MS_DEFAULT;

uint8_t ltm_frame_buffer[MAX_LTM_FRAMES_IN_BUFFER * LTM_MAX_FRAME_SIZE];
uint ltm_frames_in_buffer = 0;
uint ltm_frames_in_buffer_pnt = 0;
Expand Down Expand Up @@ -404,7 +405,7 @@ void db_read_serial_parse_transparent(int tcp_clients[], udp_conn_list_t *udp_co
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
/* 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;
Expand Down
2 changes: 1 addition & 1 deletion main/db_serial.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#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 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 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,
Expand Down
4 changes: 3 additions & 1 deletion main/globals.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@
#include "db_esp32_control.h"

#define MAX_LTM_FRAMES_IN_BUFFER 5
#define DB_BUILD_VERSION 12

#define DB_BUILD_VERSION 13
#define DB_MAJOR_VERSION 2
#define DB_MINOR_VERSION 0

Expand All @@ -45,6 +46,7 @@ extern uint8_t DB_UART_PIN_CTS;
extern uint8_t DB_UART_RTS_THRESH;
extern int32_t DB_UART_BAUD_RATE;
extern uint16_t DB_TRANS_BUF_SIZE; // Maximum packet size via ESP-NOW or WiFi in transparent or mavlink mode
extern uint16_t DB_SERIAL_READ_TIMEOUT_MS; // 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.
extern uint8_t DB_LTM_FRAME_NUM_BUFFER; // Number of LTM frames per UDP packet (min = 1; max = 5)
extern char DB_STATIC_STA_IP[IP4ADDR_STRLEN_MAX]; // user can specify static IP when in Wi-Fi client mode. If this is empty use auto IP
extern char DB_STATIC_STA_IP_GW[IP4ADDR_STRLEN_MAX];// if DB_STATIC_STA_IP is set then this must be set to the GW IP
Expand Down
3 changes: 3 additions & 0 deletions main/http_server.c
Original file line number Diff line number Diff line change
Expand Up @@ -184,6 +184,8 @@ static esp_err_t settings_post_handler(httpd_req_t *req) {

json = cJSON_GetObjectItem(root, "trans_pack_size");
if (json) DB_TRANS_BUF_SIZE = json->valueint;
json = cJSON_GetObjectItem(root, "serial_timeout");
if (json) DB_SERIAL_READ_TIMEOUT_MS = json->valueint;
json = cJSON_GetObjectItem(root, "tx_pin");
if (json) DB_UART_PIN_TX = json->valueint;
json = cJSON_GetObjectItem(root, "rx_pin");
Expand Down Expand Up @@ -540,6 +542,7 @@ static esp_err_t settings_get_handler(httpd_req_t *req) {
cJSON_AddStringToObject(root, "wifi_pass", (char *) DB_WIFI_PWD);
cJSON_AddNumberToObject(root, "ap_channel", DB_WIFI_CHANNEL);
cJSON_AddNumberToObject(root, "trans_pack_size", DB_TRANS_BUF_SIZE);
cJSON_AddNumberToObject(root, "serial_timeout", DB_SERIAL_READ_TIMEOUT_MS);
cJSON_AddNumberToObject(root, "tx_pin", DB_UART_PIN_TX);
cJSON_AddNumberToObject(root, "rx_pin", DB_UART_PIN_RX);
cJSON_AddNumberToObject(root, "cts_pin", DB_UART_PIN_CTS);
Expand Down
12 changes: 8 additions & 4 deletions main/main.c
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include "db_esp_now.h"
#include "iot_button.h"
#include "db_serial.h"
#include "globals.h"

#define NVS_NAMESPACE "settings"

Expand Down Expand Up @@ -457,6 +458,7 @@ void db_write_settings_to_nvs() {
ESP_ERROR_CHECK(nvs_set_u8(my_handle, "rts_thresh", DB_UART_RTS_THRESH));
ESP_ERROR_CHECK(nvs_set_u8(my_handle, "proto", DB_SERIAL_PROTOCOL));
ESP_ERROR_CHECK(nvs_set_u16(my_handle, "trans_pack_size", DB_TRANS_BUF_SIZE));
ESP_ERROR_CHECK(nvs_set_u16(my_handle, "serial_timeout", DB_SERIAL_READ_TIMEOUT_MS));
ESP_ERROR_CHECK(nvs_set_u8(my_handle, "ltm_per_packet", DB_LTM_FRAME_NUM_BUFFER));
ESP_ERROR_CHECK(nvs_set_str(my_handle, "ap_ip", DEFAULT_AP_IP));
ESP_ERROR_CHECK(nvs_set_str(my_handle, "ip_sta", DB_STATIC_STA_IP));
Expand Down Expand Up @@ -550,6 +552,7 @@ void db_read_settings_nvs() {
ESP_ERROR_CHECK_WITHOUT_ABORT(nvs_get_u8(my_handle, "rts_thresh", &DB_UART_RTS_THRESH));
ESP_ERROR_CHECK_WITHOUT_ABORT(nvs_get_u8(my_handle, "proto", &DB_SERIAL_PROTOCOL));
ESP_ERROR_CHECK_WITHOUT_ABORT(nvs_get_u16(my_handle, "trans_pack_size", &DB_TRANS_BUF_SIZE));
ESP_ERROR_CHECK_WITHOUT_ABORT(nvs_get_u16(my_handle, "serial_timeout", &DB_SERIAL_READ_TIMEOUT_MS));
ESP_ERROR_CHECK_WITHOUT_ABORT(nvs_get_u8(my_handle, "ltm_per_packet", &DB_LTM_FRAME_NUM_BUFFER));
// get saved UDP client
char udp_client_ip_str[INET_ADDRSTRLEN + 6];
Expand All @@ -562,10 +565,10 @@ void db_read_settings_nvs() {
nvs_close(my_handle);
ESP_LOGI(TAG,
"\tWifi Mode: %i\n\twifi_chan %i\n\tbaud %liu\n\tgpio_tx %i\n\tgpio_rx %i\n\tgpio_cts %i\n\t"
"gpio_rts %i\n\trts_thresh %i\n\tproto %i\n\ttrans_pack_size %i\n\tltm_per_packet %i",
"gpio_rts %i\n\trts_thresh %i\n\tproto %i\n\ttrans_pack_size %i\n\tltm_per_packet %i\n\tserial_timeout %i",
DB_WIFI_MODE, DB_WIFI_CHANNEL, DB_UART_BAUD_RATE, DB_UART_PIN_TX, DB_UART_PIN_RX,
DB_UART_PIN_CTS, DB_UART_PIN_RTS, DB_UART_RTS_THRESH, DB_SERIAL_PROTOCOL, DB_TRANS_BUF_SIZE,
DB_LTM_FRAME_NUM_BUFFER);
DB_LTM_FRAME_NUM_BUFFER, DB_SERIAL_READ_TIMEOUT_MS);
if (strlen(udp_client_ip_str) > 0 && udp_client_port != 0) {
// there was a saved UDP client in the NVM - add it to the udp clients list
ESP_LOGI(TAG, "Adding %s:%i to known UDP clients.", udp_client_ip_str, udp_client_port);
Expand Down Expand Up @@ -622,6 +625,7 @@ void long_press_callback(void *arg,void *usr_data) {
DB_SERIAL_PROTOCOL = DB_SERIAL_PROTOCOL_MAVLINK;
DB_TRANS_BUF_SIZE = 64;
DB_UART_RTS_THRESH = 64;
DB_SERIAL_READ_TIMEOUT_MS = DB_SERIAL_READ_TIMEOUT_MS_DEFAULT;
db_write_settings_to_nvs();
esp_restart();
}
Expand Down Expand Up @@ -655,10 +659,10 @@ void set_reset_trigger() {
void db_jtag_serial_info_print() {
uint8_t buffer[512];
int len = sprintf((char *) buffer, "\tWifi Mode: %i\n\twifi_chan %i\n\tbaud %liu\n\tgpio_tx %i\n\tgpio_rx %i\n\tgpio_cts %i\n\t"
"gpio_rts %i\n\trts_thresh %i\n\tproto %i\n\ttrans_pack_size %i\n\tltm_per_packet %i\n",
"gpio_rts %i\n\trts_thresh %i\n\tproto %i\n\ttrans_pack_size %i\n\tltm_per_packet %i\n\tserial_timeout %i\n",
DB_WIFI_MODE, DB_WIFI_CHANNEL, DB_UART_BAUD_RATE, DB_UART_PIN_TX, DB_UART_PIN_RX,
DB_UART_PIN_CTS, DB_UART_PIN_RTS, DB_UART_RTS_THRESH, DB_SERIAL_PROTOCOL, DB_TRANS_BUF_SIZE,
DB_LTM_FRAME_NUM_BUFFER);
DB_LTM_FRAME_NUM_BUFFER, DB_SERIAL_READ_TIMEOUT_MS);
write_to_serial(buffer, len);
len = sprintf((char *) buffer, "\tSSID: %s\n", DB_WIFI_SSID);
write_to_serial(buffer, len);
Expand Down

0 comments on commit a16c8a5

Please sign in to comment.