static UINT32
sysCoreInit(
void
)
{
#if 0 // gp_board static linked with kernel
/* board core init */
boardCoreInit();
#endif
apbdma0Init();
if( gp_ver.major == MACH_GPL32900 )
{
i2cInit();
}
if( gp_ver.major == MACH_GPL32900B )
{
i2cInit();
ti2cInit();
}
timerInit();
pwmInit();
adcInit();
dc2dcInit();
/* board config and board init */
boardInit();
/* display init */
displayInit();
// storageInit();
sysMknod("board", S_IFCHR);
sysMknod("chunkmem", S_IFCHR);
mknod("/dev/null", S_IFCHR|0660, makedev(MEM_MAJOR, 3));
return SP_OK;
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// initialize our libraries
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(9600);
// set uartSendByte as the output for all rprintf statements
rprintfInit(uartSendByte);
// initialize the timer system
timerInit();
// initialize vt100 library
vt100Init();
vt100ClearScreen();
// print a little intro message so we know things are working
rprintf("\r\nWelcome to Servo Test!\r\n");
// begin servo test
servoTest();
return 0;
}
void init(void)
{
// Initialize Timer
timerInit();
// Initialize LCD
lcdInit();
ourLcdControlWrite(1<<LCD_ON_CTRL | 1<<LCD_ON_DISPLAY);
// Initialize UART
uartInit();
uartSetBaudRate(CMU_BAUD);
uartSetRxHandler(packetRcv);
rprintfInit(uartSendByte);
// Initialize PWM
outb(DDRD, 0xFF); // set all port D pins to output
timer1PWMInit(8);
timer1PWMAOn();
timer1PWMBOn();
// Initialize Servos
servoInit();
// Initialize CMU
lcdWriteStr("CMUcam2 init", 0, 0);
cmuInit();
}
void gameboyInit(Gameboy *gameboy, Cartridge *cartridge)
{
CPU *cpu = malloc(sizeof(CPU));
cpuInit(cpu);
MMU *mmu = malloc(sizeof(MMU));
mmuInit(mmu, cartridge);
Timer *timer = malloc(sizeof(Timer));
timerInit(timer);
Display *display = malloc(sizeof(Display));
displayInit(display);
Joypad *joypad = malloc(sizeof(Joypad));
inputInit(joypad);
gameboy->cpu = cpu;
gameboy->mmu = mmu;
gameboy->timer = timer;
gameboy->display = display;
gameboy->joypad = joypad;
}
void _init(void)
{
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_EEPROM0);
if(ROM_EEPROMInit() == EEPROM_INIT_ERROR) {
if(ROM_EEPROMInit() != EEPROM_INIT_ERROR)
EEPROMMassErase();
}
timerInit();
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOK);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOL);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOM);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPION);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOP);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOQ);
#ifdef TARGET_IS_SNOWFLAKE_RA0
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOR);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOS);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOT);
#endif
//Unlock and commit NMI pins PD7 and PF0
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0x4C4F434B;
HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x1;
HWREG(GPIO_PORTD_BASE + GPIO_O_LOCK) = 0x4C4F434B;
HWREG(GPIO_PORTD_BASE + GPIO_O_CR) |= 0x80;
} /* void _init(void) */
void main() {
timerInit();
sei();
pwmInit();
int16_t i = 0;
uint32_t lastDetentTimestamp;
pwmSet(0);
int8_t encoderPulses = 0;
while(1) {
encoderPulses += encoderRead();
if( encoderPulses > 3 || encoderPulses < -3 ) {
int8_t encoderStep = encoderPulses / 4;
if(millis - lastDetentTimestamp < 20) {
i+= encoderStep*10;
} else if(millis - lastDetentTimestamp < 30) {
i+= encoderStep*5;
} else if(millis - lastDetentTimestamp < 40) {
i+= encoderStep*2;
} else {
i+= encoderStep;
}
if(i > 187) {
i = 187;
}
if(i < 0) {
i = 0;
}
targetPwm = (uint8_t) pgm_read_byte(&(pwm[i]));
lastDetentTimestamp = millis;
//_delay_ms(1);
encoderPulses = 0;
}
// _delay_ms(5);
}
}
/**
* @fn void userInit( void );
* @brief Configuration des périphériques
*/
void userInit( void ) {
unsigned int periphBusClk;
// Config Système
// Cache (wait states) et bus périphériques
// retourne la vitesse de travail du bus périphérque (non utilisée)
periphBusClk = SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
// Désactivation du JTAG pour accéder aux LEDs
mJTAGPortEnable( DEBUG_JTAGPORT_OFF );
// Config UART
uartInit();
// uartPutString("UART configured\r\n");
//Config GPIO
gpioInit();
// uartPutString("GPIO configured\r\n");
// Config PWM
timerInit();
pwmInit();
// uartPutString("PWM configured\r\n");
// Config SPI
spiChannel = ads7885Pic32Open( CHN_SPI, 20 );
// uartPutString("SPI configured\r\n");
// Init du l'algorithme de commande
mesure[2][pMesure]=ads7885Pic32Read( CHN_SPI,3);//data_VO;
init_stockage(mesure[2][pMesure],pwm,ref);
// Config CAN1
CAN1Init();
// uartPutString("CAN configured\r\n");
}
int main(void)
{
// initialize processor
processorInit();
// initialize timers
timerInit();
// initialize uarts
uart0Init(UART_BAUD(115200), UART_8N1, UART_FIFO_8);
uart1Init(UART_BAUD(115200), UART_8N1, UART_FIFO_8);
// initialize rprintf to use UART1 for output
rprintfInit(uart1SendByte);
// Wait for a moment to allow hardware to stabilize.
// This may be important if a serial port level-converter
// like the MAX232 is used. Charge-pump based converters
// need some time after power-up to charge before
// communication is reliable.
timerPause(50); // waits 50 milliseconds
// run the test
uartTest();
return 0;
}
int main(void) {
//spiInit(SPIDEV_1);
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
SetSysClock();
systemInit();
timerInit(); // timer must be initialized before any channel is allocated
serial0 = serial0_open();
dmaInit();
setup();
while (true) {
#ifndef EXTERNAL_DEBUG
static uint32_t dbg_start_msec;
// support reboot from host computer
if (millis()-dbg_start_msec > 100) {
dbg_start_msec = millis();
while (serialRxBytesWaiting(serial0)) {
uint8_t c = serialRead(serial0);
if (c == 'R')
systemResetToBootloader();
}
}
#endif
loop();
}
} // main
int main(void)
{
uchar i;
uchar calibrationValue;
calibrationValue = eeprom_read_byte(0); /* calibration value from last time */
if(calibrationValue != 0xff) {
OSCCAL = calibrationValue;
}
odDebugInit();
usbDeviceDisconnect();
for(i=0; i<20; i++) { /* 300 ms disconnect */
_delay_ms(15);
}
usbDeviceConnect();
wdt_enable(WDTO_1S);
timerInit();
TimerDelay = 630; /* initial 10 second delay */
usbInit();
sei();
for(;;) { /* main event loop */
wdt_reset();
usbPoll();
// lets have something waiting in here or mabe generate a random number
if(usbInterruptIsReady() && reportCount < 2) { /* we can send another key */
buildReport();
usbSetInterrupt(reportBuffer, sizeof(reportBuffer));
}
timerPoll();
}
return 0;
}
int main(void)
{
struct netEthAddr myEthAddress;
timerInit();
uartInit();
uartSetBaudRate(9600);
rprintfInit(uartSendByte);
timerPause(100);
rprintf("\r\nNetwork Stack Example\r\n");
// initialize systick timer
rprintf("Initializing Periodic Timer\r\n");
timer2SetPrescaler(TIMER_CLK_DIV1024);
timerAttach(TIMER2OVERFLOW_INT, systickHandler);
// init network stack
rprintf("Initializing Network Stack\r\n");
netstackInit(IPADDRESS, NETMASK, GATEWAY);
nicGetMacAddress(&myEthAddress.addr[0]);
rprintfProgStrM("Eth Addr is: "); netPrintEthAddr(&myEthAddress); rprintfCRLF();
rprintfProgStrM("IP Addr is: "); netPrintIPAddr(ipGetConfig()->ip); rprintfCRLF();
rprintf("Network Stack is up!\r\n");
rprintf("Starting packet receive loop\r\n");
while (1)
{
// service local stuff
serviceLocal();
// service the network
netstackService();
}
return 0;
}
void ourtosInit(uint8_t maxPriority, freq_t freq) {
int i;
/* Initialize globals.
*
* Note that we don't initialize _mainLoopStackPointer, since the timer ISR
* will always run at least once and initialize it before we try to use it.
*/
_maxPriority = maxPriority;
_started = false;
_debug = false;
_mutexesEnabled = true;
_currentTask = MAIN_LOOP_PRIORITY;
/* Initialize task array.
* Mark that all priorities are not yet devoted to either mutexes or tasks.
*/
for (i = 0; i < _maxPriority; i++) {
taskArray[i].usage = USAGE_NONE;
}
timerInit(freq);
serialInit(BAUD_9600);
}
void init() {
buildFont();
resetObjects(); // Set Player / Enemy Starting Positions
timerInit();
}
/* ------------------------------------------------------------------------- */
int main(void)
{
unsigned int i;
uchar calibrationValue;
calibrationValue = eeprom_read_byte(0); /* calibration value from last time */
if(calibrationValue != 0xff) {
OSCCAL = calibrationValue;
}
//odDebugInit();
//Production Test Routine - Turn on white LED.
DDRB |= _BV(PB1); /* output for LED */
sbi(PORTB, PB1);
for(i=0;i<20;i++) { /* 300 ms disconnect */
_delay_ms(15);
}
cbi(PORTB, PB1);
//Initialize the USB Connection with the host computer.
usbDeviceDisconnect();
for(i=0;i<20;i++){ /* 300 ms disconnect */
_delay_ms(15);
}
usbDeviceConnect();
wdt_enable(WDTO_1S);
pinsInit();
interruptsInit();
timerInit();
usbInit(); //Initialize USB comm.
sei();
for(;;)
{ /* main event loop */
wdt_reset();
usbPoll(); //Check to see if it's time to send a USB packet
if (isRecBufBusy && nextDigit==NULL)
{
#ifndef DECODE
if (rbcur>rbidx)
isRecBufBusy = 0;
else
reportChar(rbuff[rbcur++], 1);
#else
uchar command;
command = decodeBuffer();
if (0!=command)
{
if (command!=cmdPrev)
{
reportChar(pgm_read_byte(&lookupTable[command & 0b00111111]), 0); // 64 values maximum
cmdPrev = command;
cmdCounter = 1;
}
else if (++cmdCounter==3)
{
cmdCounter = 0;
cmdPrev = 0;
}
}
isRecBufBusy = 0;
#endif
}
if(usbInterruptIsReady() && nextDigit!=NULL)
{ /* we can send another key */
buildReport(); //Get the next 'key press' to send to the host.
usbSetInterrupt(reportBuffer, sizeof(reportBuffer));
if(*++nextDigit == 0xff) /* this was terminator character */
nextDigit = NULL;
}
}
return 0;
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef STM32F40_41xxx
SetSysClock();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
ledInit();
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_USART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_USART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioPeripheral = BEEP_PERIPHERAL,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
spiInit(SPI3);
spiInit(SPI4);
spiInit(SPI5);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
#if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(STM32F4DISCOVERY)
i2cInit(I2C_DEVICE_INT);
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
#ifdef I2C_DEVICE_EXT
i2cInit(I2C_DEVICE_EXT);
#endif
}
#endif
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(3);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
cliInit(&masterConfig.serialConfig);
failsafeInit(&masterConfig.rxConfig);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.batteryConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
#ifdef COLIBRI
if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) {
ledStripEnable();
}
#else
ledStripEnable();
#endif
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#endif
#if defined(SPRACINGF3) || defined(CC3D) || defined(COLIBRI) || defined(REVO)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX
//initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
//gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
//timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
#include <stdio.h>
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
GPIO_InitTypeDef GPIO_InitStruct;
int main(void) {
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_14 | GPIO_Pin_13
| GPIO_Pin_12;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOD, &GPIO_InitStruct);
printf("Hello World!\r\n");
hello();
while (1) {
static int count = 0;
static int i;
for (i = 0; i < 10000000; ++i)
;
GPIO_ToggleBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15);
printf("%d\r\n", ++count);
}
//init();
/*
while (1) {
//loop();
int x = 1;//processLoopback();
}*/
}
void main(void)
{
// Stop WDT
WDTCTL = WDTPW + WDTHOLD;
// Initialize the boards
boardInit();
clockInit();
timerInit();
flashInit();
__bis_SR_register(GIE);
// Set up the LCD
LCDInit();
GrContextInit(&g_sContext, &g_sharp96x96LCD);
GrContextForegroundSet(&g_sContext, ClrBlack);
GrContextBackgroundSet(&g_sContext, ClrWhite);
GrContextFontSet(&g_sContext, &g_sFontFixed6x8);
GrClearDisplay(&g_sContext);
GrFlush(&g_sContext);
// Intro Screen
GrStringDrawCentered(&g_sContext,
"How to use",
AUTO_STRING_LENGTH,
48,
15,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"the MSP430",
AUTO_STRING_LENGTH,
48,
35,
TRANSPARENT_TEXT);
GrStringDraw(&g_sContext,
"Graphics Library",
AUTO_STRING_LENGTH,
1,
51,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"Primitives",
AUTO_STRING_LENGTH,
48,
75,
TRANSPARENT_TEXT);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Draw pixels and lines on the display
GrStringDrawCentered(&g_sContext,
"Draw Pixels",
AUTO_STRING_LENGTH,
48,
5,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"& Lines",
AUTO_STRING_LENGTH,
48,
15,
TRANSPARENT_TEXT);
GrPixelDraw(&g_sContext, 30, 30);
GrPixelDraw(&g_sContext, 30, 32);
GrPixelDraw(&g_sContext, 32, 32);
GrPixelDraw(&g_sContext, 32, 30);
GrLineDraw(&g_sContext, 35, 35, 90, 90);
GrLineDraw(&g_sContext, 5, 80, 80, 20);
GrLineDraw(&g_sContext,
0,
GrContextDpyHeightGet(&g_sContext) - 1,
GrContextDpyWidthGet(&g_sContext) - 1,
GrContextDpyHeightGet(&g_sContext) - 1);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Draw circles on the display
GrStringDraw(&g_sContext,
"Draw Circles",
AUTO_STRING_LENGTH,
10,
5,
TRANSPARENT_TEXT);
GrCircleDraw(&g_sContext, 30, 70, 20);
GrCircleFill(&g_sContext, 60, 50, 30);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Draw rectangles on the display
GrStringDrawCentered(&g_sContext,
"Draw Rectangles",
AUTO_STRING_LENGTH,
48,
5,
TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangle1);
GrRectFill(&g_sContext, &myRectangle2);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Combining Primitive screen
GrStringDrawCentered(&g_sContext,
"Combining",
AUTO_STRING_LENGTH,
48,
15,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"Primitives to",
AUTO_STRING_LENGTH,
48,
35,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"create menus",
AUTO_STRING_LENGTH,
48,
51,
TRANSPARENT_TEXT);
GrStringDrawCentered(&g_sContext,
"and animations",
AUTO_STRING_LENGTH,
48,
75,
TRANSPARENT_TEXT);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Draw a Menu screen
GrStringDrawCentered(&g_sContext,
"Create a Menu",
AUTO_STRING_LENGTH,
48,
5,
TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangleOption1);
GrStringDraw(&g_sContext,"Option #1", 10,15,15,TRANSPARENT_TEXT);
GrRectFill(&g_sContext, &myRectangleOption2);
GrStringDraw(&g_sContext,"Option #2", 10,15,25,TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangleOption3);
GrStringDraw(&g_sContext,"Option #3", 10,15,35,TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangleOption4);
GrStringDraw(&g_sContext,"Option #4", 10,15,45,TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangleOption5);
GrStringDraw(&g_sContext,"Option #5", 10,15,55,TRANSPARENT_TEXT);
GrFlush(&g_sContext);
Delay_long();
GrClearDisplay(&g_sContext);
// Show progress bar screen
// The following animation consist on displaying a progress bar and
// updating the progress bar in increments of 25%.
GrStringDrawCentered(&g_sContext,
"Show progress",
AUTO_STRING_LENGTH,
48,
5,
TRANSPARENT_TEXT);
GrRectDraw(&g_sContext, &myRectangleFrame);
GrStringDrawCentered(&g_sContext,
"Processing...",
AUTO_STRING_LENGTH,
48,
75,
TRANSPARENT_TEXT);
GrFlush(&g_sContext);
Delay_short();
// Update display with 25 %. Initial value of "myRectangleProgress" are set
// to update bar with a 25 % increment.
GrRectFill(&g_sContext, &myRectangleProgress);
GrFlush(&g_sContext);
Delay_short();
// Set myRectangleProgress values to update progress bar with 50 %
myRectangleProgress.sXMin = 30;
myRectangleProgress.sYMin = 40;
myRectangleProgress.sXMax = 50;
myRectangleProgress.sYMax = 60;
GrRectFill(&g_sContext, &myRectangleProgress);
GrFlush(&g_sContext);
Delay_short();
// Set myRectangleProgress values to update progress bar with 75 %
myRectangleProgress.sXMin = 50;
myRectangleProgress.sYMin = 40;
myRectangleProgress.sXMax = 70;
myRectangleProgress.sYMax = 60;
GrRectFill(&g_sContext, &myRectangleProgress);
GrFlush(&g_sContext);
Delay_short();
// Set myRectangleProgress values to update progress bar with 100 %
myRectangleProgress.sXMin = 70;
myRectangleProgress.sYMin = 40;
myRectangleProgress.sXMax = 90;
myRectangleProgress.sYMax = 60;
GrRectFill(&g_sContext, &myRectangleProgress);
GrStringDrawCentered(&g_sContext,
"DONE!",
AUTO_STRING_LENGTH,
48,
85,
TRANSPARENT_TEXT);
GrFlush(&g_sContext);
Delay_long();
while(1);
}
void init(void)
{
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
// initialize IO (needed for all IO operations)
IOInitGlobal();
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
i2cSetOverclock(masterConfig.i2c_overclock);
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(500);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
if (sonarHardware) {
pwm_params.useSonar = true;
pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag;
}
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isServoOutputEnabled();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
#ifndef SKIP_RX_PWM_PPM
pwmRxInit(masterConfig.inputFilteringMode);
#endif
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmInit(&pwm_params);
mixerUsePWMIOConfiguration();
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.ioTag = IO_TAG(BEEPER),
#ifdef BEEPER_INVERTED
.isOD = false,
.isInverted = true
#else
.isOD = true,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.isOD = false;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(FURYF3) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsPreInit(&masterConfig.gpsConfig);
}
#endif
// Set gyro sampling rate divider before initialization
gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator);
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.gyro_lpf,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
}
#endif
#ifdef NAV
navigationInit(
&masterConfig.navConfig,
¤tProfile->pidProfile,
¤tProfile->rcControlsConfig,
&masterConfig.rxConfig,
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig
);
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
while (true) {
scheduler();
processLoopback();
}
}
int main(int argc, char *argv[]) {
int i, j;
int thisSocket, thatSocket;
int *confPort;
unsigned int tmpSeconds;
char *confConfig, *confEngine, *confPassword;
char magicCookie[] = "TELLUTELLUTELLUTELLUTELLUTELLU";
size_t s;
uid_t *thisUid;
gid_t *thisGid;
/*
*
* Initialize default values.
*
*/
pMainMainInfo = &mainMainInfo;
pMainThreadInfo = &mainThreadInfo;
memset(pMainMainInfo, 0, sizeof(mainMainInfo));
memset(pMainThreadInfo, 0, sizeof(mainThreadInfo));
/*
*
* Read command line and parse configuration file.
*
*/
cmdRead(argv, argc);
if((confConfig = configFetch("config_file", &i)) != NULL) {
if(configRead(confConfig) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to read configuration file");
}
}
else {
if(configRead(CONFIG_DEFAULT_FILE) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to read configuration file");
}
}
cmdRead(argv, argc);
nodeInitNames();
/*
*
* Initialize thread pool.
*
*/
if((threadPool = malloc(sizeof(struct threadInfo) * THREAD_TELSKIND)) == NULL) {
warningMessage(ERROR_FATAL, "Error occurred while trying to allocate memory for thread pool");
}
memset(threadPool, 0, sizeof(struct threadInfo) * THREAD_TELSKIND);
/*
*
* Initialize configurable subroutines.
*
*/
pMainThreadInfo->threadReady = 1;
for(j = 0; j < THREAD_TELSKIND; j++) {
threadPool[j].threadReady = 1;
threadPool[j].magicCookie = magicCookie;
threadPool[j].pMainInfo = pMainMainInfo;
if((confEngine = configFetch("storage_engine", &i)) != NULL) {
if(strncasecmp(confEngine, "plain", strlen(confEngine)) == 0) {
threadPool[j].dbInfo.connect = plainConnect;
threadPool[j].dbInfo.disconnect = plainDisconnect;
threadPool[j].dbInfo.escape = plainEscape;
threadPool[j].dbInfo.push = plainPush;
threadPool[j].dbInfo.pull = plainPull;
threadPool[j].dbInfo.round = plainRound;
threadPool[j].dbInfo.free = plainFree;
threadPool[j].dbInfo.expire = plainExpire;
threadPool[j].dbInfo.cookie = plainCookie;
threadPool[j].dbInfo.insert = plainInsert;
threadPool[j].dbInfo.login = plainLogin;
threadPool[j].dbInfo.logout = plainLogout;
threadPool[j].dbInfo.session = plainSession;
threadPool[j].dbInfo.permission = plainPermission;
pMainThreadInfo->dbInfo.connect = plainConnect;
pMainThreadInfo->dbInfo.disconnect = plainDisconnect;
pMainThreadInfo->dbInfo.escape = plainEscape;
pMainThreadInfo->dbInfo.push = plainPush;
pMainThreadInfo->dbInfo.pull = plainPull;
pMainThreadInfo->dbInfo.round = plainRound;
pMainThreadInfo->dbInfo.free = plainFree;
pMainThreadInfo->dbInfo.expire = plainExpire;
pMainThreadInfo->dbInfo.cookie = plainCookie;
pMainThreadInfo->dbInfo.insert = plainInsert;
pMainThreadInfo->dbInfo.login = plainLogin;
pMainThreadInfo->dbInfo.logout = plainLogout;
pMainThreadInfo->dbInfo.session = plainSession;
pMainThreadInfo->dbInfo.permission = plainPermission;
continue;
}
}
threadPool[j].dbInfo.connect = mysqlConnect;
threadPool[j].dbInfo.disconnect = mysqlDisconnect;
threadPool[j].dbInfo.escape = mysqlEscape;
threadPool[j].dbInfo.push = mysqlPush;
threadPool[j].dbInfo.pull = mysqlPull;
threadPool[j].dbInfo.round = mysqlRound;
threadPool[j].dbInfo.free = mysqlFree;
threadPool[j].dbInfo.expire = mysqlExpire;
threadPool[j].dbInfo.cookie = mysqlCookie;
threadPool[j].dbInfo.insert = mysqlInsert;
threadPool[j].dbInfo.login = mysqlLogin;
threadPool[j].dbInfo.logout = mysqlLogout;
threadPool[j].dbInfo.session = mysqlSession;
threadPool[j].dbInfo.permission = mysqlPermission;
pMainThreadInfo->dbInfo.connect = mysqlConnect;
pMainThreadInfo->dbInfo.disconnect = mysqlDisconnect;
pMainThreadInfo->dbInfo.escape = mysqlEscape;
pMainThreadInfo->dbInfo.push = mysqlPush;
pMainThreadInfo->dbInfo.pull = mysqlPull;
pMainThreadInfo->dbInfo.round = mysqlRound;
pMainThreadInfo->dbInfo.free = mysqlFree;
pMainThreadInfo->dbInfo.expire = mysqlExpire;
pMainThreadInfo->dbInfo.cookie = mysqlCookie;
pMainThreadInfo->dbInfo.insert = mysqlInsert;
pMainThreadInfo->dbInfo.login = mysqlLogin;
pMainThreadInfo->dbInfo.logout = mysqlLogout;
pMainThreadInfo->dbInfo.session = mysqlSession;
pMainThreadInfo->dbInfo.permission = mysqlPermission;
}
/*
*
* Initialize magick cookie.
*
*/
if((confPassword = configFetch("agent_password", &i)) != NULL) {
s = strlen(confPassword);
if(s > DATA_COOKIE_SIZE) {
s = DATA_COOKIE_SIZE;
}
strncpy(magicCookie, confPassword, s);
}
/*
*
* Initialize main thread.
*
*/
configSetUmask(0077);
if(configSetLocale(CONFIG_DEFAULT_LOCALE) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to set default locale");
}
if(configChangeRoot(CONFIG_DEFAULT_ROOT) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to change root directory");
}
if(configCloseInput() != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to close standard input");
}
if(configDaemonize() != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to daemonize process");
}
threadStack(THREAD_TELSKIND);
beginProcess(pMainThreadInfo);
/*
*
* Initialize timer.
*
*/
if(timerInit(TIMER_RESOLUTION_STATUS, 0, timerStatThreads) != 0) {
warningMessage(ERROR_FATAL, "Error occurred while trying to initialize timer");
}
/*
*
* Initialize worker threads.
*
*/
pMainMainInfo->allRunning = 0;
pMainMainInfo->theEnd = 0;
pMainMainInfo->threadEnd = 0;
for(i = 0; i < THREAD_TELSKIND; i++) {
if(threadInit(&threadPool[i], workerThread, &threadPool[i]) != 0) {
warningMessage(ERROR_FATAL, "Error occurred while trying to initialize worker thread");
}
j = 0;
while(threadPool[i].threadReady != 0) {
timerWait(&tmpSeconds, 0, THREAD_AGAIN);
if(j == 10000 || j == 20000 || j == 30000 || j == 40000 || j == 50000) {
warningMessage(ERROR_SLIGHT, "Waiting for worker threads to start taking too long, still waiting");
}
else if(j >= 60000) {
warningMessage(ERROR_FATAL, "Waiting for threads to start taking too long");
}
j++;
}
}
/*
*
* Create socket to listen.
*
*/
if((confPort = configFetch("listen_port", &i)) != NULL) {
thisSocket = netCreateListenSocket(*confPort);
}
else {
thisSocket = netCreateListenSocket(CONFIG_DEFAULT_PORT);
}
/*
*
* Create process id, shm segment, switch user and group id's.
*
*/
pidCreate();
shmCreate(DAEMON_TELSKIND, THREAD_TELSKIND);
if((thisUid = configFetch("user_id", &i)) != NULL) {
if(*thisUid != -1) {
if(uidSwitch(*thisUid) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to change user id");
}
}
}
if((thisGid = configFetch("group_id", &i)) != NULL) {
if(*thisGid != -1) {
if(gidSwitch(*thisGid) != 0) {
warningMessage(ERROR_SLIGHT, "Error occurred while trying to change group id");
}
}
}
/*
*
* Serve connected clients.
*
*/
startProcess(THREAD_TELSKIND);
pMainMainInfo->allRunning++;
pMainMainInfo->rushThreadCounter = 0;
while(pMainMainInfo->theEnd == 0) {
thatSocket = netWaitConnection(thisSocket, pMainThreadInfo);
if(pMainMainInfo->theEnd != 0) {
break;
}
mainLoop:
for(i = 0; i < THREAD_TELSKIND; i++) {
if(threadPool[i].threadReady == 0) {
threadPool[i].threadReady++;
threadPool[i].threadSocket = thatSocket;
shmUpdate(i, DAEMON_TELSKIND);
if(threadWake(&threadPool[i]) != 0) {
netCloseSocket(threadPool[i].threadSocket);
threadPool[i].threadReady = 0;
threadPool[i].threadSocket = 0;
warningMessage(ERROR_SLIGHT, "Error occurred while trying to start worker thread");
}
pMainMainInfo->rushThreadCounter = 0;
break;
}
}
if(i == THREAD_TELSKIND) {
if(pMainMainInfo->rushThreadCounter > 1000) {
pMainMainInfo->rushThreadCounter = 0;
warningMessage(ERROR_SLIGHT, "No free worker thread available, dropping agent");
netCloseSocket(thatSocket);
}
else {
pMainMainInfo->rushThreadCounter++;
if(pMainMainInfo->rushThreadCounter < 2) {
warningMessage(ERROR_SLIGHT, "No free worker thread available, it may be necessary to enlarge the thread pool");
}
timerWait(&tmpSeconds, 0, THREAD_AGAIN);
goto mainLoop;
}
}
}
/*
*
* Free allocated resources and terminate program.
*
*/
netCloseSocket(thisSocket);
pMainMainInfo->threadEnd++;
if(timerInit(TIMER_RESOLUTION_THREADSTOP, 0, timerStopThreads) == 0) {
pMainMainInfo->timerThreadAccess = 1;
for(i = 0; i < THREAD_TELSKIND; i++) {
threadWake(&threadPool[i]);
}
pMainMainInfo->timerThreadAccess = 0;
}
else {
for(i = 0; i < THREAD_TELSKIND; i++) {
threadKill(&threadPool[i]);
}
}
timesProcess(pMainThreadInfo);
pidRemove();
shmRemove(DAEMON_TELSKIND);
free(threadPool);
exitProcess(0);
exit(0);
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef STM32F40_41xxx
SetSysClock();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_USART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_USART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
spiInit(SPI3);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE_INT);
#if defined(ANYFC) || defined(COLIBRI) || defined(REVO) || defined(SPARKY2)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
#ifdef I2C_DEVICE_EXT
i2cInit(I2C_DEVICE_EXT);
#endif
}
#endif
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
#ifdef COLIBRI
if (!doesConfigurationUsePort(SERIAL_PORT_USART1)) {
ledStripEnable();
}
#else
ledStripEnable();
#endif
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1);
}
void init(void)
{
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
if (sonarHardware) {
pwm_params.useSonar = true;
pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag;
pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag;
}
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#ifdef STM32F303xC
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3);
#endif
#if defined(USE_UART2) && defined(STM32F40_41xxx)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
#if defined(USE_UART6) && defined(STM32F40_41xxx)
pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED;
// Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
pwm_params.idlePulse = 0; // brushed motors
}
#ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
#endif
#ifndef SKIP_RX_PWM_PPM
pwmRxInit(masterConfig.inputFilteringMode);
#endif
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.ioTag = IO_TAG(BEEPER),
#ifdef BEEPER_INVERTED
.isOD = false,
.isInverted = true
#else
.isOD = true,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.isOD = false;
beeperConfig.isInverted = true;
}
#endif
/* temp until PGs are implemented. */
#ifdef BLUEJAYF4
if (hardwareRevision <= BJF4_REV2) {
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
}
#endif
#ifdef CC3D
if (masterConfig.use_buzzer_p6 == 1)
beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
#ifdef USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit();
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(masterConfig.transponderData);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init(IOTAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IOTAG_NONE);
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#ifdef STM32F4
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime(gyro.targetLooptime * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
void main_init(void)
{
init();
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
case 500:
case 375:
case 250:
case 125:
accTargetLooptime = 1000;
break;
default:
case 1000:
#ifdef STM32F10X
accTargetLooptime = 1000;
#else
accTargetLooptime = 1000;
#endif
}
rescheduleTask(TASK_ACCEL, accTargetLooptime);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
// Reschedule telemetry to 500hz for Jeti Exbus
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000);
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
#ifdef OSD
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
#endif
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(systemConfig()->emf_avoidance);
#endif
i2cSetOverclock(systemConfig()->i2c_highspeed);
systemInit();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = debugConfig()->debug_mode;
#ifdef USE_EXTI
EXTIInit();
#endif
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
} else {
ledInit(true);
}
#else
ledInit(false);
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfig());
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
serialInit(feature(FEATURE_SOFTSERIAL));
mixerInit(customMotorMixer(0));
#ifdef USE_SERVOS
mixerInitServos(customServoMixer(0));
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
sonarGPIOConfig_t sonarGPIOConfig;
if (feature(FEATURE_SONAR)) {
bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC);
sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins);
sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio;
sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin;
sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio;
sonarGPIOConfig.echoPin = sonarHardware->echo_pin;
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = (
feature(FEATURE_AMPERAGE_METER)
&& batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC
);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate;
pwm_params.idlePulse = calculateMotorOff();
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit();
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
#ifdef DEBUG_PWM_CONFIGURATION
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
#endif
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#ifdef STM32F303xC
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPI3);
}
#else
spiInit(SPI3);
#endif
#endif
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.channelMask = 0;
#ifdef ADC_BATTERY
adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0);
#endif
#ifdef ADC_RSSI
adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0);
#endif
#ifdef ADC_AMPERAGE
adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0);
#endif
#ifdef ADC_POWER_12V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V);
#endif
#ifdef ADC_POWER_5V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V);
#endif
#ifdef ADC_POWER_3V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V);
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0;
#endif
adcInit(&adc_params);
#endif
initBoardAlignment();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit();
}
#endif
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5) {
gyroConfig()->gyro_sync = 0;
}
#endif
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
mspInit();
mspSerialInit();
const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom);
pidInitFilters(pidProfile());
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetPidLooptime);
#endif
imuInit();
#ifdef USE_CLI
cliInit();
#endif
failsafeInit();
rxInit(modeActivationProfile()->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit();
navigationInit(pidProfile());
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit();
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(transponderConfig()->data);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
if (feature(FEATURE_VBAT)) {
// Now that everything has powered up the voltage and cell count be determined.
voltageMeterInit();
batteryInit();
}
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeterInit();
}
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
void configureScheduler(void)
{
schedulerInit();
setTaskEnabled(TASK_SYSTEM, true);
uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
rescheduleTask(TASK_GYRO, gyroPeriodUs);
setTaskEnabled(TASK_GYRO, true);
rescheduleTask(TASK_PID, gyroPeriodUs);
setTaskEnabled(TASK_PID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
i2cSetOverclock(masterConfig.i2c_highspeed);
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
systemInit();
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
ledInit();
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL));
#ifdef USE_SERVOS
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer);
#else
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
if (feature(FEATURE_SONAR)) {
sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig);
sonarGPIOConfig_t sonarGPIOConfig = {
.gpio = SONAR_GPIO,
.triggerPin = sonarHardware->echo_pin,
.echoPin = sonarHardware->trigger_pin,
};
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit(masterConfig.inputFilteringMode);
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetLooptime);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(masterConfig.transponderData);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
/* Setup scheduler */
if (masterConfig.gyroSync) {
rescheduleTask(TASK_GYROPID, targetLooptime - INTERRUPT_WAIT_TIME);
}
else {
rescheduleTask(TASK_GYROPID, targetLooptime);
}
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
while (1) {
scheduler();
processLoopback();
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
stopMotors();
}
#ifdef TRANSPONDER
// prevent IR LEDs from burning out.
uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
transponderIrDisable();
}
#endif
while (1);
}
// Main
int main(void) {
// Init
#if defined(ENERGIA) // LaunchPad, FraunchPad and StellarPad specific
#if defined(__LM4F120H5QR__) // StellarPad specific
ROM_FPULazyStackingEnable();
timerInit();
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
#else // LaunchPad and FraunchPad specific
init();
#endif
#elif defined(CORE_TEENSY) // Teensy specific
_init_Teensyduino_internal_();
#elif defined(WIRING) // Wiring specific
boardInit();
#else // General case
init();
delay(1);
#if defined(ARDUINO) && (ARDUINO >= 100) // Arduino 1.0 + 1.5 specific
#if defined(USBCON) // Arduino 1.0 + 1.5 specific
USBDevice.attach();
#endif
#endif
#endif
// Setup
setup();
// Loop
for (;;) {
loop();
#if defined(ENERGIA)
#elif defined(MPIDE)
#elif defined(CORE_TEENSY)
#ifdef USB_SERIAL
usb_serial_class Serial;
#endif
#elif defined(WIRING)
#elif defined(ARDUINO) && (ARDUINO >= 100) // Arduino 1.0 + 1.5 specific
if (serialEventRun) serialEventRun();
#endif
}
return 0;
}
// ################ Main Program ################ //
int main(void)
{
//* --------------- Init --------------- *//
uOpioidInit();
// Init the Clock
//outputPIN(REFOCLK);
//REFOCONbits.RODIV = 4; //Div by 8
//REFOCONbits.ROSEL = 1; //PBCLK outputed
//REFOCONbits.OE = 1; //enbale ouput
//REFOCONbits.ON = 1; //enable REFOCLK
//* ------------------------------------ *//
//* ----------- Testing Space ---------- *//
timerInit(TIMER_2,0);
ocSetConfig(OC_1,OC_MODE_PWM|OC_TIMER_2);
ocSetConfig(OC_2,OC_MODE_PWM|OC_TIMER_2);
ocSetConfig(OC_3,OC_MODE_PWM|OC_TIMER_2);
pwmSetPeriod(OC_1,1000);
pwmSetPeriod(OC_2,1000);
pwmSetPeriod(OC_3,1000);
ocStart(OC_1);
ocStart(OC_2);
ocStart(OC_3);
//SPI test
outputPIN(COM0_IO2);
nrfInit(COM0_SPI_ID, &LATA, COM0_IO2);
COM0ControlTransaction.pSlave = pCOM0SlaveControl;
COM0ControlTransaction.control.all = 0;
COM0ControlTransaction.transferNb = 10;
COM0ControlTransaction.txNbRemaining = 10;
COM0ControlTransaction.rxNbDone = 0;
COM0ControlTransaction.pTX = sourceArray;
COM0ControlTransaction.pRX = checkArray;
spiStartTransaction(&COM0ControlTransaction);
//testBufCtlPtr = rBufCreate(10, sizeof(U8));
//if (uartInit(0,UART_TX_INT_TSR_EMPTY|UART_RX_INT_DATA_READY|UART_MODE_8N1,1250000) == STD_EC_SUCCESS)
// setPIN(LED_B);
//* ------------------------------------ *//
//* ----------- Loop forever ----------- *//
for (;;)
{
if ((sysTick - ledSTRed) >= LED_R_SYSTICK_INTERVAL)
{
// -- Handle boundary -- //
if (ledRed == 255)
ledRedDir = 0;
else if (ledRed == 0)
ledRedDir = 1;
// --------------------- //
// -- Duty mouvement -- //
if (ledRedDir)
ledRed++;
else
ledRed--;
// -------------------- //
pwmSetDuty(OC_1,ledRed,0xFF);
ledSTRed = sysTick;
}
if ((sysTick - ledSTGreen) >= LED_G_SYSTICK_INTERVAL)
{
// -- Handle boundary -- //
if (ledGreen == 255)
ledGreenDir = 0;
else if (ledGreen == 0)
ledGreenDir = 1;
// --------------------- //
// -- Duty mouvement -- //
if (ledGreenDir)
ledGreen++;
else
ledGreen--;
// -------------------- //
pwmSetDuty(OC_2,ledGreen,0xFF);
ledSTGreen = sysTick;
}
if ((sysTick - ledSTBlue) >= LED_B_SYSTICK_INTERVAL)
{
// -- Handle boundary -- //
if (ledBlue == 255)
ledBlueDir = 0;
else if (ledBlue == 0)
ledBlueDir = 1;
// --------------------- //
// -- Duty mouvement -- //
if (ledBlueDir)
ledBlue++;
else
ledBlue--;
// -------------------- //
pwmSetDuty(OC_3,ledBlue,0xFF);
ledSTBlue = sysTick;
}
/*
switch(ledID)
{
case 0: setPIN(LED_R); setPIN(LED_G); setPIN(LED_B); break;
case 1: clearPIN(LED_R); setPIN(LED_G); setPIN(LED_B); break;
case 2: setPIN(LED_R); clearPIN(LED_G); setPIN(LED_B); break;
case 3: setPIN(LED_R); setPIN(LED_G); clearPIN(LED_B); break;
case 4: clearPIN(LED_R); clearPIN(LED_G); setPIN(LED_B); break;
case 5: clearPIN(LED_R); setPIN(LED_G); clearPIN(LED_B); break;
case 6: setPIN(LED_R); clearPIN(LED_G); clearPIN(LED_B); break;
case 7: clearPIN(LED_R); clearPIN(LED_G); clearPIN(LED_B); break;
default: ledID = 0;
}
*/
//uartSendByte(0,0x55);
}
//* ------------------------------------ *//
}
void init(void)
{
uint8_t i;
drv_pwm_config_t pwm_params;
bool sensorsOK = false;
initPrintfSupport();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(masterConfig.emf_avoidance);
#endif
#ifdef NAZE
detectHardwareRevision();
#endif
systemInit();
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
ledInit();
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioMode = Mode_Out_OD,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioPeripheral = BEEP_PERIPHERAL,
.isInverted = false
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
#ifdef NAZE
updateHardwareRevision();
#endif
#ifdef USE_I2C
#ifdef NAZE
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
}
#else
// Configure the rest of the stuff
i2cInit(I2C_DEVICE);
#endif
#endif
#if !defined(SPARKY)
drv_adc_config_t adc_params;
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
// if gyro was not detected due to whatever reason, we give up now.
if (!sensorsOK)
failureMode(3);
LED1_ON;
LED0_OFF;
for (i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
imuInit();
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
serialInit(&masterConfig.serialConfig);
memset(&pwm_params, 0, sizeof(pwm_params));
// when using airplane/wing mixer, servo/motor outputs are remapped
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(SERIAL_PORT_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
pwmRxInit(masterConfig.inputFilteringMode);
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
failsafe = failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
¤tProfile->gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
Sonar_init();
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY))
initTelemetry();
#endif
previousTime = micros();
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
// Check battery type/voltage
if (feature(FEATURE_VBAT))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayEnablePageCycling();
#endif
}
#endif
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
int main(void) {
init();
while (1) {
loop();
processLoopback();
}
}
/*****************************************************************************
* 函 数 名 : secBoot
*
* 功能描述 : 安全启动C入口函数
*
* 输入参数 :
* 输出参数 :
*
* 返 回 值 :
*
* 其它说明 :
*
*****************************************************************************/
void secBoot(void)
{
volatile UINT32 ulBootModeVal = 0x00;
volatile tOcrShareData *pShareData = (tOcrShareData*)M3_SRAM_SHARE_DATA_ADDR;
int retVal = 0x00;
int BOOT_RST_Addr = 0x00;
#if PLATFORM==PLATFORM_PORTING
//testEfuseRead();
//testHash256();
#endif
/* 初始化 */
timerInit();
pShareData->errno = 0;
pShareData->SHA256Hash = NULL;
pShareData->RSA = NULL;
pShareData->idioIdentify = NULL;
pShareData->bSecEn = FALSE;
pShareData->bRootCaInited = FALSE;
pShareData->pRootKey = NULL;
pShareData->ulOcrInitedFlag = FALSE;
pShareData->bsp_nand_get_spec_and_save = NULL;
/* 读取Efuse安全校验配置，获取安全标志 */
if(EFUSEC_SEC_EN == (INREG32(EFUSEC_HW_CFG) & EFUSEC_SEC_EN_MASK))
{
pShareData->bSecEn = TRUE;
}
/*判断USB自举软件标志是否有效*/
if((AUTO_ENUM_FLAG_VALUE == pShareData->ulEnumFlag)
|| (SC_AUTO_ENUM_EN == (INREG32(SC_STAT0) & SC_AUTO_ENUM_EN_BITMASK)))
{
/* 软标志有效或自举管脚为低电平，先清除标志,再进入自举流程*/
/*pShareData->ulEnumFlag = 0;*/ /* 不清除标志,供探针程序检查自举原因 */
usbBoot();
}
/*读取BOOTMODE*/
ulBootModeVal = INREG32(SC_STAT0) & SC_BOOTMODE_BITMASK;
switch(ulBootModeVal)
{
case BOOT_MODE_NAND_ID:
{
print_info("\r\nNF id boot!" );
retVal = nandReadBl(M3_TCM_BL_ADDR, NAND_PARSE_ID_MODE, &(pShareData->nandspec));
break;
}
case BOOT_MODE_NAND_BOOT:
{
print_info("\r\nNF boot only!" );
/* Nand默认在Boot模式下,直接拷贝bootload映像 */
retVal = nandReadBl(M3_TCM_BL_ADDR, ONLY_NAND_BOOT_MODE, &(pShareData->nandspec));
break;
}
case BOOT_MODE_NAND_EFUSE:
{
print_info("\r\nNF efuse boot!" );
retVal = nandReadBl(M3_TCM_BL_ADDR, EFUSE_BOOT_MODE, &(pShareData->nandspec));
break;
}
#if HSIC_CONFIG==YES
case BOOT_MODE_AP_HSIC:
{
print_info("\r\nHSIC boot!" );
/*通过HSIC将bootload映像读取*/
hsicBoot();
break;
}
#endif
#if SPI_CONFIG==YES
case BOOT_MODE_SPI:
{
print_info("\r\nSPI boot!" );
/*从SPI中将bootload映像读取*/
retVal = spiDevReadBl((UINT8*)M3_TCM_BL_ADDR);
break;
}
#endif
#if EMMC_CONFIG==YES
case BOOT_MODE_EMMC:
{
print_info("\r\nEMMC boot!" );
/*搬运bootload映像*/
retVal = emmc_read_bootloader(M3_TCM_BL_ADDR);
break;
}
#endif
#if HSUART_CONFIG==YES
case BOOT_MODE_AP_HSUART:
{
print_info("\r\nHSUART boot!" );
/* 不再返回 */
retVal = apDlMain(M3_TCM_BL_ADDR);
/* 防止apDlMain返回,这里打印错误 */
print_info_with_u32("\r\napDlMain exec err, ret:" , (UINT32)retVal);
setErrno(SYS_ERR_AP_DL_RETURNS);
/* 延时复位 */
wdtRebootDelayMs(TIME_DELAY_MS_4000_FOR_AP_RETURNS);
break; /*lint !e527*/
}
#endif
default:
{
#if PLATFORM==PLATFORM_PORTING
/* PORTING版本使用此模式烧写EFUSE */
print_info_with_u32("\r\nbootmode err, will write efuse on porting..." , ulBootModeVal);
testEfuseWrite();
usbBoot();
break;
#else
/* 复位,以防止Stick形态没有串口,从复位可以得知错误 */
print_info_with_u32("\r\nbootmode err:" , (ulBootModeVal>>SC_BOOTMODE_BITPOS));
setErrno(SYS_ERR_BOOT_MODE);
/* 延时复位 */
wdtRebootDelayMs(TIME_DELAY_MS_3000_FOR_BOOT_MODE);
break;
#endif
}
}
if(OK != retVal)
{
print_info_with_u32("\r\nBL read err, ret:", (UINT32)retVal);
setErrno(retVal);
usbBoot();
}
/*判断FLASH /E2/MMC是否烧入映像*/
if(BL_CHECK_INSTRUCTION != *(volatile UINT32 *)BL_CHECK_ADDR)
{
/*映像未烧入，向AP返回Nack, 或跳入USB自举流程*/
print_info("\r\nimage not program!" );
setErrno(SEC_NO_IMAGE);
usbBoot();
}
ocrShareSave();
/*非安全启动，直接运行*/
if(!pShareData->bSecEn)
{
print_info("\r\nUnSec_boot!" );
/*非安全，跳入TCM执行*/
BOOT_RST_Addr = *(volatile UINT32 *)BOOT_RST_ADDR_PP;
go((FUNCPTR)(M3_TCM_BL_ADDR + BOOT_RST_Addr));
}
/*安全启动，进行安全校验*/
retVal = secCheck((UINT32)M3_TCM_BL_ADDR, IMAGE_TYPE_BOOTLOAD);
#ifdef START_TIME_TEST
print_info_with_u32("\r\ntime(ms):", (TIMER5_INIT_VALUE - INREG32(TIMER5_REGOFF_VALUE))/MS_TICKS);
#endif
switch(retVal)
{
case SEC_SUCCESS:
/*安全校验通过*/
print_info("\r\nSec check ok!" ); /*lint !e616*/
/* 进入下一case运行BootLoader */
case SEC_EFUSE_NOT_WRITE: /*lint !e825*/ /*EFUSE 未烧写*/
/* 跳到TCM mem执行BOOTLOAD */
BOOT_RST_Addr = *(volatile UINT32 *)BOOT_RST_ADDR_PP;
go((FUNCPTR)(M3_TCM_BL_ADDR + BOOT_RST_Addr));
break;
case SEC_SHA_CALC_ERROR:
/* Hash计算不通过 */
case SEC_OEMCA_ERROR:
/* OEM CA校验不通过 */
case SEC_IMAGE_ERROR: /* 映像校验不通过 */
case SEC_ROOT_CA_ERROR:
/* 根CA校验错误 */
case SEC_IMAGE_LEN_ERROR:/*安全版本长度错误*/
print_info("\r\nSec check err!" );
setErrno(retVal);
usbBoot();
break;
case SEC_EFUSE_READ_ERROR:
/*Efuse读取失败,使用看门狗复位,再次尝试读取*/
print_info("\r\nEfuse read err, reboot...");
setErrno(SYS_ERR_EFUSE_READ);
wdtRebootDelayMs(TIME_DELAY_MS_2000_FOR_EFUSE_READERR);
break;
default:
print_info_with_u32("\r\nunhandered ret:",(UINT32)retVal);
setErrno(SYS_ERR_SEC_UNKNOWN_RET);
/* 延时1s */
wdtRebootDelayMs(TIME_DELAY_MS_5000_FOR_SEC_UNKNOWN_RET);
/*break; */ /*for pc lint*/
}
void init(void)
{
#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
systemInit();
//i2cSetOverclock(masterConfig.i2c_overclock);
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = masterConfig.debug_mode;
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
#ifdef ALIENFLIGHTF3
ledInit(hardwareRevision == AFF3_REV_1 ? false : true);
#else
ledInit(false);
#endif
LED2_ON;
#ifdef USE_EXTI
EXTIInit();
#endif
#if defined(BUTTONS)
gpio_config_t buttonAGpioConfig = {
BUTTON_A_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_A_PORT, &buttonAGpioConfig);
gpio_config_t buttonBGpioConfig = {
BUTTON_B_PIN,
Mode_IPU,
Speed_2MHz
};
gpioInit(BUTTON_B_PORT, &buttonBGpioConfig);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow GPIO configuration to settle
if (!isMPUSoftReset()) {
uint8_t secondsRemaining = 5;
bool bothButtonsHeld;
do {
bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN);
if (bothButtonsHeld) {
if (--secondsRemaining == 0) {
resetEEPROM();
systemReset();
}
delay(1000);
LED0_TOGGLE;
}
} while (bothButtonsHeld);
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(&masterConfig.rxConfig);
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
#if !defined(USE_HAL_DRIVER)
dmaInit();
#endif
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
#ifdef USE_SERVOS
servoMixerInit(masterConfig.customServoMixer);
#endif
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d;
}
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
featureClear(FEATURE_3D);
idlePulse = 0; // brushed motors
}
#ifdef USE_QUAD_MIXER_ONLY
motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT);
#else
motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount);
#endif
#ifdef USE_SERVOS
if (isMixerUsingServos()) {
//pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
servoInit(&masterConfig.servoConfig);
}
#endif
#ifndef SKIP_RX_PWM_PPM
if (feature(FEATURE_RX_PPM)) {
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(&masterConfig.pwmConfig);
}
pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode);
#endif
mixerConfigureOutput();
#ifdef USE_SERVOS
servoConfigureOutput();
#endif
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef BEEPER
beeperInit(&masterConfig.beeperConfig);
#endif
/* temp until PGs are implemented. */
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_BST
bstInit(BST_DEVICE);
#endif
#ifdef USE_SPI
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPIDEV_3);
}
#else
spiInit(SPIDEV_3);
#endif
#endif
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4);
#endif
#endif
#ifdef VTX
vtxInit();
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_USART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.enableVBat = feature(FEATURE_VBAT);
adc_params.enableRSSI = feature(FEATURE_RSSI_ADC);
adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER);
adc_params.enableExternal1 = false;
#ifdef OLIMEXINO
adc_params.enableExternal1 = true;
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5);
#endif
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit(&masterConfig.rxConfig);
}
#endif
#ifdef USE_RTC6705
if (feature(FEATURE_VTX)) {
rtc6705_soft_spi_init();
current_vtx_channel = masterConfig.vtx_channel;
rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]);
rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power);
}
#endif
#ifdef OSD
if (feature(FEATURE_OSD)) {
osdInit();
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
masterConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
LED1_ON;
LED0_OFF;
LED2_OFF;
for (int i = 0; i < 10; i++) {
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
imuInit();
mspFcInit();
mspSerialInit();
#ifdef USE_CLI
cliInit(&masterConfig.serialConfig);
#endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit(
&masterConfig.serialConfig,
&masterConfig.gpsConfig
);
navigationInit(
&masterConfig.gpsProfile,
¤tProfile->pidProfile
);
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors);
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(masterConfig.transponderData);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init(IO_TAG_NONE);
}
#elif defined(USE_FLASH_M25P16)
m25p16_init(IO_TAG_NONE);
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
#if defined(STM32F4) || defined(STM32F7)
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM;
#else
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#endif
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif
if (masterConfig.mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles();
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
// Now that everything has powered up the voltage and cell count be determined.
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
fcTasksInit();
systemState |= SYSTEM_STATE_READY;
}
int main(int argc, char* argv[])
{
sp_bind_p bStd=0, bSsl=0;
char pidbuf[10], * strbuf;
if(ConfLoad("/etc/spanel.conf")!=1){
perror("ERROR! Failed to open config file. Maybe I'm not root?\n");
exit(1);
}
//check for pidfile
pidf = open(ConfC("PATH_PIDFILE"),O_CREAT|O_WRONLY,S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH);
if(flock(pidf,LOCK_EX|LOCK_NB)){
rep("ERROR: Pidfile is busy");
exit(1);
}
sprintf(pidbuf,"%d",getpid());
if(-1==write(pidf,pidbuf,strlen(pidbuf))) perror("write (pidf)");
fsync(pidf);
signal(SIGINT, sig_act);
signal(SIGHUP, sig_act);
signal(SIGKILL, sig_act);
signal(SIGTERM, sig_act);
srand ( time(NULL) );
spXmlStartup();
if(!db_open())
{
rep("Failed to connect to database");
finalize(1);
}else{
//init event library
event_init();
if(firsrunCheckAll())
rep("Internal DB verified");
timerInit();
timerSetAction(1,siteChecker, NULL, 2);
rep("Connected to database");
if(ConfI("PortNum"))
bStd = bind_pool_add(NULL, ConfI("PortNum"), false);
if(ConfI("PostNumSSL"))
bSsl = bind_pool_add(NULL, ConfI("PostNumSSL"), true);
if(ConfI("PortNum") && !bStd && ConfI("PostNumSSL") && !bSsl) {
rep("Failed making socket");
finalize(2);
}
//timer thread
// timerInit(0);
//start main loop
rep("SkillPanel version "SPANEL_VERSION" is up and running");
bind_one_std(bStd,true);
bind_one_std(bSsl,true);
conn_cycle();
}
finalize(0);
return 0;
}
/**
* Fonctions
*/
int main(void)
{
// timer
timerInit();
i2cInit();
// initialisation des I/O
sbi(DDRD,3); // led verte en sortie
sbi(PORTD,3); // éteindre la led
cbi(DDRD,2); // bouton d'entrée
cbi(PORTD,2);
// initialisation des blinkm
// arrêter le script
cmd[0] = 'o';
cmdSize = 1;
i2cMasterSend(0x00,cmdSize,cmd);
cmd[0] = 'c';
cmd[1] = 0x00;
cmd[2] = 0x00;
cmd[3] = 0x00;
cmdSize = 4;
i2cMasterSend(0x00,cmdSize,cmd);
delay_ms(1000);
cmd[0] = 'f';
cmd[1] = 20;
cmdSize = 2;
i2cMasterSend(0x00,cmdSize,cmd);
/** Boucle principale **/
//
while (1)
{
cbi(PORTD,3);
//
cmd[0] = 'h';
cmd[1] = hue;
cmd[2] = 0xFF;
cmd[3] = 0xFF;
cmdSize = 4;
i2cMasterSend(0x00,cmdSize,cmd);
//
if (hue < 1000)
{
delay_ms(hue);
} else {
delay_ms(700);
}
sbi(PORTD,3);
//
cmd[0] = 'c';
cmd[1] = 0x00;
cmd[2] = 0x00;
cmd[3] = 0x00;
cmdSize = 4;
i2cMasterSend(0x00,cmdSize,cmd);
//
delay_ms(500);
// interrogation du détecteur de distance
cmd[0] = 0x42; // read distance
cmdSize = 1;
i2cMasterSend(0x02,cmdSize,cmd);
i2cMasterReceive(0x03,2,buffer);
hue = ( 0x00FF & buffer[0] );
hue += ( (0x00FF & buffer[1]) <<8 );
}
return 0;
}
int init(int argc, char *argv[]) {
#if WIN32
char currentDirectory[MAX_PATH];
#endif
srand(time(0));
conInit();
loadDefaults();
timerInit();
if (!commandLineRead(argc, argv))
return 0;
if (view.quit)
return 0;
if (!state.dontExecuteDefaultScript)
configRead(findFile(CONFIG_FILE), 0);
#ifndef NO_GUI
if (!gfxInit())
return 1;
#endif
if (state.historyFrames % 2)
state.historyFrames--;
fpsInit();
#ifdef WIN32
// if we've gone this far, lets set the registry key even if it exists...
GetCurrentDirectory(MAX_PATH, currentDirectory);
setRegistryString(REGISTRY_NAME_PATH, currentDirectory);
#endif
#ifdef _OPENMP
conAdd(LHELP, "multi-threaded rendering: max threads = %d. Found %d processors.",
state.processFrameThreads, omp_get_num_procs());
#endif
#ifndef NO_STDIO_REDIRECT
// say hi (and keep stdout.txt alive on windows...)
if(!view.useStdout && !view.screenSaver)
printf("Welcome to %s.\n", GRAVIT_VERSION);
#endif
#ifdef WITHOUT_AGAR
conAdd(LHELP, "Welcome to Gravit!");
#ifndef NO_GUI
conAdd(LHELP, "Quick Start: Hit SPACE to start a new simulation!");
conAdd(LHELP, "Hold down a mouse button and move it around. Use A and Z keys, or the scroll wheel to zoom in and out.");
#endif
#endif
return 0;
}
