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;
}
