Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Merge pull request #2819 from cs8425/SITL

Browse source 
SITL in gazebosim
This commit is contained in:
Michael Keller 2017-04-06 00:04:25 +12:00 • committed by GitHub
commit 42cbbbb681
23 changed files with 2668 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

69
Makefile
View file

@ -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?) $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have you prepared a valid target.mk?)
endif 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?) $(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 endif
@ -205,7 +205,7 @@ endif
256K_TARGETS = $(F3_TARGETS) 256K_TARGETS = $(F3_TARGETS)
512K_TARGETS = $(F411_TARGETS) $(F446_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) 512K_TARGETS = $(F411_TARGETS) $(F446_TARGETS) $(F7X2RE_TARGETS) $(F7X5XE_TARGETS)
1024K_TARGETS = $(F405_TARGETS) $(F7X5XG_TARGETS) $(F7X6XG_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. # Configure default flash sizes for the targets (largest size specified gets hit first) if flash not specified already.
ifeq ($(FLASH_SIZE),) ifeq ($(FLASH_SIZE),)
@ -534,6 +534,26 @@ TARGET_FLAGS = -D$(TARGET)
# End F7 targets # 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 # Start F1 targets
else else
@ -939,6 +959,19 @@ F7EXCLUDES = drivers/bus_spi.c \
drivers/timer.c \ drivers/timer.c \
drivers/serial_uart.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 # check if target.mk supplied
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
SRC := $(STARTUP_SRC) $(STM32F4xx_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC) 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) SRC := $(STARTUP_SRC) $(STM32F30x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC)
else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS))) else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC) $(VARIANT_SRC) 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 endif
ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),) ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),)
@ -982,6 +1017,11 @@ ifeq ($(TARGET),$(filter $(TARGET),$(F7_TARGETS)))
SRC := $(filter-out ${F7EXCLUDES}, $(SRC)) SRC := $(filter-out ${F7EXCLUDES}, $(SRC))
endif endif
#SITL excludes
ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
SRC := $(filter-out ${SITLEXCLUDES}, $(SRC))
endif
ifneq ($(filter SDCARD,$(FEATURES)),) ifneq ($(filter SDCARD,$(FEATURES)),)
SRC += \ SRC += \
drivers/sdcard.c \ drivers/sdcard.c \
@ -1037,6 +1077,13 @@ OPTIMISE_SIZE := -Os
LTO_FLAGS := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED) 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 else
OPTIMISE_DEFAULT := -Ofast OPTIMISE_DEFAULT := -Ofast
@ -1097,6 +1144,24 @@ LDFLAGS = -lm \
-Wl,--no-wchar-size-warning \ -Wl,--no-wchar-size-warning \
-T$(LD_SCRIPT) -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 # No user-serviceable parts below
############################################################################### ###############################################################################

20
lib/main/dyad/LICENSE Normal file
View file

@ -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.

85
lib/main/dyad/README.md Normal file
View file

@ -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 <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.

1156
lib/main/dyad/dyad.c Normal file
View file

File diff suppressed because it is too large Load diff

106
lib/main/dyad/dyad.h Normal file
View file

@ -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

4
src/main/config/config_streamer.c
View file

@ -53,6 +53,8 @@ extern uint8_t __config_end;
# define FLASH_PAGE_SIZE ((uint32_t)0x8000) # define FLASH_PAGE_SIZE ((uint32_t)0x8000)
# elif defined(UNIT_TEST) # elif defined(UNIT_TEST)
# define FLASH_PAGE_SIZE (0x400) # define FLASH_PAGE_SIZE (0x400)
# elif defined(SIMULATOR_BUILD)
# define FLASH_PAGE_SIZE (0x400)
# else # else
# error "Flash page size not defined for target." # error "Flash page size not defined for target."
# endif # endif
@ -87,6 +89,8 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
// NOP // NOP
#elif defined(UNIT_TEST) #elif defined(UNIT_TEST)
// NOP // NOP
#elif defined(SIMULATOR_BUILD)
// NOP
#else #else
# error "Unsupported CPU" # error "Unsupported CPU"
#endif #endif

