/*
 * @brief Application Program Loop
 */
void application()
{
	// Stop Watchdog Timer
	stopWatchdogTimer();

	// Configure DCO Frequency 1 MHz
	configureDCOFrequency1MHz();

	// Configure PushButtons Inputs
	pinDigitalInput(RTCSET);
	pinDigitalInput(RTCINC);
	pinDigitalInput(RTCDEC);

	// Configure PullUps
	pinDigitalEnabledPullUp(RTCSET);
	pinDigitalEnabledPullUp(RTCINC);
	pinDigitalEnabledPullUp(RTCDEC);

	// Configure High Low Interrupt
	pinDigitalSelectHighLowTransitionInterrupt(RTCSET);
	pinDigitalSelectHighLowTransitionInterrupt(RTCINC);
	pinDigitalSelectHighLowTransitionInterrupt(RTCDEC);

	// Configure Pin Interrupts
	pinDigitalEnableInterrupt(RTCSET);
	pinDigitalEnableInterrupt(RTCINC);
	pinDigitalEnableInterrupt(RTCDEC);

	// Init PAP Motor
	papMotorInit();

	// Init ADC
	adcInit();

	// Configure A5 Input
	adcAnalogInputEnable(5);

	// RTC Init
	rtcInit();

	// Start RTC
	rtcStart();

	// Initialize LCD Module
	lcdInit();

	// Write Message Constant
	lcdWriteMessage(1,1,mensaje);
	lcdWriteMessage(3,1,mensajeADC);
	lcdWriteMessage(3,11,mensajeTemp);

	// Write Date and Time LCD Module
	lcdDataTimeFormat(2, 1, rtcGetHour(), rtcGetMinute(), rtcGetSecond());
	lcdDataDateFormat(2, 11, rtcGetDay(), rtcGetMonth(), rtcGetYear());

	// Enable Interrupts
	enableInterrupts();

	// Entry Low Power Mode 0
	entryLowPowerMode0();
}
Beispiel #2
0
void main(void) {
    rccInit();

    statusLed = digitalInit(GPIO_STATUS_LED_PORT, GPIO_STATUS_LED_PIN);
    errorLed = digitalInit(GPIO_ERROR_LED_PORT, GPIO_ERROR_LED_PIN);
#ifdef ESC_DEBUG
    tp = digitalInit(GPIO_TP_PORT, GPIO_TP_PIN);
    digitalLo(tp);
#endif

    timerInit();
    configInit();
    adcInit();
    fetInit();
    serialInit();
    canInit();
    runInit();
    cliInit();
    owInit();

    runDisarm(REASON_STARTUP);
    inputMode = ESC_INPUT_PWM;

    fetSetDutyCycle(0);
    fetBeep(200, 100);
    fetBeep(300, 100);
    fetBeep(400, 100);
    fetBeep(500, 100);
    fetBeep(400, 100);
    fetBeep(300, 100);
    fetBeep(200, 100);

    pwmInit();

    digitalHi(statusLed);
    digitalHi(errorLed);

    // self calibrating idle timer loop
    {
	uint32_t lastRunCount;
	uint32_t thisCycles, lastCycles;
        volatile uint32_t cycles;
        volatile uint32_t *DWT_CYCCNT = (uint32_t *)0xE0001004;
        volatile uint32_t *DWT_CONTROL = (uint32_t *)0xE0001000;
        volatile uint32_t *SCB_DEMCR = (uint32_t *)0xE000EDFC;

        *SCB_DEMCR = *SCB_DEMCR | 0x01000000;
        *DWT_CONTROL = *DWT_CONTROL | 1;	// enable the counter

	minCycles = 0xffff;
        while (1) {
            idleCounter++;

	    if (runCount != lastRunCount && !(runCount % (RUN_FREQ / 1000))) {
		if (commandMode == CLI_MODE)
		    cliCheck();
		else
		    binaryCheck();
		lastRunCount = runCount;
	    }

            thisCycles = *DWT_CYCCNT;
	    cycles = thisCycles - lastCycles;
	    lastCycles = thisCycles;

            // record shortest number of instructions for loop
	    totalCycles += cycles;
            if (cycles < minCycles)
                minCycles = cycles;
        }
    }
}
Beispiel #3
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;

#if 0
    // PC12, PA15
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
    GPIOA->BRR = 0x8000; // set low 15
    GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
    GPIOC->BRR = 0x1000; // set low 12
#endif

#if 0
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOB->BRR = 0x18; // set low 4 & 3
    GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif

    systemInit();
    init_printf(NULL, _putc);

    checkFirstTime(false);
    readEEPROM();

    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);

    mixerInit(); // this will set useServo var depending on mixer type
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
    pwm_params.useServos = useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
        break;
    }

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function. spektrum below will override that
    rcReadRawFunc = pwmReadRawRC;

    if (feature(FEATURE_SPEKTRUM)) {
        spektrumInit();
        rcReadRawFunc = spektrumReadRawRC;
    if (feature(FEATURE_GPS))
        gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    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;

    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    serialInit(mcfg.serial_baudrate);

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = 400;
    calibratingG = 1000;
    calibratingB = 200;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLES_25 = 1;

    // loopy
    while (1) {
        loop();
    }
}
Beispiel #4
0
//-- main
//
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;
    serialPort_t* loopbackPort = NULL;


    //-- 하드웨어 초기화
    //
    systemInit();


#ifdef USE_LAME_PRINTF
    init_printf(NULL, _putc);
#endif

    checkFirstTime(false);
    readEEPROM();


    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else 
    {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);
    initBoardAlignment();

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);

    mixerInit(); // this will set core.useServo var depending on mixer type

    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    //pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
    pwm_params.useUART = 0;
    pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
    pwm_params.useServos = core.useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
    if (feature(FEATURE_3D))
        pwm_params.idlePulse = mcfg.neutral3d;
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors
    pwm_params.servoCenterPulse = mcfg.midrc;
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
        break;
    }

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
    for (i = 0; i < RC_CHANS; i++)
        rcData[i] = 1502;
    rcReadRawFunc = pwmReadRawRC;
    core.numRCChannels = MAX_INPUTS;

    if (feature(FEATURE_SERIALRX)) {
        switch (mcfg.serialrx_type) {
            case SERIALRX_SPEKTRUM1024:
            case SERIALRX_SPEKTRUM2048:
                spektrumInit(&rcReadRawFunc);
                break;
            case SERIALRX_SBUS:
                sbusInit(&rcReadRawFunc);
                break;
            case SERIALRX_SUMD:
                sumdInit(&rcReadRawFunc);
                break;
            #if defined(SKYROVER)
            case SERIALRX_HEXAIRBOT:
                hexairbotInit(&rcReadRawFunc);
                break;
            #endif
        }
    } else { // spektrum and GPS are mutually exclusive
        // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
        // gpsInit will return if FEATURE_GPS is not enabled.
        // Sanity check below - protocols other than NMEA do not support baud rate autodetection
        if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
            mcfg.gps_baudrate = 0;
        gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    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;



    serialInit(mcfg.serial_baudrate);

	#if _DEF_MW_PORT == PORT_UART1
	core.menuport  = uartOpen(USART1, NULL, mcfg.serial_baudrate, MODE_RXTX);
	core.debugport = core.menuport;
	#else
	core.menuport  = uartOpen(USART1, NULL, mcfg.serial_baudrate, MODE_RXTX);
	core.debugport = core.menuport;
	#endif

    DEBUG_PRINT("Booting.. V140926R1\r\n");


    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    

    if (feature(FEATURE_SOFTSERIAL)) {
      setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
#ifdef SOFTSERIAL_LOOPBACK
      loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
      serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
#endif
    }

    if (feature(FEATURE_TELEMETRY))
        initTelemetry();

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = CALIBRATING_ACC_CYCLES;
    calibratingG = CALIBRATING_GYRO_CYCLES;
    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLES_25 = 1;

    //-- 센서초기화 루틴때문에 USB 통신안되는것 때문에 마지막으로 순서 변경
    //
    Hw_VCom_Init();

    DEBUG_PRINT("Start\r\n");

    //-- thread 시작
    //
    thread_main();

    while(1)
    {

    }
}
Beispiel #5
0
int_t main(void)
{
   error_t error;
   NetInterface *interface;
   OsTask *task;
   MacAddr macAddr;
#if (APP_USE_DHCP == DISABLED)
   Ipv4Addr ipv4Addr;
#endif
#if (APP_USE_SLAAC == DISABLED)
   Ipv6Addr ipv6Addr;
#endif

   //Initialize kernel
   osInitKernel();
   //Configure debug UART
   debugInit(115200);

   //Start-up message
   TRACE_INFO("\r\n");
   TRACE_INFO("**********************************\r\n");
   TRACE_INFO("*** CycloneTCP Web Server Demo ***\r\n");
   TRACE_INFO("**********************************\r\n");
   TRACE_INFO("Copyright: 2010-2015 Oryx Embedded SARL\r\n");
   TRACE_INFO("Compiled: %s %s\r\n", __DATE__, __TIME__);
   TRACE_INFO("Target: SAM3X\r\n");
   TRACE_INFO("\r\n");

   //Configure I/Os
   ioInit();
   //ADC initialization
   adcInit();

   //Initialize LCD display
   GLCD_Init();
   GLCD_SetBackColor(Blue);
   GLCD_SetTextColor(White);
   GLCD_Clear(Blue);

   //Welcome message
   lcdSetCursor(0, 0);
   printf("Web Server Demo\r\n");

   //Generate a random seed

   //PRNG initialization
   error = yarrowInit(&yarrowContext);
   //Any error to report?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to initialize PRNG!\r\n");
   }

   //Properly seed the PRNG
   error = yarrowSeed(&yarrowContext, seed, sizeof(seed));
   //Any error to report?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to seed PRNG!\r\n");
   }

   //TCP/IP stack initialization
   error = netInit();
   //Any error to report?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to initialize TCP/IP stack!\r\n");
   }

   //Configure the first Ethernet interface
   interface = &netInterface[0];

   //Set interface name
   netSetInterfaceName(interface, "eth0");
   //Set host name
   netSetHostname(interface, "WebServerDemo");
   //Select the relevant network adapter
   netSetDriver(interface, &sam3xEthDriver);
   netSetPhyDriver(interface, &dm9161PhyDriver);
   //Set external interrupt line driver
   netSetExtIntDriver(interface, &extIntDriver);
   //Set host MAC address
   macStringToAddr(APP_MAC_ADDR, &macAddr);
   netSetMacAddr(interface, &macAddr);

   //Initialize network interface
   error = netConfigInterface(interface);
   //Any error to report?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to configure interface %s!\r\n", interface->name);
   }

