Remove SITL specific defines from fc_init
Cleanup target.c indentation.
This commit is contained in:
parent
a2ebb196c7
commit
1ff5ff7d98
|
@ -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
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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"
|
||||||
|
|
||||||
|
@ -255,16 +257,24 @@ void ledInit(const statusLedConfig_t *statusLedConfig) {
|
||||||
UNUSED(statusLedConfig);
|
UNUSED(statusLedConfig);
|
||||||
printf("[led]Init...\n");
|
printf("[led]Init...\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
void timerInit(void) {
|
void timerInit(void) {
|
||||||
printf("[timer]Init...\n");
|
printf("[timer]Init...\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
void timerStart(void) {
|
void timerStart(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void failureMode(failureMode_e mode) {
|
void failureMode(failureMode_e mode) {
|
||||||
printf("[failureMode]!!! %d\n", mode);
|
printf("[failureMode]!!! %d\n", mode);
|
||||||
while(1);
|
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
|
||||||
|
@ -273,11 +283,13 @@ uint64_t nanos64_real() {
|
||||||
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);
|
||||||
|
@ -295,6 +307,7 @@ uint64_t micros64() {
|
||||||
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;
|
||||||
|
@ -321,12 +334,15 @@ 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) {
|
void delayMicroseconds(uint32_t us) {
|
||||||
microsleep(us / simRate);
|
microsleep(us / simRate);
|
||||||
}
|
}
|
||||||
|
|
||||||
void delayMicroseconds_real(uint32_t us) {
|
void delayMicroseconds_real(uint32_t us) {
|
||||||
microsleep(us);
|
microsleep(us);
|
||||||
}
|
}
|
||||||
|
|
||||||
void delay(uint32_t ms) {
|
void delay(uint32_t ms) {
|
||||||
uint64_t start = millis64();
|
uint64_t start = millis64();
|
||||||
|
|
||||||
|
@ -379,6 +395,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint
|
||||||
}
|
}
|
||||||
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++) {
|
||||||
|
@ -389,20 +406,25 @@ void servoDevInit(const servoDevConfig_t *servoConfig) {
|
||||||
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
|
||||||
|
@ -423,6 +445,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) {
|
||||||
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;
|
||||||
}
|
}
|
||||||
|
@ -438,8 +461,8 @@ 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) {
|
||||||
|
@ -458,7 +481,7 @@ void FLASH_Unlock(void) {
|
||||||
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;
|
||||||
|
@ -486,6 +509,7 @@ FLASH_Status FLASH_ErasePage(uintptr_t 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;
|
||||||
|
@ -496,4 +520,14 @@ FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
|
||||||
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");
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue