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 */
}
