Michael Keller
GitHub
commit
42cbbbb681
69
Makefile
69
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
|
||||
###############################################################################
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
|
||||
|
||||
|
||||
## 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 <stdlib.h>
|
||||
#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 <stdio.h>
|
||||
#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.
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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 <stdarg.h>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h> /* 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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Authors:
|
||||
* Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
|
||||
* Hamasaki/Timecop - Initial baseflight code
|
||||
*/
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
|
||||
#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,
|
||||
}
|
||||
};
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <netinet/in.h>
|
||||
#include <pthread.h>
|
||||
#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);
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -31,6 +31,9 @@ int main(void)
|
|||
while (true) {
|
||||
scheduler();
|
||||
processLoopback();
|
||||
#ifdef SIMULATOR_BUILD
|
||||
delayMicroseconds_real(50); // max rate 20kHz
|
||||
#endif
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -68,6 +68,10 @@
|
|||
|
||||
#define STM32F1
|
||||
|
||||
#elif defined(SITL)
|
||||
|
||||
// Nop
|
||||
|
||||
#else // STM32F10X
|
||||
#error "Invalid chipset specified. Update platform.h"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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`:
|
||||
`<real_time_update_rate>0</real_time_update_rate>`
|
||||
to
|
||||
`<real_time_update_rate>100</real_time_update_rate>`
|
||||
***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`
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#define printf printf
|
||||
#define sprintf sprintf
|
||||
|
||||
#include <errno.h>
|
||||
#include <time.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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 <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
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);
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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 <string.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
@ -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 <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
|
||||
#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
|
||||
|
||||
Loading…
Reference in New Issue