rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Remove SITL specific defines from fc_init 

Cleanup target.c indentation.
This commit is contained in:
blckmn 2017-07-02 04:40:19 +10:00
parent a2ebb196c7
commit 1ff5ff7d98
2 changed files with 291 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/fc/fc_init.c
View File

@ -315,7 +315,7 @@ void init(void)
} }
#endif #endif
#if defined(USE_SPEKTRUM_BIND) && !defined(SITL) #if defined(USE_SPEKTRUM_BIND)
if (feature(FEATURE_RX_SERIAL)) { if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) { switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM1024:
@ -338,7 +338,7 @@ void init(void)
busSwitchInit(); busSwitchInit();
#endif #endif
#if defined(USE_UART) && !defined(SITL) #if defined(USE_UART)
uartPinConfigure(serialPinConfig()); uartPinConfigure(serialPinConfig());
#endif #endif
@ -384,12 +384,12 @@ void init(void)
if (0) {} if (0) {}
#if defined(USE_PPM) #if defined(USE_PPM)
else if (feature(FEATURE_RX_PPM)) { else if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig()); ppmRxInit(ppmConfig());
} }
#endif #endif
#if defined(USE_PWM) #if defined(USE_PWM)
else if (feature(FEATURE_RX_PARALLEL_PWM)) { else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(pwmConfig()); pwmRxInit(pwmConfig());
} }
#endif #endif
@ -478,10 +478,8 @@ void init(void)
initBoardAlignment(boardAlignment()); initBoardAlignment(boardAlignment());
if (!sensorsAutodetect()) { if (!sensorsAutodetect()) {
#if !defined(SITL)
// if gyro was not detected due to whatever reason, notify and don't arm. // if gyro was not detected due to whatever reason, notify and don't arm.
failureLedCode(FAILURE_MISSING_ACC, 2); failureLedCode(FAILURE_MISSING_ACC, 2);
#endif
setArmingDisabled(ARMING_DISABLED_NO_GYRO); setArmingDisabled(ARMING_DISABLED_NO_GYRO);
} }

540
src/main/target/SITL/target.c
View File

