some fix & should be usable
use quaternion directly & we can fly in 3D mode now rename uartPort_t to tcpPort_t fix race on ACC, GYRO, IMU fix gyro scale & disable SystemLoad calculate update README.md remove some unused fix scale on 3D mode
This commit is contained in:
cs8425
parent
9053feb418
commit
efbb7520a8
|
|
@ -20,6 +20,28 @@
|
|||
|
||||
#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
|
||||
|
||||
#include "common/axis.h"
|
||||
|
|
@ -28,26 +50,55 @@
|
|||
#include "accgyro.h"
|
||||
#include "accgyro_fake.h"
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
GYRO_LOCK;
|
||||
|
||||
fakeGyroADC[X] = x;
|
||||
fakeGyroADC[Y] = y;
|
||||
fakeGyroADC[Z] = z;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYRO_SYNC)
|
||||
gyroUpdated = true;
|
||||
#endif
|
||||
GYRO_UNLOCK;
|
||||
}
|
||||
|
||||
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[Y] = fakeGyroADC[Y];
|
||||
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
||||
|
||||
GYRO_UNLOCK;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -70,7 +121,11 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
|||
gyro->intStatus = fakeGyroInitStatus;
|
||||
gyro->read = fakeGyroRead;
|
||||
gyro->temperature = fakeGyroReadTemperature;
|
||||
#if defined(SIMULATOR_BUILD)
|
||||
gyro->scale = 1.0f / 16.4f;
|
||||
#else
|
||||
gyro->scale = 1.0f;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
#endif // USE_FAKE_GYRO
|
||||
|
|
@ -80,23 +135,53 @@ bool fakeGyroDetect(gyroDev_t *gyro)
|
|||
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
ACC_LOCK;
|
||||
|
||||
fakeAccData[X] = x;
|
||||
fakeAccData[Y] = y;
|
||||
fakeAccData[Z] = z;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_ACC_SYNC)
|
||||
accUpdated = true;
|
||||
#endif
|
||||
ACC_LOCK;
|
||||
}
|
||||
|
||||
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[Y] = fakeAccData[Y];
|
||||
acc->ADCRaw[Z] = fakeAccData[Z];
|
||||
|
||||
ACC_UNLOCK;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,18 +38,18 @@
|
|||
#define BASE_PORT 5760
|
||||
|
||||
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 tcpStart = false;
|
||||
bool tcpIsStart(void) {
|
||||
return tcpStart;
|
||||
}
|
||||
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);
|
||||
}
|
||||
static void onClose(dyad_Event *e) {
|
||||
uartPort_t* s = (uartPort_t*)(e->udata);
|
||||
tcpPort_t* s = (tcpPort_t*)(e->udata);
|
||||
s->clientCount--;
|
||||
s->conn = NULL;
|
||||
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) {
|
||||
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);
|
||||
|
||||
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_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]) {
|
||||
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)
|
||||
{
|
||||
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)
|
||||
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)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t*)instance;
|
||||
tcpPort_t *s = (tcpPort_t*)instance;
|
||||
uint32_t count;
|
||||
pthread_mutex_lock(&s->rxLock);
|
||||
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)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t*)instance;
|
||||
tcpPort_t *s = (tcpPort_t*)instance;
|
||||
uint32_t bytesUsed;
|
||||
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
|
|
@ -175,7 +175,7 @@ uint32_t tcpTotalTxBytesFree(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);
|
||||
bool isEmpty = s->port.txBufferTail == s->port.txBufferHead;
|
||||
pthread_mutex_unlock(&s->txLock);
|
||||
|
|
@ -185,7 +185,7 @@ bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
|||
uint8_t tcpRead(serialPort_t *instance)
|
||||
{
|
||||
uint8_t ch;
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
tcpPort_t *s = (tcpPort_t *)instance;
|
||||
pthread_mutex_lock(&s->rxLock);
|
||||
|
||||
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)
|
||||
{
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
tcpPort_t *s = (tcpPort_t *)instance;
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
|
||||
s->port.txBuffer[s->port.txBufferHead] = ch;
|
||||
|
|
@ -215,10 +215,10 @@ void tcpWrite(serialPort_t *instance, uint8_t ch)
|
|||
tcpDataOut(s);
|
||||
}
|
||||
|
||||
void tcpDataOut(uartPort_t *instance)
|
||||
void tcpDataOut(tcpPort_t *instance)
|
||||
{
|
||||
uint32_t bytesUsed;
|
||||
uartPort_t *s = (uartPort_t *)instance;
|
||||
tcpPort_t *s = (tcpPort_t *)instance;
|
||||
if(s->conn == NULL) return;
|
||||
pthread_mutex_lock(&s->txLock);
|
||||
|
||||
|
|
@ -234,9 +234,9 @@ void tcpDataOut(uartPort_t *instance)
|
|||
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);
|
||||
|
||||
while(size--) {
|
||||
|
|
|
|||
|
|
@ -41,20 +41,20 @@ typedef struct {
|
|||
bool connected;
|
||||
uint16_t clientCount;
|
||||
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 API
|
||||
// tcpPort API
|
||||
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 tcpTotalTxBytesFree(const 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 tcpIsStart(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 uint8_t pidUpdateCountdown;
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
|
||||
if(lockMainPID() != 0) return;
|
||||
#endif
|
||||
|
||||
if (debugMode == DEBUG_CYCLETIME) {
|
||||
debug[0] = getTaskDeltaTime(TASK_SELF);
|
||||
debug[1] = averageSystemLoadPercent;
|
||||
|
|
|
|||
|
|
@ -49,6 +49,28 @@
|
|||
#include "sensors/sensors.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
|
||||
// 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][1] = 2.0f * (q2q3 - -q0q1);
|
||||
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;
|
||||
|
||||
imuComputeRotationMatrix();
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
if (pthread_mutex_init(&imuUpdateLock, NULL) != 0) {
|
||||
printf("Create imuUpdateLock error!\n");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void imuResetAccelerationSum(void)
|
||||
|
|
@ -402,6 +435,12 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
UNUSED(useYaw);
|
||||
UNUSED(rawYawError);
|
||||
#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,
|
||||
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],
|
||||
|
|
@ -416,7 +455,16 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
void imuUpdateAttitude(timeUs_t currentTimeUs)
|
||||
{
|
||||
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);
|
||||
IMU_UNLOCK;
|
||||
} else {
|
||||
acc.accSmooth[X] = 0;
|
||||
acc.accSmooth[Y] = 0;
|
||||
|
|
@ -448,12 +496,18 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|||
#ifdef SIMULATOR_BUILD
|
||||
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
|
||||
{
|
||||
IMU_LOCK;
|
||||
|
||||
attitude.values.roll = roll * 10;
|
||||
attitude.values.pitch = pitch * 10;
|
||||
attitude.values.yaw = yaw * 10;
|
||||
|
||||
IMU_UNLOCK;
|
||||
}
|
||||
void imuSetAttitudeQuat(float w, float x, float y, float z)
|
||||
{
|
||||
IMU_LOCK;
|
||||
|
||||
q0 = w;
|
||||
q1 = x;
|
||||
q2 = y;
|
||||
|
|
@ -461,6 +515,19 @@ void imuSetAttitudeQuat(float w, float x, float y, float z)
|
|||
|
||||
imuComputeRotationMatrix();
|
||||
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
|
||||
|
||||
|
|
|
|||
|
|
@ -78,5 +78,8 @@ void imuInit(void);
|
|||
void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg
|
||||
void imuSetAttitudeQuat(float w, float x, float y, float z);
|
||||
#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;
|
||||
totalWaitingTasks = 0;
|
||||
}
|
||||
#if defined(SIMULATOR_BUILD)
|
||||
averageSystemLoadPercent = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifndef SKIP_TASK_STATISTICS
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
## 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.
|
||||
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
|
||||
see here: [Installation](http://gazebosim.org/tutorials?cat=install]
|
||||
see here: [Installation](http://gazebosim.org/tutorials?cat=install)
|
||||
|
||||
### copy & modify world
|
||||
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>`
|
||||
to
|
||||
`<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).
|
||||
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.
|
||||
smaller mean more accurate, but need higher speed CPU to run as realtime.
|
||||
|
||||
### build betaflight
|
||||
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
|
||||
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).
|
||||
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
|
||||
betaflight -> gazebo `udp://127.0.0.1:9002`
|
||||
|
|
|
|||
|
|
@ -36,11 +36,15 @@
|
|||
|
||||
#include "drivers/timer.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 "flight/imu.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "fc/config.h"
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "dyad.h"
|
||||
#include "target/SITL/udplink.h"
|
||||
|
||||
|
|
@ -53,23 +57,38 @@ static pthread_t tcpWorker, udpWorker;
|
|||
static bool workerRunning = true;
|
||||
static udpLink_t stateLink, pwmLink;
|
||||
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 ACC_SCALE (256 / 9.81)
|
||||
#define GYRO_SCALE (8192 / 2000.0)
|
||||
#define ACC_SCALE (256 / 9.80665)
|
||||
#define GYRO_SCALE (16.4)
|
||||
void sendMotorUpdate() {
|
||||
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
|
||||
}
|
||||
void updateState(const fdm_packet* pkt) {
|
||||
static double last_timestamp = 0; // in seconds
|
||||
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();
|
||||
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
|
||||
last_timestamp = 0;
|
||||
last_realtime = 0;
|
||||
last_timestamp = pkt->timestamp;
|
||||
last_realtime = realtime_now;
|
||||
sendMotorUpdate();
|
||||
return;
|
||||
}
|
||||
|
||||
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
|
||||
if(deltaSim < 0) { // don't use old packet
|
||||
pthread_mutex_unlock(&updateLock);
|
||||
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]);
|
||||
|
||||
#if defined(SKIP_IMU_CALC)
|
||||
#if defined(SET_IMU_FROM_EULER)
|
||||
// set from Euler
|
||||
double qw = pkt->imu_orientation_quat[0];
|
||||
double qx = pkt->imu_orientation_quat[1];
|
||||
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);
|
||||
zf = atan2(t3, t4) * RAD2DEG;
|
||||
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
|
||||
|
||||
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;
|
||||
#if defined(SIMULATOR_IMU_SYNC)
|
||||
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_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
|
||||
|
||||
#if defined(SIMULATOR_GYROPID_SYNC)
|
||||
pthread_mutex_unlock(&mainLoopLock); // can run main loop
|
||||
#endif
|
||||
}
|
||||
|
||||
static void* udpThread(void* data) {
|
||||
|
|
@ -169,6 +210,11 @@ void systemInit(void) {
|
|||
exit(1);
|
||||
}
|
||||
|
||||
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
|
||||
printf("Create mainLoopLock error!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
|
||||
if(ret != 0) {
|
||||
printf("Create tcpWorker error!\n");
|
||||
|
|
@ -186,6 +232,9 @@ void systemInit(void) {
|
|||
printf("Create udpWorker error!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// serial can't been slow down
|
||||
rescheduleTask(TASK_SERIAL, 1);
|
||||
}
|
||||
|
||||
void systemReset(void){
|
||||
|
|
@ -221,34 +270,43 @@ void failureMode(failureMode_e mode) {
|
|||
|
||||
// Time part
|
||||
// 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() {
|
||||
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)));
|
||||
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)));
|
||||
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;
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
|
||||
return out*1e-3;
|
||||
// return micros64_real();
|
||||
}
|
||||
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;
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
|
||||
return out*1e-6;
|
||||
// return millis64_real();
|
||||
}
|
||||
|
||||
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
|
||||
bool pwmMotorsEnabled = false;
|
||||
|
|
@ -286,9 +366,9 @@ 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;
|
||||
static int16_t motorsPwm[MAX_SUPPORTED_MOTORS];
|
||||
static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
|
||||
static int16_t idlePulse;
|
||||
|
||||
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
|
||||
UNUSED(motorConfig);
|
||||
|
|
@ -324,11 +404,17 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
|
|||
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;
|
||||
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
|
||||
|
||||
double outScale = 1000.0;
|
||||
if (feature(FEATURE_3D)) {
|
||||
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"!!
|
||||
if(pthread_mutex_trylock(&updateLock) != 0) return;
|
||||
|
|
@ -351,6 +437,7 @@ char _Min_Stack_Size;
|
|||
|
||||
// fake EEPROM
|
||||
extern uint8_t __config_start;
|
||||
extern uint8_t __config_end;
|
||||
extern uint32_t __FLASH_CONFIG_Size;
|
||||
static FILE *eepromFd = NULL;
|
||||
const char *EEPROM_FILE = EEPROM_FILENAME;
|
||||
|
|
@ -374,8 +461,8 @@ void FLASH_Unlock(void) {
|
|||
lSize = ftell(eepromFd);
|
||||
rewind(eepromFd);
|
||||
|
||||
printf("[FLASH_Unlock]size = %ld\n", lSize);
|
||||
for(unsigned int i=0; i<((uintptr_t)&__FLASH_CONFIG_Size); i++){
|
||||
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
|
||||
for(unsigned int i=0; i<(uintptr_t)(&__config_end - &__config_start); i++){
|
||||
c = fgetc(eepromFd);
|
||||
if(c == EOF) break;
|
||||
eeprom[i] = (uint8_t)c;
|
||||
|
|
@ -403,8 +490,12 @@ FLASH_Status FLASH_ErasePage(uint32_t 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));
|
||||
if((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
|
||||
*((uint32_t*)(uintptr_t)addr) = Data;
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,18 +20,31 @@
|
|||
// SITL (software in the loop) simulator
|
||||
#define TARGET_BOARD_IDENTIFIER "SITL"
|
||||
|
||||
#define SIMULATOR_BUILD
|
||||
#define SIMULATOR_MULTITHREAD
|
||||
|
||||
// use simulatior's attitude directly
|
||||
// disable this if wants to test AHRS algorithm
|
||||
#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
|
||||
#define EEPROM_FILENAME "eeprom.bin"
|
||||
|
||||
#define SIMULATOR_BUILD
|
||||
|
||||
#define U_ID_0 0
|
||||
#define U_ID_1 1
|
||||
#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 USE_FAKE_ACC
|
||||
|
||||
|
|
@ -213,14 +226,14 @@ typedef enum
|
|||
|
||||
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];
|
||||
double imu_angular_velocity_rpy[3]; // rad/s -> range: +/- 8192; +/- 2000 deg/se
|
||||
double imu_linear_acceleration_xyz[3]; // m/s/s NED, body frame -> sim 1G = 9.80665, FC 1G = 256
|
||||
double imu_orientation_quat[4]; //w, x, y, z
|
||||
double velocity_xyz[3]; // m/s, earth frame
|
||||
double position_xyz[3]; // meters, NED from origin
|
||||
} fdm_packet;
|
||||
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;
|
||||
|
||||
void FLASH_Unlock(void);
|
||||
|
|
@ -228,7 +241,12 @@ void FLASH_Lock(void);
|
|||
FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
|
||||
FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data);
|
||||
|
||||
uint64_t nanos64_real();
|
||||
uint64_t micros64_real();
|
||||
uint64_t millis64_real();
|
||||
void delayMicroseconds_real(uint32_t us);
|
||||
uint64_t micros64();
|
||||
uint64_t millis64();
|
||||
|
||||
int lockMainPID(void);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue