extern void SAM_init (void) { /* C setup: initialized variables */ memcpy (&data_start, &code_end, &data_end - &data_start); /* C setup: not initialized variables */ memset (&bss_start, 0, &bss_end - &bss_start); /* the wdt mode register is write-once! */ if (SAM_WDT_DISABLE) { SAM_wdt_disable (); /* disable also writes the wdt mode register */ } else { SAM_wdt_setup (SAM_WDV_10SEC); /* initial wtv is 16 sec, but we may come from bootloader */ } SAM_pins_init (); DEV_at91_RST_set_user_reset_enable (AT91C_BASE_RSTC, 1); /* Low level debug (polling) initialization */ DEV_at91_DBGU_configure (); DEV_at91_DBGU_print_ascii ("AT91SAM7X chip ID : "); DEV_at91_DBGU_print_hex8 (AT91C_BASE_DBGU->DBGU_CIDR); DEV_at91_DBGU_print_ascii ("\n"); /* Use green, red LED for debugging */ DEV_loops_per_msec = SAM_LOOPS_PER_MSEC; SAM_digio_init (); blink_green (1); /* Abort handler initialization */ ACE_stdlib_init (&heap, abort_handler, panic_handler); /* Heap initialization */ if (USO_heap_init (&heap, &heap_start, &heap_end) == FALSE) { ACE_PANIC ("Heap init failed!"); } /* SYS FS needs heap */ if (MFS_sysfs_init () == FALSE) { ACE_PANIC ("Sysfs init failed!"); } USO_heap_install (&heap, "heap0"); SAM_digio_install (); blink_green (1); /* Go multithreading */ USO_transform (idle_run, (USO_stack_t *) &stack_start, SAM_IDLE_STACK_SIZE / sizeof (USO_stack_t)); }
/****************************************************************** * on_mode_release() * toggle between position and angle modes if MiP is paused *******************************************************************/ int on_mode_release(){ // store whether or not the controller was armed int was_armed = setpoint.arm_state; // disarm the controller if necessary if(was_armed == ARMED){ disarm_controller(); } // toggle between position and angle modes if(setpoint.core_mode == POSITION){ setpoint.core_mode = ANGLE; printf("using core_mode = ANGLE\n"); } else { setpoint.core_mode = POSITION; printf("using core_mode = POSITION\n"); } // arm the controller if it was armed before swapping modes if(was_armed == ARMED){ zero_out_controller(); arm_controller(); } blink_green(); return 0; }
void blink(const int n, const color_t c) { switch (c) { case RED: blink_red(n); break; case GREEN: blink_green(n); break; case BLUE: blink_blue(n); break; default: break; } }
/*********************************************************************** * on_mode_release() * placeholder for now, just blink led ***********************************************************************/ int on_mode_release(){ blink_green(); return 0; }
static void idle_run (void) { /* System initialization without kernel logging! * The scheduler is initialized and we are running on the idle * thread so now we are allowed to start threads! * The idle thread is not allowed to block! */ USO_buf_pool_init (&ios_buf_pool, ios_bufs, IOS_BUF_COUNT, IOS_BUF_SIZE); ACE_stdio_init (&ios_buf_pool); USO_sleep_init (); DEV_timers_init (); DEV_at91_FLASHD_initialize (SAM_MCK); SAM_uart_init_0 (); SAM_uart_init_1 (); SAM_rtc_init (); SAM_ticks_init (); SAM_sys_interrupt_init (); SAM_adc_init (); SAM_pwm_init (); SAM_events_init (); blink_green (1); ser0 = MFS_resolve("/sys/dev/serial/ser0"); if (ser0 == NULL) { ACE_PANIC ("Open ser0 fail"); } /* Initialize kernel logging */ CLI_tty_init (&tty_0, ser0, CLI_TTY_INTRANSL_CR_2_NL, CLI_TTY_OUTTRANSL_ADD_CR, "tty0", TRUE); tty0 = MFS_resolve("/sys/cli/tty0"); if (tty0 == NULL) { ACE_PANIC ("Open tty0 fail"); } CLI_tty_init (&tty_1, ser0, CLI_TTY_INTRANSL_CR_2_NL, CLI_TTY_OUTTRANSL_ADD_CR, "tty1", FALSE); CLI_switch_init(); CLI_switch_set(0,tty0); CLI_tty_select(tty0); ser1 = MFS_resolve("/sys/dev/serial/ser1"); if (ser1 == NULL) { ACE_PANIC ("Open ser1 fail"); } /* The idle thread is not allowed to block * so it is not allowed to do any printf or log output!!!! */ USO_log_init (tty0, USO_LL_INFO); blink_green (1); /* Interrupts must be enabled before starting the first thread! */ USO_enable (); blink_green (1); CLI_config_init (); SAM_config_install (); if (SAM_spi_init () != 0) { ACE_PANIC ("Spi init fail"); } else { if (SAM_mmc_install () == ACE_OK) { CLI_config_read (); } else { blink_red(2); } } /* ttyS0 is stdio for start thread, * all other threads started(derived) from start thread * will also use ttyS0 as stdio as long they don't change it. */ SAM_start (tty0); /* idle */ }