From 120fa216939e40efdf18ff342442da7a1cdf4abb Mon Sep 17 00:00:00 2001 From: cs8425 Date: Fri, 31 Mar 2017 02:35:06 +0800 Subject: [PATCH] implement SITL in gazebosim with ArduCopterPlugin need implement fake eeprom & fake IO need implement fake system function can compile, stuck in isEEPROMContentValid() EEPROM in memory work EEPROM as file should work fix some complie warning MSP over TCP work (add dyad.c) a little clean up fix FLASH_CONFIG_Size in ld script & implement some pwmout IO to simulator work!! need to check direction & scale!! can fly but Gyro buggy move dyad.c fix busy-loop (limit to max 20kHz) can simulatie in different speed now! (hard code) add option for IMU calculation add README.md move dyad.c and fix F3 overrun the flash size explanation SITL in README.md and reuse CFLAGS, ASFLAGS --- Makefile | 69 +- lib/main/dyad/LICENSE | 20 + lib/main/dyad/README.md | 85 ++ lib/main/dyad/dyad.c | 1156 +++++++++++++++++++++++ lib/main/dyad/dyad.h | 106 +++ src/main/config/config_streamer.c | 4 + src/main/drivers/bus_i2c.h | 2 + src/main/drivers/io.c | 2 + src/main/drivers/io.h | 10 + src/main/drivers/serial_tcp.c | 268 ++++++ src/main/drivers/serial_tcp.h | 60 ++ src/main/drivers/timer.h | 5 + src/main/flight/imu.c | 29 +- src/main/flight/imu.h | 7 + src/main/main.c | 3 + src/main/platform.h | 4 + src/main/target/SITL/README.md | 40 + src/main/target/SITL/parameter_group.ld | 40 + src/main/target/SITL/target.c | 411 ++++++++ src/main/target/SITL/target.h | 234 +++++ src/main/target/SITL/target.mk | 9 + src/main/target/SITL/udplink.c | 67 ++ src/main/target/SITL/udplink.h | 40 + 23 files changed, 2668 insertions(+), 3 deletions(-) create mode 100644 lib/main/dyad/LICENSE create mode 100644 lib/main/dyad/README.md create mode 100644 lib/main/dyad/dyad.c create mode 100644 lib/main/dyad/dyad.h create mode 100644 src/main/drivers/serial_tcp.c create mode 100644 src/main/drivers/serial_tcp.h create mode 100644 src/main/target/SITL/README.md create mode 100644 src/main/target/SITL/parameter_group.ld create mode 100644 src/main/target/SITL/target.c create mode 100644 src/main/target/SITL/target.h create mode 100644 src/main/target/SITL/target.mk create mode 100644 src/main/target/SITL/udplink.c create mode 100644 src/main/target/SITL/udplink.h 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 +