#if (IPV4_SUPPORT == ENABLED)
#if (APP_USE_DHCP == ENABLED)
   //Get default settings
   dhcpClientGetDefaultSettings(&dhcpClientSettings);
   //Set the network interface to be configured by DHCP
   dhcpClientSettings.interface = interface;
   //Disable rapid commit option
   dhcpClientSettings.rapidCommit = FALSE;

   //DHCP client initialization
   error = dhcpClientInit(&dhcpClientContext, &dhcpClientSettings);
   //Failed to initialize DHCP client?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to initialize DHCP client!\r\n");
   }

   //Start DHCP client
   error = dhcpClientStart(&dhcpClientContext);
   //Failed to start DHCP client?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to start DHCP client!\r\n");
   }
#else
   //Set IPv4 host address
   ipv4StringToAddr(APP_IPV4_HOST_ADDR, &ipv4Addr);
   ipv4SetHostAddr(interface, ipv4Addr);

   //Set subnet mask
   ipv4StringToAddr(APP_IPV4_SUBNET_MASK, &ipv4Addr);
   ipv4SetSubnetMask(interface, ipv4Addr);

   //Set default gateway
   ipv4StringToAddr(APP_IPV4_DEFAULT_GATEWAY, &ipv4Addr);
   ipv4SetDefaultGateway(interface, ipv4Addr);

   //Set primary and secondary DNS servers
   ipv4StringToAddr(APP_IPV4_PRIMARY_DNS, &ipv4Addr);
   ipv4SetDnsServer(interface, 0, ipv4Addr);
   ipv4StringToAddr(APP_IPV4_SECONDARY_DNS, &ipv4Addr);
   ipv4SetDnsServer(interface, 1, ipv4Addr);
#endif
#endif

#if (IPV6_SUPPORT == ENABLED)
#if (APP_USE_SLAAC == ENABLED)
   //Get default settings
   slaacGetDefaultSettings(&slaacSettings);
   //Set the network interface to be configured
   slaacSettings.interface = interface;

   //SLAAC initialization
   error = slaacInit(&slaacContext, &slaacSettings);
   //Failed to initialize SLAAC?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to initialize SLAAC!\r\n");
   }

   //Start IPv6 address autoconfiguration process
   error = slaacStart(&slaacContext);
   //Failed to start SLAAC process?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to start SLAAC!\r\n");
   }
#else
   //Set link-local address
   ipv6StringToAddr(APP_IPV6_LINK_LOCAL_ADDR, &ipv6Addr);
   ipv6SetLinkLocalAddr(interface, &ipv6Addr);

   //Set IPv6 prefix
   ipv6StringToAddr(APP_IPV6_PREFIX, &ipv6Addr);
   ipv6SetPrefix(interface, &ipv6Addr, APP_IPV6_PREFIX_LENGTH);

   //Set global address
   ipv6StringToAddr(APP_IPV6_GLOBAL_ADDR, &ipv6Addr);
   ipv6SetGlobalAddr(interface, &ipv6Addr);

   //Set router
   ipv6StringToAddr(APP_IPV6_ROUTER, &ipv6Addr);
   ipv6SetRouter(interface, &ipv6Addr);

   //Set primary and secondary DNS servers
   ipv6StringToAddr(APP_IPV6_PRIMARY_DNS, &ipv6Addr);
   ipv6SetDnsServer(interface, 0, &ipv6Addr);
   ipv6StringToAddr(APP_IPV6_SECONDARY_DNS, &ipv6Addr);
   ipv6SetDnsServer(interface, 1, &ipv6Addr);
#endif
#endif

   //Get default settings
   httpServerGetDefaultSettings(&httpServerSettings);
   //Bind HTTP server to the desired interface
   httpServerSettings.interface = &netInterface[0];
   //Listen to port 80
   httpServerSettings.port = HTTP_PORT;
   //Client connections
   httpServerSettings.maxConnections = APP_HTTP_MAX_CONNECTIONS;
   httpServerSettings.connections = httpConnections;
   //Specify the server's root directory
   strcpy(httpServerSettings.rootDirectory, "/www/");
   //Set default home page
   strcpy(httpServerSettings.defaultDocument, "index.shtm");
   //Callback functions
   httpServerSettings.authCallback = httpServerAuthCallback;
   httpServerSettings.cgiCallback = httpServerCgiCallback;
   httpServerSettings.uriNotFoundCallback = httpServerUriNotFoundCallback;

   //HTTP server initialization
   error = httpServerInit(&httpServerContext, &httpServerSettings);
   //Failed to initialize HTTP server?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to initialize HTTP server!\r\n");
   }

   //Start HTTP server
   error = httpServerStart(&httpServerContext);
   //Failed to start HTTP server?
   if(error)
   {
      //Debug message
      TRACE_ERROR("Failed to start HTTP server!\r\n");
   }

   //Create user task
   task = osCreateTask("User Task", userTask, NULL, 500, 1);
   //Failed to create the task?
   if(task == OS_INVALID_HANDLE)
   {
      //Debug message
      TRACE_ERROR("Failed to create task!\r\n");
   }

   //Create a task to blink the LED
   task = osCreateTask("Blink", blinkTask, NULL, 500, 1);
   //Failed to create the task?
   if(task == OS_INVALID_HANDLE)
   {
      //Debug message
      TRACE_ERROR("Failed to create task!\r\n");
   }

   //Start the execution of tasks
   osStartKernel();

   //This function should never return
   return 0;
}
Beispiel #6
0
/**
 * @brief   HAL initialization.
 * @details This function invokes the low level initialization code then
 *          initializes all the drivers enabled in the HAL. Finally the
 *          board-specific initialization is performed by invoking
 *          @p boardInit() (usually defined in @p board.c).
 *
 * @init
 */
