diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index ff4bebf42..90489fffd 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -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); } diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index efe3e1865..804ebe7b3 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -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, %d\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); + } } 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"); +}