Merge pull request #3403 from blckmn/SITL
Remove SITL specific defines from fc_init
This commit is contained in:
J Blackman
GitHub
commit
5aa81c81ce
|
|
@ -315,7 +315,7 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
|
||||
#if defined(USE_SPEKTRUM_BIND)
|
||||
if (feature(FEATURE_RX_SERIAL)) {
|
||||
switch (rxConfig()->serialrx_provider) {
|
||||
case SERIALRX_SPEKTRUM1024:
|
||||
|
|
@ -338,7 +338,7 @@ void init(void)
|
|||
busSwitchInit();
|
||||
#endif
|
||||
|
||||
#if defined(USE_UART) && !defined(SITL)
|
||||
#if defined(USE_UART)
|
||||
uartPinConfigure(serialPinConfig());
|
||||
#endif
|
||||
|
||||
|
|
@ -384,12 +384,12 @@ void init(void)
|
|||
if (0) {}
|
||||
#if defined(USE_PPM)
|
||||
else if (feature(FEATURE_RX_PPM)) {
|
||||
ppmRxInit(ppmConfig());
|
||||
ppmRxInit(ppmConfig());
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_PWM)
|
||||
else if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
pwmRxInit(pwmConfig());
|
||||
pwmRxInit(pwmConfig());
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -478,10 +478,8 @@ void init(void)
|
|||
initBoardAlignment(boardAlignment());
|
||||
|
||||
if (!sensorsAutodetect()) {
|
||||
#if !defined(SITL)
|
||||
// if gyro was not detected due to whatever reason, notify and don't arm.
|
||||
failureLedCode(FAILURE_MISSING_ACC, 2);
|
||||
#endif
|
||||
setArmingDisabled(ARMING_DISABLED_NO_GYRO);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,6 +43,8 @@ const timerHardware_t timerHardware[1]; // unused
|
|||
#include "fc/config.h"
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "dyad.h"
|
||||
#include "target/SITL/udplink.h"
|
||||
|
||||
|
|
@ -60,259 +62,270 @@ 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);
|
||||
return pthread_mutex_trylock(&mainLoopLock);
|
||||
}
|
||||
|
||||
#define RAD2DEG (180.0 / M_PI)
|
||||
#define ACC_SCALE (256 / 9.80665)
|
||||
#define GYRO_SCALE (16.4)
|
||||
void sendMotorUpdate() {
|
||||
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
|
||||
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
|
||||
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);
|
||||
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 = pkt->timestamp;
|
||||
last_realtime = realtime_now;
|
||||
sendMotorUpdate();
|
||||
return;
|
||||
}
|
||||
const uint64_t realtime_now = micros64_real();
|
||||
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
|
||||
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
|
||||
return;
|
||||
}
|
||||
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
|
||||
if(deltaSim < 0) { // don't use old packet
|
||||
return;
|
||||
}
|
||||
|
||||
int16_t x,y,z;
|
||||
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
|
||||
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
|
||||
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
|
||||
fakeAccSet(fakeAccDev, x, y, z);
|
||||
int16_t x,y,z;
|
||||
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
|
||||
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
|
||||
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
|
||||
fakeAccSet(fakeAccDev, x, y, z);
|
||||
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
|
||||
|
||||
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
|
||||
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
|
||||
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
|
||||
fakeGyroSet(fakeGyroDev, x, y, z);
|
||||
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
|
||||
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
|
||||
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
|
||||
fakeGyroSet(fakeGyroDev, x, y, z);
|
||||
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
|
||||
|
||||
#if defined(SKIP_IMU_CALC)
|
||||
#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];
|
||||
double qz = pkt->imu_orientation_quat[3];
|
||||
double ysqr = qy * qy;
|
||||
double xf, yf, zf;
|
||||
// set from Euler
|
||||
double qw = pkt->imu_orientation_quat[0];
|
||||
double qx = pkt->imu_orientation_quat[1];
|
||||
double qy = pkt->imu_orientation_quat[2];
|
||||
double qz = pkt->imu_orientation_quat[3];
|
||||
double ysqr = qy * qy;
|
||||
double xf, yf, zf;
|
||||
|
||||
// roll (x-axis rotation)
|
||||
double t0 = +2.0 * (qw * qx + qy * qz);
|
||||
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
|
||||
xf = atan2(t0, t1) * RAD2DEG;
|
||||
// roll (x-axis rotation)
|
||||
double t0 = +2.0 * (qw * qx + qy * qz);
|
||||
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
|
||||
xf = atan2(t0, t1) * RAD2DEG;
|
||||
|
||||
// pitch (y-axis rotation)
|
||||
double t2 = +2.0 * (qw * qy - qz * qx);
|
||||
t2 = t2 > 1.0 ? 1.0 : t2;
|
||||
t2 = t2 < -1.0 ? -1.0 : t2;
|
||||
yf = asin(t2) * RAD2DEG; // from wiki
|
||||
// pitch (y-axis rotation)
|
||||
double t2 = +2.0 * (qw * qy - qz * qx);
|
||||
t2 = t2 > 1.0 ? 1.0 : t2;
|
||||
t2 = t2 < -1.0 ? -1.0 : t2;
|
||||
yf = asin(t2) * RAD2DEG; // from wiki
|
||||
|
||||
// yaw (z-axis rotation)
|
||||
double t3 = +2.0 * (qw * qz + qx * qy);
|
||||
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
|
||||
zf = atan2(t3, t4) * RAD2DEG;
|
||||
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
|
||||
// yaw (z-axis rotation)
|
||||
double t3 = +2.0 * (qw * qz + qx * qy);
|
||||
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
|
||||
zf = atan2(t3, t4) * RAD2DEG;
|
||||
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
|
||||
#else
|
||||
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
|
||||
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(SIMULATOR_IMU_SYNC)
|
||||
imuSetHasNewData(deltaSim*1e6);
|
||||
imuUpdateAttitude(micros());
|
||||
imuSetHasNewData(deltaSim*1e6);
|
||||
imuUpdateAttitude(micros());
|
||||
#endif
|
||||
|
||||
|
||||
if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
|
||||
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);
|
||||
}
|
||||
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();
|
||||
last_timestamp = pkt->timestamp;
|
||||
last_realtime = micros64_real();
|
||||
|
||||
last_ts.tv_sec = now_ts.tv_sec;
|
||||
last_ts.tv_nsec = now_ts.tv_nsec;
|
||||
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
|
||||
pthread_mutex_unlock(&mainLoopLock); // can run main loop
|
||||
#endif
|
||||
}
|
||||
|
||||
static void* udpThread(void* data) {
|
||||
UNUSED(data);
|
||||
int n = 0;
|
||||
UNUSED(data);
|
||||
int n = 0;
|
||||
|
||||
while (workerRunning) {
|
||||
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
|
||||
if(n == sizeof(fdm_packet)) {
|
||||
while (workerRunning) {
|
||||
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
|
||||
if(n == sizeof(fdm_packet)) {
|
||||
// printf("[data]new fdm %d\n", n);
|
||||
updateState(&fdmPkt);
|
||||
}
|
||||
}
|
||||
updateState(&fdmPkt);
|
||||
}
|
||||
}
|
||||
|
||||
printf("udpThread end!!\n");
|
||||
return NULL;
|
||||
printf("udpThread end!!\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void* tcpThread(void* data) {
|
||||
UNUSED(data);
|
||||
UNUSED(data);
|
||||
|
||||
dyad_init();
|
||||
dyad_setTickInterval(0.2f);
|
||||
dyad_setUpdateTimeout(0.5f);
|
||||
dyad_init();
|
||||
dyad_setTickInterval(0.2f);
|
||||
dyad_setUpdateTimeout(0.5f);
|
||||
|
||||
while (workerRunning) {
|
||||
dyad_update();
|
||||
}
|
||||
while (workerRunning) {
|
||||
dyad_update();
|
||||
}
|
||||
|
||||
dyad_shutdown();
|
||||
printf("tcpThread end!!\n");
|
||||
return NULL;
|
||||
dyad_shutdown();
|
||||
printf("tcpThread end!!\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// system
|
||||
void systemInit(void) {
|
||||
int ret;
|
||||
int ret;
|
||||
|
||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||
printf("[system]Init...\n");
|
||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||
printf("[system]Init...\n");
|
||||
|
||||
SystemCoreClock = 500 * 1e6; // fake 500MHz
|
||||
FLASH_Unlock();
|
||||
SystemCoreClock = 500 * 1e6; // fake 500MHz
|
||||
FLASH_Unlock();
|
||||
|
||||
if (pthread_mutex_init(&updateLock, NULL) != 0) {
|
||||
printf("Create updateLock error!\n");
|
||||
exit(1);
|
||||
}
|
||||
if (pthread_mutex_init(&updateLock, NULL) != 0) {
|
||||
printf("Create updateLock error!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
|
||||
printf("Create mainLoopLock error!\n");
|
||||
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");
|
||||
exit(1);
|
||||
}
|
||||
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
|
||||
if(ret != 0) {
|
||||
printf("Create tcpWorker error!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
|
||||
printf("init PwnOut UDP link...%d\n", ret);
|
||||
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
|
||||
printf("init PwnOut UDP link...%d\n", ret);
|
||||
|
||||
ret = udpInit(&stateLink, NULL, 9003, true);
|
||||
printf("start UDP server...%d\n", ret);
|
||||
ret = udpInit(&stateLink, NULL, 9003, true);
|
||||
printf("start UDP server...%d\n", ret);
|
||||
|
||||
ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
|
||||
if(ret != 0) {
|
||||
printf("Create udpWorker error!\n");
|
||||
exit(1);
|
||||
}
|
||||
ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
|
||||
if(ret != 0) {
|
||||
printf("Create udpWorker error!\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// serial can't been slow down
|
||||
rescheduleTask(TASK_SERIAL, 1);
|
||||
// serial can't been slow down
|
||||
rescheduleTask(TASK_SERIAL, 1);
|
||||
}
|
||||
|
||||
void systemReset(void){
|
||||
printf("[system]Reset!\n");
|
||||
workerRunning = false;
|
||||
pthread_join(tcpWorker, NULL);
|
||||
pthread_join(udpWorker, NULL);
|
||||
exit(0);
|
||||
printf("[system]Reset!\n");
|
||||
workerRunning = false;
|
||||
pthread_join(tcpWorker, NULL);
|
||||
pthread_join(udpWorker, NULL);
|
||||
exit(0);
|
||||
}
|
||||
void systemResetToBootloader(void) {
|
||||
printf("[system]ResetToBootloader!\n");
|
||||
workerRunning = false;
|
||||
pthread_join(tcpWorker, NULL);
|
||||
pthread_join(udpWorker, NULL);
|
||||
exit(0);
|
||||
printf("[system]ResetToBootloader!\n");
|
||||
workerRunning = false;
|
||||
pthread_join(tcpWorker, NULL);
|
||||
pthread_join(udpWorker, NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
// drivers/light_led.c
|
||||
void ledInit(const statusLedConfig_t *statusLedConfig) {
|
||||
UNUSED(statusLedConfig);
|
||||
printf("[led]Init...\n");
|
||||
}
|
||||
void timerInit(void) {
|
||||
printf("[timer]Init...\n");
|
||||
}
|
||||
void timerStart(void) {
|
||||
}
|
||||
void failureMode(failureMode_e mode) {
|
||||
printf("[failureMode]!!! %d\n", mode);
|
||||
while(1);
|
||||
UNUSED(statusLedConfig);
|
||||
printf("[led]Init...\n");
|
||||
}
|
||||
|
||||
void timerInit(void) {
|
||||
printf("[timer]Init...\n");
|
||||
}
|
||||
|
||||
void timerStart(void) {
|
||||
}
|
||||
|
||||
void failureMode(failureMode_e mode) {
|
||||
printf("[failureMode]!!! %d\n", mode);
|
||||
while(1);
|
||||
}
|
||||
|
||||
void failureLedCode(failureMode_e mode, int repeatCount)
|
||||
{
|
||||
UNUSED(repeatCount);
|
||||
printf("Failure LED flash for: [failureMode]!!! %d\n", 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);
|
||||
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)));
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
|
||||
}
|
||||
|
||||
uint64_t millis64_real() {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
|
||||
}
|
||||
|
||||
uint64_t micros64() {
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
|
||||
return out*1e-3;
|
||||
return out*1e-3;
|
||||
// return micros64_real();
|
||||
}
|
||||
|
||||
uint64_t millis64() {
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
static uint64_t last = 0;
|
||||
static uint64_t out = 0;
|
||||
uint64_t now = nanos64_real();
|
||||
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
out += (now - last) * simRate;
|
||||
last = now;
|
||||
|
||||
return out*1e-6;
|
||||
return out*1e-6;
|
||||
// return millis64_real();
|
||||
}
|
||||
|
||||
uint32_t micros(void) {
|
||||
return micros64() & 0xFFFFFFFF;
|
||||
return micros64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
uint32_t millis(void) {
|
||||
return millis64() & 0xFFFFFFFF;
|
||||
return millis64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
void microsleep(uint32_t usec) {
|
||||
|
|
@ -321,18 +334,21 @@ void microsleep(uint32_t usec) {
|
|||
ts.tv_nsec = usec*1000UL;
|
||||
while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
|
||||
}
|
||||
void delayMicroseconds(uint32_t us) {
|
||||
microsleep(us / simRate);
|
||||
}
|
||||
void delayMicroseconds_real(uint32_t us) {
|
||||
microsleep(us);
|
||||
}
|
||||
void delay(uint32_t ms) {
|
||||
uint64_t start = millis64();
|
||||
|
||||
while ((millis64() - start) < ms) {
|
||||
microsleep(1000);
|
||||
}
|
||||
void delayMicroseconds(uint32_t us) {
|
||||
microsleep(us / simRate);
|
||||
}
|
||||
|
||||
void delayMicroseconds_real(uint32_t us) {
|
||||
microsleep(us);
|
||||
}
|
||||
|
||||
void delay(uint32_t ms) {
|
||||
uint64_t start = millis64();
|
||||
|
||||
while ((millis64() - start) < ms) {
|
||||
microsleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
// Subtract the ‘struct timespec’ values X and Y, storing the result in RESULT.
|
||||
|
|
@ -340,21 +356,21 @@ void delay(uint32_t ms) {
|
|||
// 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;
|
||||
}
|
||||
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;
|
||||
// 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;
|
||||
// Return 1 if result is negative.
|
||||
return x->tv_sec < y->tv_sec;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -369,68 +385,75 @@ 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);
|
||||
UNUSED(motorCount);
|
||||
UNUSED(motorConfig);
|
||||
UNUSED(motorCount);
|
||||
|
||||
idlePulse = _idlePulse;
|
||||
idlePulse = _idlePulse;
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
motors[motorIndex].enabled = true;
|
||||
}
|
||||
pwmMotorsEnabled = true;
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
motors[motorIndex].enabled = true;
|
||||
}
|
||||
pwmMotorsEnabled = true;
|
||||
}
|
||||
|
||||
void servoDevInit(const servoDevConfig_t *servoConfig) {
|
||||
UNUSED(servoConfig);
|
||||
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||
servos[servoIndex].enabled = true;
|
||||
}
|
||||
UNUSED(servoConfig);
|
||||
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||
servos[servoIndex].enabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
pwmOutputPort_t *pwmGetMotors(void) {
|
||||
return motors;
|
||||
return motors;
|
||||
}
|
||||
|
||||
bool pwmAreMotorsEnabled(void) {
|
||||
return pwmMotorsEnabled;
|
||||
return pwmMotorsEnabled;
|
||||
}
|
||||
|
||||
bool isMotorProtocolDshot(void)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
void pwmWriteMotor(uint8_t index, float value) {
|
||||
motorsPwm[index] = value - idlePulse;
|
||||
motorsPwm[index] = value - idlePulse;
|
||||
}
|
||||
|
||||
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
|
||||
UNUSED(motorCount);
|
||||
pwmMotorsEnabled = false;
|
||||
UNUSED(motorCount);
|
||||
pwmMotorsEnabled = false;
|
||||
}
|
||||
|
||||
void pwmCompleteMotorUpdate(uint8_t motorCount) {
|
||||
UNUSED(motorCount);
|
||||
// send to simulator
|
||||
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
|
||||
UNUSED(motorCount);
|
||||
// send to simulator
|
||||
// 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;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
|
||||
// get one "fdm_packet" can only send one "servo_packet"!!
|
||||
if(pthread_mutex_trylock(&updateLock) != 0) return;
|
||||
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
|
||||
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
|
||||
}
|
||||
|
||||
void pwmWriteServo(uint8_t index, float value) {
|
||||
servosPwm[index] = value;
|
||||
servosPwm[index] = value;
|
||||
}
|
||||
|
||||
// ADC part
|
||||
uint16_t adcGetChannel(uint8_t channel) {
|
||||
UNUSED(channel);
|
||||
return 0;
|
||||
UNUSED(channel);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// stack part
|
||||
|
|
@ -438,62 +461,73 @@ char _estack;
|
|||
char _Min_Stack_Size;
|
||||
|
||||
// fake EEPROM
|
||||
extern uint8_t __config_start;
|
||||
extern uint8_t __config_end;
|
||||
uint8_t __config_start;
|
||||
uint8_t __config_end;
|
||||
static FILE *eepromFd = NULL;
|
||||
|
||||
void FLASH_Unlock(void) {
|
||||
uint8_t * const eeprom = &__config_start;
|
||||
uint8_t * const eeprom = &__config_start;
|
||||
|
||||
if (eepromFd != NULL) {
|
||||
printf("[FLASH_Unlock] eepromFd != NULL\n");
|
||||
return;
|
||||
}
|
||||
if (eepromFd != NULL) {
|
||||
printf("[FLASH_Unlock] eepromFd != NULL\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// open or create
|
||||
eepromFd = fopen(EEPROM_FILENAME,"r+");
|
||||
if (eepromFd != NULL) {
|
||||
// obtain file size:
|
||||
fseek(eepromFd , 0 , SEEK_END);
|
||||
long lSize = ftell(eepromFd);
|
||||
rewind(eepromFd);
|
||||
// open or create
|
||||
eepromFd = fopen(EEPROM_FILENAME,"r+");
|
||||
if (eepromFd != NULL) {
|
||||
// obtain file size:
|
||||
fseek(eepromFd , 0 , SEEK_END);
|
||||
long lSize = ftell(eepromFd);
|
||||
rewind(eepromFd);
|
||||
|
||||
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
|
||||
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
|
||||
int c = fgetc(eepromFd);
|
||||
if(c == EOF) break;
|
||||
eeprom[i] = (uint8_t)c;
|
||||
}
|
||||
} else {
|
||||
eepromFd = fopen(EEPROM_FILENAME, "w+");
|
||||
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
|
||||
}
|
||||
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start));
|
||||
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
|
||||
int c = fgetc(eepromFd);
|
||||
if(c == EOF) break;
|
||||
eeprom[i] = (uint8_t)c;
|
||||
}
|
||||
} else {
|
||||
eepromFd = fopen(EEPROM_FILENAME, "w+");
|
||||
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
|
||||
}
|
||||
}
|
||||
|
||||
void FLASH_Lock(void) {
|
||||
// flush & close
|
||||
if (eepromFd != NULL) {
|
||||
const uint8_t *eeprom = &__config_start;
|
||||
fseek(eepromFd, 0, SEEK_SET);
|
||||
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
|
||||
fclose(eepromFd);
|
||||
eepromFd = NULL;
|
||||
}
|
||||
// flush & close
|
||||
if (eepromFd != NULL) {
|
||||
const uint8_t *eeprom = &__config_start;
|
||||
fseek(eepromFd, 0, SEEK_SET);
|
||||
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
|
||||
fclose(eepromFd);
|
||||
eepromFd = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
|
||||
UNUSED(Page_Address);
|
||||
UNUSED(Page_Address);
|
||||
// printf("[FLASH_ErasePage]%x\n", Page_Address);
|
||||
return FLASH_COMPLETE;
|
||||
return FLASH_COMPLETE;
|
||||
}
|
||||
|
||||
FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
|
||||
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
|
||||
*((uint32_t*)addr) = Data;
|
||||
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
|
||||
} else {
|
||||
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
|
||||
}
|
||||
return FLASH_COMPLETE;
|
||||
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
|
||||
*((uint32_t*)addr) = Data;
|
||||
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
|
||||
} else {
|
||||
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
|
||||
}
|
||||
return FLASH_COMPLETE;
|
||||
}
|
||||
|
||||
void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
|
||||
{
|
||||
UNUSED(pSerialPinConfig);
|
||||
printf("uartPinConfigure");
|
||||
}
|
||||
|
||||
void spektrumBind(rxConfig_t *rxConfig)
|
||||
{
|
||||
UNUSED(rxConfig);
|
||||
printf("spektrumBind");
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue