int acc_init() {
// Use SPI0, mode0 with lsb shifted as requested PLACE AT TOP
volatile uint32_t counter = 0;
spi_ba = spi_master_init(SPI0, SPI_MODE3, false);
if (spi_ba == 0) {
return -1;
}
//Put the ADXL345 into StandBy Mode by writing 0x00
// to the POWER_CTL register.
write_register(ADXL345_POWER_CTL, 0x00); //Standby mode
//Put the ADXL345 into +/- 4G range by writing the value 0x01
// to the DATA_FORMAT register.
write_register(ADXL345_DATA_FORMAT, 0x00);
// set up sampling rate of 50Hz
write_register(ADXL345_BW_RATE, 0x09);
fifo_init();
int_init();
//Put the ADXL345 into Measurement Mode by writing 0x08
// to the POWER_CTL register.
write_register(ADXL345_POWER_CTL, 0x08); //Measurement mode
return 0;
}
struct intercept_s *di_init( struct server *serp )
{
struct intercept_s *ip = &dgram_intercept_state ;
ip->int_socket_type = SOCK_DGRAM ;
ip->int_priv = (void *) &idgram ;
ip->int_ops = &idgram_ops ;
int_init( ip, serp ) ;
return( ip ) ;
}
s32 os_main(u32 sp)
{
struct __os_task__ *ptask;
int_init();
uart_init();
dram_init();
timer_init();
mmc_init();
PRINT_INFO("%s\n", sys_banner);
coretimer_init();
task_init();
semaphore_init();
PRINT_INFO("cpu_mode: %s; lr: 0x%x; sp: 0x%x; cpsr: 0x%x\n",
get_cpu_mode(NULL), __get_lr(), sp, __get_cpsr());
gpio_set_function(GPIO_16, OUTPUT);
gpio_set_output(GPIO_16, 0);
/* set_log_level(LOG_DEBUG); */
/* create idle task */
if ((ptask = tcb_alloc()) == NULL) {
panic();
}
tcb_init(ptask, idle_task, 0, 256);
/*os_ready_insert(ptask);*/
current_task = &tcb[IDLE_TASK_ID]; /* assume that this is idle_task */
/* create main task */
if ((ptask = tcb_alloc()) == NULL) {
panic();
}
tcb_init(ptask, main_task, 0, 100);
os_ready_insert(ptask);
/* 'slip into idle task', cause the os_main() is not a task (it's the god code of system) */
__set_sp(&(task_stack[0][TASK_STK_SIZE]));
current_task->state = TASK_RUNNING;
idle_task(0);
kassert(0);
return 0;
}
/* Called by reset exception handler to initialize the kernel and start
rolling first task. */
void
kernel_init()
{
tid_t t;
int i;
printf("Initializing kernel:\n");
printf(" Clearing kernel structures...\n");
memset(tasks, 0, sizeof(tasks));
memset(stacks, 0, sizeof(stacks));
memset(msgs, 0, sizeof(msgs));
printf(" Initializing MCBs... %d MCB(s)\n", MAX_MSGS);
for(i = 0; i < (MAX_MSGS - 1); i++)
msgs[i].next = &msgs[i+1];
free_mcbs = &msgs[0];
printf(" Initializing TCBs... %d user task(s)\n", MAX_TASKS);
tasks_entries();
for(t = 0; t <= MAX_TASKS; t++) {
tasks[t].regs.sp = (unsigned long)stacks[t] + STACK_SIZE - 4;
/* Disable EXR for kernel context */
tasks[t].regs.sr |= (t == 0 ? SPR_SR_SM : SPR_SR_TEE | SPR_SR_IEE);
tasks[t].regs.gprs[1] = t;
}
/* First task runs in seprvisor mode */
tasks[1].regs.sr |= SPR_SR_SM;
/* TID=0 is reserved for kernel use */
kernel_context = (unsigned long *)&tasks[0].regs;
/* First task to be scheduled is task TID=1 */
task_context = (unsigned long *)&tasks[1].regs;
/* Initialize initrrupt controller */
int_init();
printf(" Exceptions will be enabled when first task is dispatched.\n");
printf("Kernel initalized. Starting first user task.\n");
#if KERNEL_SYSCALL_SCHED
kernel_sched(); /* Lets schedule and dispatch our first task */
#else
tick_init(TICK_PERIOD, kernel_sched);
kernel_sched(); /* Lets schedule and dispatch our first task */
#endif
/* ... */ /* We never get here */
}
int main()
{
uart_init(JSP_BASE);
int_init();
int_add(JSP_IRQ, &uart_interrupt, NULL);
/* We can't use printf because in this simple example
we don't link C library. */
uart_print_str("Hello World.\n");
report(0xdeaddead);
or32_exit(0);
}
int main()
{
int uart0_core = 0;
int uart1_core = 1;
uart0_tx_ctrl.busy = 0;
/* Set up interrupt handler */
int_init();
/* Install UART core 0 interrupt handler */
int_add(UART0_IRQ, uart_int_handler,(void*) &uart0_core);
/* Install UART core 1 interrupt handler */
//int_add(UART1_IRQ, uart_int_handler,(void*) &uart1_core);
/* Enable interrupts in supervisor register */
mtspr (SPR_SR, mfspr (SPR_SR) | SPR_SR_IEE);
uart_init(uart0_core);
//uart_init(uart1_core);
//uart_rxint_enable(uart1_core);
uart_rxint_enable(uart0_core);
char* teststring = "\n\tHello world from UART 0\n\0";
uart0_tx_buffer(teststring);
// Do other things while we transmit
float f1, f2, f3; int i;
f1 = 0.2382; f2 = 4342.65; f3=0;
for(i=0;i<32;i++) f3 += f1*f3 + f2;
report(f3);
report(0x4aaaaa1f);
char* done_calculating = "\tDone with the number crunching!\n\0";
uart0_tx_buffer(done_calculating);
// Character '*', which will be received in the interrupt handler and cause
// the simulation to exit.
char* finish = "*\n\0";
uart0_tx_buffer(finish);
while(1); // will exit in the rx interrupt routine
}
int main(unsigned argl, const char far* args)
{
STACK_DECL char arg[128];
uint16 cmd;
int_init();
tsr_loaded = tsr_is_installed();
/* Duplicate the command line locally */
memcpy(arg, args, argl);
arg[argl] = 0;
/* Call the main TSR function */
if (tsr_loaded)
cmd = tsr(mem_remote, arg);
else
cmd = tsr(0, arg);
if (cmd == TSR_LOAD) {
if (tsr_loaded)
return EXIT_FAILURE;
if (!load())
return EXIT_FAILURE;
/* device driver hasn't the environment */
if (!tsr_sys)
freeenv();
return EXIT_TSR;
}
if (cmd == TSR_UNLOAD) {
if (!tsr_loaded)
return EXIT_FAILURE;
if (!mem_unload()) {
cputs("Can't unload\n\r");
return EXIT_FAILURE;
}
if (freemem(tsr_loaded->psp) != 0) {
cputs("Can't release the memory\n\r");
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}
if (cmd == TSR_SUCCESS)
return EXIT_SUCCESS;
else
return EXIT_FAILURE;
}
void niuniu(void)
{
int_init();
timer_init();
beeper_init();
uart_init();
timer_update();
while(1)
{
puts("haha ......\r\n");
sleep(1);
}
}
void inmate_main(void)
{
printk_uart_base = UART_BASE;
int_init();
int_set_handler(IRQ_VECTOR, irq_handler);
ioapic_init();
ioapic_pin_set_vector(ACPI_GSI, TRIGGER_LEVEL_ACTIVE_HIGH, IRQ_VECTOR);
printk("Press power button to trigger an IRQ\n"
"Note: ACPI IRQs are broken for Linux now.\n");
asm volatile("sti");
while (1)
asm volatile("hlt");
}
/*
* enables an interrupt on a pin. needs edge trigger and callback function specified
*/
void gpio_enable_interrupt(port_t p_port, pin_t p_pin, trig_t p_trig, callback_t p_callback)
{
unsigned int * custom_PCR = (unsigned int *)(PORTX_PCR_BASE + p_port * 0x1000 + p_pin * 0x4);
if(p_port == port_C)
{
gpio_c_callback[p_pin] = p_callback;
}
else if(p_port == port_D)
{
gpio_d_callback[p_pin] = p_callback;
}
//enable pin interrupt with trigger
*custom_PCR &= ~PORT_PCR_IRQC_MASK;
*custom_PCR |= PORT_PCR_IRQC(p_trig);
//config priority
int_init(INT_PORTC_PORTD, priority_1);
}
int
main ()
{
int i;
/* Initialise handler vector */
int_init();
/* Install ethernet interrupt handler, it is enabled here too */
int_add(ETH0_IRQ, oeth_interrupt, 0);
/* Enable interrupts in supervisor register */
cpu_enable_user_interrupts();
ethmac_setup(); /* Configure MAC, TX/RX BDs and enable RX and TX in MODER */
/* clear tx_done, the tx interrupt handler will set it when it's been transmitted */
tx_done = 0;
rx_done = 0;
ethphy_set_100mbit(0);
#ifndef ETH_TX_TEST_LENGTH
# define ETH_TX_START_LENGTH 40
# define ETH_TX_TEST_LENGTH 1024
# define ETH_TX_TEST_LENGTH_INCREMENT 21
//# define ETH_TX_TEST_LENGTH OETH_TX_BUFF_SIZE
#endif
for(i=ETH_TX_START_LENGTH;i<ETH_TX_TEST_LENGTH;
i+=ETH_TX_TEST_LENGTH_INCREMENT)
fill_and_tx_packet(i);
ethphy_set_10mbit(0);
for(i=ETH_TX_START_LENGTH;i<ETH_TX_TEST_LENGTH;
i+=ETH_TX_TEST_LENGTH_INCREMENT)
fill_and_tx_packet(i);
exit(0x8000000d);
}
void inmate_main(void)
{
printk_uart_base = UART_BASE;
int_init();
int_set_handler(IRQ_VECTOR, irq_handler);
ioapic_init();
ioapic_pin_set_vector(ACPI_GSI, TRIGGER_LEVEL_ACTIVE_HIGH, IRQ_VECTOR);
pm_base = comm_region->pm_timer_address - 8;
outw(inw(pm_base + PM1_ENABLE) | PM1_TMR_EN, pm_base + PM1_ENABLE);
printk("Note: ACPI IRQs are broken for Linux now.\n");
asm volatile("sti");
while (1)
asm volatile("hlt");
}
int
main ()
{
/* Initialise handler vector */
int_init();
/* Install ethernet interrupt handler, it is enabled here too */
int_add(ETH0_IRQ, oeth_interrupt, 0);
ethmac_setup(ETH0_PHY, ETH0_BUF); /* Configure MAC, TX/RX BDs and enable RX and TX in MODER */
wprint("Entering loop\n");
while (1) { }
ethmac_halt();
exit(0x8000000d);
return 0;
}
static void gpio_set_up_pushbutton_int(int core)
{
int_init();
gpio_clear_ints(core, PUSHBUTTON_LINE_NUM);
// Install interrupt handler
int_add(GPIO0_IRQ, gpio_pushbutton_int, 0);
// Enable this interrupt in PIC
int_enable(GPIO0_IRQ);
gpio_int_enable_line(core, PUSHBUTTON_LINE_NUM, 0); // Falling edge triggered line
gpio_enable_ints(core);
// Enable interrupts
cpu_enable_user_interrupts();
}
int main ()
{
/* Initialise handler vector */
int_init();
/* Install ethernet interrupt handler, it is enabled here too */
int_add(ETH0_IRQ, oeth_interrupt, 0);
ethmac_setup(ETH0_PHY, ETH0_BUF); /* Configure MAC, TX/RX BDs and enable RX and TX in MODER */
/* clear tx_done, the tx interrupt handler will set it when it's been transmitted */
while (1) {
char buf[120];
memcpy(buf, "Hello world!\n", 12);
tx_packet(buf, sizeof(buf));
}
exit(0x8000000d);
}
int main() {
// Configure ISRs
int_init();
int_add(29, (void *) int_time_cmp, 0);
int_enable();
EER = 0xF0000000; // enable all timer events;
IER = 0xF0000000; // enable all timer interrupts
/* Setup Timer A */
TOCRA = 0x80;
TPRA = 0x3F; // set prescaler, enable interrupts and start timer.
while (timer_triggered < 5) {
printf("Loop Counter: %d\n", timer_triggered);
sleep();
}
set_gpio_pin_value(0, 0);
int_disable();
print_summary(0);
return 0;
}
int main(int argc, char **argv)
{
char src1[4096] = "Hello world.";
char dst1[4096] = "1111111111111111111111111111111111";
char src2[4096] = "The whole world spread before you.";
char dst2[4096] = "2222222222222222222222222222222222";
// Attach the external interrupt handler for 'intr0'
int_init();
int_add(0, (void *)interruptHandler, NULL);
int_enable(0);
// Enable external interrupts
Uns32 spr = MFSPR(17);
spr |= 0x4;
MTSPR(17, spr);
writeReg8(DMA_BASE, DMA_CONFIGURATION, BURST_SIZE); /* reset */
LOG("initial dst1 '%s' dst2 '%s'\n", dst1, dst2);
/* write to DMAC registers to start burst */
dmaBurst(0, src1, dst1, strlen(src1)+1);
dmaBurst(1, src2, dst2, strlen(src2)+1);
/* wait for burst to complete */
LOG("Waiting for interrupts\n");
while ( interruptCount < 2 )
;
LOG("%u interrupts received\n", interruptCount);
/* check results */
LOG("DMA result dst1 '%s' dst2 '%s'\n", dst1, dst2);
return 1;
}
static int set_name(void *data, Char *pathname)
{
EDITINFO *einf = (EDITINFO *) data;
XTextProperty prop_name;
Char *name;
int namesize;
Char *stripname, *nname;
if (pathname == NULL) {
EDITINFO *tinf;
FlexArray istck;
int i=0,j;
int_init(istck);
while (aig(tinf=(EDITINFO*)next_data_with_type(MAINEDITWINDOW, &i))) {
if (!Ustrncmp(translate(EMPTYFILE),tinf->filename, Ustrlen(translate(EMPTYFILE)))) {
j = Ustrtol(tinf->filename+Ustrlen(translate(EMPTYFILE)), NULL, 10);
int_add(istck, j);
}
i++;
}
i=1;
while (int_contains(istck,i)) i++;
int_clear(istck);
nname = (Char *) malloc(sizeof(Char)*(Ustrlen(userdir) +
Ustrlen(translate("/" EMPTYFILE))
+ 5 +Ustrlen(translate(EXTENSION))));
concat_in(nname, userdir, translate("/" EMPTYFILE));
stripname = nname + Ustrlen(nname);
{ Char sb[40];
Char *s;
sb[39]=0;
s=Ultostr(i,sb+39);
Ustrcat(stripname,s);
}
Ustrcat(stripname, translate(EXTENSION));
} else
nname = pathname;
stripname = concat(strip_name(nname),NULL);
if (!Ustrcmp(stripname+Ustrlen(stripname)-Ustrlen(translate(EXTENSION)),
translate(EXTENSION)))
stripname[Ustrlen(stripname)-Ustrlen(translate(EXTENSION))] = 0;
namesize = Ustrlen(translate(EDITNAME)) + Ustrlen(stripname) + 1 +
(einf->saved ? 0 : Ustrlen(translate(CHANGED))) +
(einf->view_mode && !einf->shell ? Ustrlen(translate(VIEWCOM)) : 0) +
(einf->shell && !einf->fini ? Ustrlen(translate(RUNCOM)) : 0) +
(einf->shell && einf->fini ? Ustrlen(translate(DONECOM)) : 0);
name = (Char *) malloc((size_t) namesize*sizeof(Char) );
if (name) {
name[0]= '\0';
Ustrcat(name, translate(EDITNAME));
Ustrcat(name, stripname);
if (!einf->saved && !einf->shell) Ustrcat(name, translate(CHANGED));
if (einf->view_mode && !einf->shell) Ustrcat(name, translate(VIEWCOM));
if (einf->shell && !einf->fini) Ustrcat(name, translate(RUNCOM));
if (einf->shell && einf->fini) Ustrcat(name, translate(DONECOM));
}
{
char *n;
n= (char*)UstrtoLocale(name);
if (!name || !XStringListToTextProperty(&n, 1, &prop_name)) {
message(MP_ERROR, translate("No location for editname."));
return 0;
}
}
XSetWMName(display, einf->win_id, &prop_name);
free(einf->headername);
if (einf->pathname!=nname) free(einf->pathname);
free(einf->filename);
einf->headername = name;
einf->filename = stripname;
einf->pathname = nname;
{
char *icn;
icn = (char*)UstrtoLocale(stripname);
if (!XStringListToTextProperty(&icn, 1, &prop_name)) {
message(MP_ERROR, translate("No location for editicon."));
return 0;
}
}
XSetWMIconName(display, einf->win_id, &prop_name);
return 1;
}
basic_variant(signed char v) {
int_init(v);
}
int main(void)
{
unsigned int XXL,YXL,ZXL;
//unsigned int ADC1,ADC2,ADC3;
// int16_t ZGY;
unsigned int XGY,YGY,ZGY;
// unsigned char XYZ_buffer[100];
unsigned int timer=0;
unsigned int timer_old=0;
unsigned int diff_counter=0;
unsigned int m_IMU_count=0;
double m_XXL_sum=0;
double m_YXL_sum=0;
double m_ZXL_sum=0;
double m_XGY_sum=0;
double m_YGY_sum=0;
double m_ZGY_sum=0;
clk_init();
port_init();
USART_INIT();
timer_Init();
int_init();
// sei();
// spi_init();
/* Init SS pin as output with wired AND and pull-up. */
//120813 portC를 D로 바꿈 SPI인거 같아서
PORTD.DIRSET = PIN0_bm;
PORTD.PIN0CTRL = PORT_OPC_WIREDANDPULL_gc;
PORTD.DIRSET = PIN1_bm;
PORTD.PIN1CTRL = PORT_OPC_WIREDANDPULL_gc;
/* Set SS output to high. (No slave addressed). */
PORTD.OUTSET = PIN0_bm;
PORTD.OUTSET = PIN1_bm;
/* Instantiate pointer to ssPort. */
PORT_t *ssPort = &PORTD;
/* Initialize SPI master on port D. */
SPI_MasterInit(&spiMasterD,
&SPID,
&PORTD,
false,
SPI_MODE_3_gc,
SPI_INTLVL_OFF_gc,
false,
SPI_PRESCALER_DIV16_gc);
/* Use USARTD0 and initialize buffers. */
USART_InterruptDriver_Initialize(&USARTD0_data, &USARTD0, USART_DREINTLVL_HI_gc);
/* Enable RXC interrupt. */
USART_RxdInterruptLevel_Set(USARTD0_data.usart, USART_RXCINTLVL_HI_gc);
/* Enable PMIC interrupt level low. */
PMIC.CTRL |= PMIC_HILVLEX_bm;
/* Enable global interrupts.*/
sei();
/* Initialize ACC & Gyro */
// Init_L3G4200DH();
Init_LIS3DH();
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro_init,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
SPI_MasterSSHigh(ssPort, PIN1_bm);
// USART_Rx_Enable(&USARTD0);
// USART_Tx_Enable(&USARTD0);
double z_deg=0;
unsigned int oldT=0;
unsigned int newT=0;
unsigned int dT=0;
int i;
double x1m=0;
double x1p=0;
double P1p=1.0;
double x2m[4]={0,0,0,0};
double x2p[4]={0,0,0,0};
double P2p[4][4]={{0.1,0,0,0},
{0,0.1,0,0},
{0,0,0.1,0},
{0,0,0,0.1}};
double enc_time_cnt=0;
for(i=0;i<DELAY_COMP_G;i++)
{
m_XGY_buf[i]=0.0;
m_YGY_buf[i]=0.0;
m_ZGY_buf[i]=0.0;
m_XXL_buf[i]=0.0;
m_YXL_buf[i]=0.0;
m_ZXL_buf[i]=0.0;
m_enc_buf[i]=0.0;
m_enc_timebuf[i]=0.0;
m_T_buf[i]=0.0;
}
//char XYZ_buffer_debug[20];
//sprintf((char*)XYZ_buffer_debug,"Hello W");
//uartSendTX((unsigned char*)XYZ_buffer_debug);
while(1)
{
//if(1)
if(samplingFlag)
{
// adc_start_conversion(&ADCA, ADC_CH0);
samplingFlag=0;
timer=TCC0.CNT;
diff_counter=timer-timer_old;
/* Create data packet (SS to slave by PC0). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData,
ACC_DATA,
NUM_BYTES,
&PORTD,
PIN0_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN0_bm);
_delay_us(5);
/* Transceive packet. */
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN0_bm);
/* Create data packet (SS to slave by PC1). */
SPI_MasterCreateDataPacket(&dataPacket,
masterSendData_gyro,
GYRO_DATA,
NUM_BYTES,
&PORTD,
PIN1_bm);
/* MASTER: Pull SS line low. This has to be done since
* SPI_MasterTransceiveByte() does not control the SS line(s). */
SPI_MasterSSLow(ssPort, PIN1_bm);
/* Transceive packet. */
_delay_us(5);
SPI_MasterTransceivePacket(&spiMasterD, &dataPacket);
/* MASTER: Release SS to slave. */
_delay_us(5);
SPI_MasterSSHigh(ssPort, PIN1_bm);
timer_old=timer;
T=(double)diff_counter/2000000.0*32.0;
ACC_DATA[2]=ACC_DATA[2]+0x80;
ACC_DATA[4]=ACC_DATA[4]+0x80;
ACC_DATA[6]=ACC_DATA[6]+0x80;
YXL= (unsigned int)ACC_DATA[2]*256+ACC_DATA[1];
XXL= (unsigned int)ACC_DATA[4]*256+ACC_DATA[3];
ZXL= (unsigned int)ACC_DATA[6]*256+ACC_DATA[5];
GYRO_DATA[2]=GYRO_DATA[2]+0x80;
GYRO_DATA[4]=GYRO_DATA[4]+0x80;
GYRO_DATA[6]=GYRO_DATA[6]+0x80;
XGY= (unsigned int)GYRO_DATA[4]*256+GYRO_DATA[3];
YGY= (unsigned int)GYRO_DATA[2]*256+GYRO_DATA[1];
ZGY= (unsigned int)GYRO_DATA[6]*256+GYRO_DATA[5];
if(m_IMU_count<Tsample)
{
m_IMU_count++;
}
else if(m_IMU_count<Tsample+Bsample)
{
m_XXL_sum+=XXL;
m_YXL_sum+=YXL;
m_ZXL_sum+=ZXL;
//m_XGY_sum+=XGY;
//m_YGY_sum+=YGY;
m_ZGY_sum+=ZGY;
m_IMU_count++;
}
else if(m_IMU_count==Tsample+Bsample)
{
//SetTimer(25,1,NULL);
m_biasXXL=(double)m_XXL_sum/(double)Bsample;
m_biasYXL=(double)m_YXL_sum/(double)Bsample;
m_biasZXL=(double)m_ZXL_sum/(double)Bsample-GRAVITY_COUNT;
//m_biasXGY=(double)m_XGY_sum/(double)Bsample;
//m_biasYGY=(double)m_YGY_sum/(double)Bsample;
m_biasZGY=(double)m_ZGY_sum/(double)Bsample;
gravityVect[0]=0;
gravityVect[1]=0;
gravityVect[2]=SCALE_ZXL*GRAVITY_COUNT;
m_IMU_count++;
}
else
{
//encoder_interface(0);
//encoder_interface(1);
//host_interface();
//unsigned int a=TCC0.CNT;
//position_estimator(XXL,YXL,ZXL,XGY,YGY,ZGY);
//unsigned int b=TCC0.CNT;
//TJX=(double)(b-a)/2000000.0*32.0;
//FF_controller();
newT=TCC0.CNT;
dT=newT-oldT;
////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
int i,j,k;
m_XXL=-SCALE_XXL*((double)XXL-(double)m_biasXXL);
m_YXL=-SCALE_YXL*((double)YXL-(double)m_biasYXL);
m_ZXL=SCALE_ZXL*((double)ZXL-(double)m_biasZXL);
m_XGY=-SCALE_XGY*((double)XGY-(double)m_biasXGY);//-0.001212142/0.00015711
m_YGY=SCALE_YGY*((double)YGY-(double)m_biasYGY);//+
//if(ZGY<3000) m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY+65536.0);
//else m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);//+
m_ZGY=SCALE_ZGY*((double)ZGY-(double)m_biasZGY);
sprintf(XYZ_buffer,"%u %u %u %u %u %u \n",XXL, YXL, ZXL, XGY, YGY, ZGY);
uartC0SendTX((unsigned char*)XYZ_buffer);
}
}
}
while(true) {
nop();
}
}
void pinguino_main(void)
{
#if defined(PIC18F26J50)
// Enable the PLL and wait 2+ms until the PLL locks
u16 pll_startup_counter = 600;
OSCTUNEbits.PLLEN = 1;
while(pll_startup_counter--);
#endif
PIE1 = 0;
PIE2 = 0;
IOsetSpecial();
IOsetDigital();
IOsetRemap();
#ifdef ON_EVENT // Enable General/Peripheral interrupts
int_init(); // Disable all individual interrupts
#endif
#ifdef __USB__
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
//setup();
//#ifdef ON_EVENT
//int_start(); // Enable all defined timers interrupts
//#endif
#ifdef ANALOG
analog_init();
#endif
#ifdef __MILLIS__ // Use Timer 0
millis_init();
#endif
#ifdef SERVOSLIBRARY
servos_init();
#endif
#ifdef __USBCDC
CDC_init();
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
#ifdef __USBBULK
bulk_init();
PIE2bits.USBIE = 1;
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
#ifdef __PS2KEYB__
keyboard_init()
#endif
#if defined(__SERIAL__) || defined(SERVOSLIBRARY)
INTCONbits.PEIE = 1;
INTCONbits.GIE = 1;
#endif
/* RB : millis.c/millis_init() did already the job
#ifdef MILLIS
INTCONbits.TMR0IE= 1;
INTCONbits.GIE = 1;
#endif
*/
setup();
#ifdef ON_EVENT
int_start(); // Enable all defined timers interrupts
#endif
while (1)
loop();
}
basic_variant(signed int v) {
int_init(v);
}
int main(int argc, char **argv) {
int sockfd, lsockfd;
char *output;
pid_t pid, sid;
struct sockaddr_in local, remote;
socklen_t socksize = sizeof (struct sockaddr_in);
pid = fork();
if (pid < 0)
ERR_EXIT_ERRNO("fork error");
if (pid > 0)
exit(0);
sid = setsid();
if (sid < 0)
ERR_EXIT_ERRNO("setsid error");
//close(2), close(1), close(0);
memset(&local, 0, sizeof (local));
local.sin_family = AF_INET;
local.sin_addr.s_addr = htonl(INADDR_ANY);
local.sin_port = htons(6969);
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (!sockfd)
ERR_EXIT_ERRNO("socket error");
prog = argv[0];
ex_init();
int_init();
lpar_init();
bind(sockfd, (struct sockaddr *) &local, sizeof (struct sockaddr));
if (!sockfd)
ERR_EXIT_ERRNO("bind error");
listen(sockfd, 1);
lsockfd = accept(sockfd, (struct sockaddr *) &remote, &socksize);
if (!lsockfd)
ERR_EXIT_ERRNO("accept error");
while (lsockfd) {
lpar_update();
//output_raw(&output);
output_zperf_compat(&output);
write(lsockfd, output, strlen(output));
close(lsockfd);
lsockfd = accept(sockfd, (struct sockaddr *) &remote, &socksize);
if (!lsockfd)
ERR_EXIT_ERRNO("accept error");
}
close(lsockfd);
close(sockfd);
return 0;
}
basic_variant(signed long long v) {
int_init(v);
}
int window_init(window_t* window) { LOG
int error;
int i;
if (window->draw) {
/* initialize video */
if (!vg_init(window->video_mode)) {
printf("window_init: vg_init failed.\n");
return -1;
}
}
window->redraw = 0;
window->done = 0;
/* init font system */
error = vg_init_FreeType();
if (error) {
printf("window_init: vg_init_FreeType failed with error code %d.\n", error);
return error;
}
error = window_set_title(window, "cnix %s", "0.2");
if (error) {
printf("window_init: window_set_title failed with error code %d.\n", error);
return error;
}
error = window_set_log_message(window, "");
if (error) {
printf("window_init: window_set_log_message failed with error code %d.\n", error);
return error;
}
error = window_set_size(window, window->draw ? vg_get_h_res() : 1024, window->draw ? vg_get_v_res() : 768);
if (error) {
printf("window_init: window_set_size failed with error code %d.\n", error);
return error;
}
/* set up tabs */
window->current_tab = -1;
for (i = 0; i < TAB_COUNT; ++i)
window->tabs[i] = NULL;
//window->tabs[0] = tab_create("#1");
//window->tabs[1] = tab_create("#2");
//window->tabs[2] = tab_create("#3");
//window->tabs[3] = tab_create("#4");
//window->tabs[4] = tab_create("#5");
//window->tabs[5] = tab_create("#6");
//window->tabs[6] = tab_create("#7");
//window->tabs[7] = tab_create("#8");
//window->tabs[8] = tab_create("#9");
//window->tabs[9] = tab_create("#10");
//window->tabs[10] = tab_create("#11");
window->tabs[11] = tab_create("#console");
window->current_tab = TAB_CONSOLE;
window->prev_current_tab = window->current_tab;
window->date = NULL;
/* initialize interrupt handlers */
int_init();
/* install mouse */
error = window_install_mouse(window);
if (error) {
printf("window_init: window_install_mouse failed with error code %d.\n", error);
return error;
}
printf("window_install: mouse installed with success.\n");
/* install keyboard */
error = keyboard_install();
if (error) {
printf("window_init: keyboard_install failed with error code %d.\n", error);
return error;
}
printf("window_init: keyboard installed with success.\n");
/* set up buttons */
new_btn = new_button(869, 5, 20, 20, new_btn_draw, new_btn_click, 1, window);
open_btn = new_button(894, 5, 20, 20, open_btn_draw, open_btn_click, 1, window);
save_btn = new_button(919, 5, 20, 20, save_btn_draw, save_btn_click, 1, window);
make_btn = new_button(944, 5, 20, 20, make_btn_draw, make_btn_click, 1, window);
run_btn = new_button(969, 5, 20, 20, run_btn_draw, run_btn_click, 1, window);
close_btn = new_button(994, 5, 20, 20, close_btn_draw, close_btn_click, 1, window);
window->state = WIN_STATE_NORMAL;
return 0;
}
extern "C" int
_start(kernel_args *bootKernelArgs, int currentCPU)
{
if (bootKernelArgs->kernel_args_size != sizeof(kernel_args)
|| bootKernelArgs->version != CURRENT_KERNEL_ARGS_VERSION) {
// This is something we cannot handle right now - release kernels
// should always be able to handle the kernel_args of earlier
// released kernels.
debug_early_boot_message("Version mismatch between boot loader and "
"kernel!\n");
return -1;
}
smp_set_num_cpus(bootKernelArgs->num_cpus);
// wait for all the cpus to get here
smp_cpu_rendezvous(&sCpuRendezvous, currentCPU);
// the passed in kernel args are in a non-allocated range of memory
if (currentCPU == 0)
memcpy(&sKernelArgs, bootKernelArgs, sizeof(kernel_args));
smp_cpu_rendezvous(&sCpuRendezvous2, currentCPU);
// do any pre-booting cpu config
cpu_preboot_init_percpu(&sKernelArgs, currentCPU);
thread_preboot_init_percpu(&sKernelArgs, currentCPU);
// if we're not a boot cpu, spin here until someone wakes us up
if (smp_trap_non_boot_cpus(currentCPU, &sCpuRendezvous3)) {
// init platform
arch_platform_init(&sKernelArgs);
// setup debug output
debug_init(&sKernelArgs);
set_dprintf_enabled(true);
dprintf("Welcome to kernel debugger output!\n");
dprintf("Haiku revision: %lu\n", get_haiku_revision());
// init modules
TRACE("init CPU\n");
cpu_init(&sKernelArgs);
cpu_init_percpu(&sKernelArgs, currentCPU);
TRACE("init interrupts\n");
int_init(&sKernelArgs);
TRACE("init VM\n");
vm_init(&sKernelArgs);
// Before vm_init_post_sem() is called, we have to make sure that
// the boot loader allocated region is not used anymore
boot_item_init();
debug_init_post_vm(&sKernelArgs);
low_resource_manager_init();
// now we can use the heap and create areas
arch_platform_init_post_vm(&sKernelArgs);
lock_debug_init();
TRACE("init driver_settings\n");
driver_settings_init(&sKernelArgs);
debug_init_post_settings(&sKernelArgs);
TRACE("init notification services\n");
notifications_init();
TRACE("init teams\n");
team_init(&sKernelArgs);
TRACE("init ELF loader\n");
elf_init(&sKernelArgs);
TRACE("init modules\n");
module_init(&sKernelArgs);
TRACE("init semaphores\n");
haiku_sem_init(&sKernelArgs);
TRACE("init interrupts post vm\n");
int_init_post_vm(&sKernelArgs);
cpu_init_post_vm(&sKernelArgs);
commpage_init();
TRACE("init system info\n");
system_info_init(&sKernelArgs);
TRACE("init SMP\n");
smp_init(&sKernelArgs);
TRACE("init timer\n");
timer_init(&sKernelArgs);
TRACE("init real time clock\n");
rtc_init(&sKernelArgs);
TRACE("init condition variables\n");
condition_variable_init();
// now we can create and use semaphores
TRACE("init VM semaphores\n");
vm_init_post_sem(&sKernelArgs);
TRACE("init generic syscall\n");
generic_syscall_init();
smp_init_post_generic_syscalls();
TRACE("init scheduler\n");
scheduler_init();
TRACE("init threads\n");
thread_init(&sKernelArgs);
TRACE("init kernel daemons\n");
kernel_daemon_init();
arch_platform_init_post_thread(&sKernelArgs);
TRACE("init I/O interrupts\n");
int_init_io(&sKernelArgs);
TRACE("init VM threads\n");
vm_init_post_thread(&sKernelArgs);
low_resource_manager_init_post_thread();
TRACE("init VFS\n");
vfs_init(&sKernelArgs);
#if ENABLE_SWAP_SUPPORT
TRACE("init swap support\n");
swap_init();
#endif
TRACE("init POSIX semaphores\n");
realtime_sem_init();
xsi_sem_init();
xsi_msg_init();
// Start a thread to finish initializing the rest of the system. Note,
// it won't be scheduled before calling scheduler_start() (on any CPU).
TRACE("spawning main2 thread\n");
thread_id thread = spawn_kernel_thread(&main2, "main2",
B_NORMAL_PRIORITY, NULL);
resume_thread(thread);
// We're ready to start the scheduler and enable interrupts on all CPUs.
scheduler_enable_scheduling();
// bring up the AP cpus in a lock step fashion
TRACE("waking up AP cpus\n");
sCpuRendezvous = sCpuRendezvous2 = 0;
smp_wake_up_non_boot_cpus();
smp_cpu_rendezvous(&sCpuRendezvous, 0); // wait until they're booted
// exit the kernel startup phase (mutexes, etc work from now on out)
TRACE("exiting kernel startup\n");
gKernelStartup = false;
smp_cpu_rendezvous(&sCpuRendezvous2, 0);
// release the AP cpus to go enter the scheduler
TRACE("starting scheduler on cpu 0 and enabling interrupts\n");
scheduler_start();
enable_interrupts();
} else {
// lets make sure we're in sync with the main cpu
// the boot processor has probably been sending us
// tlb sync messages all along the way, but we've
// been ignoring them
arch_cpu_global_TLB_invalidate();
// this is run for each non boot processor after they've been set loose
cpu_init_percpu(&sKernelArgs, currentCPU);
smp_per_cpu_init(&sKernelArgs, currentCPU);
// wait for all other AP cpus to get to this point
smp_cpu_rendezvous(&sCpuRendezvous, currentCPU);
smp_cpu_rendezvous(&sCpuRendezvous2, currentCPU);
// welcome to the machine
scheduler_start();
enable_interrupts();
}
#ifdef TRACE_BOOT
// We disable interrupts for this dprintf(), since otherwise dprintf()
// would acquires a mutex, which is something we must not do in an idle
// thread, or otherwise the scheduler would be seriously unhappy.
disable_interrupts();
TRACE("main: done... begin idle loop on cpu %d\n", currentCPU);
enable_interrupts();
#endif
for (;;)
arch_cpu_idle();
return 0;
}
void init_int(void) {
int_init(&int1, (uint16_t *)&IFS1, (uint16_t *)&IEC1, (WORD*)&RPINR0, 1, 4, 1);
int_init(&int2, (uint16_t *)&IFS1, (uint16_t *)&IEC1, (WORD*)&RPINR1, 0, 13, 2);
int_init(&int3, (uint16_t *)&IFS3, (uint16_t *)&IEC3, (WORD*)&RPINR1, 1, 5, 3);
int_init(&int4, (uint16_t *)&IFS3, (uint16_t *)&IEC3, (WORD*)&RPINR2, 0, 6, 4);
}
/*
ok, this is pman init stage two. we will execute this code, and then jump to the process
manager main processing loop.
What we will do here, is setup the page pool. And initialize System services, along with structures.
Notice, we are now task 0 on the system.
*/
void pman_init_stage2()
{
UINT32 linear, physical;
struct pm_thread *pmthr = NULL;
struct pm_task *pmtsk = NULL;
int i = 0;
int init_size = 0;
/* get rid of the init stuff */
destroy_thread(INIT_THREAD_NUM);
destroy_task(INIT_TASK_NUM);
/*
Open used ports
*/
for(i = 0; i <= 12; i++)
{
open_port(i, 3, PRIV_LEVEL_ONLY);
}
/*
Init stage 1 has placed bootinfo at PMAN_MULTIBOOTINFO_PHYS
before initializing the pool we need to know memory size
and that information is there. So lets map it on our page table.
*/
linear = PMAN_MULTIBOOT_LINEAR + SARTORIS_PROCBASE_LINEAR;
physical = PMAN_MULTIBOOT_PHYS;
map_pages(PMAN_TASK, linear, physical, PMAN_MULTIBOOT_PAGES, PGATT_WRITE_ENA, 2);
/* Reallocate init image */
init_size = init_reloc();
pman_print_set_color(0x7);
pman_print("Mapping Malloc %i pages", PMAN_MALLOC_PAGES);
/* Pagein remaining pages for kmalloc */
linear = PMAN_MALLOC_LINEAR + SARTORIS_PROCBASE_LINEAR; // place after multiboot (this will invalidate the map src/dest linear address,
// we cannot use that area anymore, but it's ok, we used it for init copy only.)
physical = PMAN_MALLOC_PHYS;
map_pages(PMAN_TASK, linear, physical, PMAN_MALLOC_PAGES, PGATT_WRITE_ENA, 2);
pman_print("Initializing tasks/threads.");
/* Show MMAP information */
if(((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->flags & MB_INFO_MMAP && ((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_length > 0)
{
//Calculate multiboot mmap linear address.
//Sartoris loader left MMAP just after multiboot info structure.
((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_addr = PMAN_MULTIBOOT_LINEAR + sizeof(struct multiboot_info);
pman_print("Multiboot MMAP Size: %i ", ((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_length);
pman_print("Multiboot mmap linear address: %x", ((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_addr);
struct mmap_entry *entry = NULL;
entry = (struct mmap_entry *)((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_addr;
int kk = 0, mmlen = ((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR)->mmap_length / entry->size;
for(kk = 0; kk < mmlen; kk++)
{
pman_print("Multiboot entry size: %i start: %x end: %x type: %i", entry->size, (UINT32)entry->start, (UINT32)entry->end, entry->type);
entry = (struct mmap_entry *)((UINT32)entry + entry->size);
}
}
else
{
pman_print("No MMAP present.");
}
/* Initialize vmm subsystem */
vmm_init((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR, PMAN_INIT_RELOC_PHYS, PMAN_INIT_RELOC_PHYS + init_size);
tsk_init();
thr_init();
/* Mark SCHED_THR as taken! */
pmtsk = tsk_create(PMAN_TASK);
pmtsk->state = TSK_NORMAL;
pmthr = thr_create(SCHED_THR, pmtsk);
pmthr->state = THR_INTHNDL; // ehm... well... it IS an interrupt handler :D
pmthr->task_id = PMAN_TASK;
pmthr->state = THR_INTHNDL;
pman_print("Initializing allocator and interrupts.");
/* Initialize kernel memory allocator */
kmem_init(PMAN_MALLOC_LINEAR, PMAN_MALLOC_PAGES);
/* get our own interrupt handlers, override microkernel defaults */
int_init();
/* Initialize Scheduler subsystem */
sch_init();
pman_print("InitFS2 Service loading...");
/* Load System Services and init Loader */
loader_init((ADDR)PHYSICAL2LINEAR(PMAN_INIT_RELOC_PHYS));
//pman_print_clr(7);
pman_print("Loading finished, return INIT image memory to POOL...");
/* Put now unused Init-Fs pages onto vmm managed address space again. */
vmm_add_mem((struct multiboot_info*)PMAN_MULTIBOOT_LINEAR
,PHYSICAL2LINEAR(PMAN_INIT_RELOC_PHYS)
,PHYSICAL2LINEAR(PMAN_INIT_RELOC_PHYS + init_size));
pman_print("Signals Initialization...");
/* Initialize global signals container */
init_signals();
pman_print("Commands Initialization...");
/* Initialize Commands subsystem. */
cmd_init();
pman_print_set_color(12);
pman_print("PMAN: Initialization step 2 completed.");
/* Create Scheduler int handler */
if(create_int_handler(32, SCHED_THR, FALSE, 0) < 0)
pman_print_and_stop("Could not create Scheduler thread.");
/* This is it, we are finished! */
process_manager();
}
/*---------------------------------------------------------------------------*/
usys os_init (usys mem_start, usys mem_size)
{
usys stat ;
/* Disable all Interrupts */
cpu_disable_ints() ;
/* The global kernel data structre is statically allocated by */
/* the linker. This prevent the need for it to get allocated by */
/* the Memory Unit which is itself not initialized. */
memset (&kernel, 0, sizeof (kernel_t)) ;
/* Initialize the Memory Unit */
if (mem_init (mem_start, mem_size) != GOOD)
{
printf ("ERR: In os_init(): mem_init() Failed !!!\n") ;
return BAD ;
}
/* Initialize the Task Unit */
if (task_init () != GOOD)
{
printf ("ERR: In os_init(): task_init () Failed !!!\n") ;
return BAD ;
}
/* The memory and task units are now initialized. The kernel */
/* task and the interrupt task can now be created. They are */
/* created with the highest priority. */
kernel.int_mode = YES ; /* Fix this HACK <-- */
printf ("Akalon: Creating Idle Task\n") ;
stat = task_new (0, 0, 1024,0,0, task_idle, 0,0,0, "idle_task",
(usys *) (void *) &kernel.task_idle) ;
if (stat != GOOD)
{
printf ("ERR: In os_init(): Creating Idle Task. Stat = %d\n", stat) ;
return BAD ;
}
/* Initialize the Interrupt Unit */
if (int_init () != GOOD)
{
printf ("ERR: In os_init(): int_init () Failed !!!\n") ;
return BAD ;
}
/* Initialize the Semaphore Unit */
if (sem_init () != GOOD)
{
printf ("ERR: In os_init(): sem_init() Failed !!!\n") ;
return BAD ;
}
/* Initialize the Timer Unit */
if (timer_init () != GOOD)
{
printf ("ERR: In os_init(): timer_init() Failed !!!\n") ;
return BAD ;
}
/* Now, run the Idle Task. Except in the case of */
/* an Error, this call will never return back. */
kernel.task_idle->state = TASK_RUNNING ;
cpu_task_first (kernel.task_idle->stack_ptr) ;
/* We did return back...something has gone wrong. */
printf ("ERR: os_init(). Cannot Run Idle Task !!!\n") ;
return BAD ;
} /* End of function os_init() */
basic_variant(char v) {
int_init(v);
}
