TEST_RESULT_T test(TEST_PARAMETER_T *ptTestParam) { TEST_RESULT_T tTestResult; SQITEST_PARAMETER_T *ptTestParams; QSI_CFG_T tQsiCfg; const unsigned char * const pucSqiRomAddress = (const unsigned char * const)(HOSTADDR(sqirom)); systime_init(); ptTestParams = (SQITEST_PARAMETER_T*)(ptTestParam->pvInitParams); /* Switch off SYS led. */ rdy_run_setLEDs(RDYRUN_OFF); /* Say "hi". */ uprintf("\f. *** SQI test start ***\n"); uprintf(". offset: 0x%08x\n", ptTestParams->ulOffset); uprintf(". size: 0x%08x\n", ptTestParams->ulSize); uprintf(". buffer: 0x%08x\n", ptTestParams->pucBuffer); tTestResult = boot_sqi_xip(&tQsiCfg); if( tTestResult==TEST_RESULT_OK ) { memcpy(ptTestParams->pucBuffer, pucSqiRomAddress+ptTestParams->ulOffset, ptTestParams->ulSize); } return tTestResult; }
int main() { // Initialize the hardware sp_timer_init(); led_init(); adc_init(); systime_init(); // Initalize the routines for preemption spk_init(); // Initialize the data_ready semaphore TASK_SEM_INIT(&data_ready, 0); // Create the fft_task sp_create( fft_task, 1 ); // Init the radio cc1k_radio_init(); // Create the sampling_task sp_create( sampling_task, 2 ); /* if( sp_node_address == 1 ) { sp_create( test_radio, 1 ); led_yellow_toggle(); } else { sp_create( test_receiver, 1 ); led_yellow_toggle(); } */ //ENABLE_INT(); // Call the scheduler spk_sched(); return 0; }
int main() { led_init(); led_on(); systime_init(); console_init(); puts("===== BL RESET =====\r\n"); uint32_t t_last_blink = SYSTIME; uint32_t t_start = SYSTIME; #define BLINK_HALF_PERIOD 100000 #define BOOT_TIMEOUT 200000 while (1) { if (SYSTIME - t_last_blink > BLINK_HALF_PERIOD) { t_last_blink += BLINK_HALF_PERIOD; led_toggle(); } if (/*g_rs485_boot_requested ||*/ (SYSTIME - t_start > BOOT_TIMEOUT /*&& !flash_writes_occurred()*/)) { run_application(); } } return 0; }
/* Initialisation */ void rcx_init (void) { init_timer(&async, &dispatch[0]); init_power(); systime_init(); init_sensors(); //init_motors init_buttons(); // Buttons & display pins init_serial(0, 0, 1, 1); }
void hardware_init (void){ init_IO(); /** * @brief set watchdog timer * we use hardware fuse bit to enforce watchdog is always on * and ask timer to reset watchdog * Set up the longest watchdog here */ //! Ram - From the AVR datasheet, the typical watchdog reset interval is 1.9 seconds at 3V Vcc for the max. prescalar value // __asm__ __volatile__ ("wdr"); // WDTCR = (1 << WDCE) | (1 << WDE); //! Ram - Start the timed sequence required to change the watchdog pre-scaler values // WDTCR = (1 << WDE) | (1 << WDP2) | (1 << WDP1) | (1 << WDP0); //! Ram - Set the pre-scalar values (Must be done within 4 clock cycles) (compiler must pre-compute const ?) //! component level init //! we do led initialization in sos_watchdog_processing systime_init(); // SYSTEM TIMER timer_hardware_init(DEFAULT_INTERVAL, DEFAULT_SCALE); #ifdef USE_UART1 SET_FLASH_SELECT_DD_OUT(); SET_FLASH_OUT_DD_OUT(); SET_FLASH_CLK_DD_OUT(); SET_FLASH_SELECT(); #endif // UART uart_system_init(); #ifndef NO_SOS_UART //! Initalize uart comm channel sos_uart_init(); #endif // I2C i2c_system_init(); #ifndef NO_SOS_I2C //! Initalize i2c comm channel sos_i2c_init(); //! Initialize the I2C Comm Manager // Ram - Assuming that it is turned on // by default with the SOS_I2C component sos_i2c_mgr_init(); #endif // ADC adc_proc_init(); #ifndef SOS_EMU // radio_init(NON_BEACONED_PAN); //hubert: use mac_init for vmac mac_init(); #endif }
int main() { led_init(); led_on(); console_init(); puts("===== APP ENTRY =====\r\n"); systime_init(); while (1) { delay_ms(500); led_toggle(); } return 0; }
TEST_RESULT_T test_main(TEST_PARAMETER_T *ptTestParameter) { TEST_RESULT_T tTestResult; USER_PARAMETER_T *ptUserParameter; /* Initialize the SYSTIME unit. * This is important for delays and SYS LED blink sequences. */ systime_init(); /* Show a welcome message including the version. */ uprintf("\f. *** Test skeleton by [email protected] ***\n"); uprintf("V" VERSION_ALL "\n\n"); /* Switch off SYS LED. */ rdy_run_setLEDs(RDYRUN_OFF); /* Show the address of the test parameter. */ uprintf("ptTestParameter: 0x%08x\n", ptTestParameter); /* Get the user parameter. */ ptUserParameter = (USER_PARAMETER_T*)(ptTestParameter->pvUserParameter); /* Show the address of the user parameter. */ uprintf("ptUserParameter: 0x%08x\n", ptUserParameter); /* Call the test routine. */ tTestResult = test_routine(ptUserParameter); /* Set the RDY/RUN LED to green if the result of the test is "OK". * Set it to yellow if an error occured. */ if( tTestResult==TEST_RESULT_OK ) { rdy_run_setLEDs(RDYRUN_GREEN); } else { rdy_run_setLEDs(RDYRUN_YELLOW); } return tTestResult; }
TEST_RESULT_T test(TEST_PARAMETER_T *ptTestParam) { TEST_RESULT_T tTestResult; CRCTEST_PARAMETER_T *ptTestParams; const unsigned char *pucCnt; const unsigned char *pucEnd; unsigned long ulCrc32; systime_init(); uprintf("\f. *** CRC test by [email protected] ***\n"); uprintf("V" VERSION_ALL "\n\n"); /* Switch off SYS led. */ rdy_run_setLEDs(RDYRUN_OFF); /* Get the test parameter. */ ptTestParams = (CRCTEST_PARAMETER_T*)(ptTestParam->pvInitParams); /* Build the CRC for the requested area. */ pucCnt = ptTestParams->pucAreaStart; pucEnd = ptTestParams->pucAreaEnd; uprintf(". area start: 0x%08x\n", ptTestParams->pucAreaStart); uprintf(". area end : 0x%08x\n\n", ptTestParams->pucAreaEnd); uprintf("Calculating CRC32 ...\n"); crc_init_crc32(); while( pucCnt<pucEnd ) { crc_update( *(pucCnt++) ); } ulCrc32 = crc_get_crc32(); ptTestParams->ulCrc32 = ulCrc32; uprintf("CRC32 = 0x%08x\n", ulCrc32); rdy_run_setLEDs(RDYRUN_GREEN); tTestResult = TEST_RESULT_OK; return tTestResult; }
/* This is your main function for the test, your control starts here. */ NETX_CONSOLEAPP_RESULT_T netx_consoleapp_main(NETX_CONSOLEAPP_PARAMETER_T *ptTestParam) { /* this is the result of the test */ NETX_CONSOLEAPP_RESULT_T tTestResult; /* this is the input parameter from the xml file */ unsigned long ulParameter; /* Init all modules. */ systime_init(); /* the input parameter is */ ulParameter = (unsigned long)ptTestParam->pvInitParams; /* say hi */ uprintf(". *** test skeleton start ***\n"); uprintf(". Parameter Address: 0x%08x\n", (unsigned long)ptTestParam); uprintf(". Parameter: 0x%08x\n", ulParameter); /* Print a very long line (longer than 1 USB packet). */ uprintf("012345678901234567890123456789012345678901234567890123456789012345678901234567\n"); uprintf("000000000011111111112222222222333333333344444444445555555555666666666677777777\n"); /* Print messages with a specific delay. */ delay_print(100, 0); delay_print( 8, 500); delay_print( 4, 1000); delay_print( 2, 2000); /* write parameter to return message */ ptTestParam->pvReturnMessage = (void*)ulParameter; /* test result is "ok" */ tTestResult = NETX_CONSOLEAPP_RESULT_OK; return tTestResult; }
int main() { led_init(); led_on(); console_init(); printf("********************************\r\nAPP ENTRY\r\n"); systime_init(); usb_init(); dmxl_init(); __enable_irq(); while (1) { /* while (g_num_usb_sof < 500) { } g_num_usb_sof = 0; //led_toggle(); g_status_pkt.t = systime_usecs(); usb_tx(1, (const uint8_t *)&g_status_pkt, sizeof(g_status_pkt)); //printf("systime %d\r\n", (int)systime_usecs()); */ dmxl_tick(); } return 0; }
void uart_monitor(void) { #if ASIC_TYP==ASIC_TYP_NETX56 unsigned long ulRomId; unsigned long ulConsoleDevice; #endif #if ASIC_TYP==ASIC_TYP_NETX50 unsigned long ulConsoleDevice; #endif systime_init(); #if ASIC_TYP==ASIC_TYP_NETX500 /* Both ASICs in this group can not use the ROM routines for UART * communication. * * The netX500 and netX100 ROM code UART put routine converts LF (0x0a) * to CR LF (0x0d 0x0a). It is not possible to send binary data with * it. Replace the vectors with custom routines. */ /* Initialize the UART. */ uart_init(&tUartCfg); /* Set the new vectors. */ tSerialV1Vectors.fn.fnGet = uart_get; tSerialV1Vectors.fn.fnPut = uart_put; tSerialV1Vectors.fn.fnPeek = uart_peek; tSerialV1Vectors.fn.fnFlush = uart_flush; #elif ASIC_TYP==ASIC_TYP_NETX10 /* The netX10 ROM code uses areas in bank0 around offset 0x8180. This * is outside the RAM area reserved for the monitor code. */ /* Set the new vectors. */ tSerialV1Vectors.fn.fnGet = uart_get; tSerialV1Vectors.fn.fnPut = uart_put; tSerialV1Vectors.fn.fnPeek = uart_peek; tSerialV1Vectors.fn.fnFlush = uart_flush; #elif ASIC_TYP==ASIC_TYP_NETX50 /* Compare vectors to netx50 USB. This one needs special treatment. */ if( memcmp(&tSerialV2Vectors, &tSerialNetx50UsbVectors, sizeof(SERIAL_V2_COMM_FN_T))==0 ) { /* USB CDC */ usb_init(); ulConsoleDevice = (unsigned long)CONSOLE_DEVICE_USB; } else { /* UART */ /* In this mode the USB endpoints are in the state "not configured". */ tReceiveEpState = USB_EndpointState_Unconfigured; /* Copy the ROM code vectors to an internal buffer. */ memcpy(&tSerialV1Vectors, &tSerialV2Vectors, sizeof(SERIAL_V2_COMM_FN_T)); ulConsoleDevice = (unsigned long)CONSOLE_DEVICE_UART; } transport_set_vectors(ulConsoleDevice); #elif ASIC_TYP==ASIC_TYP_NETX56 ulRomId = aulRomId[2]; if( ulRomId==ROM_CODE_ID_NETX56 ) { ulConsoleDevice = aulConsoleDevices_netx56[0]; } else if( ulRomId==ROM_CODE_ID_NETX56B ) { ulConsoleDevice = aulConsoleDevices_netx56b[0]; } else { ulConsoleDevice = (unsigned long)CONSOLE_DEVICE_NONE; } if( ulConsoleDevice!=((unsigned long)CONSOLE_DEVICE_USB) && ulConsoleDevice!=((unsigned long)CONSOLE_DEVICE_UART0) ) { while(1) {}; } transport_set_vectors(ulConsoleDevice); #else # error "Unknown ASIC_TYP!" #endif transport_init(); while(1) { transport_loop(); } }
void reset_vector(void) { g_stack[0] = 0; // need to put a reference in here to the stack array // to make sure the linker brings it in. I'm sure there // is a more elegant way to do this, but this seems to work EFC->EEFC_FMR = EEFC_FMR_FWS(5); // slow down flash for our blazing speed WDT->WDT_MR = WDT_MR_WDDIS; // disable watchdog for now // TODO: a block of code which can be ifdef'd in and out to source the // slow clock from a 32 kHz crystal rather than the (relatively) inaccurate // internal RC oscillator PMC->PMC_MCKR = (PMC->PMC_MCKR & ~(uint32_t)PMC_MCKR_CSS_Msk) | PMC_MCKR_CSS_MAIN_CLK; PMC->CKGR_MOR = CKGR_MOR_KEY_PASSWD | CKGR_MOR_MOSCXTST(0x10) | // startup time: slowclock*8*this CKGR_MOR_MOSCRCEN | // keep main on-chip RC oscillator on ! CKGR_MOR_MOSCXTEN; // crystal oscillator enable (not select) while (!(PMC->PMC_SR & PMC_SR_MOSCSELS)) { } // spin until stable while (!(PMC->PMC_SR & PMC_SR_MCKRDY)) { } // spin until selected PMC->CKGR_MOR = CKGR_MOR_KEY_PASSWD | // "password" hard-wired in logic CKGR_MOR_MOSCXTST(0x10) | // startup time: slowclock*8*this CKGR_MOR_MOSCRCEN | // keep main on-chip RC oscillator on ! CKGR_MOR_MOSCXTEN; // main crystal oscillator enable while (!(PMC->PMC_SR & PMC_SR_MOSCXTS)) { } // busy wait // switch to main crystal oscillator PMC->CKGR_MOR = CKGR_MOR_KEY_PASSWD | CKGR_MOR_MOSCXTST(0x10) | CKGR_MOR_MOSCRCEN | // keep on-chip RC oscillator on ! CKGR_MOR_MOSCXTEN | CKGR_MOR_MOSCSEL; while (!(PMC->PMC_SR & PMC_SR_MOSCSELS) || !(PMC->PMC_SR & PMC_SR_MCKRDY) ) { } // spin until stable PMC->PMC_MCKR = (PMC->PMC_MCKR & ~(uint32_t)PMC_MCKR_CSS_Msk) | PMC_MCKR_CSS_MAIN_CLK; while (!(PMC->PMC_SR & PMC_SR_MCKRDY)) { } // spin until selected // now, let's measure the frequency of the main crystal oscillator PMC->CKGR_MCFR = CKGR_MCFR_CCSS | // measure the crystal oscillator CKGR_MCFR_RCMEAS ; // start a new measurement // PLLA must output between 150 MHz and 500 MHz // board has 12 MHz crystal; let's multiply by 24 for 288 MHz PLL freq #define MUL 23 PMC->CKGR_PLLAR = CKGR_PLLAR_ONE | // per datasheet, must set 1<<29 CKGR_PLLAR_MULA(MUL) | // pll = crystal * (mul+1)/div CKGR_PLLAR_DIVA(1) | CKGR_PLLAR_PLLACOUNT(0x3f); while (!(PMC->PMC_SR & PMC_SR_LOCKA)) { } // spin until lock // don't use a divider... use the PLL output as CPU clock and divide CPU // clock by 2 to get 144 MHz for the master clock PMC->PMC_MCKR = PMC_MCKR_CSS_MAIN_CLK | // | PMC_MCKR_MDIV_PCK_DIV2; while (!(PMC->PMC_SR & PMC_SR_MCKRDY)) { } // spin until ready // finally, dividers are all set up, so let's switch CPU to the PLLA output PMC->PMC_MCKR = PMC_MCKR_CSS_PLLA_CLK | PMC_MCKR_MDIV_PCK_DIV2; while (!(PMC->PMC_SR & PMC_SR_MCKRDY)) { } // spin until selected // now we're running the CPU at 288 MHz and the system at 144 MHz uint32_t *pSrc, *pDest; // set up data segment pSrc = &_etext; pDest = &_srelocate; if (pSrc != pDest) for (; pDest < &_erelocate; ) *pDest++ = *pSrc++; // set up bss segment for (pDest = &_szero; pDest < &_ezero; ) *pDest++ = 0; // set vector table base address (if needed) pSrc = (uint32_t *)&_sfixed; SCB->VTOR = ( (uint32_t)pSrc & SCB_VTOR_TBLOFF_Msk ); // 7 LSB's are 0 if (((uint32_t)pSrc >= IRAM_ADDR) && ((uint32_t)pSrc < IRAM_ADDR+IRAM_SIZE)) SCB->VTOR |= 1 << 29; // TBLBASE bit enable_fpu(); __libc_init_array(); static char metal_stdout_buf[1024]; setvbuf(stdout, metal_stdout_buf, _IOLBF, sizeof(metal_stdout_buf)); systime_init(); led_init(); console_init(); main(); while (1) { } // hopefully we never get here... }
//------------------------------------------------------------------------- // FUNCTION DECLARATION //------------------------------------------------------------------------- void hardware_init(void) { init_IO(); // WATCHDOG SETUP FOR AVR #ifndef DISABLE_WDT /* * Hardware Fuse Bit ensures WDT is always ON * The following timed sequence sets up a WDT * with the longest timeout period. */ __asm__ __volatile__ ("wdr"); WDTCR = (1 << WDCE) | (1 << WDE); WDTCR = (1 << WDE) | (1 << WDP2) | (1 << WDP1) | (1 << WDP0); #else /* * WDT may need to be disabled during debugging etc. * You must also unset the WDT fuse. If it is set it * is not possible to disable the WDT in software. * Setting the fuses to ff9fff will allow it to be disabled. */ __asm__ __volatile__ ("wdr"); WDTCR = (1 << WDCE) | (1 << WDE); WDTCR = 0; #endif //DISABLE_MICA2_WDT // LEDS // We do led initialization in sos_watchdog_processing // LOCAL TIME systime_init(); // SYSTEM TIMER timer_hardware_init(DEFAULT_INTERVAL, DEFAULT_SCALE); // UART #ifdef USE_UART1 SET_FLASH_SELECT_DD_OUT(); SET_FLASH_SELECT(); #endif uart_system_init(); #ifndef NO_SOS_UART //! Initalize uart comm channel sos_uart_init(); #endif // I2C // always initalize the i2c system i2c_system_init(); #ifndef NO_SOS_I2C //! Initalize i2c comm channel sos_i2c_init(); //! Initialize the I2C Comm Manager // Ram - Assuming that it is turned on // by default with the SOS_I2C component sos_i2c_mgr_init(); #endif // ADC adc_proc_init(); // RADIO #ifndef SOS_EMU cc1k_radio_init(); #ifndef DISABLE_RADIO cc1k_radio_start(); #ifdef RADIO_XMIT_POWER cc1k_cnt_SetRFPower(RADIO_XMIT_POWER); #endif//RADIO_XMIT_POWER #else cc1k_radio_stop(); #endif//DISABLE_RADIO #endif//SOS_EMU // EXTERNAL FLASH #ifndef USE_UART1 exflash_init(); #endif // MICA2 PERIPHERALS (Optional) #ifdef SOS_MICA2_PERIPHERAL mica2_peripheral_init(); #endif //TODO: We may want to move this out of the mica2 hardware (Roy) one_wire_init(); }
void kmain(void) { int reset_after_shutdown=0; #ifdef CONF_DKEY int c; #endif /* Install the text.hi segment in the correct place. The * firmware loader puts it in the bss segment, we copy it * to it's final location. */ memcpy(&__text_hi, &__bss, &__etext_hi - &__text_hi); reset_vector = rom_reset_vector; /* Turn off motor, since writing to hitext manipulates motors */ motor_controller = 0; memset(&__bss, 0, &__bss_end - &__bss); #ifdef CONF_MM mm_init(); #endif while (1) { power_init(); #ifdef CONF_AUTOSHUTOFF shutoff_init(); #endif lcd_init(); #ifdef CONF_DSOUND dsound_init(); #endif #ifdef CONF_TIME systime_init(); #endif #ifdef CONF_DSENSOR ds_init(); #endif #ifdef CONF_DMOTOR dm_init(); #endif #ifdef CONF_LNP lnp_init(); lnp_logical_init(); #endif #ifdef CONF_TM tm_init(); #endif #ifdef CONF_PROGRAM program_init(); #endif show_on(); // wait till power key released // #ifdef CONF_DKEY dkey_multi=KEY_ANY; while((c=dkey_multi) & KEY_ONOFF); #else while (PRESSED(dbutton(), BUTTON_ONOFF)); delay(100); #endif cls(); #ifndef CONF_PROGRAM lcd_show(man_run); #ifndef CONF_LCD_REFRESH lcd_refresh(); #endif #endif // run app // #ifdef CONF_TM #ifndef CONF_PROGRAM execi(&main,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE); #endif tm_start(); #else main(0,0); #endif show_off(); // ON/OFF + PROGRAM -> erase firmware #ifdef CONF_DKEY while((c=dkey_multi) & KEY_ONOFF) if(c&KEY_PRGM) reset_after_shutdown=1; #else while (PRESSED(dbutton(), BUTTON_ONOFF)) if (PRESSED(dbutton(), BUTTON_PROGRAM)) reset_after_shutdown=1; #endif #ifdef CONF_PROGRAM program_shutdown(); #endif #ifdef CONF_LNP lnp_logical_shutdown(); #endif #ifdef CONF_DMOTOR dm_shutdown(); #endif #ifdef CONF_DSENSOR ds_shutdown(); #endif #ifdef CONF_TIME systime_shutdown(); #endif if (reset_after_shutdown) rom_reset(); lcd_clear(); lcd_power_off(); power_off(); } }
int main(void) { uint32_t start_time; uint32_t end_time; uint16_t max_current; uint16_t max_voltage; uint8_t on_off = 0; const button_status_t* bstatus; // safety delay _delay_ms(200); load_fuses_from_eeprom(&max_current,&max_voltage); systime_init(); sound_init(); uart_init(); ui_measure_init(); buttons_init(); lcd_init(); sei(); lcd_write("abcdefghijklmnop","ABCDEFGHIJKLMNOP"); dbg_trace1("Project name: %s\r\n", FIRMWARE_NAME); dbg_trace1("Version: %s\r\n", FIRMWARE_VERSION); dbg_trace1("Last update date: %s\r\n", FIRMWARE_LAST_UPDATE_DATE); dbg_trace1("Last update time: %s\r\n", FIRMWARE_LAST_UPDATE_TIME); dbg_trace1("Author: %s\r\n\r\n", FIRMWARE_AUTHOR); dbg_trace1("Maximal current is: %u\r\n", max_current); dbg_trace1("Maximal voltage is: %u\r\n\r\n", max_voltage); sound_play_intro(); start_time = systime_get_time(); end_time = start_time + 100; while (1) { if ((bstatus = buttons_routine(&bstatus))) { if (bstatus->butt1_redge) { increment_current(&max_current); sound_beep_high(); store_fuses_to_eeprom(max_current,max_voltage); dbg_trace1("current fuse set to %u mA\n", max_current); } else if (bstatus->butt2_redge) { decrement_current(&max_current); sound_beep_low(); store_fuses_to_eeprom(max_current,max_voltage); dbg_trace1("current fuse set to %u mA\n", max_current); } else if (bstatus->butt3_redge) { increment_voltage(&max_voltage); sound_beep_high(); store_fuses_to_eeprom(max_current,max_voltage); dbg_trace1("voltage fuse set to %u mV\n", max_voltage); } else if (bstatus->butt4_redge) { decrement_voltage(&max_voltage); sound_beep_low(); store_fuses_to_eeprom(max_current,max_voltage); dbg_trace1("voltage fuse set to %u mV\n", max_voltage); } else if (bstatus->butt5_redge) { if (on_off) { on_off = 0; dbg_trace0("power supply is off\n"); sound_beep_low(); } else { on_off = 1; dbg_trace0("power supply is on\n"); sound_beep_high(); } } } if (systime_is_time_passed(start_time, end_time)) { start_time = end_time; end_time = start_time + 100; uint16_t voltage = ui_measure_get_voltage(); uint16_t current = ui_measure_get_current(); char c[32]; char v[32]; sprintf(v,"%u",voltage); sprintf(c,"%u",current); lcd_write(v,c); dbg_trace2("%u\t %u\n", current, voltage); } sound_routine(); // watch dog reset a sleep } }
int main() { led_init(); led_on(); console_init(); printf("===== APP ENTRY =====\r\n"); systime_init(); enc_init(); usb_init(); halls_init(); therm_init(); //enc_print_regs(); printf("entering blink loop...\r\n"); __enable_irq(); usb_tx(1, g_tx_buf, sizeof(g_tx_buf)); uint16_t raw_angle = 0, prev_raw_angle = 0; float raw_vel = 0; float filt_vel[3] = {0}; float filt_angle[3] = {0}; //float raw_vel = 0, filt_vel = 0, filt_angle = 0; bool filter_init = false; float unwrapped_raw = 0, prev_unwrapped_raw = 0; uint32_t t = 0, t_last_led_blink = 0; const float pos_gain[3] = { 0.9f, 0.99f, 0.999f }; const float vel_gain[3] = { 0.99f, 0.999f, 0.9999f }; int wraps = 0; uint32_t t_last_therm_reading = 0; g_therm_celsius = therm_celsius(); while (1) { if (SYSTIME - t_last_therm_reading > 1000) { g_therm_celsius = therm_celsius(); t_last_therm_reading = SYSTIME; } if (SYSTIME - t_last_led_blink > 100000) { t_last_led_blink = SYSTIME; led_toggle(); /* printf("\n\n"); printf("gintsts = 0x%08x\r\n", (unsigned)USB_OTG_FS->GINTSTS); printf("dctl = 0x%08x\r\n", (unsigned)g_usbd_dbg->DCTL); printf("dsts = 0x%08x\r\n", (unsigned)g_usbd_dbg->DSTS); printf("dtxfsts1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DTXFSTS); printf("diepctl1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPCTL); printf("diepint1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPINT); printf("dieptsiz1= 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPTSIZ); */ } raw_angle = enc_poll_angle(); t = SYSTIME; if (filter_init) { int diff = raw_angle - prev_raw_angle; if (diff > 8000) wraps--; else if (diff < -8000) wraps++; unwrapped_raw = (float)raw_angle + wraps * 16384; // calculate raw_vel in ticks/usec for numerical stability // TODO: use a better timebase, since we're polling @ 100 khz so there // is extreme quantization on the microsecond clock float dt_usecs = (float)(t - g_t_angle) * 1000000.0f; if (dt_usecs < 1.0f) dt_usecs = 1.0f; // todo: this leads to bad numerical stability after lots of wraps // need to re-work this crap raw_vel = (unwrapped_raw - prev_unwrapped_raw) / dt_usecs; for (int i = 0; i < 3; i++) { filt_angle[i] = pos_gain[i] * filt_angle[i] + (1.0f - pos_gain[i]) * unwrapped_raw; filt_vel[i] = vel_gain[i] * filt_vel[i] + (1.0f - vel_gain[i]) * raw_vel * 1000000.0f; } } else { filter_init = true; for (int i = 0; i < 3; i++) { filt_angle[i] = raw_angle; filt_vel[i] = 0; } } prev_raw_angle = raw_angle; prev_unwrapped_raw = unwrapped_raw; __disable_irq(); g_t_angle = t; g_raw_angle = raw_angle; for (int i = 0; i < 3; i++) { g_angle[i] = filt_angle[i]; g_vel[i] = filt_vel[i]; // * 0.000001f; // convert to ticks / sec } g_num_samp++; __enable_irq(); } return 0; }