diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index a827af26f777f..0262eeecffa02 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -675,6 +675,8 @@ def build(bld): common_src += bld.path.ant_glob('libraries/AP_HAL_ChibiOS/hwdef/common/*.mk') common_src += bld.path.ant_glob('modules/ChibiOS/os/hal/**/*.[ch]') common_src += bld.path.ant_glob('modules/ChibiOS/os/hal/**/*.mk') + common_src += bld.path.ant_glob('modules/ChibiOS/ext/lwip/src/**/*.[ch]') + common_src += bld.path.ant_glob('modules/ChibiOS/ext/lwip/src/core/**/*.[ch]') if bld.env.ROMFS_FILES: common_src += [bld.bldnode.find_or_declare('ap_romfs_embedded.h')] diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index fb78e2c186b53..5c76db5d35d88 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4515,7 +4515,7 @@ def AerobaticsScripting(self): self.customise_SITL_commandline( [], model=model, - defaults_filepath="", + defaults_filepath="Tools/autotest/models/plane-3d.parm", wipe=True) self.context_push() diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index c0ab14b986904..e62ec5920827c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -333,6 +333,30 @@ AP_Filesystem_Backend::FormatStatus AP_Filesystem::get_format_status(void) const } #endif +/* + stat wrapper for scripting + */ +bool AP_Filesystem::stat(const char *pathname, stat_t &stbuf) +{ + struct stat st; + if (fs.stat(pathname, &st) != 0) { + return false; + } + stbuf.size = st.st_size; + stbuf.mode = st.st_mode; + // these wrap in 2038 + stbuf.atime = st.st_atime; + stbuf.ctime = st.st_ctime; + stbuf.mtime = st.st_mtime; + return true; +} + +// get_singleton for scripting +AP_Filesystem *AP_Filesystem::get_singleton(void) +{ + return &fs; +} + namespace AP { AP_Filesystem &FS() diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index 585a2f5e2eb2c..b139713a751f5 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -82,6 +82,20 @@ class AP_Filesystem { int fsync(int fd); int32_t lseek(int fd, int32_t offset, int whence); int stat(const char *pathname, struct stat *stbuf); + + // stat variant for scripting + typedef struct { + uint32_t size; + int32_t mode; + uint32_t mtime; + uint32_t atime; + uint32_t ctime; + bool is_directory(void) const { + return (mode & S_IFMT) == S_IFDIR; + } + } stat_t; + bool stat(const char *pathname, stat_t &stbuf); + int unlink(const char *pathname); int mkdir(const char *pathname); int rename(const char *oldpath, const char *newpath); @@ -121,7 +135,10 @@ class AP_Filesystem { load a full file. Use delete to free the data */ FileData *load_file(const char *filename); - + + // get_singleton for scripting + static AP_Filesystem *get_singleton(void); + private: struct Backend { const char *prefix; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 31d5265f358f3..9eadd7f607b40 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -52,6 +52,9 @@ static const SysFileList sysfs_file_list[] = { #endif {"crash_dump.bin"}, {"storage.bin"}, +#if AP_FILESYSTEM_SYS_FLASH_ENABLED + {"flash.bin"}, +#endif }; int8_t AP_Filesystem_Sys::file_in_sysfs(const char *fname) { @@ -152,6 +155,13 @@ int AP_Filesystem_Sys::open(const char *fname, int flags, bool allow_absolute_pa r.str->set_buffer((char*)ptr, size, size); } } +#if AP_FILESYSTEM_SYS_FLASH_ENABLED + if (strcmp(fname, "flash.bin") == 0) { + void *ptr = (void*)0x08000000; + const size_t size = BOARD_FLASH_SIZE*1024; + r.str->set_buffer((char*)ptr, size, size); + } +#endif if (r.str->get_length() == 0) { errno = r.str->has_failed_allocation()?ENOMEM:ENOENT; diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index 75f3aa001c224..63cf31f2084f6 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -50,3 +50,6 @@ #define AP_FILESYSTEM_FILE_READING_ENABLED (AP_FILESYSTEM_FILE_WRITING_ENABLED || AP_FILESYSTEM_ROMFS_ENABLED) #endif +#ifndef AP_FILESYSTEM_SYS_FLASH_ENABLED +#define AP_FILESYSTEM_SYS_FLASH_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#endif diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 039e0685df98a..5a6169053472d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -58,6 +58,12 @@ int AP_Filesystem_Posix::open(const char *fname, int flags, bool allow_absolute_ if (! allow_absolute_paths) { fname = map_filename(fname); } + struct stat st; + if (::stat(fname, &st) == 0 && + ((st.st_mode & S_IFMT) != S_IFREG && (st.st_mode & S_IFMT) != S_IFLNK)) { + // only allow links and files + return -1; + } // we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage return ::open(fname, flags | O_CLOEXEC, 0644); } diff --git a/libraries/AP_Filesystem/posix_compat.cpp b/libraries/AP_Filesystem/posix_compat.cpp index 3ec501ef393bc..6dee4f1653431 100644 --- a/libraries/AP_Filesystem/posix_compat.cpp +++ b/libraries/AP_Filesystem/posix_compat.cpp @@ -30,14 +30,6 @@ #include #include -struct apfs_file { - int fd; - bool error; - bool eof; - int16_t unget; - char *tmpfile_name; -}; - #define CHECK_STREAM(stream, ret) while (stream == NULL || stream->fd < 0) { errno = EBADF; return ret; } #define modecmp(str, pat) (strcmp(str, pat) == 0 ? 1: 0) @@ -170,7 +162,8 @@ int apfs_fseek(APFS_FILE *stream, long offset, int whence) { CHECK_STREAM(stream, EOF); stream->eof = false; - return AP::FS().lseek(stream->fd, offset, whence); + AP::FS().lseek(stream->fd, offset, whence); + return 0; } int apfs_ferror(APFS_FILE *stream) diff --git a/libraries/AP_Filesystem/posix_compat.h b/libraries/AP_Filesystem/posix_compat.h index 5fa0fba3caa8f..4ccf56b5c5f43 100644 --- a/libraries/AP_Filesystem/posix_compat.h +++ b/libraries/AP_Filesystem/posix_compat.h @@ -20,6 +20,8 @@ #include #include #include +#include +#include #ifdef __cplusplus extern "C" { @@ -29,7 +31,13 @@ extern "C" { these are here to allow lua to build on HAL_ChibiOS */ -typedef struct apfs_file APFS_FILE; +typedef struct apfs_file { + int fd; + bool error; + bool eof; + int16_t unget; + char *tmpfile_name; +} APFS_FILE; APFS_FILE *apfs_fopen(const char *pathname, const char *mode); int apfs_fprintf(APFS_FILE *stream, const char *format, ...); diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp index a5beacca66d05..b27a3195d0a66 100644 --- a/libraries/AP_HAL/utility/Socket.cpp +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -52,14 +52,7 @@ SocketAPM::SocketAPM(bool _datagram, int _fd) : SocketAPM::~SocketAPM() { - if (fd != -1) { - CALL_PREFIX(close)(fd); - fd = -1; - } - if (fd_in != -1) { - CALL_PREFIX(close)(fd_in); - fd_in = -1; - } + close(); } void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr) @@ -79,6 +72,9 @@ void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockadd */ bool SocketAPM::connect(const char *address, uint16_t port) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; int ret; int one = 1; @@ -163,6 +159,9 @@ bool SocketAPM::connect(const char *address, uint16_t port) */ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); @@ -194,9 +193,13 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim */ bool SocketAPM::bind(const char *address, uint16_t port) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); + reuseaddress(); if (CALL_PREFIX(bind)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { return false; } @@ -209,6 +212,9 @@ bool SocketAPM::bind(const char *address, uint16_t port) */ bool SocketAPM::reuseaddress(void) const { + if (fd == -1) { + return false; + } int one = 1; return (CALL_PREFIX(setsockopt)(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1); } @@ -218,6 +224,9 @@ bool SocketAPM::reuseaddress(void) const */ bool SocketAPM::set_blocking(bool blocking) const { + if (fd == -1) { + return false; + } int fcntl_ret; if (blocking) { fcntl_ret = CALL_PREFIX(fcntl)(fd, F_SETFL, CALL_PREFIX(fcntl)(fd, F_GETFL, 0) & ~O_NONBLOCK); @@ -238,6 +247,9 @@ bool SocketAPM::set_blocking(bool blocking) const */ bool SocketAPM::set_cloexec() const { + if (fd == -1) { + return false; + } #ifdef FD_CLOEXEC return (CALL_PREFIX(fcntl)(fd, F_SETFD, FD_CLOEXEC) != -1); #else @@ -250,7 +262,10 @@ bool SocketAPM::set_cloexec() const */ ssize_t SocketAPM::send(const void *buf, size_t size) const { - return CALL_PREFIX(send)(fd, buf, size, 0); + if (fd == -1) { + return -1; + } + return CALL_PREFIX(send)(fd, buf, size, MSG_NOSIGNAL); } /* @@ -258,6 +273,9 @@ ssize_t SocketAPM::send(const void *buf, size_t size) const */ ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uint16_t port) { + if (fd == -1) { + return -1; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); return CALL_PREFIX(sendto)(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); @@ -277,6 +295,10 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) ssize_t ret; ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len); if (ret <= 0) { + if (!datagram && connected && ret == 0) { + // remote host has closed connection + connected = false; + } return ret; } if (fd_in != -1) { @@ -322,6 +344,9 @@ const char *SocketAPM::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint void SocketAPM::set_broadcast(void) const { + if (fd == -1) { + return; + } int one = 1; CALL_PREFIX(setsockopt)(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); } @@ -336,6 +361,9 @@ bool SocketAPM::pollin(uint32_t timeout_ms) FD_ZERO(&fds); int fin = get_read_fd(); + if (fin == -1) { + return false; + } FD_SET(fin, &fds); tv.tv_sec = timeout_ms / 1000; @@ -353,6 +381,9 @@ bool SocketAPM::pollin(uint32_t timeout_ms) */ bool SocketAPM::pollout(uint32_t timeout_ms) { + if (fd == -1) { + return false; + } fd_set fds; struct timeval tv; @@ -373,6 +404,9 @@ bool SocketAPM::pollout(uint32_t timeout_ms) */ bool SocketAPM::listen(uint16_t backlog) const { + if (fd == -1) { + return false; + } return CALL_PREFIX(listen)(fd, (int)backlog) == 0; } @@ -382,6 +416,9 @@ bool SocketAPM::listen(uint16_t backlog) const */ SocketAPM *SocketAPM::accept(uint32_t timeout_ms) { + if (fd == -1) { + return nullptr; + } if (!pollin(timeout_ms)) { return nullptr; } @@ -391,7 +428,12 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms) if (newfd == -1) { return nullptr; } - return new SocketAPM(false, newfd); + auto *ret = new SocketAPM(false, newfd); + if (ret != nullptr) { + ret->connected = true; + ret->reuseaddress(); + } + return ret; } /* @@ -405,4 +447,33 @@ bool SocketAPM::is_multicast_address(struct sockaddr_in &addr) const return haddr >= mc_lower && haddr <= mc_upper; } +void SocketAPM::close(void) +{ + if (fd != -1) { + CALL_PREFIX(close)(fd); + fd = -1; + } + if (fd_in != -1) { + CALL_PREFIX(close)(fd_in); + fd_in = -1; + } +} + +/* + duplicate a socket, giving a new object with the same contents, + the fd in the old object is set to -1 + */ +SocketAPM *SocketAPM::duplicate(void) +{ + auto *ret = new SocketAPM(datagram, fd); + if (ret == nullptr) { + return nullptr; + } + ret->fd_in = fd_in; + ret->connected = connected; + fd = -1; + fd_in = -1; + return ret; +} + #endif // AP_NETWORKING_BACKEND_ANY diff --git a/libraries/AP_HAL/utility/Socket.h b/libraries/AP_HAL/utility/Socket.h index 70e4a1cdea158..a7bbf45e5ba1c 100644 --- a/libraries/AP_HAL/utility/Socket.h +++ b/libraries/AP_HAL/utility/Socket.h @@ -69,6 +69,9 @@ class SocketAPM { // start listening for new tcp connections bool listen(uint16_t backlog) const; + // close socket + void close(void); + // accept a new connection. Only valid for TCP connections after // listen has been used. A new socket is returned SocketAPM *accept(uint32_t timeout_ms); @@ -78,6 +81,10 @@ class SocketAPM { return fd_in != -1? fd_in : fd; } + // create a new socket with same fd, but new memory + // the old socket gets fd of -1 + SocketAPM *duplicate(void); + bool is_connected(void) const { return connected; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h b/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h index 7002236e2eda9..94f839feaa0f7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h @@ -49,7 +49,6 @@ ----------------------------------------------- */ -#define LWIP_PLATFORM_DIAG(x) do {__wrap_printf x; } while(0) // #define LWIP_DEBUG #define U16_F "u" #define X8_F "x" @@ -57,13 +56,29 @@ #define LWIP_STATS_DISPLAY 1 #define ETHARP_STATS 1 #define LWIP_IGMP 1 -#define MEMP_NUM_NETCONN 10 // up to 10 sockets -#define MEMP_NUM_TCP_PCB 10 +#define MEMP_NUM_NETCONN 50 // up to 50 sockets +#define MEMP_NUM_TCP_PCB 200 #define MEM_LIBC_MALLOC 1 #define MEMP_MEM_MALLOC 1 #define SO_REUSE 1 #define SO_REUSE_RXTOALL 1 -#define DHCP_DEBUG LWIP_DBG_ON +#define DEFAULT_ACCEPTMBOX_SIZE 20 +#define MEMP_NUM_PBUF 64 +#define TCP_WND 12000 +#define TCP_SND_BUF 12000 + +/* + map LWIP debugging onto ap_networking_printf to allow for easier + redirection to a file or dedicated serial port + */ +#ifdef __cplusplus +extern "C" { +#endif +int ap_networking_printf(const char *fmt, ...); +#ifdef __cplusplus +} +#endif +#define LWIP_PLATFORM_DIAG(x) do {ap_networking_printf x; } while(0) #ifndef LWIP_IPV6 // This uses an additional 18KB Flash diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index df2f207e9c5ab..95856166fd17f 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -8,6 +8,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -19,6 +20,8 @@ extern const AP_HAL::HAL& hal; #include #endif +#include + #if AP_NETWORKING_BACKEND_SITL #include "AP_Networking_SITL.h" #endif @@ -150,6 +153,9 @@ void AP_Networking::init() // init network mapped serialmanager ports ports_init(); + + // register sendfile handler + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Networking::sendfile_check, void)); } /* @@ -282,6 +288,77 @@ void AP_Networking::startup_wait(void) const #endif } +/* + send the rest of a file to a socket + */ +bool AP_Networking::sendfile(SocketAPM *sock, int fd) +{ + WITH_SEMAPHORE(sem); + if (sendfile_buf == nullptr) { + sendfile_buf = (uint8_t *)malloc(AP_NETWORKING_SENDFILE_BUFSIZE); + if (sendfile_buf == nullptr) { + return false; + } + } + for (auto &s : sendfiles) { + if (s.sock == nullptr) { + s.sock = sock->duplicate(); + if (s.sock == nullptr) { + return false; + } + s.fd = fd; + return true; + } + } + return false; +} + +void AP_Networking::SendFile::close(void) +{ + AP::FS().close(fd); + delete sock; + sock = nullptr; +} + +#include +/* + check for sendfile updates + */ +void AP_Networking::sendfile_check(void) +{ + if (sendfile_buf == nullptr) { + return; + } + WITH_SEMAPHORE(sem); + bool none_active = true; + for (auto &s : sendfiles) { + if (s.sock == nullptr) { + continue; + } + none_active = false; + if (!s.sock->pollout(0)) { + continue; + } + const auto nread = AP::FS().read(s.fd, sendfile_buf, AP_NETWORKING_SENDFILE_BUFSIZE); + if (nread <= 0) { + s.close(); + continue; + } + const auto nsent = s.sock->send(sendfile_buf, nread); + if (nsent <= 0) { + s.close(); + continue; + } + if (nsent < nread) { + AP::FS().lseek(s.fd, nsent - nread, SEEK_CUR); + } + } + if (none_active) { + free(sendfile_buf); + sendfile_buf = nullptr; + } +} + AP_Networking *AP_Networking::singleton; namespace AP @@ -292,4 +369,31 @@ AP_Networking &network() } } +/* + debug printfs from LWIP + */ +int ap_networking_printf(const char *fmt, ...) +{ +#ifdef AP_NETWORKING_LWIP_DEBUG_PORT + static AP_HAL::UARTDriver *uart; + if (uart == nullptr) { + uart = hal.serial(AP_NETWORKING_LWIP_DEBUG_PORT); + if (uart == nullptr) { + return -1; + } + uart->begin(921600, 0, 50000); + } + va_list ap; + va_start(ap, fmt); + uart->vprintf(fmt, ap); + va_end(ap); +#else + va_list ap; + va_start(ap, fmt); + hal.console->vprintf(fmt, ap); + va_end(ap); +#endif + return 0; +} + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 1359b3abe5572..5ed3232924407 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -188,6 +188,11 @@ class AP_Networking static uint32_t convert_netmask_bitcount_to_ip(const uint32_t netmask_bitcount); static uint8_t convert_netmask_ip_to_bitcount(const uint32_t netmask_ip); + /* + send contents of a file to a socket then close both socket and file + */ + bool sendfile(SocketAPM *sock, int fd); + static const struct AP_Param::GroupInfo var_info[]; private: @@ -311,6 +316,16 @@ class AP_Networking // ports for registration with serial manager Port ports[AP_NETWORKING_NUM_PORTS]; + // support for sendfile() + struct SendFile { + SocketAPM *sock; + int fd; + void close(void); + } sendfiles[AP_NETWORKING_NUM_SENDFILES]; + + uint8_t *sendfile_buf; + void sendfile_check(void); + void ports_init(void); }; @@ -319,4 +334,8 @@ namespace AP AP_Networking &network(); }; +extern "C" { +int ap_networking_printf(const char *fmt, ...); +} + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 5d32334510dc5..00ba6d50c1e60 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -89,3 +89,11 @@ #ifndef AP_NETWORKING_NUM_PORTS #define AP_NETWORKING_NUM_PORTS 4 #endif + +#ifndef AP_NETWORKING_NUM_SENDFILES +#define AP_NETWORKING_NUM_SENDFILES 20 +#endif + +#ifndef AP_NETWORKING_SENDFILE_BUFSIZE +#define AP_NETWORKING_SENDFILE_BUFSIZE 10000 +#endif diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index 41d9f5b294470..77c763b528e0c 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -94,15 +94,8 @@ bool AP_RTC::get_utc_usec(uint64_t &usec) const return true; } -bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const +void AP_RTC::clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const { - // get time of day in ms - uint64_t time_ms = 0; - if (!get_utc_usec(time_ms)) { - return false; - } - time_ms /= 1000U; - // separate time into ms, sec, min, hour and days but all expressed in milliseconds ms = time_ms % 1000; uint32_t sec_ms = (time_ms % (60 * 1000)) - ms; @@ -113,32 +106,104 @@ bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uin sec = sec_ms / 1000; min = min_ms / (60 * 1000); hour = hour_ms / (60 * 60 * 1000); +} +bool AP_RTC::clock_s_to_date_fields(const uint32_t utc_sec32, uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint8_t &wday) const +{ + const time_t utc_sec = utc_sec32; + struct tm* tm = gmtime(&utc_sec); + if (tm == nullptr) { + return false; + } + year = tm->tm_year+1900; /* Year, 20xx. */ + month = tm->tm_mon; /* Month. [0-11] */ + day = tm->tm_mday; /* Day. [1-31] */ + hour = tm->tm_hour; /* Hours. [0-23] */ + min = tm->tm_min; /* Minutes. [0-59] */ + sec = tm->tm_sec; /* Seconds. [0-60] (1 leap second) */ + wday = tm->tm_wday; /* week day, [0-6] */ + return true; +} + +/* + return true for leap years + */ +bool AP_RTC::_is_leap(uint32_t y) +{ + y += 1900; + return (y % 4) == 0 && ((y % 100) != 0 || (y % 400) == 0); +} + +/* + implementation of timegm() (from Samba) +*/ +uint32_t AP_RTC::_timegm(struct tm &tm) +{ + static const uint8_t ndays[2][12] = { + {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}, + {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}}; + uint32_t res = 0; + + if (tm.tm_mon > 12 || + tm.tm_mday > 31 || + tm.tm_min > 60 || + tm.tm_sec > 60 || + tm.tm_hour > 24) { + /* invalid tm structure */ + return 0; + } + + for (auto i = 70; i < tm.tm_year; i++) { + res += _is_leap(i) ? 366 : 365; + } + + for (auto i = 0; i < tm.tm_mon; i++) { + res += ndays[_is_leap(tm.tm_year)][i]; + } + res += tm.tm_mday - 1U; + res *= 24U; + res += tm.tm_hour; + res *= 60U; + res += tm.tm_min; + res *= 60U; + res += tm.tm_sec; + return res; +} + +uint32_t AP_RTC::date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const +{ + struct tm tm {}; + tm.tm_year = year - 1900; + tm.tm_mon = month; + tm.tm_mday = day; + tm.tm_hour = hour; + tm.tm_min = min; + tm.tm_sec = sec; + return _timegm(tm); +} + +bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const +{ + // get time of day in ms + uint64_t time_ms; + if (!get_utc_usec(time_ms)) { + return false; + } + time_ms /= 1000U; + clock_ms_to_hms_fields(time_ms, hour, min, sec, ms); return true; } bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const { // get local time of day in ms - uint64_t time_ms = 0; - uint64_t ms_local = 0; + uint64_t time_ms; if (!get_utc_usec(time_ms)) { return false; } time_ms /= 1000U; - ms_local = time_ms + (tz_min * 60000); - - // separate time into ms, sec, min, hour and days but all expressed in milliseconds - ms = ms_local % 1000; - uint32_t sec_ms = (ms_local % (60 * 1000)) - ms; - uint32_t min_ms = (ms_local % (60 * 60 * 1000)) - sec_ms - ms; - uint32_t hour_ms = (ms_local % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms; - - // convert times as milliseconds into appropriate units - sec = sec_ms / 1000; - min = min_ms / (60 * 1000); - hour = hour_ms / (60 * 60 * 1000); - + const uint64_t ms_local = time_ms + (tz_min * 60000); + clock_ms_to_hms_fields(ms_local, hour, min, sec, ms); return true; } diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index d03846bf6f29b..a35887532b547 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -61,7 +61,10 @@ class AP_RTC { HAL_Semaphore &get_semaphore(void) { return rsem; } - + + bool clock_s_to_date_fields(const uint32_t utc_sec32, uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint8_t &wday) const; + uint32_t date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const; + private: static AP_RTC *_singleton; @@ -70,6 +73,10 @@ class AP_RTC { source_type rtc_source_type = SOURCE_NONE; int64_t rtc_shift; + void clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const; + + static bool _is_leap(uint32_t y); + static uint32_t _timegm(struct tm &tm); }; namespace AP { diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 347a7d440fd2d..82c6f220cbf78 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -53,6 +53,10 @@ static_assert(SCRIPTING_STACK_SIZE <= SCRIPTING_STACK_MAX_SIZE, "Scripting requi #define SCRIPTING_ENABLE_DEFAULT 0 #endif +#if AP_NETWORKING_ENABLED +#include +#endif + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_Scripting::var_info[] = { @@ -287,6 +291,17 @@ void AP_Scripting::thread(void) { } num_pwm_source = 0; +#if AP_NETWORKING_ENABLED + // clear allocated sockets + for (uint8_t i=0; i #include #include "AP_Scripting_CANSensor.h" +#include #ifndef SCRIPTING_MAX_NUM_I2C_DEVICE #define SCRIPTING_MAX_NUM_I2C_DEVICE 4 @@ -31,6 +32,13 @@ #define SCRIPTING_MAX_NUM_PWM_SOURCE 4 +#if AP_NETWORKING_ENABLED +#ifndef SCRIPTING_MAX_NUM_NET_SOCKET +#define SCRIPTING_MAX_NUM_NET_SOCKET 50 +#endif +class SocketAPM; +#endif + class AP_Scripting { public: @@ -111,6 +119,12 @@ class AP_Scripting int get_current_ref() { return current_ref; } void set_current_ref(int ref) { current_ref = ref; } +#if AP_NETWORKING_ENABLED + // SocketAPM storage + uint8_t num_net_sockets; + SocketAPM *_net_sockets[SCRIPTING_MAX_NUM_NET_SOCKET]; +#endif + struct mavlink_msg { mavlink_message_t msg; mavlink_channel_t chan; diff --git a/libraries/AP_Scripting/applets/WebExamples/test.lua b/libraries/AP_Scripting/applets/WebExamples/test.lua new file mode 100644 index 0000000000000..97a5cc4a9e086 --- /dev/null +++ b/libraries/AP_Scripting/applets/WebExamples/test.lua @@ -0,0 +1,9 @@ +--[[ +example lua cgi file for cgi-bin/ folder +--]] +return [[ +test-from-cgi + +working!! +]] + diff --git a/libraries/AP_Scripting/applets/WebExamples/test.shtml b/libraries/AP_Scripting/applets/WebExamples/test.shtml new file mode 100644 index 0000000000000..96546a427fe52 --- /dev/null +++ b/libraries/AP_Scripting/applets/WebExamples/test.shtml @@ -0,0 +1,16 @@ + + + + + +

Server Side Scripting Test

+ + + + + + + +
RollPitchYaw
+ + diff --git a/libraries/AP_Scripting/applets/net_webserver.lua b/libraries/AP_Scripting/applets/net_webserver.lua new file mode 100644 index 0000000000000..4355fa6006e87 --- /dev/null +++ b/libraries/AP_Scripting/applets/net_webserver.lua @@ -0,0 +1,955 @@ +--[[ + example script to test lua socket API +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 47 +PARAM_TABLE_PREFIX = "WEB_" + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +-- Setup Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'net_test: could not add param table') + +--[[ + // @Param: WEB_ENABLE + // @DisplayName: enable web server + // @Description: enable web server + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local WEB_ENABLE = bind_add_param('ENABLE', 1, 1) + +--[[ + // @Param: WEB_BIND_PORT + // @DisplayName: web server TCP port + // @Description: web server TCP port + // @Range: 1 65535 + // @User: Standard +--]] +local WEB_BIND_PORT = bind_add_param('BIND_PORT', 2, 8080) + +--[[ + // @Param: WEB_DEBUG + // @DisplayName: web server debugging + // @Description: web server debugging + // @Values: 0:Disabled,1:Enabled + // @User: Advanced +--]] +local WEB_DEBUG = bind_add_param('DEBUG', 3, 0) + +--[[ + // @Param: WEB_BLOCK_SIZE + // @DisplayName: web server block size + // @Description: web server block size for download + // @Range: 1 65535 + // @User: Advanced +--]] +local WEB_BLOCK_SIZE = bind_add_param('BLOCK_SIZE', 4, 10240) + +--[[ + // @Param: WEB_TIMEOUT + // @DisplayName: web server timeout + // @Description: timeout for inactive connections + // @Units: s + // @Range: 0.1 60 + // @User: Advanced +--]] +local WEB_TIMEOUT = bind_add_param('TIMEOUT', 5, 2.0) + +if WEB_ENABLE:get() ~= 1 then + gcs:send_text(MAV_SEVERITY.INFO, "WebServer: disabled") + return +end + +local BRD_RTC_TZ_MIN = Parameter("BRD_RTC_TZ_MIN") + +gcs:send_text(MAV_SEVERITY.INFO, string.format("WebServer: starting on port %u", WEB_BIND_PORT:get())) + +local sock_listen = Socket(0) +local clients = {} + +local DOCTYPE = "" +local SERVER_VERSION = "net_webserver 1.0" +local CONTENT_TEXT_HTML = "text/html;charset=UTF-8" +local CONTENT_OCTET_STREAM = "application/octet-stream" + +local HIDDEN_FOLDERS = { "@SYS", "@ROMFS", "@MISSION", "@PARAM" } + +local MNT_PREFIX = "/mnt" +local MNT_PREFIX2 = MNT_PREFIX .. "/" + +local MIME_TYPES = { + ["apj"] = CONTENT_OCTET_STREAM, + ["dat"] = CONTENT_OCTET_STREAM, + ["o"] = CONTENT_OCTET_STREAM, + ["obj"] = CONTENT_OCTET_STREAM, + ["lua"] = "text/x-lua", + ["py"] = "text/x-python", + ["shtml"] = CONTENT_TEXT_HTML, + ["js"] = "text/javascript", + -- thanks to https://developer.mozilla.org/en-US/docs/Web/HTTP/Basics_of_HTTP/MIME_types/Common_types + ["aac"] = "audio/aac", + ["abw"] = "application/x-abiword", + ["arc"] = "application/x-freearc", + ["avif"] = "image/avif", + ["avi"] = "video/x-msvideo", + ["azw"] = "application/vnd.amazon.ebook", + ["bin"] = "application/octet-stream", + ["bmp"] = "image/bmp", + ["bz"] = "application/x-bzip", + ["bz2"] = "application/x-bzip2", + ["cda"] = "application/x-cdf", + ["csh"] = "application/x-csh", + ["css"] = "text/css", + ["csv"] = "text/csv", + ["doc"] = "application/msword", + ["docx"] = "application/vnd.openxmlformats-officedocument.wordprocessingml.document", + ["eot"] = "application/vnd.ms-fontobject", + ["epub"] = "application/epub+zip", + ["gz"] = "application/gzip", + ["gif"] = "image/gif", + ["htm"] = CONTENT_TEXT_HTML, + ["html"] = CONTENT_TEXT_HTML, + ["ico"] = "image/vnd.microsoft.icon", + ["ics"] = "text/calendar", + ["jar"] = "application/java-archive", + ["jpeg"] = "image/jpeg", + ["json"] = "application/json", + ["jsonld"] = "application/ld+json", + ["mid"] = "audio/x-midi", + ["mjs"] = "text/javascript", + ["mp3"] = "audio/mpeg", + ["mp4"] = "video/mp4", + ["mpeg"] = "video/mpeg", + ["mpkg"] = "application/vnd.apple.installer+xml", + ["odp"] = "application/vnd.oasis.opendocument.presentation", + ["ods"] = "application/vnd.oasis.opendocument.spreadsheet", + ["odt"] = "application/vnd.oasis.opendocument.text", + ["oga"] = "audio/ogg", + ["ogv"] = "video/ogg", + ["ogx"] = "application/ogg", + ["opus"] = "audio/opus", + ["otf"] = "font/otf", + ["png"] = "image/png", + ["pdf"] = "application/pdf", + ["php"] = "application/x-httpd-php", + ["ppt"] = "application/vnd.ms-powerpoint", + ["pptx"] = "application/vnd.openxmlformats-officedocument.presentationml.presentation", + ["rar"] = "application/vnd.rar", + ["rtf"] = "application/rtf", + ["sh"] = "application/x-sh", + ["svg"] = "image/svg+xml", + ["tar"] = "application/x-tar", + ["tif"] = "image/tiff", + ["tiff"] = "image/tiff", + ["ts"] = "video/mp2t", + ["ttf"] = "font/ttf", + ["txt"] = "text/plain", + ["vsd"] = "application/vnd.visio", + ["wav"] = "audio/wav", + ["weba"] = "audio/webm", + ["webm"] = "video/webm", + ["webp"] = "image/webp", + ["woff"] = "font/woff", + ["woff2"] = "font/woff2", + ["xhtml"] = "application/xhtml+xml", + ["xls"] = "application/vnd.ms-excel", + ["xlsx"] = "application/vnd.openxmlformats-officedocument.spreadsheetml.sheet", + ["xml"] = "default.", + ["xul"] = "application/vnd.mozilla.xul+xml", + ["zip"] = "application/zip", + ["3gp"] = "video", + ["3g2"] = "video", + ["7z"] = "application/x-7z-compressed", +} + +--[[ + builtin dynamic pages +--]] +local DYNAMIC_PAGES = { + +-- main home page +["/"] = [[ + + + + + + ArduPilot + + + +

ArduPilot Web Server

+ + +
+ +
+

Controller Status

+
+ + +]], + +-- board status section on front page +["@DYNAMIC/board_status.shtml"] = [[ + + + + + + + +
Firmware
GIT Hash
Uptime
Arm Status
AHRS Location
GPS Location
+]] +} + +--[[ + builtin javascript library functions +--]] +JS_LIBRARY = { + ["dynamic_load"] = [[ + function dynamic_load(div_id, uri, period_ms) { + var xhr = new XMLHttpRequest(); + xhr.open('GET', uri); + + xhr.setRequestHeader("Cache-Control", "no-cache, no-store, max-age=0"); + xhr.setRequestHeader("Expires", "Tue, 01 Jan 1980 1:00:00 GMT"); + xhr.setRequestHeader("Pragma", "no-cache"); + + xhr.onload = function () { + if (xhr.status === 200) { + var output = document.getElementById(div_id); + if (uri.endsWith('.shtml') || uri.endsWith('.html')) { + output.innerHTML = xhr.responseText; + } else { + output.textContent = xhr.responseText; + } + } + setTimeout(function() { dynamic_load(div_id,uri, period_ms); }, period_ms); + } + xhr.send(); + } +]] +} + +if not sock_listen:bind("0.0.0.0", WEB_BIND_PORT:get()) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("WebServer: failed to bind to TCP %u", WEB_BIND_PORT:get())) + return +end + +if not sock_listen:listen(20) then + gcs:send_text(MAV_SEVERITY.ERROR, "WebServer: failed to listen") + return +end + +function hms_uptime() + local s = (millis()/1000):toint() + local min = math.floor(s / 60) % 60 + local hr = math.floor(s / 3600) + return string.format("%u hours %u minutes %u seconds", hr, min, s%60) +end + +--[[ + split string by pattern +--]] +local function split(str, pattern) + local ret = {} + for s in string.gmatch(str, pattern) do + table.insert(ret, s) + end + return ret +end + +--[[ + return true if a string ends in the 2nd string +--]] +local function endswith(str, s) + local len1 = #str + local len2 = #s + return string.sub(str,1+len1-len2,len1) == s +end + +--[[ + return true if a string starts with the 2nd string +--]] +local function startswith(str, s) + return string.sub(str,1,#s) == s +end + +local debug_count=0 + +function DEBUG(txt) + if WEB_DEBUG:get() ~= 0 then + gcs:send_text(MAV_SEVERITY.DEBUG, txt .. string.format(" [%u]", debug_count)) + debug_count = debug_count + 1 + end +end + +--[[ + return index of element in a table +--]] +function table_index(t,el) + for i,v in ipairs(t) do + if v == el then + return i + end + end + return nil +end + +--[[ + return true if a table contains a given element +--]] +function table_contains(t,el) + local i = table_index(t, el) + return i ~= nil +end + +function is_hidden_dir(path) + return table_contains(HIDDEN_FOLDERS, path) +end + +local DAYS = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat" } +local MONTHS = { "Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec" } + +function isdirectory(path) + local s = fs:stat(path) + return s and s:is_directory() +end + +--[[ + time string for directory listings +--]] +function file_timestring(path) + local s = fs:stat(path) + if not s then + return "" + end + local mtime = s:mtime() + mtime = mtime + BRD_RTC_TZ_MIN:get()*60 + local year, month, day, hour, min, sec, _ = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%04u-%02u-%02u %02u:%02u", year, month+1, day, hour, min, sec) +end + +--[[ + time string for Last-Modified +--]] +function file_timestring_http(mtime) + local year, month, day, hour, min, sec, wday = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%s, %02u %s %u %02u:%02u:%02u GMT", + DAYS[wday+1], + day, + MONTHS[month+1], + year, + hour, + min, + sec) +end + +--[[ + parse a http time string to a uint32_t seconds timestamp +--]] +function file_timestring_http_parse(tstring) + local dayname, day, monthname, year, hour, min, sec = + string.match(tstring, + '(%w%w%w), (%d+) (%w%w%w) (%d%d%d%d) (%d%d):(%d%d):(%d%d) GMT') + if not dayname then + return nil + end + local mon = table_index(MONTHS, monthname) + return rtc:date_fields_to_clock_s(year, mon-1, day, hour, min, sec) +end + +--[[ + return true if path exists and is not a directory +--]] +function file_exists(path) + local s = fs:stat(path) + if not s then + return false + end + return not s:is_directory() +end + +--[[ + substitute variables of form {xxx} from a table + from http://lua-users.org/wiki/StringInterpolation +--]] +function substitute_vars(s, vars) + s = (string.gsub(s, "({([^}]+)})", + function(whole,i) + return vars[i] or whole + end)) + return s +end + +--[[ + lat or lon as a string, working around limited type in ftoa_engine +--]] +function latlon_str(ll) + local ipart = tonumber(string.match(tostring(ll*1.0e-7), '(.*[.]).*')) + local fpart = math.abs(ll - ipart*10000000) + return string.format("%d.%u", ipart, fpart, ipart*10000000, ll) +end + +--[[ + location string for home page +--]] +function location_string(loc) + return substitute_vars([[{lat} {lon} {alt}]], + { ["lat"] = latlon_str(loc:lat()), + ["lon"] = latlon_str(loc:lng()), + ["alt"] = string.format("%.1fm", loc:alt()*1.0e-2) }) +end + +--[[ + client class for open connections +--]] +local function Client(_sock, _idx) + local self = {} + + self.closed = false + + local sock = _sock + local idx = _idx + local have_header = false + local header = "" + local header_lines = {} + local header_vars = {} + local run = nil + local protocol = nil + local file = nil + local start_time = millis() + local offset = 0 + + function self.read_header() + local s = sock:recv(2048) + if not s then + local now = millis() + if not sock:is_connected() or now - start_time > WEB_TIMEOUT:get()*1000 then + -- EOF while looking for header + DEBUG(string.format("%u: EOF", idx)) + self.remove() + return false + end + return false + end + if not s or #s == 0 then + return false + end + header = header .. s + local eoh = string.find(s, '\r\n\r\n') + if eoh then + DEBUG(string.format("%u: got header", idx)) + have_header = true + header_lines = split(header, "[^\r\n]+") + -- blocking for reply + sock:set_blocking(true) + return true + end + return false + end + + function self.sendstring(s) + sock:send(s, #s) + end + + function self.sendline(s) + self.sendstring(s .. "\r\n") + end + + --[[ + send a string with variable substitution using {varname} + --]] + function self.sendstring_vars(s, vars) + self.sendstring(substitute_vars(s, vars)) + end + + function self.send_header(code, codestr, vars) + self.sendline(string.format("%s %u %s", protocol, code, codestr)) + self.sendline(string.format("Server: %s", SERVER_VERSION)) + for k,v in pairs(vars) do + self.sendline(string.format("%s: %s", k, v)) + end + self.sendline("Connection: close") + self.sendline("") + end + + -- get size of a file + function self.file_size(fname) + local s = fs:stat(fname) + if not s then + return 0 + end + local ret = s:size():toint() + DEBUG(string.format("%u: size of '%s' -> %u", idx, fname, ret)) + return ret + end + + + --[[ + return full path with .. resolution + --]] + function self.full_path(path, name) + DEBUG(string.format("%u: full_path(%s,%s)", idx, path, name)) + local ret = path + if path == "/" and startswith(name,"@") then + return name + end + if name == ".." then + if path == "/" then + return "/" + end + if endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + local dir, _ = string.match(path, '(.*/)(.*)') + if not dir then + return path + end + return dir + end + if not endswith(ret, "/") then + ret = ret .. "/" + end + ret = ret .. name + DEBUG(string.format("%u: full_path(%s,%s) -> %s", idx, path, name, ret)) + return ret + end + + function self.directory_list(path) + sock:set_blocking(true) + if startswith(path, "/@") then + path = string.sub(path, 2, #path-1) + end + DEBUG(string.format("%u: directory_list(%s)", idx, path)) + local dlist = dirlist(path) + if not dlist then + dlist = {} + end + if not table_contains(dlist, "..") then + -- on ChibiOS we don't get .. + table.insert(dlist, "..") + end + if path == "/" then + for _,v in ipairs(HIDDEN_FOLDERS) do + table.insert(dlist, v) + end + end + + table.sort(dlist) + self.send_header(200, "OK", {["Content-Type"]=CONTENT_TEXT_HTML}) + self.sendline(DOCTYPE) + self.sendstring_vars([[ + + + Index of {path} + + +

Index of {path}

+ + +]], {path=path}) + for _,d in ipairs(dlist) do + local skip = d == "." + if not skip then + local fullpath = self.full_path(path, d) + local name = d + local sizestr = "0" + local stat = fs:stat(fullpath) + local size = stat and stat:size() or 0 + if is_hidden_dir(fullpath) or (stat and stat:is_directory()) then + name = name .. "/" + elseif size >= 100*1000*1000 then + sizestr = string.format("%uM", (size/(1000*1000)):toint()) + else + sizestr = tostring(size) + end + local modtime = file_timestring(fullpath) + self.sendstring_vars([[ +]], { name=name, size=sizestr, modtime=modtime }) + end + end + self.sendstring([[ +
NameLast modifiedSize
{name}{modtime}{size}
+ + +]]) + end + + -- send file content + function self.send_file() + if not sock:pollout(0) then + return + end + local chunk = WEB_BLOCK_SIZE:get() + local b = file:read(chunk) + sock:set_blocking(true) + if b and #b > 0 then + local sent = sock:send(b, #b) + if sent == -1 then + run = nil + self.remove() + return + end + if sent < #b then + file:seek(offset+sent) + end + offset = offset + sent + end + if not b or #b < chunk then + -- EOF + DEBUG(string.format("%u: sent file", idx)) + run = nil + self.remove() + return + end + end + + --[[ + load whole file as a string + --]] + function self.load_file() + local chunk = WEB_BLOCK_SIZE:get() + local ret = "" + while true do + local b = file:read(chunk) + if not b or #b == 0 then + break + end + ret = ret .. b + end + return ret + end + + --[[ + evaluate some lua code and return as a string + --]] + function self.evaluate(code) + local eval_code = "function eval_func()\n" .. code .. "\nend\n" + local f, errloc, err = load(eval_code, "eval_func", "t", _ENV) + if not f then + DEBUG(string.format("load failed: err=%s errloc=%s", err, errloc)) + return nil + end + local success, err2 = pcall(f) + if not success then + DEBUG(string.format("pcall failed: err=%s", err2)) + return nil + end + local ok, s2 = pcall(eval_func) + eval_func = nil + if ok then + return s2 + end + return nil + end + + --[[ + process a file as a lua CGI + --]] + function self.send_cgi() + sock:set_blocking(true) + local contents = self.load_file() + local s = self.evaluate(contents) + if s then + self.sendstring(s) + end + self.remove() + end + + --[[ + send file content with server side processsing + files ending in .shtml can have embedded lua lika this: + + + + Using 'lstr' a return tostring(yourcode) is added to the code + automatically + --]] + function self.send_processed_file(dynamic_page) + sock:set_blocking(true) + local contents + if dynamic_page then + contents = file + else + contents = self.load_file() + end + while #contents > 0 do + local pat1 = "(.-)[<][?]lua[ \n](.-)[?][>](.*)" + local pat2 = "(.-)[<][?]lstr[ \n](.-)[?][>](.*)" + local p1, p2, p3 = string.match(contents, pat1) + if not p1 then + p1, p2, p3 = string.match(contents, pat2) + if not p1 then + break + end + p2 = "return tostring(" .. p2 .. ")" + end + self.sendstring(p1) + local s2 = self.evaluate(p2) + if s2 then + self.sendstring(s2) + end + contents = p3 + end + self.sendstring(contents) + self.remove() + end + + -- return a content type + function self.content_type(path) + if path == "/" then + return MIME_TYPES["html"] + end + local _, ext = string.match(path, '(.*[.])(.*)') + ext = string.lower(ext) + local ret = MIME_TYPES[ext] + if not ret then + return CONTENT_OCTET_STREAM + end + return ret + end + + -- perform a file download + function self.file_download(path) + if startswith(path, "/@") then + path = string.sub(path, 2, #path) + end + DEBUG(string.format("%u: file_download(%s)", idx, path)) + file = DYNAMIC_PAGES[path] + dynamic_page = file ~= nil + if not dynamic_page then + file = io.open(path,"rb") + if not file then + DEBUG(string.format("%u: Failed to open '%s'", idx, path)) + return false + end + end + local vars = {["Content-Type"]=self.content_type(path)} + local cgi_processing = startswith(path, "/cgi-bin/") and endswith(path, ".lua") + local server_side_processing = endswith(path, ".shtml") + local stat = fs:stat(path) + if not startswith(path, "@") and + not server_side_processing and + not cgi_processing and stat and + not dynamic_page then + local fsize = stat:size() + local mtime = stat:mtime() + vars["Content-Length"]= tostring(fsize) + local modtime = file_timestring_http(mtime) + if modtime then + vars["Last-Modified"] = modtime + end + local if_modified_since = header_vars['If-Modified-Since'] + if if_modified_since then + local tsec = file_timestring_http_parse(if_modified_since) + if tsec and tsec >= mtime then + DEBUG(string.format("%u: Not modified: %s %s", idx, modtime, if_modified_since)) + self.send_header(304, "Not Modified", vars) + return true + end + end + end + self.send_header(200, "OK", vars) + if server_side_processing or dynamic_page then + DEBUG(string.format("%u: shtml processing %s", idx, path)) + run = self.send_processed_file(dynamic_page) + elseif cgi_processing then + DEBUG(string.format("%u: CGI processing %s", idx, path)) + run = self.send_cgi + elseif sock:sendfile(file) then + return true + else + run = self.send_file + end + return true + end + + function self.not_found() + self.send_header(404, "Not found", {}) + end + + function self.moved_permanently(relpath) + if not startswith(relpath, "/") then + relpath = "/" .. relpath + end + local location = string.format("http://%s%s", header_vars['Host'], relpath) + DEBUG(string.format("%u: Redirect -> %s", idx, location)) + self.send_header(301, "Moved Permanently", {["Location"]=location}) + end + + -- process a single request + function self.process_request() + local h1 = header_lines[1] + if not h1 or #h1 == 0 then + DEBUG(string.format("%u: empty request", idx)) + return + end + local cmd = split(header_lines[1], "%S+") + if not cmd or #cmd < 3 then + DEBUG(string.format("bad request: %s", header_lines[1])) + return + end + if cmd[1] ~= "GET" then + DEBUG(string.format("bad op: %s", cmd[1])) + return + end + protocol = cmd[3] + if protocol ~= "HTTP/1.0" and protocol ~= "HTTP/1.1" then + DEBUG(string.format("bad protocol: %s", protocol)) + return + end + local path = cmd[2] + DEBUG(string.format("%u: path='%s'", idx, path)) + + -- extract header variables + for i = 2,#header_lines do + local key, var = string.match(header_lines[i], '(.*): (.*)') + if key then + header_vars[key] = var + end + end + + if DYNAMIC_PAGES[path] ~= nil then + self.file_download(path) + return + end + + if path == MNT_PREFIX then + path = "/" + end + if startswith(path, MNT_PREFIX2) then + path = string.sub(path,#MNT_PREFIX2,#path) + end + + if isdirectory(path) and + not endswith(path,"/") and + header_vars['Host'] and + not is_hidden_dir(path) then + self.moved_permanently(path .. "/") + return + end + + if path ~= "/" and endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + + if startswith(path,"/@") then + path = string.sub(path, 2, #path) + end + + -- see if we have an index file + if isdirectory(path) and file_exists(path .. "/index.html") then + DEBUG(string.format("%u: found index.html", idx)) + if self.file_download(path .. "/index.html") then + return + end + end + + -- see if it is a directory + if (path == "/" or + DYNAMIC_PAGES[path] == nil) and + (endswith(path,"/") or + isdirectory(path) or + is_hidden_dir(path)) then + self.directory_list(path) + return + end + + -- or a file + if self.file_download(path) then + return + end + self.not_found(path) + end + + -- update the client + function self.update() + if run then + run() + return + end + if not have_header then + if not self.read_header() then + return + end + end + self.process_request() + if not run then + -- nothing more to do + self.remove() + end + end + + function self.remove() + DEBUG(string.format("%u: removing client OFFSET=%u", idx, offset)) + sock:close() + self.closed = true + end + + -- return the instance + return self +end + +--[[ + see if any new clients want to connect +--]] +local function check_new_clients() + while sock_listen:pollin(0) do + local sock = sock_listen:accept() + if not sock then + return + end + -- non-blocking for header read + sock:set_blocking(false) + -- find free client slot + for i = 1, #clients+1 do + if clients[i] == nil then + local idx = i + local client = Client(sock, idx) + DEBUG(string.format("%u: New client", idx)) + clients[idx] = client + end + end + end +end + +--[[ + check for client activity +--]] +local function check_clients() + for idx,client in ipairs(clients) do + if not client.closed then + client.update() + end + if client.closed then + table.remove(clients,idx) + end + end +end + +local function update() + check_new_clients() + check_clients() + return update,5 +end + +return update,100 diff --git a/libraries/AP_Scripting/applets/net_webserver.md b/libraries/AP_Scripting/applets/net_webserver.md new file mode 100644 index 0000000000000..fa45efe780b46 --- /dev/null +++ b/libraries/AP_Scripting/applets/net_webserver.md @@ -0,0 +1,91 @@ +# Web Server Application + +This implements a web server for boards that have networking support. + +# Parameters + +The web server has a small number of parameters + +## WEB_ENABLE + +This must be set to 1 to enable the web server + +## WEB_BIND_PORT + +This sets the network port to use for the server. It defaults to 8080 + +## WEB_DEBUG + +This enables verbose debugging + +## WEB_BLOCK_SIZE + +This sets the block size for network and file read/write +operations. Setting a larger value can increase performance at the +cost of more memory + +## WEB_TIMEOUT + +This sets the timeout in seconds for inactive client connections. + +# Operation + +By default the web server serves the root of your microSD card. You +can include html, javascript (*.js), image files etc on your microSD +to create a full web server with any structure you want. + +## Server Side Scripting + +The web server supports embedding lua script elements inside html +files for files with a filename of *.shtml. Here is an example: + +``` + + + + + +

Server Side Scripting Test

+ + + + + + + +
RollPitchYaw
+ + +``` +In this example we are using two forms of embedded lua scripts. The +first form starts with " 0 then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_server(%s): got input '%s'", name, r)) + end +end + +local function update() + test_echo("TCP", sock_tcp_echo) + test_echo("UDP", sock_udp_echo) + test_server("TCP", sock_tcp_in) + test_server("UDP", sock_udp_in) + counter = counter + 1 + return update,1000 +end + +return update,100 diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a37476a5252d9..3c0c37a7a936a 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -536,6 +536,23 @@ ap_object AP_HAL::I2CDevice method write_register boolean uint8_t'skip_check uin ap_object AP_HAL::I2CDevice manual read_registers AP_HAL__I2CDevice_read_registers 2 ap_object AP_HAL::I2CDevice method set_address void uint8_t'skip_check +include AP_HAL/utility/Socket.h depends (AP_NETWORKING_ENABLED==1) +global manual Socket lua_get_SocketAPM 1 depends (AP_NETWORKING_ENABLED==1) + +ap_object SocketAPM depends (AP_NETWORKING_ENABLED==1) +ap_object SocketAPM method connect boolean string uint16_t'skip_check +ap_object SocketAPM method bind boolean string uint16_t'skip_check +ap_object SocketAPM method send int32_t string uint32_t'skip_check +ap_object SocketAPM method listen boolean uint8_t'skip_check +ap_object SocketAPM method set_blocking boolean boolean +ap_object SocketAPM method is_connected boolean +ap_object SocketAPM method pollout boolean uint32_t'skip_check +ap_object SocketAPM method pollin boolean uint32_t'skip_check +ap_object SocketAPM method reuseaddress boolean +ap_object SocketAPM manual sendfile SocketAPM_sendfile 1 +ap_object SocketAPM manual close SocketAPM_close 0 +ap_object SocketAPM manual recv SocketAPM_recv 1 +ap_object SocketAPM manual accept SocketAPM_accept 1 ap_object AP_HAL::AnalogSource depends !defined(HAL_DISABLE_ADC_DRIVER) ap_object AP_HAL::AnalogSource method set_pin boolean uint8_t'skip_check @@ -814,3 +831,23 @@ singleton AC_Fence depends AP_FENCE_ENABLED singleton AC_Fence rename fence singleton AC_Fence method get_breaches uint8_t singleton AC_Fence method get_breach_time uint32_t + +include AP_Filesystem/AP_Filesystem.h depends AP_FILESYSTEM_FILE_READING_ENABLED +include AP_Filesystem/AP_Filesystem_config.h +userdata AP_Filesystem::stat_t depends AP_FILESYSTEM_FILE_READING_ENABLED +userdata AP_Filesystem::stat_t rename stat_t +userdata AP_Filesystem::stat_t field size uint32_t read +userdata AP_Filesystem::stat_t field mode int32_t read +userdata AP_Filesystem::stat_t field mtime uint32_t read +userdata AP_Filesystem::stat_t field atime uint32_t read +userdata AP_Filesystem::stat_t field ctime uint32_t read +userdata AP_Filesystem::stat_t method is_directory boolean + +singleton AP_Filesystem rename fs +singleton AP_Filesystem method stat boolean string AP_Filesystem::stat_t'Null + +include AP_RTC/AP_RTC.h depends AP_RTC_ENABLED +include AP_RTC/AP_RTC_config.h +singleton AP_RTC rename rtc +singleton AP_RTC method clock_s_to_date_fields boolean uint32_t'skip_check uint16_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null +singleton AP_RTC method date_fields_to_clock_s uint32_t uint16_t'skip_check int8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 1ff491ed74078..0059fb1150994 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -15,6 +15,12 @@ #include #include +#include +#if AP_NETWORKING_ENABLED +#include +#endif +#include "lua/src/lauxlib.h" + extern const AP_HAL::HAL& hal; extern "C" { @@ -755,6 +761,155 @@ int lua_get_PWMSource(lua_State *L) { return 1; } +#if AP_NETWORKING_ENABLED +/* + allocate a SocketAPM + */ +int lua_get_SocketAPM(lua_State *L) { + binding_argcheck(L, 1); + const uint8_t datagram = get_uint8_t(L, 1); + auto *scripting = AP::scripting(); + + if (scripting->num_net_sockets >= SCRIPTING_MAX_NUM_NET_SOCKET) { + return luaL_argerror(L, 1, "no sockets available"); + } + + auto *sock = new SocketAPM(datagram); + if (sock == nullptr) { + return luaL_argerror(L, 1, "SocketAPM device nullptr"); + } + scripting->_net_sockets[scripting->num_net_sockets] = sock; + + new_SocketAPM(L); + *((SocketAPM**)luaL_checkudata(L, -1, "SocketAPM")) = scripting->_net_sockets[scripting->num_net_sockets]; + + scripting->num_net_sockets++; + + return 1; +} + +/* + socket close + */ +int SocketAPM_close(lua_State *L) { + binding_argcheck(L, 1); + + SocketAPM *ud = *check_SocketAPM(L, 1); + + auto *scripting = AP::scripting(); + + if (scripting->num_net_sockets == 0) { + return luaL_argerror(L, 1, "socket close error"); + } + + // clear allocated socket + for (uint8_t i=0; i_net_sockets[i] == ud) { + ud->close(); + delete ud; + scripting->_net_sockets[i] = nullptr; + scripting->num_net_sockets--; + break; + } + } + + return 0; +} + +/* + socket sendfile, for offloading file send to AP_Networking + */ +int SocketAPM_sendfile(lua_State *L) { + binding_argcheck(L, 2); + + SocketAPM *ud = *check_SocketAPM(L, 1); + + auto *scripting = AP::scripting(); + + if (scripting->num_net_sockets == 0) { + return luaL_argerror(L, 1, "sendfile error"); + } + + auto *p = (luaL_Stream *)luaL_checkudata(L, 2, LUA_FILEHANDLE); + int fd = p->f->fd; + bool ret = false; + + // find the socket + for (uint8_t i=0; i_net_sockets[i] == ud) { + ret = AP::network().sendfile(ud, fd); + if (ret) { + // remove from scripting, leave socket and fd open + p->f->fd = -1; + scripting->_net_sockets[i] = nullptr; + scripting->num_net_sockets--; + } + break; + } + } + + lua_pushboolean(L, ret); + return 1; +} + +/* + receive from a socket to a lua string + */ +int SocketAPM_recv(lua_State *L) { + binding_argcheck(L, 2); + + SocketAPM * ud = *check_SocketAPM(L, 1); + + const uint16_t count = get_uint16_t(L, 2); + uint8_t *data = (uint8_t*)malloc(count); + if (data == nullptr) { + return 0; + } + + const auto ret = ud->recv(data, count, 0); + if (ret < 0) { + free(data); + return 0; + } + + // push to lua string + lua_pushlstring(L, (const char *)data, ret); + free(data); + + return 1; +} + +/* + TCP socket accept() call + */ +int SocketAPM_accept(lua_State *L) { + binding_argcheck(L, 1); + + SocketAPM * ud = *check_SocketAPM(L, 1); + + auto *scripting = AP::scripting(); + if (scripting->num_net_sockets >= SCRIPTING_MAX_NUM_NET_SOCKET) { + return luaL_argerror(L, 1, "no sockets available"); + } + + auto *sock = ud->accept(0); + if (sock == nullptr) { + return 0; + } + + scripting->_net_sockets[scripting->num_net_sockets] = sock; + + new_SocketAPM(L); + *((SocketAPM**)luaL_checkudata(L, -1, "SocketAPM")) = scripting->_net_sockets[scripting->num_net_sockets]; + + scripting->num_net_sockets++; + + return 1; +} + +#endif // AP_NETWORKING_ENABLED + + int lua_get_current_ref() { auto *scripting = AP::scripting(); diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 8eef08bca35ac..6bc459dc27bf2 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -14,6 +14,11 @@ int lua_dirlist(lua_State *L); int lua_removefile(lua_State *L); int SRV_Channels_get_safety_state(lua_State *L); int lua_get_PWMSource(lua_State *L); +int lua_get_SocketAPM(lua_State *L); +int SocketAPM_recv(lua_State *L); +int SocketAPM_accept(lua_State *L); +int SocketAPM_close(lua_State *L); +int SocketAPM_sendfile(lua_State *L); int lua_mavlink_init(lua_State *L); int lua_mavlink_receive_chan(lua_State *L); int lua_mavlink_register_rx_msgid(lua_State *L);