From efbb7520a875f89ae2e66e824ff80d35557a8098 Mon Sep 17 00:00:00 2001 From: cs8425 Date: Thu, 6 Apr 2017 00:41:40 +0800 Subject: [PATCH] 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 --- src/main/drivers/accgyro_fake.c | 87 ++++++++++++++++- src/main/drivers/serial_tcp.c | 30 +++--- src/main/drivers/serial_tcp.h | 10 +- src/main/fc/fc_core.c | 4 + src/main/flight/imu.c | 67 +++++++++++++ src/main/flight/imu.h | 3 + src/main/scheduler/scheduler.c | 3 + src/main/target/SITL/README.md | 17 +++- src/main/target/SITL/target.c | 165 +++++++++++++++++++++++++------- src/main/target/SITL/target.h | 34 +++++-- 10 files changed, 350 insertions(+), 70 deletions(-) diff --git a/src/main/drivers/accgyro_fake.c b/src/main/drivers/accgyro_fake.c index 105f6a33e..3add29d66 100644 --- a/src/main/drivers/accgyro_fake.c +++ b/src/main/drivers/accgyro_fake.c @@ -20,6 +20,28 @@ #include "platform.h" +#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) +#include +#include +#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; } diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 93c1657a2..578e51b97 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -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--) { diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h index 80c6d727f..65cef9c84 100644 --- a/src/main/drivers/serial_tcp.h +++ b/src/main/drivers/serial_tcp.h @@ -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); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 57b5c30f1..b39e42ff8 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 78fe82a54..3d8f6a9f7 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -49,6 +49,28 @@ #include "sensors/sensors.h" #include "sensors/sonar.h" +#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD) +#include +#include +#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 diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 41aebab9e..048d96ae0 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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 diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 4ff2fda49..d42ff4089 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -126,6 +126,9 @@ void taskSystem(timeUs_t currentTimeUs) totalWaitingTasksSamples = 0; totalWaitingTasks = 0; } +#if defined(SIMULATOR_BUILD) + averageSystemLoadPercent = 0; +#endif } #ifndef SKIP_TASK_STATISTICS diff --git a/src/main/target/SITL/README.md b/src/main/target/SITL/README.md index 241c37739..9eb7de084 100644 --- a/src/main/target/SITL/README.md +++ b/src/main/target/SITL/README.md @@ -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`: `0` to `100` -***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` diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 29bb62eae..e46a8261f 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -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; } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 1c63017ba..075359779 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -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);