void halInit(void) {

  hal_lld_init();

#if HAL_USE_TM || defined(__DOXYGEN__)
  tmInit();
#endif
#if HAL_USE_PAL || defined(__DOXYGEN__)
  palInit(&pal_default_config);
#endif
#if HAL_USE_ADC || defined(__DOXYGEN__)
  adcInit();
#endif
#if HAL_USE_CAN || defined(__DOXYGEN__)
  canInit();
#endif
#if HAL_USE_EXT || defined(__DOXYGEN__)
  extInit();
#endif
#if HAL_USE_GPT || defined(__DOXYGEN__)
  gptInit();
#endif
#if HAL_USE_I2C || defined(__DOXYGEN__)
  i2cInit();
#endif
#if HAL_USE_ICU || defined(__DOXYGEN__)
  icuInit();
#endif
#if HAL_USE_MAC || defined(__DOXYGEN__)
  macInit();
#endif
#if HAL_USE_PWM || defined(__DOXYGEN__)
  pwmInit();
#endif
#if HAL_USE_SERIAL || defined(__DOXYGEN__)
  sdInit();
#endif
#if HAL_USE_SDC || defined(__DOXYGEN__)
  sdcInit();
#endif
#if HAL_USE_SPI || defined(__DOXYGEN__)
  spiInit();
#endif
#if HAL_USE_UART || defined(__DOXYGEN__)
  uartInit();
#endif
#if HAL_USE_USB || defined(__DOXYGEN__)
  usbInit();
#endif
#if HAL_USE_MMC_SPI || defined(__DOXYGEN__)
  mmcInit();
#endif
#if HAL_USE_SERIAL_USB || defined(__DOXYGEN__)
  sduInit();
#endif
#if HAL_USE_RTC || defined(__DOXYGEN__)
  rtcInit();
#endif
  /* Board specific initialization.*/
  boardInit();
}
Beispiel #7
0
//----------------------------------------------------------
//      System initialization
//----------------------------------------------------------
static inline void initSystem(void)
{
    bool success;
    (void)success;

    // disable interrupts: disabled on msp430 by default, but other systems might need this
    DISABLE_INTS();

    // stop the watchdog: GCC disables it by default, but other compilers might not be so helpful
    watchdogStop();

    // TODO: init dynamic memory
    // platformMemInit();

    // basic, platform-specific initialization: timers, platform-specific drivers (?)
    initPlatform();

    // start energy accounting (as soon as timers are initialized)
    energyConsumerOn(ENERGY_CONSUMER_MCU);

#ifdef USE_PRINT
    // init printing to serial (makes sense only after clock has been calibrated)
    if (printInit != NULL) printInit();
#endif

    INIT_PRINTF("starting MansOS...\n");

#ifdef USE_LEDS
    INIT_PRINTF("init LED(s)...\n");
    ledsInit();
#endif
#ifdef USE_BEEPER
    beeperInit();
#endif
#ifdef RAMTEXT_START
    if ((MemoryAddress_t)&_end > RAMTEXT_START) {
        // Panic right aways on RAM overflow.
        // In case this happens, you might want to increase the address
        // specified by CONST_RAMTEXT_START in config file
        assertionFailed("Overflow between .data and .ramtext sections", __FILE__, __LINE__);
    }
#endif
#ifdef USE_ADC
    if (adcInit != NULL) {
        INIT_PRINTF("init ADC...\n");
        adcInit();
    }
#endif
#ifdef USE_RANDOM
    INIT_PRINTF("init RNG...\n");
    randomInit();
#endif
#if USE_ALARMS
    INIT_PRINTF("init alarms...\n");
    initAlarms();
#endif
#ifdef USE_RADIO
    INIT_PRINTF("init radio...\n");
    radioInit();
#endif
#ifdef USE_ADDRESSING
    INIT_PRINTF("init communication stack...\n");
    networkingInit();
#endif
#ifdef USE_EXT_FLASH
    INIT_PRINTF("init external flash...\n");
    extFlashInit();
#endif
#ifdef USE_SDCARD
    INIT_PRINTF("init SD card...\n");
    sdcardInit();
#endif
#ifdef USE_EEPROM
    INIT_PRINTF("init EEPROM...\n");
    eepromInit();
#endif
#ifdef USE_ISL29003
    INIT_PRINTF("init ISL light sensor...\n");
    success = islInit();
    if (!success) {
        INIT_PRINTF("ISL init failed!\n");
    }
#endif
#ifdef USE_ADS1115
    INIT_PRINTF("init ADS111x ADC converter chip...\n");
    adsInit();
#endif
#if USE_ADS8638
    INIT_PRINTF("init ADS8638 ADC converter chip...\n");
    ads8638Init();
#endif
#if USE_ADS8328
    INIT_PRINTF("init ADS8328 ADC converter chip...\n");
    ads8328Init();
#endif
#if USE_AD5258
    INIT_PRINTF("init AD5258 digital potentiometer...\n");
    ad5258Init();
#endif
#if USE_DAC7718
    INIT_PRINTF("init DAC7718 DAC converter chip...\n");
    dac7718Init();
#endif
#if USE_ISL1219
    INIT_PRINTF("init ISL1219 real-time clock chip...\n");
    isl1219Init();
#endif
#ifdef USE_HUMIDITY
    INIT_PRINTF("init humidity sensor...\n");
    humidityInit();
#endif
#ifdef USE_ACCEL
    INIT_PRINTF("init accelerometer...\n");
    accelInit();
#endif
#ifdef USE_TIMESYNC
    INIT_PRINTF("init base station time sync...\n");
    timesyncInit();
#endif
#ifdef USE_SMP
    INIT_PRINTF("init SSMP...\n");
    smpInit();
#endif
#ifdef USE_REPROGRAMMING
    INIT_PRINTF("init reprogramming...\n");
    bootParamsInit();
#endif
#ifdef USE_DCO_RECALIBRATION
    extern void dcoRecalibrationInit(void);
    INIT_PRINTF("init DCO recalibration...\n");
    dcoRecalibrationInit();
#endif
#ifdef USE_FS
    INIT_PRINTF("init file system...\n");
    fsInit();
#endif
#ifdef USE_FATFS
    INIT_PRINTF("init FAT file system...\n");
    fatFsInit();
    INIT_PRINTF("init POSIX-like file routines...\n");
    posixStdioInit();
#endif
#ifdef USE_WMP
    INIT_PRINTF("init WMP...\n");
    wmpInit();
#endif
#ifdef USE_SEAL_NET
    INIT_PRINTF("init SEAL networking...\n");
    sealNetInit();
#endif

    INIT_PRINTF("starting the application...\n");
}
Beispiel #8
0
int main()
{
	// PORTB output for LCD
	DDRB = 0xff;
	PORTB = 0xff;

#ifdef BOARD2
	// PORTC PC0-4 output, PC5 input
	DDRC = 0x1f;
	PORTC = 0x00;
	sbi(PORTC, MUTE);
#endif
#ifdef BOARD1
	// PORTC PC0,2-5 output, PC1 input
	DDRC = 0x3d;
	PORTC = 0x00;
	sbi(PORTC, MUTE);
#endif

	// PORTD is input with pullup
	DDRD = 0x00;
	PORTD = 0xff;

	lcdInit();
	adcInit();

	readGlobalSettings();
	toneCount = 5*F_CPU/tone;

	initInterrupts();
    initPLL();

	sprintf(str, "PE1JPD 23cm v%s", version);
	lcdCursor(0,0);
	lcdStr(str);
	_delay_ms(500);

	for (;;) {
		switch(mode) {
			case VFO:
				mode = Vfo();
				writeGlobalSettings();
				break;
			case MEMORY:
				mode = Memory();
				writeGlobalSettings();
				break;
			case SPECTRUM:
				mode = Spectrum();
				break;
			case MENU:
				mode = Menu(mode);
				break;
			case MEMORY_MENU:
				mode = MemoryMenu(mode);
				break;
			default:
				mode = VFO;
				break;
		}
	}
}
int main() {
	initialize();
	UBRR0H = UBRRH_VALUE;
	UBRR0L = UBRRL_VALUE;

	UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0);

	UCSR0C = (1<<UCSZ00) | (1<<UCSZ01);


	// enable interrupts for the timer
	sei();

	/* Timer aufsetzen: nach 1 ms soll der Interrupt ausgelöst werden. */
	/* 8 bit counter (TIMER0) */
	/* normal mode */
	TCCR0A = 0;
	/* CLK/64 */
	TCCR0B = (1 << CS01) | (1 << CS00);
	/* timer ticks: 250 */
	TCNT0 = 5;
	TIMSK0 = (1 << TOIE0);
	TIFR0 = (1 << TOV0);

	adcInit();

	int16_t ii = 0;
	int16_t jj = 0;

	wdt_enable(WDTO_2S);

/*
	for(;;i++) {
		uart_bin(PINB);
		uart_puts(" ");
		uart_bin(PIND);
		uart_puts(" ");
		uart_hex16(i);
		uart_puts("\r\n");
		_delay_ms(1000);
		wdt_reset();
	}
*/

	uart_puts("Starting up\r\n"); 
	
	for (ii = 0; ii < 4; ii++) {
		positions[ii] = 0;
	}

	// Count the number of cycles the autofocus is stuck in mid-state
	uint8_t mid_state_counter = 0;

	// Count the number of successive movements in the same direction before increasing speed
	uint16_t movement_counter = 0;

	uint16_t current_delay = INITIAL_DELAY;

	enum movement{NONE, UP, DOWN, TILT_DOWN, TILT_UP, LEVEL, EMERGENCY} last_movement, movement;
	last_movement = NONE;
	movement = NONE;

	// Reset autofocus position

	/*
	   while(1) {  
	   adc_start_conversion(7);
	   const uint16_t autofocus_result = adc_wait();
	   uart_hex16(autofocus_result);
	   wdt_reset();
	   uart_puts("\r\n");
	   }
	// */

	uint8_t autofocus_hit_counter = 0;

	movement_counter = 0;
	current_delay = INITIAL_DELAY;
	
	initialize();
	adcInit();
	
	adc_start_conversion();
	uint16_t autofocus_result = adc_wait();

	uint16_t no_movement_counter = 0;
	
	autofocusPosition = 2*HEIGHT_LIMIT;

	for(ii = 0; ; ii++){
		wdt_reset();
		//*
		if (current_delay > 1) {
			for (jj = 0; jj < current_delay; jj++) {
				_delay_us(1);
			}
		}
		// */
		#ifdef DEBUG
		uart_hex16(current_delay);
		uart_puts("\r\n");
		#endif
		// True if autofocus endstop not yet hit.
		const bool autofocus_switch_clear = (autofocus_result <= OPEN_VALUE + INTERVAL_SIZE && autofocus_result >= OPEN_VALUE - INTERVAL_SIZE);
		bool autofocus_clear = true;

		if (!autofocus_switch_clear) {
			if (autofocus_hit_counter > 10) {
				autofocus_clear = false;
				if ((ii % 1024) == 0) {
					uart_puts("Autofocus hit at: ");
					printPositions();
					autofocusPosition = positions[0];
					uart_puts("\r\n");
				}
				if (autofocus_result < SHORT_CIRCUIT_VALUE) {
					uart_puts("Autofocus has short circuit, value is: ");
					uart_hex16(autofocus_result);
					uart_puts("\r\n");
				}
				else if (autofocus_result > CABLE_BROKEN_VALUE) {
					uart_puts("Autofocus cable is broken, value is: ");
					uart_hex16(autofocus_result);
					uart_puts("\r\n");
				}
				else if(autofocus_result > OPEN_VALUE + INTERVAL_SIZE && autofocus_result < CLOSED_VALUE - INTERVAL_SIZE) {
					if (mid_state_counter > 10) {
						uart_puts("Autofocus stuck in mid-state, value is: 0x");
						uart_hex16(autofocus_result);
						uart_puts("\r\n");
					}
					else {
						mid_state_counter++;
					}
				}
			}
			// Increment autofocus_hit_counter in order to detect series of hit autofocus.
			if (autofocus_hit_counter < 20) {
				autofocus_hit_counter++;
			}
		}
		else {
			mid_state_counter = 0;
			autofocus_hit_counter = 0;
		}
		movement = NONE;
		if (!buttonEmergency()) {
			if (buttonDown()) {
				if (!anyStopReached()) {
					stepAllDown();
					movement = DOWN;
				}
				_delay_us(2.08);
			}
			else if (buttonUp()) {
				if (!anyHeightLimitReached() && autofocus_clear) {
					stepAllUp();
					movement = UP;
				}
			}
			else if (buttonTiltFrontUp()) {
				if (!anyStopReached() && !anyHeightLimitReached() && (positions[3]-positions[1] < TILT_LIMIT) && autofocus_clear) {
					stepUp3();
					stepDown1();
					movement = TILT_UP;
				}
				_delay_us(0.96);
			}
			else if (buttonTiltFrontDown()) {
				if (!anyStopReached() && !anyHeightLimitReached() && (positions[1]-positions[3] < TILT_LIMIT) && autofocus_clear) {
					stepDown3();
					stepUp1();
					movement = TILT_DOWN;
				}
				_delay_us(1.52);
			}
			else if (buttonLevel()) {
				if (!anyStopReached() && autofocus_clear) {
					if (positions[1] > positions[3]+1) {
						stepDown1();
						stepUp3();
						movement = LEVEL;
						_delay_us(0.72);
					}
					else if (positions[1]+1 < positions[3]) {
						stepDown3();
						stepUp1();
						movement = LEVEL;
					}
				}
				_delay_us(2.22);
			}
			else if (buttonFocus()) {
				if ((positions[0] > (autofocusPosition - FOCUS_DISTANCE)) && !anyStopReached()) {
					stepAllDown();
					movement = DOWN;
				}
				else if ((positions[0] < (int32_t)(autofocusPosition - FOCUS_DISTANCE)) && autofocus_clear) {
					stepAllUp();
					movement = UP;
				}
				// Needed to fulfill the 2us hold constraint of the step pin
			}
		}
		if (movement == last_movement && movement != NONE) {
			if (movement_counter > 500) {
				current_delay--;
				if (current_delay < MIN_DELAY) {
					current_delay = MIN_DELAY;
				}
				movement_counter = 0;
			}
			movement_counter += current_delay;
		}
		else {
			current_delay = INITIAL_DELAY;
			movement_counter = 0;
		}

		// Do all the emergency stuff
		if (buttonEmergency()) {
			_delay_us(17);
			movement = EMERGENCY;
			if (buttonEmergency() && buttonFocus()) {
				gotoEndstops();
			}
			if (buttonUp()) {
				if (buttonTiltFrontDown()) {
					stepUp1();
				}
				if (buttonLevel()) {
					stepUp2();
				}
				if (buttonTiltFrontUp()) {
					stepUp3();
				}
			}
			if (buttonDown()) {
				if (buttonTiltFrontDown()) {
					stepDown1();
				}
				if (buttonLevel()) {
					stepDown2();
				}
				if (buttonTiltFrontUp()) {
					stepDown3();
				}
				
			}
			if (buttonLevel()) {
				int32_t maxPosition = max(positions[1], max(positions[2], positions[3]));
				if (positions[1] < maxPosition) {
					stepUp1();
				}
				if (positions[2] < maxPosition) {
					stepUp2();
				}
				if (positions[3] < maxPosition) {
					stepUp3();
				}
			}
			// In case of emergency, introduce a safe delay for the step hold time
			_delay_us(2);
		}

		last_movement = movement;
		if (movement == NONE) {
			no_movement_counter++;
			if (no_movement_counter > 2e3) {
				sleep();
				no_movement_counter = 0;
				printPositions();
				uart_puts("\r\n");
			}
		}
		else {
			no_movement_counter = 0;
			if (sleeping) {
				wakeup();
				_delay_ms(100);
			}
		}
		if(!(ADCSRA & (1<<ADSC))) {   // auf Abschluss der Konvertierung warten
			autofocus_result = ADCW;
			adc_start_conversion();
		}
		_delay_us(1.08);
		unsetStep1;
		unsetStep2;
		unsetStep3;
	}

}
int main(void)
{
//***********************************DEBUG********************************************/
#if 0

nmeaPOS p1,p2;
nmeaINFO info;
info.lat = 5547.1206;
info.lon = 4906.2111;
nmea_info2pos(&info, &p1);
info.lat = 5547.1221;
info.lon = 4906.208;
nmea_info2pos(&info, &p2);

m += 23;

u32 t    = nmea_distance(&p1, &p2);
if(m)
#endif
//***********************************END OF DEBUG********************************************/
  NVIC_Configuration();	//for  all peripheria
  if (SysTick_Config(SystemCoreClock / 1000))  //1ms
     { 
       /* Capture error */ 
       while (1);
     }
  Delay(500);


  USBIniPin();
  	  
  signUSBMass = USBDetectPin();
  signUSBMass = 0;  //deb
  #if not defined (VER_3)
    USBCommonIni(); 
  #endif
  if(signUSBMass)
    {
	 #if defined (VER_3)
	 USBCommonIni();
	 #endif
     while (bDeviceState != CONFIGURED);
	}
  else //if(!signUSBMass)
   {

  	 /* Flash unlock */
     FLASH_Unlock();
     /* Clear All pending flags */
     FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);	

//***********************************DEBUG********************************************/

     
//***********************************END OF DEBUG********************************************/
  /* Enable CRC clock */
  RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
    
  ledDioGPIOInit();
  led_dn(BOARD_LED_ON);
  led_mid(BOARD_LED_ON);
#if defined (VER_3)
  led_up(BOARD_LED_ON);
  ibuttonInit();
  rfmodemInit();
#endif
  Delay(1000);
  alarmInit();
  BKPInit();

  //timer6Init();

  //rs485Init();
#if not defined (VER_3)
  ais326dq_init();
#endif

  //ais326dq_data(&ais326dq_out);
  /*ADC*/
  adcInit();
  /*GPS*/
  gpsInit();

  /* reading settings */
  readConfig();
  /*MODEM*/
  gprsModemInit();
  gprsModemOn(innerState.activeSIMCard);
//***********************************DEBUG********************************************/
  //GSMSpeaker(1);
//***********************************END OF DEBUG********************************************/

#ifndef BRIDGE_USB_GSM
  setupGSM();
  ftpGSMPrepare();
  packetsIni();
#endif

  led_dn(BOARD_LED_OFF);
  led_mid(BOARD_LED_OFF);
#if defined (VER_3)
  led_up(BOARD_LED_OFF);
#endif

  rtc_init();
  rtc_gettime(&rtc);

#if 1			  /* WATCH DOG */
  /* IWDG timeout equal to 3.27 sec (the timeout may varies due to LSI frequency dispersion) */
  /* Enable write access to IWDG_PR and IWDG_RLR registers */
  IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
  /* IWDG counter clock: 40KHz(LSI) / 32 = 1.25 KHz */
  IWDG_SetPrescaler(IWDG_Prescaler_64);	//32
  /* Set counter reload value to 0xFFF */
  IWDG_SetReload(0xFFF);
  /* Reload IWDG counter */
  IWDG_ReloadCounter();
  /* Enable IWDG (the LSI oscillator will be enabled by hardware) */
  IWDG_Enable();
  setTimerIWDG(ONE_SEC);
#endif

  initSD();
#if defined (VER_3)
  //DACInit();
#endif

  /* Log  */
  saveSDInfo((u8 *)"TURN ON BLOCK ",strlen((u8 *)"TURN ON BLOCK "), SD_SENDED, SD_TYPE_MSG_LOG );
  //saveSDInfo((u8 *)readRTCTime(&rtc),strlen((const char *)readRTCTime(&rtc)), SD_SENDED, SD_TYPE_MSG_LOG );
#if defined (VER_3)  
  //DACSpeaker(1);
  //wp_play("0:/sound.wav");
  //DACSpeaker(0);
#endif 

  }	 //if(!signUSBMass)

  while (1)
  {
    if(!signUSBMass)
	  {
		 monitorWatchDog();

		 #ifndef BRIDGE_USB_GPS
	     if(!innerState.bootFTPStarted)
	         gpsHandling();
		 #endif

		 #ifndef BRIDGE_USB_GSM
		 if(!innerState.flagTmpDebug)
		    loopGSM();
			loopFTP();
            UpdatingFlash();
			if(!innerState.bootFTPStarted)
			   naviPacketHandle();
			rcvPacketHandle();
            rcvPacketHandleUSB();
		 #endif 

		 #if !defined (VER_3)
			 buttonScan();
		     accelScan();
		 #endif
		 
		 //rs485Analyse();
		 handleFOS();
		 executeDelayedCmd();
#if defined (VER_3)
		 #if 0 
		 if(innerState.flagDebug)
		 {
  			DACSpeaker(1);
	        /* Start DAC Channel1 conversion by software */
		    //a += 300;
    		//DAC_SetChannel1Data(DAC_Align_12b_R, 4000);
    		//DAC_SetChannel1Data(DAC_Align_12b_L, a);  //for saw
    		//DAC_SetChannel1Data(DAC_Align_8b_R, a);
			
			//DAC_SetChannel1Data(DAC_Align_12b_R, 4095);
			//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
			//for (a = 0; a<4095; ++a)
			//for(;;)
			//  DAC_SetChannel1Data(DAC_Align_12b_R, 0);

			//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
			//for ( ; ; )
			//{
			//   DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE);
			//}
 

	        DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE);  //debugga
		 }
		 else
		 {
		     DACSpeaker(0);
		 }
	  #endif
#endif
	 }
	else
	  handleUSBPresent();
  }	   //while(1)
}