2
src/main/drivers/bus_i2c.h
View file

@ -40,7 +40,9 @@ typedef enum I2CDevice {
} I2CDevice; } I2CDevice;
typedef struct i2cDevice_s { typedef struct i2cDevice_s {
#if !defined(SIMULATOR_BUILD)
I2C_TypeDef *dev; I2C_TypeDef *dev;
#endif
ioTag_t scl; ioTag_t scl;
ioTag_t sda; ioTag_t sda;
rccPeriphTag_t rcc; rccPeriphTag_t rcc;

2
src/main/drivers/io.c
View file

@ -145,6 +145,8 @@ uint32_t IO_EXTI_Line(IO_t io)
return 1 << IO_GPIOPinIdx(io); return 1 << IO_GPIOPinIdx(io);
#elif defined(STM32F7) #elif defined(STM32F7)
return 1 << IO_GPIOPinIdx(io); return 1 << IO_GPIOPinIdx(io);
#elif defined(SIMULATOR_BUILD)
return 1;
#else #else
# error "Unknown target type" # error "Unknown target type"
#endif #endif

10
src/main/drivers/io.h
View file

@ -88,6 +88,16 @@
# define IOCFG_IPU 0 # define IOCFG_IPU 0
# define IOCFG_IN_FLOATING 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 #else
# warning "Unknown TARGET" # warning "Unknown TARGET"
#endif #endif

268
src/main/drivers/serial_tcp.c Normal file
View file

@ -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,
}
};

60
src/main/drivers/serial_tcp.h Normal file
View file

@ -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);

5
src/main/drivers/timer.h
View file

@ -50,6 +50,11 @@ typedef uint32_t timCCR_t;
typedef uint32_t timCCER_t; typedef uint32_t timCCER_t;
typedef uint32_t timSR_t; typedef uint32_t timSR_t;
typedef uint32_t timCNT_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 #else
#error "Unknown CPU defined" #error "Unknown CPU defined"
#endif #endif

29
src/main/flight/imu.c
View file

@ -395,6 +395,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC)
UNUSED(imuMahonyAHRSupdate);
UNUSED(useAcc);
UNUSED(useMag);
UNUSED(useYaw);
UNUSED(rawYawError);
#else
imuMahonyAHRSupdate(deltaT * 1e-6f, imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]), 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], useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
@ -402,7 +409,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useYaw, rawYawError); useYaw, rawYawError);
imuUpdateEulerAngles(); imuUpdateEulerAngles();
#endif
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
} }
@ -437,3 +444,23 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
angle = 900; angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); 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

7
src/main/flight/imu.h
View file

@ -73,3 +73,10 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
void imuResetAccelerationSum(void); void imuResetAccelerationSum(void);
void imuInit(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

3
src/main/main.c
View file

@ -31,6 +31,9 @@ int main(void)
while (true) { while (true) {
scheduler(); scheduler();
processLoopback(); processLoopback();
#ifdef SIMULATOR_BUILD
delayMicroseconds_real(50); // max rate 20kHz
#endif
} }
return 0; return 0;
} }

4
src/main/platform.h
View file

@ -68,6 +68,10 @@
#define STM32F1 #define STM32F1
#elif defined(SITL)
// Nop
#else // STM32F10X #else // STM32F10X
#error "Invalid chipset specified. Update platform.h" #error "Invalid chipset specified. Update platform.h"
#endif #endif

40
src/main/target/SITL/README.md Normal file
View file

@ -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`

40
src/main/target/SITL/parameter_group.ld Normal file
View file

@ -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;

411
src/main/target/SITL/target.c Normal file
View file

@ -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;
}

234
src/main/target/SITL/target.h Normal file
View file

@ -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);

9
src/main/target/SITL/target.mk Normal file
View file

@ -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

67
src/main/target/SITL/udplink.c Normal file
View file

@ -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;
}

40
src/main/target/SITL/udplink.h Normal file
View file

@ -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