@ -43,6 +43,8 @@ const timerHardware_t timerHardware[1]; // unused
#include "fc/config.h" #include "fc/config.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
#include "rx/rx.h"
#include "dyad.h" #include "dyad.h"
#include "target/SITL/udplink.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 timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
int lockMainPID(void) { int lockMainPID(void) {
return pthread_mutex_trylock(&mainLoopLock); return pthread_mutex_trylock(&mainLoopLock);
} }
#define RAD2DEG (180.0 / M_PI) #define RAD2DEG (180.0 / M_PI)
#define ACC_SCALE (256 / 9.80665) #define ACC_SCALE (256 / 9.80665)
#define GYRO_SCALE (16.4) #define GYRO_SCALE (16.4)
void sendMotorUpdate() { void sendMotorUpdate() {
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); 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 static struct timespec last_ts; // last packet
struct timespec now_ts; struct timespec now_ts;
clock_gettime(CLOCK_MONOTONIC, &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 = pkt->timestamp; last_timestamp = pkt->timestamp;
last_realtime = realtime_now; last_realtime = realtime_now;
sendMotorUpdate(); sendMotorUpdate();
return; 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
return; return;
} }
int16_t x,y,z; int16_t x,y,z;
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE; x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE; y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE; z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
fakeAccSet(fakeAccDev, x, y, z); 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]); // 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; x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG; y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG; z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
fakeGyroSet(fakeGyroDev, x, y, z); 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]); // 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) #if defined(SET_IMU_FROM_EULER)
// set 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];
double qz = pkt->imu_orientation_quat[3]; double qz = pkt->imu_orientation_quat[3];
double ysqr = qy * qy; double ysqr = qy * qy;
double xf, yf, zf; double xf, yf, zf;
// roll (x-axis rotation) // roll (x-axis rotation)
double t0 = +2.0 * (qw * qx + qy * qz); double t0 = +2.0 * (qw * qx + qy * qz);
double t1 = +1.0 - 2.0 * (qx * qx + ysqr); double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
xf = atan2(t0, t1) * RAD2DEG; xf = atan2(t0, t1) * RAD2DEG;
// pitch (y-axis rotation) // pitch (y-axis rotation)
double t2 = +2.0 * (qw * qy - qz * qx); double t2 = +2.0 * (qw * qy - qz * qx);
t2 = t2 > 1.0 ? 1.0 : t2; t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2; t2 = t2 < -1.0 ? -1.0 : t2;
yf = asin(t2) * RAD2DEG; // from wiki yf = asin(t2) * RAD2DEG; // from wiki
// yaw (z-axis rotation) // yaw (z-axis rotation)
double t3 = +2.0 * (qw * qz + qx * qy); double t3 = +2.0 * (qw * qz + qx * qy);
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 #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
#endif #endif
#if defined(SIMULATOR_IMU_SYNC) #if defined(SIMULATOR_IMU_SYNC)
imuSetHasNewData(deltaSim*1e6); imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros()); imuUpdateAttitude(micros());
#endif #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; // simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts; struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts); timeval_sub(&out_ts, &now_ts, &last_ts);
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec); 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); // 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();
last_ts.tv_sec = now_ts.tv_sec; last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec; 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) #if defined(SIMULATOR_GYROPID_SYNC)
pthread_mutex_unlock(&mainLoopLock); // can run main loop pthread_mutex_unlock(&mainLoopLock); // can run main loop
#endif #endif
} }
static void* udpThread(void* data) { static void* udpThread(void* data) {
UNUSED(data); UNUSED(data);
int n = 0; int n = 0;
while (workerRunning) { while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100); n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
if(n == sizeof(fdm_packet)) { if(n == sizeof(fdm_packet)) {
// printf("[data]new fdm %d\n", n); // printf("[data]new fdm %d\n", n);
updateState(&fdmPkt); updateState(&fdmPkt);
} }
} }
printf("udpThread end!!\n"); printf("udpThread end!!\n");
return NULL; return NULL;
} }
static void* tcpThread(void* data) { static void* tcpThread(void* data) {
UNUSED(data); UNUSED(data);
dyad_init(); dyad_init();
dyad_setTickInterval(0.2f); dyad_setTickInterval(0.2f);
dyad_setUpdateTimeout(0.5f); dyad_setUpdateTimeout(0.5f);
while (workerRunning) { while (workerRunning) {
dyad_update(); dyad_update();
} }
dyad_shutdown(); dyad_shutdown();
printf("tcpThread end!!\n"); printf("tcpThread end!!\n");
return NULL; return NULL;
} }
// system // system
void systemInit(void) { void systemInit(void) {
int ret; int ret;
clock_gettime(CLOCK_MONOTONIC, &start_time); clock_gettime(CLOCK_MONOTONIC, &start_time);
printf("[system]Init...\n"); printf("[system]Init...\n");
SystemCoreClock = 500 * 1e6; // fake 500MHz SystemCoreClock = 500 * 1e6; // fake 500MHz
FLASH_Unlock(); FLASH_Unlock();
if (pthread_mutex_init(&updateLock, NULL) != 0) { if (pthread_mutex_init(&updateLock, NULL) != 0) {
printf("Create updateLock error!\n"); printf("Create updateLock error!\n");
exit(1); exit(1);
} }
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
printf("Create mainLoopLock error!\n"); printf("Create mainLoopLock error!\n");
exit(1); 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");
exit(1); exit(1);
} }
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false); ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
printf("init PwnOut UDP link...%d\n", ret); printf("init PwnOut UDP link...%d\n", ret);
ret = udpInit(&stateLink, NULL, 9003, true); ret = udpInit(&stateLink, NULL, 9003, true);
printf("start UDP server...%d\n", ret); printf("start UDP server...%d\n", ret);
ret = pthread_create(&udpWorker, NULL, udpThread, NULL); ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
if(ret != 0) { if(ret != 0) {
printf("Create udpWorker error!\n"); printf("Create udpWorker error!\n");
exit(1); exit(1);
} }
// serial can't been slow down // serial can't been slow down
rescheduleTask(TASK_SERIAL, 1); rescheduleTask(TASK_SERIAL, 1);
} }
void systemReset(void){ void systemReset(void){
printf("[system]Reset!\n"); printf("[system]Reset!\n");
workerRunning = false; workerRunning = false;
pthread_join(tcpWorker, NULL); pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL); pthread_join(udpWorker, NULL);
exit(0); exit(0);
} }
void systemResetToBootloader(void) { void systemResetToBootloader(void) {
printf("[system]ResetToBootloader!\n"); printf("[system]ResetToBootloader!\n");
workerRunning = false; workerRunning = false;
pthread_join(tcpWorker, NULL); pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL); pthread_join(udpWorker, NULL);
exit(0); exit(0);
} }
// drivers/light_led.c // drivers/light_led.c
void ledInit(const statusLedConfig_t *statusLedConfig) { void ledInit(const statusLedConfig_t *statusLedConfig) {
UNUSED(statusLedConfig); UNUSED(statusLedConfig);
printf("[led]Init...\n"); 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 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 // Time part
// Thanks ArduPilot // Thanks ArduPilot
uint64_t nanos64_real() { uint64_t nanos64_real() {
struct timespec ts; struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts); clock_gettime(CLOCK_MONOTONIC, &ts);
return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec); 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)) - (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() { 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)) - (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() { uint64_t micros64() {
static uint64_t last = 0; static uint64_t last = 0;
static uint64_t out = 0; static uint64_t out = 0;
uint64_t now = nanos64_real(); uint64_t now = nanos64_real();
out += (now - last) * simRate; out += (now - last) * simRate;
last = now; last = now;
return out*1e-3; return out*1e-3;
// return micros64_real(); // return micros64_real();
} }
uint64_t millis64() { uint64_t millis64() {
static uint64_t last = 0; static uint64_t last = 0;
static uint64_t out = 0; static uint64_t out = 0;
uint64_t now = nanos64_real(); uint64_t now = nanos64_real();
out += (now - last) * simRate; out += (now - last) * simRate;
last = now; last = now;
return out*1e-6; return out*1e-6;
// return millis64_real(); // return millis64_real();
} }
uint32_t micros(void) { uint32_t micros(void) {
return micros64() & 0xFFFFFFFF; return micros64() & 0xFFFFFFFF;
} }
uint32_t millis(void) { uint32_t millis(void) {
return millis64() & 0xFFFFFFFF; return millis64() & 0xFFFFFFFF;
} }
void microsleep(uint32_t usec) { void microsleep(uint32_t usec) {
@ -321,18 +334,21 @@ void microsleep(uint32_t usec) {
ts.tv_nsec = usec*1000UL; ts.tv_nsec = usec*1000UL;
while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ; 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) { void delayMicroseconds(uint32_t us) {
microsleep(1000); 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. // 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 // result = x - y
// from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html // 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) { int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) {
unsigned int s_carry = 0; unsigned int s_carry = 0;
unsigned int ns_carry = 0; unsigned int ns_carry = 0;
// Perform the carry for the later subtraction by updating y. // Perform the carry for the later subtraction by updating y.
if (x->tv_nsec < y->tv_nsec) { if (x->tv_nsec < y->tv_nsec) {
int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1; int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
ns_carry += 1000000000 * nsec; ns_carry += 1000000000 * nsec;
s_carry += nsec; s_carry += nsec;
} }
// Compute the time remaining to wait. tv_usec is certainly positive. // Compute the time remaining to wait. tv_usec is certainly positive.
result->tv_sec = x->tv_sec - y->tv_sec - s_carry; result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry; result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
// Return 1 if result is negative. // Return 1 if result is negative.
return x->tv_sec < y->tv_sec; return x->tv_sec < y->tv_sec;
} }
@ -369,68 +385,75 @@ static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
static int16_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);
UNUSED(motorCount); UNUSED(motorCount);
idlePulse = _idlePulse; idlePulse = _idlePulse;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true; motors[motorIndex].enabled = true;
} }
pwmMotorsEnabled = true; pwmMotorsEnabled = true;
} }
void servoDevInit(const servoDevConfig_t *servoConfig) { void servoDevInit(const servoDevConfig_t *servoConfig) {
UNUSED(servoConfig); UNUSED(servoConfig);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
servos[servoIndex].enabled = true; servos[servoIndex].enabled = true;
} }
} }
pwmOutputPort_t *pwmGetMotors(void) { pwmOutputPort_t *pwmGetMotors(void) {
return motors; return motors;
} }
bool pwmAreMotorsEnabled(void) { bool pwmAreMotorsEnabled(void) {
return pwmMotorsEnabled; return pwmMotorsEnabled;
} }
bool isMotorProtocolDshot(void) bool isMotorProtocolDshot(void)
{ {
return false; return false;
} }
void pwmWriteMotor(uint8_t index, float value) { void pwmWriteMotor(uint8_t index, float value) {
motorsPwm[index] = value - idlePulse; motorsPwm[index] = value - idlePulse;
} }
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
UNUSED(motorCount); UNUSED(motorCount);
pwmMotorsEnabled = false; pwmMotorsEnabled = false;
} }
void pwmCompleteMotorUpdate(uint8_t motorCount) { void pwmCompleteMotorUpdate(uint8_t motorCount) {
UNUSED(motorCount); UNUSED(motorCount);
// send to simulator // send to simulator
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
double outScale = 1000.0; double outScale = 1000.0;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
outScale = 500.0; outScale = 500.0;
} }
pwmPkt.motor_speed[3] = motorsPwm[0] / outScale; pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
pwmPkt.motor_speed[0] = motorsPwm[1] / outScale; pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
pwmPkt.motor_speed[1] = motorsPwm[2] / outScale; pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
pwmPkt.motor_speed[2] = motorsPwm[3] / 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;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
} }
void pwmWriteServo(uint8_t index, float value) { void pwmWriteServo(uint8_t index, float value) {
servosPwm[index] = value; servosPwm[index] = value;
} }
// ADC part // ADC part
uint16_t adcGetChannel(uint8_t channel) { uint16_t adcGetChannel(uint8_t channel) {
UNUSED(channel); UNUSED(channel);
return 0; return 0;
} }
// stack part // stack part
@ -438,62 +461,73 @@ char _estack;
char _Min_Stack_Size; char _Min_Stack_Size;
// fake EEPROM // fake EEPROM
extern uint8_t __config_start; uint8_t __config_start;
extern uint8_t __config_end; uint8_t __config_end;
static FILE *eepromFd = NULL; static FILE *eepromFd = NULL;
void FLASH_Unlock(void) { void FLASH_Unlock(void) {
uint8_t * const eeprom = &__config_start; uint8_t * const eeprom = &__config_start;
if (eepromFd != NULL) { if (eepromFd != NULL) {
printf("[FLASH_Unlock] eepromFd != NULL\n"); printf("[FLASH_Unlock] eepromFd != NULL\n");
return; return;
} }
// open or create // open or create
eepromFd = fopen(EEPROM_FILENAME,"r+"); eepromFd = fopen(EEPROM_FILENAME,"r+");
if (eepromFd != NULL) { if (eepromFd != NULL) {
// obtain file size: // obtain file size:
fseek(eepromFd , 0 , SEEK_END); fseek(eepromFd , 0 , SEEK_END);
long lSize = ftell(eepromFd); long lSize = ftell(eepromFd);
rewind(eepromFd); rewind(eepromFd);
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start)); printf("[FLASH_Unlock]size = %ld, %d\n", lSize, (uintptr_t)(&__config_end - &__config_start));
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) { for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
int c = fgetc(eepromFd); int c = fgetc(eepromFd);
if(c == EOF) break; if(c == EOF) break;
eeprom[i] = (uint8_t)c; eeprom[i] = (uint8_t)c;
} }
} else { } else {
eepromFd = fopen(EEPROM_FILENAME, "w+"); eepromFd = fopen(EEPROM_FILENAME, "w+");
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd); fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
} }
} }
void FLASH_Lock(void) { void FLASH_Lock(void) {
// flush & close // flush & close
if (eepromFd != NULL) { if (eepromFd != NULL) {
const uint8_t *eeprom = &__config_start; const uint8_t *eeprom = &__config_start;
fseek(eepromFd, 0, SEEK_SET); fseek(eepromFd, 0, SEEK_SET);
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd); fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
fclose(eepromFd); fclose(eepromFd);
eepromFd = NULL; eepromFd = NULL;
} }
} }
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) { FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
UNUSED(Page_Address); UNUSED(Page_Address);
// printf("[FLASH_ErasePage]%x\n", 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) { FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) { if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
*((uint32_t*)addr) = Data; *((uint32_t*)addr) = Data;
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr)); printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
} else { } else {
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr); printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
} }
return FLASH_COMPLETE; return FLASH_COMPLETE;
} }
void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
{
UNUSED(pSerialPinConfig);
printf("uartPinConfigure");
}
void spektrumBind(rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
printf("spektrumBind");
}