diff --git a/Makefile b/Makefile index bfa206309..2ab2c34f0 100644 --- a/Makefile +++ b/Makefile @@ -197,7 +197,7 @@ ifeq ($(filter $(TARGET),$(VALID_TARGETS)),) $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?) endif -ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),) +ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS) $(SITL_TARGETS)),) $(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411 or F7x5. Have you prepared a valid target.mk?) endif @@ -205,7 +205,7 @@ endif 256K_TARGETS = $(F3_TARGETS) 512K_TARGETS = $(F411_TARGETS) $(F446_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) 1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_TARGETS) -2048K_TARGETS = $(F7X5XI_TARGETS) +2048K_TARGETS = $(F7X5XI_TARGETS) $(SITL_TARGETS) # Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already. ifeq ($(FLASH_SIZE),) @@ -534,6 +534,26 @@ TARGET_FLAGS = -D$(TARGET) # End F7 targets # +# Start SITL targets +else ifeq ($(TARGET),$(filter $(TARGET), $(SITL_TARGETS))) + +INCLUDE_DIRS := $(INCLUDE_DIRS) \ + $(ROOT)/lib/main/dyad + +SITL_SRC := $(ROOT)/lib/main/dyad/dyad.c + +#Flags +ARCH_FLAGS = +DEVICE_FLAGS = +LD_SCRIPT = src/main/target/SITL/parameter_group.ld +STARTUP_SRC = + +TARGET_FLAGS = -D$(TARGET) + +ARM_SDK_PREFIX = + +# End SITL targets +# # Start F1 targets else @@ -939,6 +959,19 @@ F7EXCLUDES = drivers/bus_spi.c \ drivers/timer.c \ drivers/serial_uart.c +SITLEXCLUDES = \ + drivers/adc.c \ + drivers/bus_spi.c \ + drivers/bus_i2c.c \ + drivers/dma.c \ + drivers/pwm_output.c \ + drivers/timer.c \ + drivers/light_led.c \ + drivers/system.c \ + drivers/rcc.c \ + drivers/serial_uart.c \ + + # check if target.mk supplied ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) SRC := $(STARTUP_SRC) $(STM32F4xx_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC) @@ -948,6 +981,8 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS))) SRC := $(STARTUP_SRC) $(STM32F30x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC) else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC) +else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS))) +SRC := $(TARGET_SRC) $(SITL_SRC) $(VARIANT_SRC) endif ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),) @@ -982,6 +1017,11 @@ ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS))) SRC := $(filter-out ${F7EXCLUDES}, $(SRC)) endif +#SITL excludes +ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS))) +SRC := $(filter-out ${SITLEXCLUDES}, $(SRC)) +endif + ifneq ($(filter SDCARD,$(FEATURES)),) SRC += \ drivers/sdcard.c \ @@ -1037,6 +1077,13 @@ OPTIMISE_SIZE := -Os LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) +else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS))) +OPTIMISE_DEFAULT := -Ofast +OPTIMISE_SPEED := -Ofast +OPTIMISE_SIZE := -Os + +LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) + else OPTIMISE_DEFAULT := -Ofast @@ -1097,6 +1144,24 @@ LDFLAGS = -lm \ -Wl,--no-wchar-size-warning \ -T$(LD_SCRIPT) +#SITL compile +ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS))) +LDFLAGS = \ + -lm \ + -lpthread \ + -lc \ + -lrt \ + $(ARCH_FLAGS) \ + $(LTO_FLAGS) \ + $(DEBUG_FLAGS) \ + -static \ + -static-libgcc \ + -Wl,-gc-sections,-Map,$(TARGET_MAP) \ + -Wl,-L$(LINKER_DIR) \ + -Wl,--cref \ + -T$(LD_SCRIPT) +endif + ############################################################################### # No user-serviceable parts below ############################################################################### diff --git a/lib/main/dyad/LICENSE b/lib/main/dyad/LICENSE new file mode 100644 index 000000000..5818e8db0 --- /dev/null +++ b/lib/main/dyad/LICENSE @@ -0,0 +1,20 @@ +Copyright (c) 2016 rxi + + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/lib/main/dyad/README.md b/lib/main/dyad/README.md new file mode 100644 index 000000000..23902ae4c --- /dev/null +++ b/lib/main/dyad/README.md @@ -0,0 +1,85 @@ + +![dyad.c](https://cloud.githubusercontent.com/assets/3920290/3942261/5de470e4-255d-11e4-95a9-5f97fa9f3a57.png) + +## Overview +Dyad.c is an asynchronous networking library which aims to be lightweight, +portable and easy to use. It can be used both to create small standalone +servers and to provide network support to existing projects. + +## Getting started +The [dyad.c](src/dyad.c?raw=1) and [dyad.h](src/dyad.h?raw=1) files can be +dropped into an existing project; if you're using Windows you will also have to +link to `ws2_32`. + +An overview of the API can be found at [doc/api.md](doc/api.md). + +Usage examples can be found at [example/](example/). + +## Server example +A simple server which listens on port 8000 and echoes whatever is sent to it: +```c +#include +#include "dyad.h" + +static void onData(dyad_Event *e) { + dyad_write(e->stream, e->data, e->size); +} + +static void onAccept(dyad_Event *e) { + dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, NULL); + dyad_writef(e->remote, "Echo server\r\n"); +} + +int main(void) { + dyad_init(); + + dyad_Stream *serv = dyad_newStream(); + dyad_addListener(serv, DYAD_EVENT_ACCEPT, onAccept, NULL); + dyad_listen(serv, 8000); + + while (dyad_getStreamCount() > 0) { + dyad_update(); + } + + dyad_shutdown(); + return 0; +} +``` + +## Client example +A simple example program which connects to a +[daytime](http://en.wikipedia.org/wiki/Daytime_Protocol) server and prints the +response: +```c +#include +#include "dyad.h" + +static void onConnect(dyad_Event *e) { + printf("connected: %s\n", e->msg); +} + +static void onData(dyad_Event *e) { + printf("%s", e->data); +} + +int main(void) { + dyad_init(); + + dyad_Stream *s = dyad_newStream(); + dyad_addListener(s, DYAD_EVENT_CONNECT, onConnect, NULL); + dyad_addListener(s, DYAD_EVENT_DATA, onData, NULL); + dyad_connect(s, "time-nw.nist.gov", 13); + + while (dyad_getStreamCount() > 0) { + dyad_update(); + } + + dyad_shutdown(); + return 0; +} +``` + + +## License +This library is free software; you can redistribute it and/or modify it under +the terms of the MIT license. See [LICENSE](LICENSE) for details. diff --git a/lib/main/dyad/dyad.c b/lib/main/dyad/dyad.c new file mode 100644 index 000000000..b7bfd68e1 --- /dev/null +++ b/lib/main/dyad/dyad.c @@ -0,0 +1,1156 @@ +/** + * Copyright (c) 2016 rxi + * + * This library is free software; you can redistribute it and/or modify it + * under the terms of the MIT license. See LICENSE for details. + */ + +#ifdef _WIN32 + #define _WIN32_WINNT 0x501 + #ifndef _CRT_SECURE_NO_WARNINGS + #define _CRT_SECURE_NO_WARNINGS + #endif + #include + #include + #include +#else + #define _POSIX_C_SOURCE 200809L + #ifdef __APPLE__ + #define _DARWIN_UNLIMITED_SELECT + #endif + #include + #include + #include + #include + #include + #include + #include + #include + #include +#endif +#include +#include +#include +#include +#include +#include +#include + +#include "dyad.h" + +#define DYAD_VERSION "0.2.1" + + +#ifdef _WIN32 + #define close(a) closesocket(a) + #define getsockopt(a,b,c,d,e) getsockopt((a),(b),(c),(char*)(d),(e)) + #define setsockopt(a,b,c,d,e) setsockopt((a),(b),(c),(char*)(d),(e)) + #define select(a,b,c,d,e) select((int)(a),(b),(c),(d),(e)) + #define bind(a,b,c) bind((a),(b),(int)(c)) + #define connect(a,b,c) connect((a),(b),(int)(c)) + + #undef errno + #define errno WSAGetLastError() + + #undef EWOULDBLOCK + #define EWOULDBLOCK WSAEWOULDBLOCK + + const char *inet_ntop(int af, const void *src, char *dst, socklen_t size) { + union { struct sockaddr sa; struct sockaddr_in sai; + struct sockaddr_in6 sai6; } addr; + int res; + memset(&addr, 0, sizeof(addr)); + addr.sa.sa_family = af; + if (af == AF_INET6) { + memcpy(&addr.sai6.sin6_addr, src, sizeof(addr.sai6.sin6_addr)); + } else { + memcpy(&addr.sai.sin_addr, src, sizeof(addr.sai.sin_addr)); + } + res = WSAAddressToStringA(&addr.sa, sizeof(addr), 0, dst, (LPDWORD) &size); + if (res != 0) return NULL; + return dst; + } +#endif + +#ifndef INVALID_SOCKET + #define INVALID_SOCKET -1 +#endif + + +/*===========================================================================*/ +/* Memory */ +/*===========================================================================*/ + +static void panic(const char *fmt, ...); + +static void *dyad_realloc(void *ptr, int n) { + ptr = realloc(ptr, n); + if (!ptr && n != 0) { + panic("out of memory"); + } + return ptr; +} + + +static void dyad_free(void *ptr) { + free(ptr); +} + + +/*===========================================================================*/ +/* Vec (dynamic array) */ +/*===========================================================================*/ + +static void vec_expand(char **data, int *length, int *capacity, int memsz) { + if (*length + 1 > *capacity) { + if (*capacity == 0) { + *capacity = 1; + } else { + *capacity <<= 1; + } + *data = dyad_realloc(*data, *capacity * memsz); + } +} + +static void vec_splice( + char **data, int *length, int *capacity, int memsz, int start, int count +) { + (void) capacity; + memmove(*data + start * memsz, + *data + (start + count) * memsz, + (*length - start - count) * memsz); +} + + +#define Vec(T)\ + struct { T *data; int length, capacity; } + + +#define vec_unpack(v)\ + (char**)&(v)->data, &(v)->length, &(v)->capacity, sizeof(*(v)->data) + + +#define vec_init(v)\ + memset((v), 0, sizeof(*(v))) + + +#define vec_deinit(v)\ + dyad_free((v)->data) + + +#define vec_clear(v)\ + ((v)->length = 0) + + +#define vec_push(v, val)\ + ( vec_expand(vec_unpack(v)),\ + (v)->data[(v)->length++] = (val) ) + + +#define vec_splice(v, start, count)\ + ( vec_splice(vec_unpack(v), start, count),\ + (v)->length -= (count) ) + + + +/*===========================================================================*/ +/* SelectSet */ +/*===========================================================================*/ + +/* A wrapper around the three fd_sets used for select(). The fd_sets' allocated + * memory is automatically expanded to accommodate fds as they are added. + * + * On Windows fd_sets are implemented as arrays; the FD_xxx macros are not used + * by the wrapper and instead the fd_set struct is manipulated directly. The + * wrapper should perform better than the normal FD_xxx macros, given that we + * don't bother with the linear search which FD_SET would perform to check for + * duplicates. + * + * On non-Windows platforms the sets are assumed to be bit arrays. The FD_xxx + * macros are not used in case their implementation attempts to do bounds + * checking; instead we manipulate the fd_sets' bits directly. + */ + +enum { + SELECT_READ, + SELECT_WRITE, + SELECT_EXCEPT, + SELECT_MAX +}; + +typedef struct { + int capacity; + dyad_Socket maxfd; + fd_set *fds[SELECT_MAX]; +} SelectSet; + +#define DYAD_UNSIGNED_BIT (sizeof(unsigned) * CHAR_BIT) + + +static void select_deinit(SelectSet *s) { + int i; + for (i = 0; i < SELECT_MAX; i++) { + dyad_free(s->fds[i]); + s->fds[i] = NULL; + } + s->capacity = 0; +} + + +static void select_grow(SelectSet *s) { + int i; + int oldCapacity = s->capacity; + s->capacity = s->capacity ? s->capacity << 1 : 1; + for (i = 0; i < SELECT_MAX; i++) { + s->fds[i] = dyad_realloc(s->fds[i], s->capacity * sizeof(fd_set)); + memset(s->fds[i] + oldCapacity, 0, + (s->capacity - oldCapacity) * sizeof(fd_set)); + } +} + + +static void select_zero(SelectSet *s) { + int i; + if (s->capacity == 0) return; + s->maxfd = 0; + for (i = 0; i < SELECT_MAX; i++) { +#if _WIN32 + s->fds[i]->fd_count = 0; +#else + memset(s->fds[i], 0, s->capacity * sizeof(fd_set)); +#endif + } +} + + +static void select_add(SelectSet *s, int set, dyad_Socket fd) { +#ifdef _WIN32 + fd_set *f; + if (s->capacity == 0) select_grow(s); + while ((unsigned) (s->capacity * FD_SETSIZE) < s->fds[set]->fd_count + 1) { + select_grow(s); + } + f = s->fds[set]; + f->fd_array[f->fd_count++] = fd; +#else + unsigned *p; + while (s->capacity * FD_SETSIZE < fd) { + select_grow(s); + } + p = (unsigned*) s->fds[set]; + p[fd / DYAD_UNSIGNED_BIT] |= 1 << (fd % DYAD_UNSIGNED_BIT); + if (fd > s->maxfd) s->maxfd = fd; +#endif +} + + +static int select_has(SelectSet *s, int set, dyad_Socket fd) { +#ifdef _WIN32 + unsigned i; + fd_set *f; + if (s->capacity == 0) return 0; + f = s->fds[set]; + for (i = 0; i < f->fd_count; i++) { + if (f->fd_array[i] == fd) { + return 1; + } + } + return 0; +#else + unsigned *p; + if (s->maxfd < fd) return 0; + p = (unsigned*) s->fds[set]; + return p[fd / DYAD_UNSIGNED_BIT] & (1 << (fd % DYAD_UNSIGNED_BIT)); +#endif +} + + +/*===========================================================================*/ +/* Core */ +/*===========================================================================*/ + +typedef struct { + int event; + dyad_Callback callback; + void *udata; +} Listener; + + +struct dyad_Stream { + int state, flags; + dyad_Socket sockfd; + char *address; + int port; + int bytesSent, bytesReceived; + double lastActivity, timeout; + Vec(Listener) listeners; + Vec(char) lineBuffer; + Vec(char) writeBuffer; + dyad_Stream *next; +}; + +#define DYAD_FLAG_READY (1 << 0) +#define DYAD_FLAG_WRITTEN (1 << 1) + + +static dyad_Stream *dyad_streams; +static int dyad_streamCount; +static char dyad_panicMsgBuffer[128]; +static dyad_PanicCallback panicCallback; +static SelectSet dyad_selectSet; +static double dyad_updateTimeout = 1; +static double dyad_tickInterval = 1; +static double dyad_lastTick = 0; + + +static void panic(const char *fmt, ...) { + va_list args; + va_start(args, fmt); + vsprintf(dyad_panicMsgBuffer, fmt, args); + va_end(args); + if (panicCallback) { + panicCallback(dyad_panicMsgBuffer); + } else { + printf("dyad panic: %s\n", dyad_panicMsgBuffer); + } + exit(EXIT_FAILURE); +} + + +static dyad_Event createEvent(int type) { + dyad_Event e; + memset(&e, 0, sizeof(e)); + e.type = type; + return e; +} + + +static void stream_destroy(dyad_Stream *stream); + +static void destroyClosedStreams(void) { + dyad_Stream *stream = dyad_streams; + while (stream) { + if (stream->state == DYAD_STATE_CLOSED) { + dyad_Stream *next = stream->next; + stream_destroy(stream); + stream = next; + } else { + stream = stream->next; + } + } +} + + +static void stream_emitEvent(dyad_Stream *stream, dyad_Event *e); + +static void updateTickTimer(void) { + /* Update tick timer */ + if (dyad_lastTick == 0) { + dyad_lastTick = dyad_getTime(); + } + while (dyad_lastTick < dyad_getTime()) { + /* Emit event on all streams */ + dyad_Stream *stream; + dyad_Event e = createEvent(DYAD_EVENT_TICK); + e.msg = "a tick has occured"; + stream = dyad_streams; + while (stream) { + stream_emitEvent(stream, &e); + stream = stream->next; + } + dyad_lastTick += dyad_tickInterval; + } +} + + +static void updateStreamTimeouts(void) { + double currentTime = dyad_getTime(); + dyad_Stream *stream; + dyad_Event e = createEvent(DYAD_EVENT_TIMEOUT); + e.msg = "stream timed out"; + stream = dyad_streams; + while (stream) { + if (stream->timeout) { + if (currentTime - stream->lastActivity > stream->timeout) { + stream_emitEvent(stream, &e); + dyad_close(stream); + } + } + stream = stream->next; + } +} + + + +/*===========================================================================*/ +/* Stream */ +/*===========================================================================*/ + +static void stream_destroy(dyad_Stream *stream) { + dyad_Event e; + dyad_Stream **next; + /* Close socket */ + if (stream->sockfd != INVALID_SOCKET) { + close(stream->sockfd); + } + /* Emit destroy event */ + e = createEvent(DYAD_EVENT_DESTROY); + e.msg = "the stream has been destroyed"; + stream_emitEvent(stream, &e); + /* Remove from list and decrement count */ + next = &dyad_streams; + while (*next != stream) { + next = &(*next)->next; + } + *next = stream->next; + dyad_streamCount--; + /* Destroy and free */ + vec_deinit(&stream->listeners); + vec_deinit(&stream->lineBuffer); + vec_deinit(&stream->writeBuffer); + dyad_free(stream->address); + dyad_free(stream); +} + + +static void stream_emitEvent(dyad_Stream *stream, dyad_Event *e) { + int i; + e->stream = stream; + for (i = 0; i < stream->listeners.length; i++) { + Listener *listener = &stream->listeners.data[i]; + if (listener->event == e->type) { + e->udata = listener->udata; + listener->callback(e); + } + /* Check to see if this listener was removed: If it was we decrement `i` + * since the next listener will now be in this ones place */ + if (listener != &stream->listeners.data[i]) { + i--; + } + } +} + + +static void stream_error(dyad_Stream *stream, const char *msg, int err) { + char buf[256]; + dyad_Event e = createEvent(DYAD_EVENT_ERROR); + if (err) { + sprintf(buf, "%.160s (%.80s)", msg, strerror(err)); + e.msg = buf; + } else { + e.msg = msg; + } + stream_emitEvent(stream, &e); + dyad_close(stream); +} + + +static void stream_initAddress(dyad_Stream *stream) { + union { struct sockaddr sa; struct sockaddr_storage sas; + struct sockaddr_in sai; struct sockaddr_in6 sai6; } addr; + socklen_t size; + memset(&addr, 0, sizeof(addr)); + size = sizeof(addr); + dyad_free(stream->address); + stream->address = NULL; + if (getpeername(stream->sockfd, &addr.sa, &size) == -1) { + if (getsockname(stream->sockfd, &addr.sa, &size) == -1) { + return; + } + } + if (addr.sas.ss_family == AF_INET6) { + stream->address = dyad_realloc(NULL, INET6_ADDRSTRLEN); + inet_ntop(AF_INET6, &addr.sai6.sin6_addr, stream->address, + INET6_ADDRSTRLEN); + stream->port = ntohs(addr.sai6.sin6_port); + } else { + stream->address = dyad_realloc(NULL, INET_ADDRSTRLEN); + inet_ntop(AF_INET, &addr.sai.sin_addr, stream->address, INET_ADDRSTRLEN); + stream->port = ntohs(addr.sai.sin_port); + } +} + + +static void stream_setSocketNonBlocking(dyad_Stream *stream, int opt) { +#ifdef _WIN32 + u_long mode = opt; + ioctlsocket(stream->sockfd, FIONBIO, &mode); +#else + int flags = fcntl(stream->sockfd, F_GETFL); + fcntl(stream->sockfd, F_SETFL, + opt ? (flags | O_NONBLOCK) : (flags & ~O_NONBLOCK)); +#endif +} + + +static void stream_setSocket(dyad_Stream *stream, dyad_Socket sockfd) { + stream->sockfd = sockfd; + stream_setSocketNonBlocking(stream, 1); + stream_initAddress(stream); +} + + +static int stream_initSocket( + dyad_Stream *stream, int domain, int type, int protocol +) { + stream->sockfd = socket(domain, type, protocol); + if (stream->sockfd == INVALID_SOCKET) { + stream_error(stream, "could not create socket", errno); + return -1; + } + stream_setSocket(stream, stream->sockfd); + return 0; +} + + +static int stream_hasListenerForEvent(dyad_Stream *stream, int event) { + int i; + for (i = 0; i < stream->listeners.length; i++) { + Listener *listener = &stream->listeners.data[i]; + if (listener->event == event) { + return 1; + } + } + return 0; +} + + +static void stream_handleReceivedData(dyad_Stream *stream) { + for (;;) { + /* Receive data */ + dyad_Event e; + char data[8192]; + int size = recv(stream->sockfd, data, sizeof(data) - 1, 0); + if (size <= 0) { + if (size == 0 || errno != EWOULDBLOCK) { + /* Handle disconnect */ + dyad_close(stream); + return; + } else { + /* No more data */ + return; + } + } + data[size] = 0; + /* Update status */ + stream->bytesReceived += size; + stream->lastActivity = dyad_getTime(); + /* Emit data event */ + e = createEvent(DYAD_EVENT_DATA); + e.msg = "received data"; + e.data = data; + e.size = size; + stream_emitEvent(stream, &e); + /* Check stream state in case it was closed during one of the data event + * handlers. */ + if (stream->state != DYAD_STATE_CONNECTED) { + return; + } + + /* Handle line event */ + if (stream_hasListenerForEvent(stream, DYAD_EVENT_LINE)) { + int i, start; + char *buf; + for (i = 0; i < size; i++) { + vec_push(&stream->lineBuffer, data[i]); + } + start = 0; + buf = stream->lineBuffer.data; + for (i = 0; i < stream->lineBuffer.length; i++) { + if (buf[i] == '\n') { + dyad_Event e; + buf[i] = '\0'; + e = createEvent(DYAD_EVENT_LINE); + e.msg = "received line"; + e.data = &buf[start]; + e.size = i - start; + /* Check and strip carriage return */ + if (e.size > 0 && e.data[e.size - 1] == '\r') { + e.data[--e.size] = '\0'; + } + stream_emitEvent(stream, &e); + start = i + 1; + /* Check stream state in case it was closed during one of the line + * event handlers. */ + if (stream->state != DYAD_STATE_CONNECTED) { + return; + } + } + } + if (start == stream->lineBuffer.length) { + vec_clear(&stream->lineBuffer); + } else { + vec_splice(&stream->lineBuffer, 0, start); + } + } + } +} + + +static void stream_acceptPendingConnections(dyad_Stream *stream) { + for (;;) { + dyad_Stream *remote; + dyad_Event e; + int err = 0; + dyad_Socket sockfd = accept(stream->sockfd, NULL, NULL); + if (sockfd == INVALID_SOCKET) { + err = errno; + if (err == EWOULDBLOCK) { + /* No more waiting sockets */ + return; + } + } + /* Create client stream */ + remote = dyad_newStream(); + remote->state = DYAD_STATE_CONNECTED; + /* Set stream's socket */ + stream_setSocket(remote, sockfd); + /* Emit accept event */ + e = createEvent(DYAD_EVENT_ACCEPT); + e.msg = "accepted connection"; + e.remote = remote; + stream_emitEvent(stream, &e); + /* Handle invalid socket -- the stream is still made and the ACCEPT event + * is still emitted, but its shut immediately with an error */ + if (remote->sockfd == INVALID_SOCKET) { + stream_error(remote, "failed to create socket on accept", err); + return; + } + } +} + + +static int stream_flushWriteBuffer(dyad_Stream *stream) { + stream->flags &= ~DYAD_FLAG_WRITTEN; + if (stream->writeBuffer.length > 0) { + /* Send data */ + int size = send(stream->sockfd, stream->writeBuffer.data, + stream->writeBuffer.length, 0); + if (size <= 0) { + if (errno == EWOULDBLOCK) { + /* No more data can be written */ + return 0; + } else { + /* Handle disconnect */ + dyad_close(stream); + return 0; + } + } + if (size == stream->writeBuffer.length) { + vec_clear(&stream->writeBuffer); + } else { + vec_splice(&stream->writeBuffer, 0, size); + } + /* Update status */ + stream->bytesSent += size; + stream->lastActivity = dyad_getTime(); + } + + if (stream->writeBuffer.length == 0) { + dyad_Event e; + /* If this is a 'closing' stream we can properly close it now */ + if (stream->state == DYAD_STATE_CLOSING) { + dyad_close(stream); + return 0; + } + /* Set ready flag and emit 'ready for data' event */ + stream->flags |= DYAD_FLAG_READY; + e = createEvent(DYAD_EVENT_READY); + e.msg = "stream is ready for more data"; + stream_emitEvent(stream, &e); + } + /* Return 1 to indicate that more data can immediately be written to the + * stream's socket */ + return 1; +} + + + +/*===========================================================================*/ +/* API */ +/*===========================================================================*/ + +/*---------------------------------------------------------------------------*/ +/* Core */ +/*---------------------------------------------------------------------------*/ + +void dyad_update(void) { + dyad_Stream *stream; + struct timeval tv; + + destroyClosedStreams(); + updateTickTimer(); + updateStreamTimeouts(); + + /* Create fd sets for select() */ + select_zero(&dyad_selectSet); + + stream = dyad_streams; + while (stream) { + switch (stream->state) { + case DYAD_STATE_CONNECTED: + select_add(&dyad_selectSet, SELECT_READ, stream->sockfd); + if (!(stream->flags & DYAD_FLAG_READY) || + stream->writeBuffer.length != 0 + ) { + select_add(&dyad_selectSet, SELECT_WRITE, stream->sockfd); + } + break; + case DYAD_STATE_CLOSING: + select_add(&dyad_selectSet, SELECT_WRITE, stream->sockfd); + break; + case DYAD_STATE_CONNECTING: + select_add(&dyad_selectSet, SELECT_WRITE, stream->sockfd); + select_add(&dyad_selectSet, SELECT_EXCEPT, stream->sockfd); + break; + case DYAD_STATE_LISTENING: + select_add(&dyad_selectSet, SELECT_READ, stream->sockfd); + break; + } + stream = stream->next; + } + + /* Init timeout value and do select */ + #ifdef _MSC_VER + #pragma warning(push) + /* Disable double to long implicit conversion warning, + * because the type of timeval's fields don't agree across platforms */ + #pragma warning(disable: 4244) + #endif + tv.tv_sec = dyad_updateTimeout; + tv.tv_usec = (dyad_updateTimeout - tv.tv_sec) * 1e6; + #ifdef _MSC_VER + #pragma warning(pop) + #endif + + select(dyad_selectSet.maxfd + 1, + dyad_selectSet.fds[SELECT_READ], + dyad_selectSet.fds[SELECT_WRITE], + dyad_selectSet.fds[SELECT_EXCEPT], + &tv); + + /* Handle streams */ + stream = dyad_streams; + while (stream) { + switch (stream->state) { + + case DYAD_STATE_CONNECTED: + if (select_has(&dyad_selectSet, SELECT_READ, stream->sockfd)) { + stream_handleReceivedData(stream); + if (stream->state == DYAD_STATE_CLOSED) { + break; + } + } + /* Fall through */ + + case DYAD_STATE_CLOSING: + if (select_has(&dyad_selectSet, SELECT_WRITE, stream->sockfd)) { + stream_flushWriteBuffer(stream); + } + break; + + case DYAD_STATE_CONNECTING: + if (select_has(&dyad_selectSet, SELECT_WRITE, stream->sockfd)) { + /* Check socket for error */ + int optval = 0; + socklen_t optlen = sizeof(optval); + dyad_Event e; + getsockopt(stream->sockfd, SOL_SOCKET, SO_ERROR, &optval, &optlen); + if (optval != 0) goto connectFailed; + /* Handle succeselful connection */ + stream->state = DYAD_STATE_CONNECTED; + stream->lastActivity = dyad_getTime(); + stream_initAddress(stream); + /* Emit connect event */ + e = createEvent(DYAD_EVENT_CONNECT); + e.msg = "connected to server"; + stream_emitEvent(stream, &e); + } else if ( + select_has(&dyad_selectSet, SELECT_EXCEPT, stream->sockfd) + ) { + /* Handle failed connection */ +connectFailed: + stream_error(stream, "could not connect to server", 0); + } + break; + + case DYAD_STATE_LISTENING: + if (select_has(&dyad_selectSet, SELECT_READ, stream->sockfd)) { + stream_acceptPendingConnections(stream); + } + break; + } + + /* If data was just now written to the stream we should immediately try to + * send it */ + if ( + stream->flags & DYAD_FLAG_WRITTEN && + stream->state != DYAD_STATE_CLOSED + ) { + stream_flushWriteBuffer(stream); + } + + stream = stream->next; + } +} + + +void dyad_init(void) { +#ifdef _WIN32 + WSADATA dat; + int err = WSAStartup(MAKEWORD(2, 2), &dat); + if (err != 0) { + panic("WSAStartup failed (%d)", err); + } +#else + /* Stops the SIGPIPE signal being raised when writing to a closed socket */ + signal(SIGPIPE, SIG_IGN); +#endif +} + + +void dyad_shutdown(void) { + /* Close and destroy all the streams */ + while (dyad_streams) { + dyad_close(dyad_streams); + stream_destroy(dyad_streams); + } + /* Clear up everything */ + select_deinit(&dyad_selectSet); +#ifdef _WIN32 + WSACleanup(); +#endif +} + + +const char *dyad_getVersion(void) { + return DYAD_VERSION; +} + + +double dyad_getTime(void) { +#ifdef _WIN32 + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + return (ft.dwHighDateTime * 4294967296.0 / 1e7) + ft.dwLowDateTime / 1e7; +#else + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec + tv.tv_usec / 1e6; +#endif +} + + +int dyad_getStreamCount(void) { + return dyad_streamCount; +} + + +void dyad_setTickInterval(double seconds) { + dyad_tickInterval = seconds; +} + + +void dyad_setUpdateTimeout(double seconds) { + dyad_updateTimeout = seconds; +} + + +dyad_PanicCallback dyad_atPanic(dyad_PanicCallback func) { + dyad_PanicCallback old = panicCallback; + panicCallback = func; + return old; +} + + +/*---------------------------------------------------------------------------*/ +/* Stream */ +/*---------------------------------------------------------------------------*/ + +dyad_Stream *dyad_newStream(void) { + dyad_Stream *stream = dyad_realloc(NULL, sizeof(*stream)); + memset(stream, 0, sizeof(*stream)); + stream->state = DYAD_STATE_CLOSED; + stream->sockfd = INVALID_SOCKET; + stream->lastActivity = dyad_getTime(); + /* Add to list and increment count */ + stream->next = dyad_streams; + dyad_streams = stream; + dyad_streamCount++; + return stream; +} + + +void dyad_addListener( + dyad_Stream *stream, int event, dyad_Callback callback, void *udata +) { + Listener listener; + listener.event = event; + listener.callback = callback; + listener.udata = udata; + vec_push(&stream->listeners, listener); +} + + +void dyad_removeListener( + dyad_Stream *stream, int event, dyad_Callback callback, void *udata +) { + int i = stream->listeners.length; + while (i--) { + Listener *x = &stream->listeners.data[i]; + if (x->event == event && x->callback == callback && x->udata == udata) { + vec_splice(&stream->listeners, i, 1); + } + } +} + + +void dyad_removeAllListeners(dyad_Stream *stream, int event) { + if (event == DYAD_EVENT_NULL) { + vec_clear(&stream->listeners); + } else { + int i = stream->listeners.length; + while (i--) { + if (stream->listeners.data[i].event == event) { + vec_splice(&stream->listeners, i, 1); + } + } + } +} + + +void dyad_close(dyad_Stream *stream) { + dyad_Event e; + if (stream->state == DYAD_STATE_CLOSED) return; + stream->state = DYAD_STATE_CLOSED; + /* Close socket */ + if (stream->sockfd != INVALID_SOCKET) { + close(stream->sockfd); + stream->sockfd = INVALID_SOCKET; + } + /* Emit event */ + e = createEvent(DYAD_EVENT_CLOSE); + e.msg = "stream closed"; + stream_emitEvent(stream, &e); + /* Clear buffers */ + vec_clear(&stream->lineBuffer); + vec_clear(&stream->writeBuffer); +} + + +void dyad_end(dyad_Stream *stream) { + if (stream->state == DYAD_STATE_CLOSED) return; + if (stream->writeBuffer.length > 0) { + stream->state = DYAD_STATE_CLOSING; + } else { + dyad_close(stream); + } +} + + +int dyad_listenEx( + dyad_Stream *stream, const char *host, int port, int backlog +) { + struct addrinfo hints, *ai = NULL; + int err, optval; + char buf[64]; + dyad_Event e; + + /* Get addrinfo */ + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + sprintf(buf, "%d", port); + err = getaddrinfo(host, buf, &hints, &ai); + if (err) { + stream_error(stream, "could not get addrinfo", errno); + goto fail; + } + /* Init socket */ + err = stream_initSocket(stream, ai->ai_family, ai->ai_socktype, + ai->ai_protocol); + if (err) goto fail; + /* Set SO_REUSEADDR so that the socket can be immediately bound without + * having to wait for any closed socket on the same port to timeout */ + optval = 1; + setsockopt(stream->sockfd, SOL_SOCKET, SO_REUSEADDR, + &optval, sizeof(optval)); + /* Bind and listen */ + err = bind(stream->sockfd, ai->ai_addr, ai->ai_addrlen); + if (err) { + stream_error(stream, "could not bind socket", errno); + goto fail; + } + err = listen(stream->sockfd, backlog); + if (err) { + stream_error(stream, "socket failed on listen", errno); + goto fail; + } + stream->state = DYAD_STATE_LISTENING; + stream->port = port; + stream_initAddress(stream); + /* Emit listening event */ + e = createEvent(DYAD_EVENT_LISTEN); + e.msg = "socket is listening"; + stream_emitEvent(stream, &e); + freeaddrinfo(ai); + return 0; + fail: + if (ai) freeaddrinfo(ai); + return -1; +} + + +int dyad_listen(dyad_Stream *stream, int port) { + return dyad_listenEx(stream, NULL, port, 511); +} + + +int dyad_connect(dyad_Stream *stream, const char *host, int port) { + struct addrinfo hints, *ai = NULL; + int err; + char buf[64]; + + /* Resolve host */ + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + sprintf(buf, "%d", port); + err = getaddrinfo(host, buf, &hints, &ai); + if (err) { + stream_error(stream, "could not resolve host", 0); + goto fail; + } + /* Start connecting */ + err = stream_initSocket(stream, ai->ai_family, ai->ai_socktype, + ai->ai_protocol); + if (err) goto fail; + connect(stream->sockfd, ai->ai_addr, ai->ai_addrlen); + stream->state = DYAD_STATE_CONNECTING; + freeaddrinfo(ai); + return 0; +fail: + if (ai) freeaddrinfo(ai); + return -1; +} + + +void dyad_write(dyad_Stream *stream, const void *data, int size) { + const char *p = data; + while (size--) { + vec_push(&stream->writeBuffer, *p++); + } + stream->flags |= DYAD_FLAG_WRITTEN; +} + + +void dyad_vwritef(dyad_Stream *stream, const char *fmt, va_list args) { + char buf[512]; + char *str; + char f[] = "%_"; + FILE *fp; + int c; + while (*fmt) { + if (*fmt == '%') { + fmt++; + switch (*fmt) { + case 'r': + fp = va_arg(args, FILE*); + if (fp == NULL) { + str = "(null)"; + goto writeStr; + } + while ((c = fgetc(fp)) != EOF) { + vec_push(&stream->writeBuffer, c); + } + break; + case 'c': + vec_push(&stream->writeBuffer, va_arg(args, int)); + break; + case 's': + str = va_arg(args, char*); + if (str == NULL) str = "(null)"; + writeStr: + while (*str) { + vec_push(&stream->writeBuffer, *str++); + } + break; + case 'b': + str = va_arg(args, char*); + c = va_arg(args, int); + while (c--) { + vec_push(&stream->writeBuffer, *str++); + } + break; + default: + f[1] = *fmt; + switch (*fmt) { + case 'f': + case 'g': sprintf(buf, f, va_arg(args, double)); break; + case 'd': + case 'i': sprintf(buf, f, va_arg(args, int)); break; + case 'x': + case 'X': sprintf(buf, f, va_arg(args, unsigned)); break; + case 'p': sprintf(buf, f, va_arg(args, void*)); break; + default : buf[0] = *fmt; buf[1] = '\0'; + } + str = buf; + goto writeStr; + } + } else { + vec_push(&stream->writeBuffer, *fmt); + } + fmt++; + } + stream->flags |= DYAD_FLAG_WRITTEN; +} + + +void dyad_writef(dyad_Stream *stream, const char *fmt, ...) { + va_list args; + va_start(args, fmt); + dyad_vwritef(stream, fmt, args); + va_end(args); +} + + +void dyad_setTimeout(dyad_Stream *stream, double seconds) { + stream->timeout = seconds; +} + + +void dyad_setNoDelay(dyad_Stream *stream, int opt) { + opt = !!opt; + setsockopt(stream->sockfd, IPPROTO_TCP, TCP_NODELAY, &opt, sizeof(opt)); +} + + +int dyad_getState(dyad_Stream *stream) { + return stream->state; +} + + +const char *dyad_getAddress(dyad_Stream *stream) { + return stream->address ? stream->address : ""; +} + + +int dyad_getPort(dyad_Stream *stream) { + return stream->port; +} + + +int dyad_getBytesSent(dyad_Stream *stream) { + return stream->bytesSent; +} + + +int dyad_getBytesReceived(dyad_Stream *stream) { + return stream->bytesReceived; +} + + +dyad_Socket dyad_getSocket(dyad_Stream *stream) { + return stream->sockfd; +} diff --git a/lib/main/dyad/dyad.h b/lib/main/dyad/dyad.h new file mode 100644 index 000000000..0f29a798d --- /dev/null +++ b/lib/main/dyad/dyad.h @@ -0,0 +1,106 @@ +/** + * Copyright (c) 2016 rxi + * + * This library is free software; you can redistribute it and/or modify it + * under the terms of the MIT license. See LICENSE for details. + */ + +#ifndef DYAD_H +#define DYAD_H + +#include + +#ifdef _WIN32 + #include /* For SOCKET */ +#endif + + +#ifdef __cplusplus +extern "C" { +#endif + +#if _WIN32 +typedef SOCKET dyad_Socket; +#else +typedef int dyad_Socket; +#endif + +struct dyad_Stream; +typedef struct dyad_Stream dyad_Stream; + +typedef struct { + int type; + void *udata; + dyad_Stream *stream; + dyad_Stream *remote; + const char *msg; + char *data; + int size; +} dyad_Event; + +typedef void (*dyad_Callback)(dyad_Event*); +typedef void (*dyad_PanicCallback)(const char*); + +enum { + DYAD_EVENT_NULL, + DYAD_EVENT_DESTROY, + DYAD_EVENT_ACCEPT, + DYAD_EVENT_LISTEN, + DYAD_EVENT_CONNECT, + DYAD_EVENT_CLOSE, + DYAD_EVENT_READY, + DYAD_EVENT_DATA, + DYAD_EVENT_LINE, + DYAD_EVENT_ERROR, + DYAD_EVENT_TIMEOUT, + DYAD_EVENT_TICK +}; + +enum { + DYAD_STATE_CLOSED, + DYAD_STATE_CLOSING, + DYAD_STATE_CONNECTING, + DYAD_STATE_CONNECTED, + DYAD_STATE_LISTENING +}; + + +void dyad_init(void); +void dyad_update(void); +void dyad_shutdown(void); +const char *dyad_getVersion(void); +double dyad_getTime(void); +int dyad_getStreamCount(void); +void dyad_setTickInterval(double seconds); +void dyad_setUpdateTimeout(double seconds); +dyad_PanicCallback dyad_atPanic(dyad_PanicCallback func); + +dyad_Stream *dyad_newStream(void); +int dyad_listen(dyad_Stream *stream, int port); +int dyad_listenEx(dyad_Stream *stream, const char *host, int port, + int backlog); +int dyad_connect(dyad_Stream *stream, const char *host, int port); +void dyad_addListener(dyad_Stream *stream, int event, + dyad_Callback callback, void *udata); +void dyad_removeListener(dyad_Stream *stream, int event, + dyad_Callback callback, void *udata); +void dyad_removeAllListeners(dyad_Stream *stream, int event); +void dyad_end(dyad_Stream *stream); +void dyad_close(dyad_Stream *stream); +void dyad_write(dyad_Stream *stream, const void *data, int size); +void dyad_vwritef(dyad_Stream *stream, const char *fmt, va_list args); +void dyad_writef(dyad_Stream *stream, const char *fmt, ...); +void dyad_setTimeout(dyad_Stream *stream, double seconds); +void dyad_setNoDelay(dyad_Stream *stream, int opt); +int dyad_getState(dyad_Stream *stream); +const char *dyad_getAddress(dyad_Stream *stream); +int dyad_getPort(dyad_Stream *stream); +int dyad_getBytesSent(dyad_Stream *stream); +int dyad_getBytesReceived(dyad_Stream *stream); +dyad_Socket dyad_getSocket(dyad_Stream *stream); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/src/main/config/config_streamer.c b/src/main/config/config_streamer.c index 114e70616..a22a3cd06 100644 --- a/src/main/config/config_streamer.c +++ b/src/main/config/config_streamer.c @@ -53,6 +53,8 @@ extern uint8_t __config_end; # define FLASH_PAGE_SIZE ((uint32_t)0x8000) # elif defined(UNIT_TEST) # define FLASH_PAGE_SIZE (0x400) +# elif defined(SIMULATOR_BUILD) +# define FLASH_PAGE_SIZE (0x400) # else # error "Flash page size not defined for target." # endif @@ -87,6 +89,8 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size) // NOP #elif defined(UNIT_TEST) // NOP +#elif defined(SIMULATOR_BUILD) + // NOP #else # error "Unsupported CPU" #endif diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index f1f687833..bbabedcc3 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -40,7 +40,9 @@ typedef enum I2CDevice { } I2CDevice; typedef struct i2cDevice_s { +#if !defined(SIMULATOR_BUILD) I2C_TypeDef *dev; +#endif ioTag_t scl; ioTag_t sda; rccPeriphTag_t rcc; diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 219d97ddf..6718634fe 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -145,6 +145,8 @@ uint32_t IO_EXTI_Line(IO_t io) return 1 << IO_GPIOPinIdx(io); #elif defined(STM32F7) return 1 << IO_GPIOPinIdx(io); +#elif defined(SIMULATOR_BUILD) + return 1; #else # error "Unknown target type" #endif diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 4d17b9d2a..d6164febc 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -88,6 +88,16 @@ # define IOCFG_IPU 0 # define IOCFG_IN_FLOATING 0 +#elif defined(SIMULATOR_BUILD) + +# define IOCFG_OUT_PP 0 +# define IOCFG_OUT_OD 0 +# define IOCFG_AF_PP 0 +# define IOCFG_AF_OD 0 +# define IOCFG_IPD 0 +# define IOCFG_IPU 0 +# define IOCFG_IN_FLOATING 0 + #else # warning "Unknown TARGET" #endif diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c new file mode 100644 index 000000000..93c1657a2 --- /dev/null +++ b/src/main/drivers/serial_tcp.c @@ -0,0 +1,268 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/* + * Authors: + * Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups. + * Hamasaki/Timecop - Initial baseflight code +*/ +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "common/utils.h" + +#include "io/serial.h" +#include "serial_tcp.h" + +#define BASE_PORT 5760 + +const struct serialPortVTable uartVTable[]; // Forward +static uartPort_t tcpSerialPorts[SERIAL_PORT_COUNT]; +static bool portInited[SERIAL_PORT_COUNT]; +static bool tcpStart = false; +bool tcpIsStart(void) { + return tcpStart; +} +static void onData(dyad_Event *e) { + uartPort_t* s = (uartPort_t*)(e->udata); + tcpDataIn(s, (uint8_t*)e->data, e->size); +} +static void onClose(dyad_Event *e) { + uartPort_t* s = (uartPort_t*)(e->udata); + s->clientCount--; + s->conn = NULL; + fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); + if(s->clientCount == 0) { + s->connected = false; + } +} +static void onAccept(dyad_Event *e) { + uartPort_t* s = (uartPort_t*)(e->udata); + fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount); + + s->connected = true; + if(s->clientCount > 0) { + dyad_close(e->remote); + return; + } + s->clientCount++; + fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount); + s->conn = e->remote; + dyad_setNoDelay(e->remote, 1); + dyad_setTimeout(e->remote, 120); + dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata); + dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata); +} +static uartPort_t* tcpReconfigure(uartPort_t *s, int id) +{ + if(portInited[id]) { + fprintf(stderr, "port had initialed!!\n"); + return s; + } + + if (pthread_mutex_init(&s->txLock, NULL) != 0) { + fprintf(stderr, "TX mutex init failed - %d\n", errno); + // TODO: clean up & re-init + return NULL; + } + if (pthread_mutex_init(&s->rxLock, NULL) != 0) { + fprintf(stderr, "RX mutex init failed - %d\n", errno); + // TODO: clean up & re-init + return NULL; + } + + tcpStart = true; + portInited[id] = true; + + s->connected = false; + s->clientCount = 0; + s->id = id; + s->conn = NULL; + s->serv = dyad_newStream(); + dyad_setNoDelay(s->serv, 1); + dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s); + + if(dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) { + fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id+1); + } else { + fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id+1); + } + return s; +} + +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + uartPort_t *s = NULL; + +#if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8) + uintptr_t id = ((uintptr_t)USARTx - 1); + s = tcpReconfigure(&tcpSerialPorts[id], id); +#else + return (serialPort_t *)s; +#endif + + s->port.vTable = uartVTable; + + // common serial initialisation code should move to serialPort::init() + s->port.rxBufferHead = s->port.rxBufferTail = 0; + s->port.txBufferHead = s->port.txBufferTail = 0; + s->port.rxBufferSize = RX_BUFFER_SIZE; + s->port.txBufferSize = TX_BUFFER_SIZE; + s->port.rxBuffer = s->rxBuffer; + s->port.txBuffer = s->txBuffer; + + // callback works for IRQ-based RX ONLY + s->port.rxCallback = rxCallback; + s->port.mode = mode; + s->port.baudRate = baudRate; + s->port.options = options; + + return (serialPort_t *)s; +} + +uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance) +{ + uartPort_t *s = (uartPort_t*)instance; + uint32_t count; + pthread_mutex_lock(&s->rxLock); + if (s->port.rxBufferHead >= s->port.rxBufferTail) { + count = s->port.rxBufferHead - s->port.rxBufferTail; + } else { + count = s->port.rxBufferSize + s->port.rxBufferHead - s->port.rxBufferTail; + } + pthread_mutex_unlock(&s->rxLock); + + return count; +} + +uint32_t tcpTotalTxBytesFree(const serialPort_t *instance) +{ + uartPort_t *s = (uartPort_t*)instance; + uint32_t bytesUsed; + + pthread_mutex_lock(&s->txLock); + if (s->port.txBufferHead >= s->port.txBufferTail) { + bytesUsed = s->port.txBufferHead - s->port.txBufferTail; + } else { + bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail; + } + bytesUsed = (s->port.txBufferSize - 1) - bytesUsed; + pthread_mutex_unlock(&s->txLock); + + return bytesUsed; +} + +bool isTcpTransmitBufferEmpty(const serialPort_t *instance) +{ + uartPort_t *s = (uartPort_t *)instance; + pthread_mutex_lock(&s->txLock); + bool isEmpty = s->port.txBufferTail == s->port.txBufferHead; + pthread_mutex_unlock(&s->txLock); + return isEmpty; +} + +uint8_t tcpRead(serialPort_t *instance) +{ + uint8_t ch; + uartPort_t *s = (uartPort_t *)instance; + pthread_mutex_lock(&s->rxLock); + + ch = s->port.rxBuffer[s->port.rxBufferTail]; + if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) { + s->port.rxBufferTail = 0; + } else { + s->port.rxBufferTail++; + } + pthread_mutex_unlock(&s->rxLock); + + return ch; +} + +void tcpWrite(serialPort_t *instance, uint8_t ch) +{ + uartPort_t *s = (uartPort_t *)instance; + pthread_mutex_lock(&s->txLock); + + s->port.txBuffer[s->port.txBufferHead] = ch; + if (s->port.txBufferHead + 1 >= s->port.txBufferSize) { + s->port.txBufferHead = 0; + } else { + s->port.txBufferHead++; + } + pthread_mutex_unlock(&s->txLock); + + tcpDataOut(s); +} + +void tcpDataOut(uartPort_t *instance) +{ + uint32_t bytesUsed; + uartPort_t *s = (uartPort_t *)instance; + if(s->conn == NULL) return; + pthread_mutex_lock(&s->txLock); + + if (s->port.txBufferHead < s->port.txBufferTail) { + bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail; + dyad_write(s->conn, (const void *)(s->port.txBuffer + s->port.txBufferTail), s->port.txBufferSize - s->port.txBufferTail); + s->port.txBufferTail = 0; + } + bytesUsed = s->port.txBufferHead - s->port.txBufferTail; + dyad_write(s->conn, (const void *)(&(s->port.txBuffer[s->port.txBufferTail])), bytesUsed); + s->port.txBufferTail = s->port.txBufferHead; + + pthread_mutex_unlock(&s->txLock); +} + +void tcpDataIn(uartPort_t *instance, uint8_t* ch, int size) +{ + uartPort_t *s = (uartPort_t *)instance; + pthread_mutex_lock(&s->rxLock); + + while(size--) { +// printf("%c", *ch); + s->port.rxBuffer[s->port.rxBufferHead] = *(ch++); + if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) { + s->port.rxBufferHead = 0; + } else { + s->port.rxBufferHead++; + } + } + pthread_mutex_unlock(&s->rxLock); +// printf("\n"); +} + +const struct serialPortVTable uartVTable[] = { + { + .serialWrite = tcpWrite, + .serialTotalRxWaiting = tcpTotalRxBytesWaiting, + .serialTotalTxFree = tcpTotalTxBytesFree, + .serialRead = tcpRead, + .serialSetBaudRate = NULL, + .isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty, + .setMode = NULL, + .writeBuf = NULL, + .beginWrite = NULL, + .endWrite = NULL, + } +}; diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h new file mode 100644 index 000000000..80c6d727f --- /dev/null +++ b/src/main/drivers/serial_tcp.h @@ -0,0 +1,60 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include +#include "dyad.h" +// Since serial ports can be used for any function these buffer sizes should be equal +// The two largest things that need to be sent are: 1, MSP responses, 2, UBLOX SVINFO packet. + +// Size must be a power of two due to various optimizations which use 'and' instead of 'mod' +// Various serial routines return the buffer occupied size as uint8_t which would need to be extended in order to +// increase size further. +#define RX_BUFFER_SIZE 1400 +#define TX_BUFFER_SIZE 1400 + +typedef struct { + serialPort_t port; + volatile uint8_t rxBuffer[RX_BUFFER_SIZE]; + volatile uint8_t txBuffer[TX_BUFFER_SIZE]; + + dyad_Stream *serv; + dyad_Stream *conn; + pthread_mutex_t txLock; + pthread_mutex_t rxLock; + bool connected; + uint16_t clientCount; + uint8_t id; +} uartPort_t; + +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options); + +// serialPort API +void tcpWrite(serialPort_t *instance, uint8_t ch); +void tcpDataIn(uartPort_t *instance, uint8_t* ch, int size); +uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance); +uint32_t tcpTotalTxBytesFree(const serialPort_t *instance); +uint8_t tcpRead(serialPort_t *instance); +void tcpDataOut(uartPort_t *instance); +bool isTcpTransmitBufferEmpty(const serialPort_t *s); + +bool tcpIsStart(void); +bool* tcpGetUsed(void); +uartPort_t* tcpGetPool(void); + diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d3c34f5c1..1c9f9f415 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -50,6 +50,11 @@ typedef uint32_t timCCR_t; typedef uint32_t timCCER_t; typedef uint32_t timSR_t; typedef uint32_t timCNT_t; +#elif defined(SIMULATOR_BUILD) +typedef uint32_t timCCR_t; +typedef uint32_t timCCER_t; +typedef uint32_t timSR_t; +typedef uint32_t timCNT_t; #else #error "Unknown CPU defined" #endif diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 40f0a24d7..78fe82a54 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -395,6 +395,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) } #endif +#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) + UNUSED(imuMahonyAHRSupdate); + UNUSED(useAcc); + UNUSED(useMag); + UNUSED(useYaw); + UNUSED(rawYawError); +#else imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]), useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z], @@ -402,7 +409,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) useYaw, rawYawError); imuUpdateEulerAngles(); - +#endif imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } @@ -437,3 +444,23 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) angle = 900; return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); } + +#ifdef SIMULATOR_BUILD +void imuSetAttitudeRPY(float roll, float pitch, float yaw) +{ + attitude.values.roll = roll * 10; + attitude.values.pitch = pitch * 10; + attitude.values.yaw = yaw * 10; +} +void imuSetAttitudeQuat(float w, float x, float y, float z) +{ + q0 = w; + q1 = x; + q2 = y; + q3 = z; + + imuComputeRotationMatrix(); + imuUpdateEulerAngles(); +} +#endif + diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index e36d7d47e..41aebab9e 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -73,3 +73,10 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); void imuResetAccelerationSum(void); void imuInit(void); + +#ifdef SIMULATOR_BUILD +void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg +void imuSetAttitudeQuat(float w, float x, float y, float z); +#endif + + diff --git a/src/main/main.c b/src/main/main.c index 0d27dea6b..fdedb9ced 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -31,6 +31,9 @@ int main(void) while (true) { scheduler(); processLoopback(); +#ifdef SIMULATOR_BUILD + delayMicroseconds_real(50); // max rate 20kHz +#endif } return 0; } diff --git a/src/main/platform.h b/src/main/platform.h index f5d3c1347..c582ab505 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -68,6 +68,10 @@ #define STM32F1 +#elif defined(SITL) + +// Nop + #else // STM32F10X #error "Invalid chipset specified. Update platform.h" #endif diff --git a/src/main/target/SITL/README.md b/src/main/target/SITL/README.md new file mode 100644 index 000000000..241c37739 --- /dev/null +++ b/src/main/target/SITL/README.md @@ -0,0 +1,40 @@ +## SITL in gazebo 8 with ArduCopterPlugin +SITL (software in the loop) simulator allows you to run betaflight/cleanflight without any hardware. +Currently only tested on Ubuntu 16.04, gcc (Ubuntu 5.4.0-6ubuntu1~16.04.4) 5.4.0 20160609. + +### install gazebo 8 +see here: [Installation](http://gazebosim.org/tutorials?cat=install] + +### copy & modify world +for Ubunutu 16.04: +`cp /usr/share/gazebo-8/worlds/iris_arducopter_demo.world .` + +change `real_time_update_rate` in `iris_arducopter_demo.world`: +`0` +to +`100` +***this MUST set to non-zero*** + +`100` mean what speed your computer should run in (Hz). +Faster computer can set to a higher rate. +see [here](http://gazebosim.org/tutorials?tut=modifying_world&cat=build_world#PhysicsProperties) for detail. +`max_step_size` should NOT higher than `0.0025` as I tested. + +### build betaflight +run `make TARGET=SITL` + +### start and run +1. start betaflight: `./obj/main/betaflight_SITL.elf` +2. start gazebo: `gazebo --verbose ./iris_arducopter_demo.world` +3. connect your transmitter and fly/test, I used a app to send `MSP_SET_RAW_RC`, code available [here](https://github.com/cs8425/msp-controller). + +### note +betaflight -> gazebo `udp://127.0.0.1:9002` +gazebo -> betaflight `udp://127.0.0.1:9003` + +UARTx will bind on `tcp://127.0.0.1:576x` when port been open. + +`eeprom.bin`, size 8192 Byte, is for config saving. +size can be changed in `src/main/target/SITL/parameter_group.ld` >> `__FLASH_CONFIG_Size` + + diff --git a/src/main/target/SITL/parameter_group.ld b/src/main/target/SITL/parameter_group.ld new file mode 100644 index 000000000..210f611be --- /dev/null +++ b/src/main/target/SITL/parameter_group.ld @@ -0,0 +1,40 @@ + + +SECTIONS { + /* BLOCK: on Windows (PE) output section must be page-aligned. Use 4-byte alignment otherwise */ + /* SUBALIGN: force 4-byte alignment of input sections for pg_registry. + Gcc defaults to 32 bytes; padding is then inserted between object files, breaking the init structure. */ + .pg_registry BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) : SUBALIGN(4) + { + PROVIDE_HIDDEN (__pg_registry_start = . ); + PROVIDE_HIDDEN (___pg_registry_start = . ); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = . ); + PROVIDE_HIDDEN (___pg_registry_end = . ); + + PROVIDE_HIDDEN (__pg_resetdata_start = . ); + PROVIDE_HIDDEN (___pg_resetdata_start = . ); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = . ); + PROVIDE_HIDDEN (___pg_resetdata_end = . ); + } +} +INSERT AFTER .text; + + +__FLASH_CONFIG_Size = 0x2000; /* 8kB */ +SECTIONS { +/* .FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) : SUBALIGN(4)*/ + .FLASH_CONFIG : + { + PROVIDE_HIDDEN (__config_start = . ); + . = . + __FLASH_CONFIG_Size; + PROVIDE_HIDDEN (__config_end = . ); + } +} +INSERT AFTER .bss; + + + + diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c new file mode 100644 index 000000000..29bb62eae --- /dev/null +++ b/src/main/target/SITL/target.c @@ -0,0 +1,411 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include +#define printf printf +#define sprintf sprintf + +#include +#include +#include + +#include "drivers/io.h" +#include "drivers/dma.h" +#include "drivers/serial.h" +#include "drivers/serial_tcp.h" +#include "drivers/system.h" +#include "drivers/pwm_output.h" +#include "drivers/light_led.h" + +#include "drivers/timer.h" +#include "drivers/timer_def.h" +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {}; + +#include "drivers/accgyro_fake.h" +#include "flight/imu.h" + +#include "dyad.h" +#include "target/SITL/udplink.h" + +static fdm_packet fdmPkt; +static servo_packet pwmPkt; + +static struct timespec start_time; +static double simRate = 1.0; +static pthread_t tcpWorker, udpWorker; +static bool workerRunning = true; +static udpLink_t stateLink, pwmLink; +static pthread_mutex_t updateLock; + +#define RAD2DEG (180.0 / M_PI) +#define ACC_SCALE (256 / 9.81) +#define GYRO_SCALE (8192 / 2000.0) +void updateState(const fdm_packet* pkt) { + static double last_timestamp = 0; // in seconds + static uint64_t last_realtime = 0; // in uS + + const uint64_t realtime_now = micros64_real(); + if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout + last_timestamp = 0; + last_realtime = 0; + } + + const double deltaSim = pkt->timestamp - last_timestamp; // in seconds + if(deltaSim < 0) { // don't use old packet + pthread_mutex_unlock(&updateLock); + return; + } + + int16_t x,y,z; + x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; + y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; + z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; + fakeAccSet(x, y, z); +// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]); + + x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG; + y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; + z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; + fakeGyroSet(x, y, z); +// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]); + +#if defined(SKIP_IMU_CALC) + double qw = pkt->imu_orientation_quat[0]; + double qx = pkt->imu_orientation_quat[1]; + double qy = pkt->imu_orientation_quat[2]; + double qz = pkt->imu_orientation_quat[3]; + double ysqr = qy * qy; + double xf, yf, zf; + + // roll (x-axis rotation) + double t0 = +2.0 * (qw * qx + qy * qz); + double t1 = +1.0 - 2.0 * (qx * qx + ysqr); + xf = atan2(t0, t1) * RAD2DEG; + + // pitch (y-axis rotation) + double t2 = +2.0 * (qw * qy - qz * qx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + yf = asin(t2) * RAD2DEG; // from wiki + + // yaw (z-axis rotation) + double t3 = +2.0 * (qw * qz + qx * qy); + double t4 = +1.0 - 2.0 * (ysqr + qz * qz); + zf = atan2(t3, t4) * RAD2DEG; + imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!! +#endif + + if(last_realtime != 0 && deltaSim < 0.02 && deltaSim > 0) { // should run faster than 50Hz + simRate = simRate * 0.99 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.01; + } + last_timestamp = pkt->timestamp; + last_realtime = micros64_real(); +// printf("simRate = %lf\n", simRate); + + pthread_mutex_unlock(&updateLock); // can send PWM output now +} + +static void* udpThread(void* data) { + UNUSED(data); + int n = 0; + + while (workerRunning) { + n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); + if(n == sizeof(fdm_packet)) { +// printf("[data]new fdm %d\n", n); + updateState(&fdmPkt); + } + } + + printf("udpThread end!!\n"); + return NULL; +} + +static void* tcpThread(void* data) { + UNUSED(data); + + dyad_init(); + dyad_setTickInterval(0.2f); + dyad_setUpdateTimeout(0.5f); + + while (workerRunning) { + dyad_update(); + } + + dyad_shutdown(); + printf("tcpThread end!!\n"); + return NULL; +} + +// system +void systemInit(void) { + int ret; + + clock_gettime(CLOCK_MONOTONIC, &start_time); + printf("[system]Init...\n"); + + SystemCoreClock = 500 * 1e6; // fake 500MHz + FLASH_Unlock(); + + if (pthread_mutex_init(&updateLock, NULL) != 0) { + printf("Create updateLock error!\n"); + exit(1); + } + + ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL); + if(ret != 0) { + printf("Create tcpWorker error!\n"); + exit(1); + } + + ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); + printf("init PwnOut UDP link...%d\n", ret); + + ret = udpInit(&stateLink, NULL, 9003, true); + printf("start UDP server...%d\n", ret); + + ret = pthread_create(&udpWorker, NULL, udpThread, NULL); + if(ret != 0) { + printf("Create udpWorker error!\n"); + exit(1); + } +} + +void systemReset(void){ + printf("[system]Reset!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); +} +void systemResetToBootloader(void) { + printf("[system]ResetToBootloader!\n"); + workerRunning = false; + pthread_join(tcpWorker, NULL); + pthread_join(udpWorker, NULL); + exit(0); +} + +// drivers/light_led.c +void ledInit(const statusLedConfig_t *statusLedConfig) { + UNUSED(statusLedConfig); + printf("[led]Init...\n"); +} +void timerInit(void) { + printf("[timer]Init...\n"); +} +void timerStart(void) { +} +void failureMode(failureMode_e mode) { + printf("[failureMode]!!! %d\n", mode); + while(1); +} + + +// Time part +// Thanks ArduPilot +uint64_t micros64_real() { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - + (start_time.tv_sec + + (start_time.tv_nsec*1.0e-9))); +} +uint64_t millis64_real() { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - + (start_time.tv_sec + + (start_time.tv_nsec*1.0e-9))); +} + +uint64_t micros64() { + static uint64_t last_time = 0; + uint64_t now = micros64_real(); + uint64_t out = last_time + ((now - last_time) * simRate); + last_time = out; + return out; +} +uint64_t millis64() { + static uint64_t last_time = 0; + uint64_t now = millis64_real(); + uint64_t out = last_time + ((now - last_time) * simRate); + last_time = out; + return out; +} + +uint32_t micros(void) { + return micros64() & 0xFFFFFFFF; +} + +uint32_t millis(void) { + return millis64() & 0xFFFFFFFF; +} + +void microsleep(uint32_t usec) { + struct timespec ts; + ts.tv_sec = 0; + ts.tv_nsec = usec*1000UL; + while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ; +} +void delayMicroseconds(uint32_t us) { + microsleep(us / simRate); +} +void delayMicroseconds_real(uint32_t us) { + microsleep(us); +} +void delay(uint32_t ms) { + uint64_t start = millis64(); + + while ((millis64() - start) < ms) { + microsleep(1000); + } +} + + +// PWM part +bool pwmMotorsEnabled = false; +static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; + +// real value to send +static uint16_t motorsPwm[MAX_SUPPORTED_MOTORS]; +static uint16_t servosPwm[MAX_SUPPORTED_SERVOS]; +static uint16_t idlePulse; + +void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) { + UNUSED(motorConfig); + UNUSED(motorCount); + + idlePulse = _idlePulse; + + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + motors[motorIndex].enabled = true; + } + pwmMotorsEnabled = true; +} +void servoDevInit(const servoDevConfig_t *servoConfig) { + UNUSED(servoConfig); + for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + servos[servoIndex].enabled = true; + } +} + +pwmOutputPort_t *pwmGetMotors(void) { + return motors; +} +bool pwmAreMotorsEnabled(void) { + return pwmMotorsEnabled; +} +void pwmWriteMotor(uint8_t index, uint16_t value) { + motorsPwm[index] = value - idlePulse; +} +void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { + UNUSED(motorCount); + pwmMotorsEnabled = false; +} +void pwmCompleteMotorUpdate(uint8_t motorCount) { + UNUSED(motorCount); + // send to simulator + // for gazebo8 ArduCopterPlugin remap, range = [0.0, 1.0] + pwmPkt.motor_speed[3] = motorsPwm[0] / 1000.0f; + pwmPkt.motor_speed[0] = motorsPwm[1] / 1000.0f; + pwmPkt.motor_speed[1] = motorsPwm[2] / 1000.0f; + pwmPkt.motor_speed[2] = motorsPwm[3] / 1000.0f; + + // get one "fdm_packet" can only send one "servo_packet"!! + if(pthread_mutex_trylock(&updateLock) != 0) return; + udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); +// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); +} +void pwmWriteServo(uint8_t index, uint16_t value) { + servosPwm[index] = value; +} + +// ADC part +uint16_t adcGetChannel(uint8_t channel) { + UNUSED(channel); + return 0; +} + +// stack part +char _estack; +char _Min_Stack_Size; + +// fake EEPROM +extern uint8_t __config_start; +extern uint32_t __FLASH_CONFIG_Size; +static FILE *eepromFd = NULL; +const char *EEPROM_FILE = EEPROM_FILENAME; + +void FLASH_Unlock(void) { + uint8_t * const eeprom = &__config_start; + + if(eepromFd != NULL) { + printf("[FLASH_Unlock] eepromFd != NULL\n"); + return; + } + + // open or create + eepromFd = fopen(EEPROM_FILE,"r+"); + if(eepromFd != NULL) { + long lSize; + int c; + + // obtain file size: + fseek(eepromFd , 0 , SEEK_END); + lSize = ftell(eepromFd); + rewind(eepromFd); + + printf("[FLASH_Unlock]size = %ld\n", lSize); + for(unsigned int i=0; i<((uintptr_t)&__FLASH_CONFIG_Size); i++){ + c = fgetc(eepromFd); + if(c == EOF) break; + eeprom[i] = (uint8_t)c; + } + }else{ + eepromFd = fopen(EEPROM_FILE, "w+"); + fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd); + } +} +void FLASH_Lock(void) { + // flush & close + if(eepromFd != NULL) { + const uint8_t *eeprom = &__config_start; + fseek(eepromFd, 0, SEEK_SET); + fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd); + fflush(eepromFd); + fclose(eepromFd); + eepromFd = NULL; + } +} + +FLASH_Status FLASH_ErasePage(uint32_t Page_Address) { + UNUSED(Page_Address); +// printf("[FLASH_ErasePage]%x\n", Page_Address); + return FLASH_COMPLETE; +} +FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data) { + *((uint32_t*)(uintptr_t)addr) = Data; +// printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr)); + return FLASH_COMPLETE; +} + + diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h new file mode 100644 index 000000000..1c63017ba --- /dev/null +++ b/src/main/target/SITL/target.h @@ -0,0 +1,234 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +// SITL (software in the loop) simulator +#define TARGET_BOARD_IDENTIFIER "SITL" + +// use simulatior's attitude directly +#define SKIP_IMU_CALC + +// file name to save config +#define EEPROM_FILENAME "eeprom.bin" + +#define SIMULATOR_BUILD + +#define U_ID_0 0 +#define U_ID_1 1 +#define U_ID_2 2 + +#define ACC +#define USE_FAKE_ACC + +#define GYRO +#define USE_FAKE_GYRO + +#define MAG +#define USE_FAKE_MAG + +#define BARO +#define USE_FAKE_BARO + +#define USABLE_TIMER_CHANNEL_COUNT 0 + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 +#define USE_UART7 +#define USE_UART8 + +//#define USE_SOFTSERIAL1 +//#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_FEATURE FEATURE_RX_MSP +#define DEFAULT_FEATURES (FEATURE_GPS | FEATURE_TELEMETRY) + +#define USE_PARAMETER_GROUPS + +#undef STACK_CHECK // I think SITL don't need this +#undef USE_DASHBOARD +#undef TELEMETRY_LTM +#undef USE_ADC +#undef USE_VCP +#undef USE_PPM +#undef USE_PWM +#undef SERIAL_RX +#undef USE_SERIALRX_CRSF +#undef USE_SERIALRX_IBUS +#undef USE_SERIALRX_SBUS +#undef USE_SERIALRX_SPEKTRUM +#undef USE_SERIALRX_SUMD +#undef USE_SERIALRX_SUMH +#undef USE_SERIALRX_XBUS +#undef LED_STRIP +#undef TELEMETRY_FRSKY +#undef TELEMETRY_HOTT +#undef TELEMETRY_SMARTPORT +#undef TELEMETRY_MAVLINK +#undef USE_RESOURCE_MGMT +#undef CMS +#undef TELEMETRY_CRSF +#undef TELEMETRY_IBUS +#undef TELEMETRY_JETIEXBUS +#undef TELEMETRY_SRXL +#undef USE_SERIALRX_JETIEXBUS +#undef VTX_COMMON +#undef VTX_CONTROL +#undef VTX_SMARTAUDIO +#undef VTX_TRAMP + +#undef USE_I2C +#undef USE_SPI + +#define FLASH_SIZE 2048 + +# define DEFIO_PORT_USED_COUNT 0 +# define DEFIO_PORT_USED_LIST /* empty */ +# define DEFIO_PORT_OFFSET_LIST /* empty */ + +#define LED_STRIP_TIMER 1 +#define SOFTSERIAL_1_TIMER 2 +#define SOFTSERIAL_2_TIMER 3 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff + +#define WS2811_DMA_TC_FLAG (void *)1 +#define WS2811_DMA_HANDLER_IDENTIFER 0 + + +// belows are internal stuff +#include +#include + +uint32_t SystemCoreClock; + +#define UNUSED(x) (void)(x) + +typedef enum +{ + Mode_TEST = 0x0, + Mode_Out_PP = 0x10 +} GPIO_Mode; + +typedef enum {RESET = 0, SET = !RESET} FlagStatus, ITStatus; +typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; +typedef enum {TEST_IRQ = 0 } IRQn_Type; +typedef enum { + EXTI_Trigger_Rising = 0x08, + EXTI_Trigger_Falling = 0x0C, + EXTI_Trigger_Rising_Falling = 0x10 +} EXTITrigger_TypeDef; + +typedef struct +{ + uint32_t IDR; + uint32_t ODR; + uint32_t BSRR; + uint32_t BRR; +} GPIO_TypeDef; + +#define GPIOA_BASE (0x0000) + +typedef struct +{ + void* test; +} TIM_TypeDef; + +typedef struct +{ + void* test; +} TIM_OCInitTypeDef; + +typedef struct { + void* test; +} DMA_TypeDef; + +typedef struct { + void* test; +} DMA_Channel_TypeDef; + +uint8_t DMA_GetFlagStatus(void *); +void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState ); +void DMA_ClearFlag(uint32_t); + +typedef struct +{ + void* test; +} SPI_TypeDef; + +typedef struct +{ + void* test; +} USART_TypeDef; + +#define USART1 ((USART_TypeDef *)0x0001) +#define USART2 ((USART_TypeDef *)0x0002) +#define USART3 ((USART_TypeDef *)0x0003) +#define USART4 ((USART_TypeDef *)0x0004) +#define USART5 ((USART_TypeDef *)0x0005) +#define USART6 ((USART_TypeDef *)0x0006) +#define USART7 ((USART_TypeDef *)0x0007) +#define USART8 ((USART_TypeDef *)0x0008) + +#define UART4 ((USART_TypeDef *)0x0004) +#define UART5 ((USART_TypeDef *)0x0005) +#define UART7 ((USART_TypeDef *)0x0007) +#define UART8 ((USART_TypeDef *)0x0008) + +typedef struct +{ + void* test; +} I2C_TypeDef; + +typedef enum +{ + FLASH_BUSY = 1, + FLASH_ERROR_PG, + FLASH_ERROR_WRP, + FLASH_COMPLETE, + FLASH_TIMEOUT +} FLASH_Status; + +typedef struct { + double timestamp; // in seconds + double imu_angular_velocity_rpy[3]; // range: +/- 8192; +/- 2000 deg/se + double imu_linear_acceleration_xyz[3]; // sim 1G = 9.81 -> FC 1G = 256 + double imu_orientation_quat[4]; + double velocity_xyz[3]; + double position_xyz[3]; +} fdm_packet; +typedef struct { + float motor_speed[4]; // [0.0, 1.0] +} servo_packet; + +void FLASH_Unlock(void); +void FLASH_Lock(void); +FLASH_Status FLASH_ErasePage(uint32_t Page_Address); +FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data); + +uint64_t micros64_real(); +uint64_t millis64_real(); +void delayMicroseconds_real(uint32_t us); + diff --git a/src/main/target/SITL/target.mk b/src/main/target/SITL/target.mk new file mode 100644 index 000000000..5eef6ce06 --- /dev/null +++ b/src/main/target/SITL/target.mk @@ -0,0 +1,9 @@ +SITL_TARGETS += $(TARGET) +#FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro_fake.c \ + drivers/barometer_fake.c \ + drivers/compass_fake.c \ + drivers/serial_tcp.c + diff --git a/src/main/target/SITL/udplink.c b/src/main/target/SITL/udplink.c new file mode 100644 index 000000000..ef9fb37cf --- /dev/null +++ b/src/main/target/SITL/udplink.c @@ -0,0 +1,67 @@ +/** + * Copyright (c) 2017 cs8425 + * + * This library is free software; you can redistribute it and/or modify it + * under the terms of the MIT license. + */ + +#include + +#include + +#include "udplink.h" + +int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) { + int one = 1; + + if((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + return -2; + } + + setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind + fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock + + link->isServer = isServer; + memset(&link->si, 0, sizeof(link->si)); + link->si.sin_family = AF_INET; + link->si.sin_port = htons(port); + link->port = port; + + if(addr == NULL) { + link->si.sin_addr.s_addr = htonl(INADDR_ANY); + }else{ + link->si.sin_addr.s_addr = inet_addr(addr); + } + + if(isServer) { + if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) { + return -1; + } + } + return 0; +} + +int udpSend(udpLink_t* link, const void* data, size_t size) { + return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si)); +} + +int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms) { + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(link->fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + + if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) { + return -1; + } + + socklen_t len; + int ret; + ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len); + return ret; +} + diff --git a/src/main/target/SITL/udplink.h b/src/main/target/SITL/udplink.h new file mode 100644 index 000000000..4ade937cb --- /dev/null +++ b/src/main/target/SITL/udplink.h @@ -0,0 +1,40 @@ +/** + * Copyright (c) 2017 cs8425 + * + * This library is free software; you can redistribute it and/or modify it + * under the terms of the MIT license. + */ + +#ifndef __UDPLINK_H +#define __UDPLINK_H + +#include +#include +#include + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + int fd; + struct sockaddr_in si; + struct sockaddr_in recv; + int port; + char* addr; + bool isServer; +} udpLink_t; + +int udpInit(udpLink_t* link, const char* addr, int port, bool isServer); +int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms); +int udpSend(udpLink_t* link, const void* data, size_t size); + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif +