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;
}
Exemple #3
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();
}
Exemple #4
0
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;
}
Exemple #5
0
    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) */
Exemple #6
0
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);
	}
}
Exemple #7
0
/**
 * @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;
}
Exemple #9
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
Exemple #10
0
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;
}
Exemple #11
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;
}
Exemple #12
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);
}
Exemple #13
0
 void init() {
   buildFont();
   resetObjects();                                           // Set Player / Enemy Starting Positions
   timerInit();
 }
Exemple #14
0
/* ------------------------------------------------------------------------- */
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;
}
Exemple #15
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(
            &currentProfile->gpsProfile,
            &currentProfile->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();
    }*/
}
Exemple #16
0
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);

}
Exemple #17
0
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,
            &currentProfile->pidProfile,
            &currentProfile->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();
    }
}
Exemple #18
0
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);
}
Exemple #19
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(
            &currentProfile->gpsProfile,
            &currentProfile->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);
}
Exemple #20
0
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,
            &currentProfile->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
}
Exemple #21
0
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
}
Exemple #22
0
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(
            &currentProfile->gpsProfile,
            &currentProfile->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);
}
Exemple #23
0
// 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;
}
Exemple #24
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);
	}
	//* ------------------------------------ *//
}
Exemple #25
0
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(
            &currentProfile->gpsProfile,
            &currentProfile->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*/

    }
Exemple #27
0
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,
            &currentProfile->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;
}
Exemple #28
0
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;
}
Exemple #29
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;	
}
Exemple #30
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;

}