#include <string.h>
#include <sys/param.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_mac.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_netif.h"
#include "protocol_examples_common.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "lwip/dns.h"
#include "lwip/ip_addr.h"
#include "lwip/netdb.h"
#include "board_version.h"
#include "board_priority.h"
#include "board_utils.h"
#include "board_xor_enc.h"
#include "board_hp_check.h"
#include "board_sta_wps.h"
#include "board_ap_wps.h"
#include "board_gpio_out.h"
#include "board_gpio_in.h"
#include "board_nvs.h"
#include "board_led.h"
#include "board_motor.h"
#include "board_i2c.h"
#include "board_adc.h"
#include "board_uart0.h"
#include "board_uart1.h"
#include "board_mavlink.h"
#include "board_queue.h"
#include "board_scan_wifi.h"
#include "board_com_check.h"
#include "board_boot_verify.h"
#include "board_servo_ctl.h"
#include "board_protocol.h"
#define EXAMPLE_ESP_WIFI_SSID_PREFIX "R0X-G28"
#define EXAMPLE_ESP_WIFI_PASS "12345678"
#define EXAMPLE_ESP_WIFI_CHANNEL (1)
#define EXAMPLE_MAX_STA_CONN (2)
#define TCP_SERVER_PORT (19345)
#define KEEPALIVE_IDLE (5)
#define KEEPALIVE_INTERVAL (5)
#define KEEPALIVE_COUNT (3)
#define UDP_SERVER_PORT (19346)
static const char *TAG = "BOT-TAG";
static int global_tcpsock_handle = -1;
static int global_udpsock_handle = -1;
static void do_tcpsock_close(void) {
if(global_tcpsock_handle > 0) {
ESP_LOGE(TAG, "Shutting down tcp socket...");
shutdown(global_tcpsock_handle, 0);
close(global_tcpsock_handle);
global_tcpsock_handle = -1;
}
}
static void do_tcpsock_write(uint8_t*buf,uint8_t len) {
if(global_tcpsock_handle <= 0) return;
int to_write = len;
while (to_write > 0) {
int written = send(global_tcpsock_handle, buf + (len - to_write), to_write, 0);
if (written < 0) {
ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
return;
}
to_write -= written;
}
}
static void do_tcpsock_recv(void) {
int len;
unsigned char rx_buffer[128];
do {
len = recv(global_tcpsock_handle, rx_buffer, sizeof(rx_buffer) - 1, 0);
if (len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
vTaskDelay(100 / portTICK_PERIOD_MS);
} else if (len == 0) {
ESP_LOGW(TAG, "Connection closed");
} else {
cmd_resolve_low_speed(rx_buffer,len);
}
} while (len > 0);
}
static void do_udpsock_close(void) {
if (global_udpsock_handle > 0) {
ESP_LOGE(TAG, "Shutting down udp socket and restarting...");
shutdown(global_udpsock_handle, 0);
close(global_udpsock_handle);
global_udpsock_handle = -1;
}
}
static void tcp_server_task(void *pvParameters) {
char addr_str[128];
int addr_family = (int)pvParameters;
int ip_protocol = 0;
int keepAlive = 1;
int option = 1;
int keepIdle = KEEPALIVE_IDLE;
int keepInterval = KEEPALIVE_INTERVAL;
int keepCount = KEEPALIVE_COUNT;
struct sockaddr_storage dest_addr;
#ifdef CONFIG_EXAMPLE_IPV4
if (addr_family == AF_INET) {
struct sockaddr_in *dest_addr_ip4 = (struct sockaddr_in *)&dest_addr;
dest_addr_ip4->sin_addr.s_addr = htonl(INADDR_ANY);
dest_addr_ip4->sin_family = AF_INET;
dest_addr_ip4->sin_port = htons(TCP_SERVER_PORT);
ip_protocol = IPPROTO_IP;
}
#endif
#ifdef CONFIG_EXAMPLE_IPV6
if (addr_family == AF_INET6) {
struct sockaddr_in6 *dest_addr_ip6 = (struct sockaddr_in6 *)&dest_addr;
bzero(&dest_addr_ip6->sin6_addr.un, sizeof(dest_addr_ip6->sin6_addr.un));
dest_addr_ip6->sin6_family = AF_INET6;
dest_addr_ip6->sin6_port = htons(PORT);
ip_protocol = IPPROTO_IPV6;
}
#endif
int listen_sock = socket(addr_family, SOCK_STREAM, ip_protocol);
if (listen_sock < 0) {
ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
vTaskDelete(NULL);
return;
}
int opt = 1;
setsockopt(listen_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
#if defined(CONFIG_EXAMPLE_IPV4) && defined(CONFIG_EXAMPLE_IPV6)
setsockopt(listen_sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, sizeof(opt));
#endif
ESP_LOGI(TAG, "tcp Socket created");
int err = bind(listen_sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
if (err != 0) {
ESP_LOGE(TAG, "tcp Socket unable to bind: errno %d", errno);
ESP_LOGE(TAG, "IPPROTO: %d", addr_family);
goto CLEAN_UP;
}
ESP_LOGI(TAG, "tcp Socket bound, port %d", TCP_SERVER_PORT);
err = listen(listen_sock, 1);
if (err != 0) {
ESP_LOGE(TAG, "Error occurred during listen: errno %d", errno);
goto CLEAN_UP;
}
while (1) {
ESP_LOGI(TAG, "Socket listening");
generate_com_check();
hp_check_load();
struct sockaddr_storage source_addr;
socklen_t addr_len = sizeof(source_addr);
global_tcpsock_handle = accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len);
if (global_tcpsock_handle < 0) {
ESP_LOGE(TAG, "Unable to accept connection: errno %d", errno);
break;
}
setsockopt(global_tcpsock_handle, SOL_SOCKET, SO_KEEPALIVE, &keepAlive, sizeof(int));
setsockopt(global_tcpsock_handle, IPPROTO_TCP, TCP_KEEPIDLE, &keepIdle, sizeof(int));
setsockopt(global_tcpsock_handle, IPPROTO_TCP, TCP_KEEPINTVL, &keepInterval, sizeof(int));
setsockopt(global_tcpsock_handle, IPPROTO_TCP, TCP_KEEPCNT, &keepCount, sizeof(int));
setsockopt(global_tcpsock_handle, IPPROTO_TCP, TCP_NODELAY, &option, sizeof(int));
#ifdef CONFIG_EXAMPLE_IPV4
if (source_addr.ss_family == PF_INET) {
inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr, addr_str, sizeof(addr_str) - 1);
}
#endif
#ifdef CONFIG_EXAMPLE_IPV6
if (source_addr.ss_family == PF_INET6) {
inet6_ntoa_r(((struct sockaddr_in6 *)&source_addr)->sin6_addr, addr_str, sizeof(addr_str) - 1);
}
#endif
ESP_LOGI(TAG, "Socket accepted ip address: %s", addr_str);
do_tcpsock_recv();
do_tcpsock_close();
do_udpsock_close();
}
CLEAN_UP:
close(listen_sock);
vTaskDelete(NULL);
}
static void udp_server_task(void *pvParameters) {
unsigned char rx_buffer[128];
char addr_str[128];
int addr_family = (int)pvParameters;
int ip_protocol = 0;
struct sockaddr_in6 dest_addr;
while (1) {
if (addr_family == AF_INET) {
struct sockaddr_in *dest_addr_ip4 = (struct sockaddr_in *)&dest_addr;
dest_addr_ip4->sin_addr.s_addr = htonl(INADDR_ANY);
dest_addr_ip4->sin_family = AF_INET;
dest_addr_ip4->sin_port = htons(UDP_SERVER_PORT);
ip_protocol = IPPROTO_IP;
} else if (addr_family == AF_INET6) {
bzero(&dest_addr.sin6_addr.un, sizeof(dest_addr.sin6_addr.un));
dest_addr.sin6_family = AF_INET6;
dest_addr.sin6_port = htons(UDP_SERVER_PORT);
ip_protocol = IPPROTO_IPV6;
}
global_udpsock_handle = socket(addr_family, SOCK_DGRAM, ip_protocol);
if (global_udpsock_handle < 0) {
ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
vTaskDelay(100 / portTICK_PERIOD_MS);
continue;
}
ESP_LOGI(TAG, "UDP Socket created");
#if defined(CONFIG_LWIP_NETBUF_RECVINFO) && !defined(CONFIG_EXAMPLE_IPV6)
int enable = 1;
lwip_setsockopt(sock, IPPROTO_IP, IP_PKTINFO, &enable, sizeof(enable));
#endif
#if defined(CONFIG_EXAMPLE_IPV4) && defined(CONFIG_EXAMPLE_IPV6)
if (addr_family == AF_INET6) {
int opt = 1;
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &opt, sizeof(opt));
}
#endif
int opt = 1;
setsockopt(global_udpsock_handle, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
int err = bind(global_udpsock_handle, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
if (err < 0) {
ESP_LOGE(TAG, "udp Socket unable to bind: errno %d", errno);
close(global_udpsock_handle);
vTaskDelay(100 / portTICK_PERIOD_MS);
continue;
}
ESP_LOGI(TAG, "udp Socket bound, port %d",UDP_SERVER_PORT);
struct sockaddr_storage source_addr;
socklen_t socklen = sizeof(source_addr);
#if defined(CONFIG_LWIP_NETBUF_RECVINFO) && !defined(CONFIG_EXAMPLE_IPV6)
struct iovec iov;
struct msghdr msg;
struct cmsghdr *cmsgtmp;
u8_t cmsg_buf[CMSG_SPACE(sizeof(struct in_pktinfo))];
iov.iov_base = rx_buffer;
iov.iov_len = sizeof(rx_buffer);
msg.msg_control = cmsg_buf;
msg.msg_controllen = sizeof(cmsg_buf);
msg.msg_flags = 0;
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_name = (struct sockaddr *)&source_addr;
msg.msg_namelen = socklen;
#endif
ESP_LOGI(TAG, "udp start Waiting for data");
while (1) {
if(global_tcpsock_handle <= 0) {
vTaskDelay(100 / portTICK_PERIOD_MS);
continue;
}
#if defined(CONFIG_LWIP_NETBUF_RECVINFO) && !defined(CONFIG_EXAMPLE_IPV6)
int len = recvmsg(global_udpsock_handle, &msg, 0);
#else
int len = recvfrom(global_udpsock_handle, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&source_addr, &socklen);
#endif
if (len < 0) {
ESP_LOGE(TAG, "udp recvfrom failed: errno %d", errno);
vTaskDelay(100 / portTICK_PERIOD_MS);
break;
}
else {
if (source_addr.ss_family == PF_INET) {
inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr, addr_str, sizeof(addr_str) - 1);
#if defined(CONFIG_LWIP_NETBUF_RECVINFO) && !defined(CONFIG_EXAMPLE_IPV6)
for ( cmsgtmp = CMSG_FIRSTHDR(&msg); cmsgtmp != NULL; cmsgtmp = CMSG_NXTHDR(&msg, cmsgtmp) ) {
if ( cmsgtmp->cmsg_level == IPPROTO_IP && cmsgtmp->cmsg_type == IP_PKTINFO ) {
struct in_pktinfo *pktinfo;
pktinfo = (struct in_pktinfo*)CMSG_DATA(&msg);
ESP_LOGI(TAG, "dest ip: %s\n", inet_ntoa(pktinfo->ipi_addr));
}
}
#endif
} else if (source_addr.ss_family == PF_INET6) {
inet6_ntoa_r(((struct sockaddr_in6 *)&source_addr)->sin6_addr, addr_str, sizeof(addr_str) - 1);
}
cmd_resolve_high_speed(rx_buffer, len);
}
}
}
vTaskDelete(NULL);
}
static void misc_task_demonstrate(void* arg) {
uint8_t reusebuf[128];
uint8_t reusebuflen = 0;
for(;;) {
if(board_queue_recv(reusebuf, &reusebuflen)) {
do_tcpsock_write(reusebuf, reusebuflen);
}
if(global_tcpsock_handle > 0) {
if(hp_check_timeout()) {
do_tcpsock_close();
}
}
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
static void wifi_event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data) {
switch (event_id) {
case WIFI_EVENT_AP_START:
ESP_LOGI(TAG, "WIFI_EVENT_AP_START");
break;
case WIFI_EVENT_AP_STADISCONNECTED: {
ESP_LOGI(TAG, "WIFI_EVENT_AP_STADISCONNECTED");
wifi_event_ap_stadisconnected_t* event = (wifi_event_ap_stadisconnected_t*) event_data;
ESP_LOGI(TAG, "station "MACSTR" leave, AID=%d", MAC2STR(event->mac), event->aid);
}
break;
case WIFI_EVENT_AP_STACONNECTED: {
ESP_LOGI(TAG, "WIFI_EVENT_AP_STACONNECTED");
wifi_event_ap_staconnected_t* event = (wifi_event_ap_staconnected_t*) event_data;
ESP_LOGI(TAG, "station "MACSTR" join, AID=%d", MAC2STR(event->mac), event->aid);
}
break;
case WIFI_EVENT_AP_WPS_RG_SUCCESS: {
ESP_LOGI(TAG, "WIFI_EVENT_AP_WPS_RG_SUCCESS");
wifi_event_ap_wps_rg_success_t *evt = (wifi_event_ap_wps_rg_success_t *)event_data;
ESP_LOGI(TAG, "station "MACSTR" WPS successful", MAC2STR(evt->peer_macaddr));
ap_wps_set_rg_status(true);
}
break;
case WIFI_EVENT_AP_WPS_RG_FAILED: {
ESP_LOGI(TAG, "WIFI_EVENT_AP_WPS_RG_FAILED");
wifi_event_ap_wps_fail_reason_t *evt = (wifi_event_ap_wps_fail_reason_t *)event_data;
ESP_LOGI(TAG, "station "MACSTR" WPS failed, reason=%d", MAC2STR(evt->peer_macaddr), evt->reason);
ap_wps_restart();
}
break;
case WIFI_EVENT_AP_WPS_RG_TIMEOUT: {
ESP_LOGI(TAG, "WIFI_EVENT_AP_WPS_RG_TIMEOUT");
ap_wps_restart();
}
break;
default:
break;
}
}
static void wifi_init_softap(void) {
esp_netif_t*mynetif = esp_netif_create_default_wifi_ap();
esp_netif_ip_info_t ipInfo;
IP4_ADDR(&ipInfo.ip, 192,168,28,1);
IP4_ADDR(&ipInfo.gw, 192,168,28,1);
IP4_ADDR(&ipInfo.netmask, 255,255,255,0);
esp_netif_dhcps_stop(mynetif);
esp_netif_set_ip_info(mynetif, &ipInfo);
esp_netif_dhcps_start(mynetif);
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));
uint8_t local_mac[6];
esp_read_mac(local_mac,ESP_MAC_WIFI_SOFTAP);
uint8_t buf[32] = {0};
sprintf((char*)buf,"%s-%02X%02X%02X",EXAMPLE_ESP_WIFI_SSID_PREFIX,local_mac[3],local_mac[4],local_mac[5]);
wifi_config_t wifi_config = {
.ap = {
.ssid = EXAMPLE_ESP_WIFI_SSID_PREFIX,
.ssid_len = strlen(EXAMPLE_ESP_WIFI_SSID_PREFIX),
.channel = EXAMPLE_ESP_WIFI_CHANNEL,
.password = EXAMPLE_ESP_WIFI_PASS,
.max_connection = EXAMPLE_MAX_STA_CONN,
#ifdef CONFIG_ESP_WIFI_SOFTAP_SAE_SUPPORT
.authmode = WIFI_AUTH_WPA3_PSK,
.sae_pwe_h2e = WPA3_SAE_PWE_BOTH,
#else
.authmode = WIFI_AUTH_WPA2_PSK,
#endif
.pmf_cfg = {
.required = true,
},
},
};
wifi_config.ap.ssid_len = strlen((char*)buf);
memcpy(wifi_config.ap.ssid,buf,wifi_config.ap.ssid_len);
wifi_config.ap.channel = sta_wifi_get_best_channel();
if (strlen(EXAMPLE_ESP_WIFI_PASS) == 0) {
wifi_config.ap.authmode = WIFI_AUTH_OPEN;
}
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_LOGI(TAG, "wifi_init_softap finished. SSID:%s password:%s channel:%d", buf, EXAMPLE_ESP_WIFI_PASS, wifi_config.ap.channel);
}
void app_main(void) {
esp_err_t ret;
ret = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
ESP_ERROR_CHECK(nvs_flash_erase());
ret = nvs_flash_init();
}
ESP_ERROR_CHECK(ret);
board_boot_verify();
sta_wifi_scan();
ESP_LOGI(TAG, "ESP_WIFI_MODE_AP");
wifi_init_softap();
board_queue_init();
uart1_init();
motor_init();
#ifdef CONFIG_EXAMPLE_IPV4
xTaskCreate(tcp_server_task, "tcp_server", 4096, (void*)AF_INET, TCP_SERVER_TASK_PRIORITY, NULL);
#endif
#ifdef CONFIG_EXAMPLE_IPV4
xTaskCreate(udp_server_task, "udp_server", 4096, (void*)AF_INET, UDP_SERVER_TASK_PRIORITY, NULL);
#endif
xTaskCreate(misc_task_demonstrate, "misc_task_demonstrate", 4096, NULL, MISC_TASK_PRIORITY, NULL);
}