Merge pull request #2841 from cs8425/SITL-fix
SITL: some fix & should be usable
This commit is contained in:
commit
53edcba0d8
|
@ -20,6 +20,28 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#define printf printf
|
||||||
|
#define sprintf sprintf
|
||||||
|
|
||||||
|
#define ACC_LOCK pthread_mutex_unlock(&accUpdateLock)
|
||||||
|
#define ACC_UNLOCK pthread_mutex_unlock(&accUpdateLock)
|
||||||
|
|
||||||
|
#define GYRO_LOCK pthread_mutex_unlock(&gyroUpdateLock)
|
||||||
|
#define GYRO_UNLOCK pthread_mutex_unlock(&gyroUpdateLock)
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
#define ACC_LOCK
|
||||||
|
#define ACC_UNLOCK
|
||||||
|
|
||||||
|
#define GYRO_LOCK
|
||||||
|
#define GYRO_UNLOCK
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef USE_FAKE_GYRO
|
#ifdef USE_FAKE_GYRO
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
@ -28,26 +50,55 @@
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
#include "accgyro_fake.h"
|
#include "accgyro_fake.h"
|
||||||
|
|
||||||
|
|
||||||
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
static pthread_mutex_t gyroUpdateLock;
|
||||||
|
#endif
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
|
||||||
|
static bool gyroUpdated = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
static void fakeGyroInit(gyroDev_t *gyro)
|
static void fakeGyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
UNUSED(gyro);
|
UNUSED(gyro);
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
if (pthread_mutex_init(&gyroUpdateLock, NULL) != 0) {
|
||||||
|
printf("Create gyroUpdateLock error!\n");
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
||||||
{
|
{
|
||||||
|
GYRO_LOCK;
|
||||||
|
|
||||||
fakeGyroADC[X] = x;
|
fakeGyroADC[X] = x;
|
||||||
fakeGyroADC[Y] = y;
|
fakeGyroADC[Y] = y;
|
||||||
fakeGyroADC[Z] = z;
|
fakeGyroADC[Z] = z;
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
|
||||||
|
gyroUpdated = true;
|
||||||
|
#endif
|
||||||
|
GYRO_UNLOCK;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeGyroRead(gyroDev_t *gyro)
|
static bool fakeGyroRead(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
|
GYRO_LOCK;
|
||||||
|
#if defined(SIMULATOR_GYRO_SYNC)
|
||||||
|
if(gyroUpdated == false) {
|
||||||
|
GYRO_UNLOCK;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
gyroUpdated = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
gyro->gyroADCRaw[X] = fakeGyroADC[X];
|
gyro->gyroADCRaw[X] = fakeGyroADC[X];
|
||||||
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
|
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
|
||||||
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
||||||
|
|
||||||
|
GYRO_UNLOCK;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -70,7 +121,11 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
||||||
gyro->intStatus = fakeGyroInitStatus;
|
gyro->intStatus = fakeGyroInitStatus;
|
||||||
gyro->read = fakeGyroRead;
|
gyro->read = fakeGyroRead;
|
||||||
gyro->temperature = fakeGyroReadTemperature;
|
gyro->temperature = fakeGyroReadTemperature;
|
||||||
|
#if defined(SIMULATOR_BUILD)
|
||||||
|
gyro->scale = 1.0f / 16.4f;
|
||||||
|
#else
|
||||||
gyro->scale = 1.0f;
|
gyro->scale = 1.0f;
|
||||||
|
#endif
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif // USE_FAKE_GYRO
|
#endif // USE_FAKE_GYRO
|
||||||
|
@ -80,23 +135,53 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
||||||
|
|
||||||
static int16_t fakeAccData[XYZ_AXIS_COUNT];
|
static int16_t fakeAccData[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
static pthread_mutex_t accUpdateLock;
|
||||||
|
#endif
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
|
||||||
|
static bool accUpdated = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
static void fakeAccInit(accDev_t *acc)
|
static void fakeAccInit(accDev_t *acc)
|
||||||
{
|
{
|
||||||
UNUSED(acc);
|
UNUSED(acc);
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
if (pthread_mutex_init(&accUpdateLock, NULL) != 0) {
|
||||||
|
printf("Create accUpdateLock error!\n");
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void fakeAccSet(int16_t x, int16_t y, int16_t z)
|
void fakeAccSet(int16_t x, int16_t y, int16_t z)
|
||||||
{
|
{
|
||||||
|
ACC_LOCK;
|
||||||
|
|
||||||
fakeAccData[X] = x;
|
fakeAccData[X] = x;
|
||||||
fakeAccData[Y] = y;
|
fakeAccData[Y] = y;
|
||||||
fakeAccData[Z] = z;
|
fakeAccData[Z] = z;
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
|
||||||
|
accUpdated = true;
|
||||||
|
#endif
|
||||||
|
ACC_LOCK;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool fakeAccRead(accDev_t *acc)
|
static bool fakeAccRead(accDev_t *acc)
|
||||||
{
|
{
|
||||||
|
ACC_LOCK;
|
||||||
|
#if defined(SIMULATOR_ACC_SYNC)
|
||||||
|
if(accUpdated == false) {
|
||||||
|
ACC_UNLOCK;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
accUpdated = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
acc->ADCRaw[X] = fakeAccData[X];
|
acc->ADCRaw[X] = fakeAccData[X];
|
||||||
acc->ADCRaw[Y] = fakeAccData[Y];
|
acc->ADCRaw[Y] = fakeAccData[Y];
|
||||||
acc->ADCRaw[Z] = fakeAccData[Z];
|
acc->ADCRaw[Z] = fakeAccData[Z];
|
||||||
|
|
||||||
|
ACC_UNLOCK;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -38,18 +38,18 @@
|
||||||
#define BASE_PORT 5760
|
#define BASE_PORT 5760
|
||||||
|
|
||||||
const struct serialPortVTable uartVTable[]; // Forward
|
const struct serialPortVTable uartVTable[]; // Forward
|
||||||
static uartPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
|
static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
|
||||||
static bool portInited[SERIAL_PORT_COUNT];
|
static bool portInited[SERIAL_PORT_COUNT];
|
||||||
static bool tcpStart = false;
|
static bool tcpStart = false;
|
||||||
bool tcpIsStart(void) {
|
bool tcpIsStart(void) {
|
||||||
return tcpStart;
|
return tcpStart;
|
||||||
}
|
}
|
||||||
static void onData(dyad_Event *e) {
|
static void onData(dyad_Event *e) {
|
||||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
tcpDataIn(s, (uint8_t*)e->data, e->size);
|
tcpDataIn(s, (uint8_t*)e->data, e->size);
|
||||||
}
|
}
|
||||||
static void onClose(dyad_Event *e) {
|
static void onClose(dyad_Event *e) {
|
||||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
s->clientCount--;
|
s->clientCount--;
|
||||||
s->conn = NULL;
|
s->conn = NULL;
|
||||||
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1, s->connected, s->clientCount);
|
||||||
|
@ -58,7 +58,7 @@ static void onClose(dyad_Event *e) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
static void onAccept(dyad_Event *e) {
|
static void onAccept(dyad_Event *e) {
|
||||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||||
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
|
fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1, s->clientCount);
|
||||||
|
|
||||||
s->connected = true;
|
s->connected = true;
|
||||||
|
@ -74,7 +74,7 @@ static void onAccept(dyad_Event *e) {
|
||||||
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
|
dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
|
||||||
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
|
dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
|
||||||
}
|
}
|
||||||
static uartPort_t* tcpReconfigure(uartPort_t *s, int id)
|
static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
|
||||||
{
|
{
|
||||||
if(portInited[id]) {
|
if(portInited[id]) {
|
||||||
fprintf(stderr, "port had initialed!!\n");
|
fprintf(stderr, "port had initialed!!\n");
|
||||||
|
@ -113,7 +113,7 @@ static uartPort_t* tcpReconfigure(uartPort_t *s, int id)
|
||||||
|
|
||||||
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options)
|
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options)
|
||||||
{
|
{
|
||||||
uartPort_t *s = NULL;
|
tcpPort_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)
|
#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);
|
uintptr_t id = ((uintptr_t)USARTx - 1);
|
||||||
|
@ -143,7 +143,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallbac
|
||||||
|
|
||||||
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
||||||
{
|
{
|
||||||
uartPort_t *s = (uartPort_t*)instance;
|
tcpPort_t *s = (tcpPort_t*)instance;
|
||||||
uint32_t count;
|
uint32_t count;
|
||||||
pthread_mutex_lock(&s->rxLock);
|
pthread_mutex_lock(&s->rxLock);
|
||||||
if (s->port.rxBufferHead >= s->port.rxBufferTail) {
|
if (s->port.rxBufferHead >= s->port.rxBufferTail) {
|
||||||
|
@ -158,7 +158,7 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
||||||
|
|
||||||
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
||||||
{
|
{
|
||||||
uartPort_t *s = (uartPort_t*)instance;
|
tcpPort_t *s = (tcpPort_t*)instance;
|
||||||
uint32_t bytesUsed;
|
uint32_t bytesUsed;
|
||||||
|
|
||||||
pthread_mutex_lock(&s->txLock);
|
pthread_mutex_lock(&s->txLock);
|
||||||
|
@ -175,7 +175,7 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
||||||
|
|
||||||
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
||||||
{
|
{
|
||||||
uartPort_t *s = (uartPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
pthread_mutex_lock(&s->txLock);
|
pthread_mutex_lock(&s->txLock);
|
||||||
bool isEmpty = s->port.txBufferTail == s->port.txBufferHead;
|
bool isEmpty = s->port.txBufferTail == s->port.txBufferHead;
|
||||||
pthread_mutex_unlock(&s->txLock);
|
pthread_mutex_unlock(&s->txLock);
|
||||||
|
@ -185,7 +185,7 @@ bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
||||||
uint8_t tcpRead(serialPort_t *instance)
|
uint8_t tcpRead(serialPort_t *instance)
|
||||||
{
|
{
|
||||||
uint8_t ch;
|
uint8_t ch;
|
||||||
uartPort_t *s = (uartPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
pthread_mutex_lock(&s->rxLock);
|
pthread_mutex_lock(&s->rxLock);
|
||||||
|
|
||||||
ch = s->port.rxBuffer[s->port.rxBufferTail];
|
ch = s->port.rxBuffer[s->port.rxBufferTail];
|
||||||
|
@ -201,7 +201,7 @@ uint8_t tcpRead(serialPort_t *instance)
|
||||||
|
|
||||||
void tcpWrite(serialPort_t *instance, uint8_t ch)
|
void tcpWrite(serialPort_t *instance, uint8_t ch)
|
||||||
{
|
{
|
||||||
uartPort_t *s = (uartPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
pthread_mutex_lock(&s->txLock);
|
pthread_mutex_lock(&s->txLock);
|
||||||
|
|
||||||
s->port.txBuffer[s->port.txBufferHead] = ch;
|
s->port.txBuffer[s->port.txBufferHead] = ch;
|
||||||
|
@ -215,10 +215,10 @@ void tcpWrite(serialPort_t *instance, uint8_t ch)
|
||||||
tcpDataOut(s);
|
tcpDataOut(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
void tcpDataOut(uartPort_t *instance)
|
void tcpDataOut(tcpPort_t *instance)
|
||||||
{
|
{
|
||||||
uint32_t bytesUsed;
|
uint32_t bytesUsed;
|
||||||
uartPort_t *s = (uartPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
if(s->conn == NULL) return;
|
if(s->conn == NULL) return;
|
||||||
pthread_mutex_lock(&s->txLock);
|
pthread_mutex_lock(&s->txLock);
|
||||||
|
|
||||||
|
@ -234,9 +234,9 @@ void tcpDataOut(uartPort_t *instance)
|
||||||
pthread_mutex_unlock(&s->txLock);
|
pthread_mutex_unlock(&s->txLock);
|
||||||
}
|
}
|
||||||
|
|
||||||
void tcpDataIn(uartPort_t *instance, uint8_t* ch, int size)
|
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
|
||||||
{
|
{
|
||||||
uartPort_t *s = (uartPort_t *)instance;
|
tcpPort_t *s = (tcpPort_t *)instance;
|
||||||
pthread_mutex_lock(&s->rxLock);
|
pthread_mutex_lock(&s->rxLock);
|
||||||
|
|
||||||
while(size--) {
|
while(size--) {
|
||||||
|
|
|
@ -41,20 +41,20 @@ typedef struct {
|
||||||
bool connected;
|
bool connected;
|
||||||
uint16_t clientCount;
|
uint16_t clientCount;
|
||||||
uint8_t id;
|
uint8_t id;
|
||||||
} uartPort_t;
|
} tcpPort_t;
|
||||||
|
|
||||||
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options);
|
serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||||
|
|
||||||
// serialPort API
|
// tcpPort API
|
||||||
void tcpWrite(serialPort_t *instance, uint8_t ch);
|
void tcpWrite(serialPort_t *instance, uint8_t ch);
|
||||||
void tcpDataIn(uartPort_t *instance, uint8_t* ch, int size);
|
void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size);
|
||||||
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance);
|
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance);
|
||||||
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance);
|
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance);
|
||||||
uint8_t tcpRead(serialPort_t *instance);
|
uint8_t tcpRead(serialPort_t *instance);
|
||||||
void tcpDataOut(uartPort_t *instance);
|
void tcpDataOut(tcpPort_t *instance);
|
||||||
bool isTcpTransmitBufferEmpty(const serialPort_t *s);
|
bool isTcpTransmitBufferEmpty(const serialPort_t *s);
|
||||||
|
|
||||||
bool tcpIsStart(void);
|
bool tcpIsStart(void);
|
||||||
bool* tcpGetUsed(void);
|
bool* tcpGetUsed(void);
|
||||||
uartPort_t* tcpGetPool(void);
|
tcpPort_t* tcpGetPool(void);
|
||||||
|
|
||||||
|
|
|
@ -602,6 +602,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
static bool runTaskMainSubprocesses;
|
static bool runTaskMainSubprocesses;
|
||||||
static uint8_t pidUpdateCountdown;
|
static uint8_t pidUpdateCountdown;
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
|
||||||
|
if(lockMainPID() != 0) return;
|
||||||
|
#endif
|
||||||
|
|
||||||
if (debugMode == DEBUG_CYCLETIME) {
|
if (debugMode == DEBUG_CYCLETIME) {
|
||||||
debug[0] = getTaskDeltaTime(TASK_SELF);
|
debug[0] = getTaskDeltaTime(TASK_SELF);
|
||||||
debug[1] = averageSystemLoadPercent;
|
debug[1] = averageSystemLoadPercent;
|
||||||
|
|
|
@ -49,6 +49,28 @@
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/sonar.h"
|
#include "sensors/sonar.h"
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
#define printf printf
|
||||||
|
#define sprintf sprintf
|
||||||
|
|
||||||
|
static pthread_mutex_t imuUpdateLock;
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_IMU_SYNC)
|
||||||
|
static uint32_t imuDeltaT = 0;
|
||||||
|
static bool imuUpdated = false;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define IMU_LOCK pthread_mutex_unlock(&imuUpdateLock)
|
||||||
|
#define IMU_UNLOCK pthread_mutex_unlock(&imuUpdateLock)
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
#define IMU_LOCK
|
||||||
|
#define IMU_UNLOCK
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
// the limit (in degrees/second) beyond which we stop integrating
|
// the limit (in degrees/second) beyond which we stop integrating
|
||||||
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
||||||
|
@ -110,6 +132,11 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
||||||
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
|
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
|
||||||
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
|
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
|
||||||
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
|
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) && !defined(SET_IMU_FROM_EULER)
|
||||||
|
rMat[1][0] = -2.0f * (q1q2 - -q0q3);
|
||||||
|
rMat[2][0] = -2.0f * (q1q3 + -q0q2);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -142,6 +169,12 @@ void imuInit(void)
|
||||||
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
|
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
|
||||||
|
|
||||||
imuComputeRotationMatrix();
|
imuComputeRotationMatrix();
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
|
||||||
|
printf("Create imuUpdateLock error!\n");
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void imuResetAccelerationSum(void)
|
void imuResetAccelerationSum(void)
|
||||||
|
@ -402,6 +435,12 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
||||||
UNUSED(useYaw);
|
UNUSED(useYaw);
|
||||||
UNUSED(rawYawError);
|
UNUSED(rawYawError);
|
||||||
#else
|
#else
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
|
||||||
|
// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
|
||||||
|
deltaT = imuDeltaT;
|
||||||
|
#endif
|
||||||
|
|
||||||
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],
|
||||||
|
@ -416,7 +455,16 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
||||||
void imuUpdateAttitude(timeUs_t currentTimeUs)
|
void imuUpdateAttitude(timeUs_t currentTimeUs)
|
||||||
{
|
{
|
||||||
if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
|
if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) {
|
||||||
|
IMU_LOCK;
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
|
||||||
|
if(imuUpdated == false){
|
||||||
|
IMU_UNLOCK;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
imuUpdated = false;
|
||||||
|
#endif
|
||||||
imuCalculateEstimatedAttitude(currentTimeUs);
|
imuCalculateEstimatedAttitude(currentTimeUs);
|
||||||
|
IMU_UNLOCK;
|
||||||
} else {
|
} else {
|
||||||
acc.accSmooth[X] = 0;
|
acc.accSmooth[X] = 0;
|
||||||
acc.accSmooth[Y] = 0;
|
acc.accSmooth[Y] = 0;
|
||||||
|
@ -448,12 +496,18 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
||||||
#ifdef SIMULATOR_BUILD
|
#ifdef SIMULATOR_BUILD
|
||||||
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
|
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
|
||||||
{
|
{
|
||||||
|
IMU_LOCK;
|
||||||
|
|
||||||
attitude.values.roll = roll * 10;
|
attitude.values.roll = roll * 10;
|
||||||
attitude.values.pitch = pitch * 10;
|
attitude.values.pitch = pitch * 10;
|
||||||
attitude.values.yaw = yaw * 10;
|
attitude.values.yaw = yaw * 10;
|
||||||
|
|
||||||
|
IMU_UNLOCK;
|
||||||
}
|
}
|
||||||
void imuSetAttitudeQuat(float w, float x, float y, float z)
|
void imuSetAttitudeQuat(float w, float x, float y, float z)
|
||||||
{
|
{
|
||||||
|
IMU_LOCK;
|
||||||
|
|
||||||
q0 = w;
|
q0 = w;
|
||||||
q1 = x;
|
q1 = x;
|
||||||
q2 = y;
|
q2 = y;
|
||||||
|
@ -461,6 +515,19 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
|
||||||
|
|
||||||
imuComputeRotationMatrix();
|
imuComputeRotationMatrix();
|
||||||
imuUpdateEulerAngles();
|
imuUpdateEulerAngles();
|
||||||
|
|
||||||
|
IMU_UNLOCK;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
|
||||||
|
void imuSetHasNewData(uint32_t dt)
|
||||||
|
{
|
||||||
|
IMU_LOCK;
|
||||||
|
|
||||||
|
imuUpdated = true;
|
||||||
|
imuDeltaT = dt;
|
||||||
|
|
||||||
|
IMU_UNLOCK;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -78,5 +78,8 @@ void imuInit(void);
|
||||||
void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg
|
void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg
|
||||||
void imuSetAttitudeQuat(float w, float x, float y, float z);
|
void imuSetAttitudeQuat(float w, float x, float y, float z);
|
||||||
#endif
|
#endif
|
||||||
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
|
||||||
|
void imuSetHasNewData(uint32_t dt);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -126,6 +126,9 @@ void taskSystem(timeUs_t currentTimeUs)
|
||||||
totalWaitingTasksSamples = 0;
|
totalWaitingTasksSamples = 0;
|
||||||
totalWaitingTasks = 0;
|
totalWaitingTasks = 0;
|
||||||
}
|
}
|
||||||
|
#if defined(SIMULATOR_BUILD)
|
||||||
|
averageSystemLoadPercent = 0;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef SKIP_TASK_STATISTICS
|
#ifndef SKIP_TASK_STATISTICS
|
||||||
|
|
|
@ -1,9 +1,9 @@
|
||||||
## SITL in gazebo 8 with ArduCopterPlugin
|
## SITL in gazebo 8 with ArduCopterPlugin
|
||||||
SITL (software in the loop) simulator allows you to run betaflight/cleanflight without any hardware.
|
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.
|
Currently only tested on Ubuntu 16.04, x86_64, gcc (Ubuntu 5.4.0-6ubuntu1~16.04.4) 5.4.0 20160609.
|
||||||
|
|
||||||
### install gazebo 8
|
### install gazebo 8
|
||||||
see here: [Installation](http://gazebosim.org/tutorials?cat=install]
|
see here: [Installation](http://gazebosim.org/tutorials?cat=install)
|
||||||
|
|
||||||
### copy & modify world
|
### copy & modify world
|
||||||
for Ubunutu 16.04:
|
for Ubunutu 16.04:
|
||||||
|
@ -13,20 +13,29 @@ change `real_time_update_rate` in `iris_arducopter_demo.world`:
|
||||||
`<real_time_update_rate>0</real_time_update_rate>`
|
`<real_time_update_rate>0</real_time_update_rate>`
|
||||||
to
|
to
|
||||||
`<real_time_update_rate>100</real_time_update_rate>`
|
`<real_time_update_rate>100</real_time_update_rate>`
|
||||||
***this MUST set to non-zero***
|
***this suggest set to non-zero***
|
||||||
|
|
||||||
`100` mean what speed your computer should run in (Hz).
|
`100` mean what speed your computer should run in (Hz).
|
||||||
Faster computer can set to a higher rate.
|
Faster computer can set to a higher rate.
|
||||||
see [here](http://gazebosim.org/tutorials?tut=modifying_world&cat=build_world#PhysicsProperties) for detail.
|
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.
|
`max_step_size` should NOT higher than `0.0025` as I tested.
|
||||||
|
smaller mean more accurate, but need higher speed CPU to run as realtime.
|
||||||
|
|
||||||
### build betaflight
|
### build betaflight
|
||||||
run `make TARGET=SITL`
|
run `make TARGET=SITL`
|
||||||
|
|
||||||
|
### settings
|
||||||
|
to avoid simulation speed slow down, suggest to set some settings belows:
|
||||||
|
|
||||||
|
In `configuration` page:
|
||||||
|
|
||||||
|
1. `ESC/Motor`: `PWM`, disable `Motor PWM speed Sparted from PID speed`
|
||||||
|
2. `PID loop frequency` as high as it can.
|
||||||
|
|
||||||
### start and run
|
### start and run
|
||||||
1. start betaflight: `./obj/main/betaflight_SITL.elf`
|
1. start betaflight: `./obj/main/betaflight_SITL.elf`
|
||||||
2. start gazebo: `gazebo --verbose ./iris_arducopter_demo.world`
|
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).
|
4. 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
|
### note
|
||||||
betaflight -> gazebo `udp://127.0.0.1:9002`
|
betaflight -> gazebo `udp://127.0.0.1:9002`
|
||||||
|
|
|
@ -36,11 +36,15 @@
|
||||||
|
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/timer_def.h"
|
#include "drivers/timer_def.h"
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
|
const timerHardware_t timerHardware[1]; // unused
|
||||||
|
|
||||||
#include "drivers/accgyro_fake.h"
|
#include "drivers/accgyro_fake.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
|
|
||||||
|
#include "config/feature.h"
|
||||||
|
#include "fc/config.h"
|
||||||
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "dyad.h"
|
#include "dyad.h"
|
||||||
#include "target/SITL/udplink.h"
|
#include "target/SITL/udplink.h"
|
||||||
|
|
||||||
|
@ -53,23 +57,38 @@ static pthread_t tcpWorker, udpWorker;
|
||||||
static bool workerRunning = true;
|
static bool workerRunning = true;
|
||||||
static udpLink_t stateLink, pwmLink;
|
static udpLink_t stateLink, pwmLink;
|
||||||
static pthread_mutex_t updateLock;
|
static pthread_mutex_t updateLock;
|
||||||
|
static pthread_mutex_t mainLoopLock;
|
||||||
|
|
||||||
|
int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
|
||||||
|
|
||||||
|
int lockMainPID(void) {
|
||||||
|
return pthread_mutex_trylock(&mainLoopLock);
|
||||||
|
}
|
||||||
|
|
||||||
#define RAD2DEG (180.0 / M_PI)
|
#define RAD2DEG (180.0 / M_PI)
|
||||||
#define ACC_SCALE (256 / 9.81)
|
#define ACC_SCALE (256 / 9.80665)
|
||||||
#define GYRO_SCALE (8192 / 2000.0)
|
#define GYRO_SCALE (16.4)
|
||||||
|
void sendMotorUpdate() {
|
||||||
|
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
|
||||||
|
}
|
||||||
void updateState(const fdm_packet* pkt) {
|
void updateState(const fdm_packet* pkt) {
|
||||||
static double last_timestamp = 0; // in seconds
|
static double last_timestamp = 0; // in seconds
|
||||||
static uint64_t last_realtime = 0; // in uS
|
static uint64_t last_realtime = 0; // in uS
|
||||||
|
static struct timespec last_ts; // last packet
|
||||||
|
|
||||||
|
struct timespec now_ts;
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &now_ts);
|
||||||
|
|
||||||
const uint64_t realtime_now = micros64_real();
|
const uint64_t realtime_now = micros64_real();
|
||||||
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
|
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
|
||||||
last_timestamp = 0;
|
last_timestamp = pkt->timestamp;
|
||||||
last_realtime = 0;
|
last_realtime = realtime_now;
|
||||||
|
sendMotorUpdate();
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
|
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
|
||||||
if(deltaSim < 0) { // don't use old packet
|
if(deltaSim < 0) { // don't use old packet
|
||||||
pthread_mutex_unlock(&updateLock);
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -87,6 +106,8 @@ void updateState(const fdm_packet* pkt) {
|
||||||
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
|
// 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)
|
#if defined(SKIP_IMU_CALC)
|
||||||
|
#if defined(SET_IMU_FROM_EULER)
|
||||||
|
// set from Euler
|
||||||
double qw = pkt->imu_orientation_quat[0];
|
double qw = pkt->imu_orientation_quat[0];
|
||||||
double qx = pkt->imu_orientation_quat[1];
|
double qx = pkt->imu_orientation_quat[1];
|
||||||
double qy = pkt->imu_orientation_quat[2];
|
double qy = pkt->imu_orientation_quat[2];
|
||||||
|
@ -110,16 +131,36 @@ void updateState(const fdm_packet* pkt) {
|
||||||
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
|
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
|
||||||
zf = atan2(t3, t4) * RAD2DEG;
|
zf = atan2(t3, t4) * RAD2DEG;
|
||||||
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
|
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
|
||||||
|
#else
|
||||||
|
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if(last_realtime != 0 && deltaSim < 0.02 && deltaSim > 0) { // should run faster than 50Hz
|
#if defined(SIMULATOR_IMU_SYNC)
|
||||||
simRate = simRate * 0.99 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.01;
|
imuSetHasNewData(deltaSim*1e6);
|
||||||
|
imuUpdateAttitude(micros());
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
|
||||||
|
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
|
||||||
|
struct timespec out_ts;
|
||||||
|
timeval_sub(&out_ts, &now_ts, &last_ts);
|
||||||
|
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
|
||||||
}
|
}
|
||||||
|
// printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
|
||||||
|
|
||||||
last_timestamp = pkt->timestamp;
|
last_timestamp = pkt->timestamp;
|
||||||
last_realtime = micros64_real();
|
last_realtime = micros64_real();
|
||||||
// printf("simRate = %lf\n", simRate);
|
|
||||||
|
last_ts.tv_sec = now_ts.tv_sec;
|
||||||
|
last_ts.tv_nsec = now_ts.tv_nsec;
|
||||||
|
|
||||||
pthread_mutex_unlock(&updateLock); // can send PWM output now
|
pthread_mutex_unlock(&updateLock); // can send PWM output now
|
||||||
|
|
||||||
|
#if defined(SIMULATOR_GYROPID_SYNC)
|
||||||
|
pthread_mutex_unlock(&mainLoopLock); // can run main loop
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static void* udpThread(void* data) {
|
static void* udpThread(void* data) {
|
||||||
|
@ -169,6 +210,11 @@ void systemInit(void) {
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
|
||||||
|
printf("Create mainLoopLock error!\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
|
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
|
||||||
if(ret != 0) {
|
if(ret != 0) {
|
||||||
printf("Create tcpWorker error!\n");
|
printf("Create tcpWorker error!\n");
|
||||||
|
@ -186,6 +232,9 @@ void systemInit(void) {
|
||||||
printf("Create udpWorker error!\n");
|
printf("Create udpWorker error!\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// serial can't been slow down
|
||||||
|
rescheduleTask(TASK_SERIAL, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
void systemReset(void){
|
void systemReset(void){
|
||||||
|
@ -221,34 +270,43 @@ void failureMode(failureMode_e mode) {
|
||||||
|
|
||||||
// Time part
|
// Time part
|
||||||
// Thanks ArduPilot
|
// Thanks ArduPilot
|
||||||
|
uint64_t nanos64_real() {
|
||||||
|
struct timespec ts;
|
||||||
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
|
return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
|
||||||
|
}
|
||||||
uint64_t micros64_real() {
|
uint64_t micros64_real() {
|
||||||
struct timespec ts;
|
struct timespec ts;
|
||||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
|
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
|
||||||
(start_time.tv_sec +
|
|
||||||
(start_time.tv_nsec*1.0e-9)));
|
|
||||||
}
|
}
|
||||||
uint64_t millis64_real() {
|
uint64_t millis64_real() {
|
||||||
struct timespec ts;
|
struct timespec ts;
|
||||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) -
|
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
|
||||||
(start_time.tv_sec +
|
|
||||||
(start_time.tv_nsec*1.0e-9)));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t micros64() {
|
uint64_t micros64() {
|
||||||
static uint64_t last_time = 0;
|
static uint64_t last = 0;
|
||||||
uint64_t now = micros64_real();
|
static uint64_t out = 0;
|
||||||
uint64_t out = last_time + ((now - last_time) * simRate);
|
uint64_t now = nanos64_real();
|
||||||
last_time = out;
|
|
||||||
return out;
|
out += (now - last) * simRate;
|
||||||
|
last = now;
|
||||||
|
|
||||||
|
return out*1e-3;
|
||||||
|
// return micros64_real();
|
||||||
}
|
}
|
||||||
uint64_t millis64() {
|
uint64_t millis64() {
|
||||||
static uint64_t last_time = 0;
|
static uint64_t last = 0;
|
||||||
uint64_t now = millis64_real();
|
static uint64_t out = 0;
|
||||||
uint64_t out = last_time + ((now - last_time) * simRate);
|
uint64_t now = nanos64_real();
|
||||||
last_time = out;
|
|
||||||
return out;
|
out += (now - last) * simRate;
|
||||||
|
last = now;
|
||||||
|
|
||||||
|
return out*1e-6;
|
||||||
|
// return millis64_real();
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t micros(void) {
|
uint32_t micros(void) {
|
||||||
|
@ -279,6 +337,28 @@ void delay(uint32_t ms) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Subtract the ‘struct timespec’ values X and Y, storing the result in RESULT.
|
||||||
|
// Return 1 if the difference is negative, otherwise 0.
|
||||||
|
// result = x - y
|
||||||
|
// from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
|
||||||
|
int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) {
|
||||||
|
unsigned int s_carry = 0;
|
||||||
|
unsigned int ns_carry = 0;
|
||||||
|
// Perform the carry for the later subtraction by updating y.
|
||||||
|
if (x->tv_nsec < y->tv_nsec) {
|
||||||
|
int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
|
||||||
|
ns_carry += 1000000000 * nsec;
|
||||||
|
s_carry += nsec;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the time remaining to wait. tv_usec is certainly positive.
|
||||||
|
result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
|
||||||
|
result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
|
||||||
|
|
||||||
|
// Return 1 if result is negative.
|
||||||
|
return x->tv_sec < y->tv_sec;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// PWM part
|
// PWM part
|
||||||
bool pwmMotorsEnabled = false;
|
bool pwmMotorsEnabled = false;
|
||||||
|
@ -286,9 +366,9 @@ static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
|
||||||
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
|
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
|
||||||
|
|
||||||
// real value to send
|
// real value to send
|
||||||
static uint16_t motorsPwm[MAX_SUPPORTED_MOTORS];
|
static int16_t motorsPwm[MAX_SUPPORTED_MOTORS];
|
||||||
static uint16_t servosPwm[MAX_SUPPORTED_SERVOS];
|
static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
|
||||||
static uint16_t idlePulse;
|
static int16_t idlePulse;
|
||||||
|
|
||||||
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
|
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
|
||||||
UNUSED(motorConfig);
|
UNUSED(motorConfig);
|
||||||
|
@ -324,11 +404,17 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
|
||||||
void pwmCompleteMotorUpdate(uint8_t motorCount) {
|
void pwmCompleteMotorUpdate(uint8_t motorCount) {
|
||||||
UNUSED(motorCount);
|
UNUSED(motorCount);
|
||||||
// send to simulator
|
// send to simulator
|
||||||
// for gazebo8 ArduCopterPlugin remap, range = [0.0, 1.0]
|
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
|
||||||
pwmPkt.motor_speed[3] = motorsPwm[0] / 1000.0f;
|
|
||||||
pwmPkt.motor_speed[0] = motorsPwm[1] / 1000.0f;
|
double outScale = 1000.0;
|
||||||
pwmPkt.motor_speed[1] = motorsPwm[2] / 1000.0f;
|
if (feature(FEATURE_3D)) {
|
||||||
pwmPkt.motor_speed[2] = motorsPwm[3] / 1000.0f;
|
outScale = 500.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
|
||||||
|
pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
|
||||||
|
pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
|
||||||
|
pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
|
||||||
|
|
||||||
// get one "fdm_packet" can only send one "servo_packet"!!
|
// get one "fdm_packet" can only send one "servo_packet"!!
|
||||||
if(pthread_mutex_trylock(&updateLock) != 0) return;
|
if(pthread_mutex_trylock(&updateLock) != 0) return;
|
||||||
|
@ -351,6 +437,7 @@ char _Min_Stack_Size;
|
||||||
|
|
||||||
// fake EEPROM
|
// fake EEPROM
|
||||||
extern uint8_t __config_start;
|
extern uint8_t __config_start;
|
||||||
|
extern uint8_t __config_end;
|
||||||
extern uint32_t __FLASH_CONFIG_Size;
|
extern uint32_t __FLASH_CONFIG_Size;
|
||||||
static FILE *eepromFd = NULL;
|
static FILE *eepromFd = NULL;
|
||||||
const char *EEPROM_FILE = EEPROM_FILENAME;
|
const char *EEPROM_FILE = EEPROM_FILENAME;
|
||||||
|
@ -374,8 +461,8 @@ void FLASH_Unlock(void) {
|
||||||
lSize = ftell(eepromFd);
|
lSize = ftell(eepromFd);
|
||||||
rewind(eepromFd);
|
rewind(eepromFd);
|
||||||
|
|
||||||
printf("[FLASH_Unlock]size = %ld\n", lSize);
|
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
|
||||||
for(unsigned int i=0; i<((uintptr_t)&__FLASH_CONFIG_Size); i++){
|
for(unsigned int i=0; i<(uintptr_t)(&__config_end - &__config_start); i++){
|
||||||
c = fgetc(eepromFd);
|
c = fgetc(eepromFd);
|
||||||
if(c == EOF) break;
|
if(c == EOF) break;
|
||||||
eeprom[i] = (uint8_t)c;
|
eeprom[i] = (uint8_t)c;
|
||||||
|
@ -403,8 +490,12 @@ FLASH_Status FLASH_ErasePage(uint32_t Page_Address) {
|
||||||
return FLASH_COMPLETE;
|
return FLASH_COMPLETE;
|
||||||
}
|
}
|
||||||
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data) {
|
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data) {
|
||||||
|
if((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
|
||||||
*((uint32_t*)(uintptr_t)addr) = Data;
|
*((uint32_t*)(uintptr_t)addr) = Data;
|
||||||
// printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
|
// printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
|
||||||
|
}else{
|
||||||
|
printf("[FLASH_ProgramWord]Out of Range!! 0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
|
||||||
|
}
|
||||||
return FLASH_COMPLETE;
|
return FLASH_COMPLETE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -20,18 +20,31 @@
|
||||||
// SITL (software in the loop) simulator
|
// SITL (software in the loop) simulator
|
||||||
#define TARGET_BOARD_IDENTIFIER "SITL"
|
#define TARGET_BOARD_IDENTIFIER "SITL"
|
||||||
|
|
||||||
|
#define SIMULATOR_BUILD
|
||||||
|
#define SIMULATOR_MULTITHREAD
|
||||||
|
|
||||||
// use simulatior's attitude directly
|
// use simulatior's attitude directly
|
||||||
|
// disable this if wants to test AHRS algorithm
|
||||||
#define SKIP_IMU_CALC
|
#define SKIP_IMU_CALC
|
||||||
|
|
||||||
|
//#define SIMULATOR_ACC_SYNC
|
||||||
|
//#define SIMULATOR_GYRO_SYNC
|
||||||
|
//#define SIMULATOR_IMU_SYNC
|
||||||
|
//#define SIMULATOR_GYROPID_SYNC
|
||||||
|
|
||||||
// file name to save config
|
// file name to save config
|
||||||
#define EEPROM_FILENAME "eeprom.bin"
|
#define EEPROM_FILENAME "eeprom.bin"
|
||||||
|
|
||||||
#define SIMULATOR_BUILD
|
|
||||||
|
|
||||||
#define U_ID_0 0
|
#define U_ID_0 0
|
||||||
#define U_ID_1 1
|
#define U_ID_1 1
|
||||||
#define U_ID_2 2
|
#define U_ID_2 2
|
||||||
|
|
||||||
|
#undef TASK_GYROPID_DESIRED_PERIOD
|
||||||
|
#define TASK_GYROPID_DESIRED_PERIOD 100
|
||||||
|
|
||||||
|
#undef SCHEDULER_DELAY_LIMIT
|
||||||
|
#define SCHEDULER_DELAY_LIMIT 1
|
||||||
|
|
||||||
#define ACC
|
#define ACC
|
||||||
#define USE_FAKE_ACC
|
#define USE_FAKE_ACC
|
||||||
|
|
||||||
|
@ -213,14 +226,14 @@ typedef enum
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
double timestamp; // in seconds
|
double timestamp; // in seconds
|
||||||
double imu_angular_velocity_rpy[3]; // range: +/- 8192; +/- 2000 deg/se
|
double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se
|
||||||
double imu_linear_acceleration_xyz[3]; // sim 1G = 9.81 -> FC 1G = 256
|
double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256
|
||||||
double imu_orientation_quat[4];
|
double imu_orientation_quat[4]; //w, x, y, z
|
||||||
double velocity_xyz[3];
|
double velocity_xyz[3]; // m/s, earth frame
|
||||||
double position_xyz[3];
|
double position_xyz[3]; // meters, NED from origin
|
||||||
} fdm_packet;
|
} fdm_packet;
|
||||||
typedef struct {
|
typedef struct {
|
||||||
float motor_speed[4]; // [0.0, 1.0]
|
float motor_speed[4]; // normal: [0.0, 1.0], 3D: [-1.0, 1.0]
|
||||||
} servo_packet;
|
} servo_packet;
|
||||||
|
|
||||||
void FLASH_Unlock(void);
|
void FLASH_Unlock(void);
|
||||||
|
@ -228,7 +241,12 @@ void FLASH_Lock(void);
|
||||||
FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
|
FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
|
||||||
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data);
|
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data);
|
||||||
|
|
||||||
|
uint64_t nanos64_real();
|
||||||
uint64_t micros64_real();
|
uint64_t micros64_real();
|
||||||
uint64_t millis64_real();
|
uint64_t millis64_real();
|
||||||
void delayMicroseconds_real(uint32_t us);
|
void delayMicroseconds_real(uint32_t us);
|
||||||
|
uint64_t micros64();
|
||||||
|
uint64_t millis64();
|
||||||
|
|
||||||
|
int lockMainPID(void);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue