/*------------------------------------------------------------------ * qrtest.c -- test QR engines and sensors * * reads ae[0-3] from stdin * (q,w,e,r increment ae[0-3], a,s,d,f decrement) * * prints time[us],ae[0-3],sax,say,saz,sp,sq,sr on stdout * * Arjan J.C. van Gemund * * Extended by Matias Escudero June 2010 * (wireless + some X32 features) * * Modified by Arjan van Gemund Nov 2010 * (200Hz ae update ISR instead of 1300Hz QR ISR) * * Modified by Arjan van Gemund Feb 2011 * (new QR peripheral + 500Hz ae update ISR instead of 1300Hz QR ISR) * *------------------------------------------------------------------ */ #include #include "x32.h" #include "assert.h" /* define some peripheral short hands */ #define X32_timer_per peripherals[PERIPHERAL_TIMER1_PERIOD] #define X32_leds peripherals[PERIPHERAL_LEDS] #define X32_ms_clock peripherals[PERIPHERAL_MS_CLOCK] #define X32_us_clock peripherals[PERIPHERAL_US_CLOCK] #define X32_QR_a0 peripherals[PERIPHERAL_XUFO_A0] #define X32_QR_a1 peripherals[PERIPHERAL_XUFO_A1] #define X32_QR_a2 peripherals[PERIPHERAL_XUFO_A2] #define X32_QR_a3 peripherals[PERIPHERAL_XUFO_A3] #define X32_QR_s0 peripherals[PERIPHERAL_XUFO_S0] #define X32_QR_s1 peripherals[PERIPHERAL_XUFO_S1] #define X32_QR_s2 peripherals[PERIPHERAL_XUFO_S2] #define X32_QR_s3 peripherals[PERIPHERAL_XUFO_S3] #define X32_QR_s4 peripherals[PERIPHERAL_XUFO_S4] #define X32_QR_s5 peripherals[PERIPHERAL_XUFO_S5] #define X32_QR_timestamp peripherals[PERIPHERAL_XUFO_TIMESTAMP] #define X32_rs232_data peripherals[PERIPHERAL_PRIMARY_DATA] #define X32_rs232_stat peripherals[PERIPHERAL_PRIMARY_STATUS] #define X32_rs232_char (X32_rs232_stat & 0x02) #define X32_wireless_data peripherals[PERIPHERAL_WIRELESS_DATA] #define X32_wireless_stat peripherals[PERIPHERAL_WIRELESS_STATUS] #define X32_wireless_char (X32_wireless_stat & 0x02) #define X32_button peripherals[PERIPHERAL_BUTTONS] #define X32_switches peripherals[PERIPHERAL_SWITCHES] // RX FIFO #define FIFOSIZE 16 char fifo[FIFOSIZE]; int iptr, optr; // Global vars char c; int demo_done; int ae0, ae1, ae2, ae3; int s0, s1, s2, s3, s4, s5, timestamp; int isr_qr_counter; int isr_qr_time; int button; int inst; // Some functions void toggle_led(int); void delay_ms(int); void delay_us(int); /*------------------------------------------------------------------ * isr_button -- QR button interrupt handler *------------------------------------------------------------------ */ void isr_button(void) { button = 1; // just signal event } /*------------------------------------------------------------------ * isr_timer -- timer #1 interrupt handler -- update ae regs QR *------------------------------------------------------------------ */ void isr_timer(void) { int ae_index; /* send actuator values to ae0..3 QR peripheral regs */ X32_QR_a0 = ae0; X32_QR_a1 = ae1; X32_QR_a2 = ae2; X32_QR_a3 = ae3; } /*------------------------------------------------------------------ * isr_qr_link -- QR link rx interrupt handler *------------------------------------------------------------------ */ void isr_qr_link(void) { /* X32 time stamp */ isr_qr_time = X32_us_clock; /* get sensor and timestamp values */ s0 = X32_QR_s0; s1 = X32_QR_s1; s2 = X32_QR_s2; s3 = X32_QR_s3; s4 = X32_QR_s4; s5 = X32_QR_s5; timestamp = X32_QR_timestamp; /* verify the presence of interrupts (1300 Hz) */ isr_qr_counter++; if (isr_qr_counter % 500 == 0) { toggle_led(2); } } /*------------------------------------------------------------------ * isr_rs232_rx -- rs232 rx interrupt handler * dump in the same fifo as wireless link *------------------------------------------------------------------ */ void isr_rs232_rx(void) { int c; /* signal interrupt */ toggle_led(3); /* may have received > 1 char before IRQ is serviced so loop */ while (X32_rs232_char) { fifo[iptr++] = X32_rs232_data; if (iptr >= FIFOSIZE) iptr = 0; } } /*------------------------------------------------------------------ * isr_wireless_rx -- wireless rx interrupt handler * dump in the same fifo as wired link *------------------------------------------------------------------ */ void isr_wireless_rx(void) { int c; /* signal interrupt */ toggle_led(4); /* may have received > 1 char before IRQ is serviced so loop */ while (X32_wireless_char) { fifo[iptr++] = X32_wireless_data; if (iptr >= FIFOSIZE) iptr = 0; } } /*------------------------------------------------------------------ * getchar -- read char from rx fifo, return -1 if no char available *------------------------------------------------------------------ */ int getchar(void) { int c; if (optr == iptr) { return -1; } c = fifo[optr++]; if (optr >= FIFOSIZE) { optr = 0; } return c; } /*------------------------------------------------------------------ * delay_ms -- busy-wait for ms milliseconds *------------------------------------------------------------------ */ void delay_ms(int ms) { int time = X32_ms_clock; while(X32_ms_clock - time < ms) { ; } } /*------------------------------------------------------------------ * delay_us -- busy-wait for us milliseconds *------------------------------------------------------------------ */ void delay_us(int us) { int time = X32_us_clock; while(X32_us_clock - time < us) { ; } } /*------------------------------------------------------------------ * toggle_led -- toggle led # i on QR *------------------------------------------------------------------ */ void toggle_led(int i) { X32_leds = (X32_leds ^ (1 << i)); } /*------------------------------------------------------------------ * process_key -- process command keys *------------------------------------------------------------------ */ void process_key(char c) { switch (c) { case 'q': ae0 += 20; break; case 'a': ae0 -= 20; if (ae0 < 0) ae0 = 0; break; case 'w': ae1 += 20; break; case 's': ae1 -= 20; if (ae1 < 0) ae1 = 0; break; case 'e': ae2 += 20; break; case 'd': ae2 -= 20; if (ae2 < 0) ae2 = 0; break; case 'r': ae3 += 20; break; case 'f': ae3 -= 20; if (ae3 < 0) ae3 = 0; break; default: demo_done = 1; } } /*------------------------------------------------------------------ * print_state -- print all sensors and actuators *------------------------------------------------------------------ */ void print_state(void) { int i; char text[100]; /* send telemetry to wired link */ printf("%d | ",isr_qr_time); printf("%3d %3d %3d %3d | ",ae0,ae1,ae2,ae3); printf("%3d %3d %3d %3d %3d %3d | %d\r\n", s0,s1,s2,s3,s4,s5,timestamp); /* send subset to wireless if QR switches set */ sprintf(text, "%d %d %d %d \r\n",ae0,ae1,ae2,ae3); i = 0; while( text[i] != 0) { delay_ms(1); // don't overload wireless link if (X32_switches == 0x03) { X32_wireless_data = text[i]; } i++; } } /*------------------------------------------------------------------ * main -- do the test *------------------------------------------------------------------ */ int main() { /* prepare QR rx interrupt handler and associated variables */ SET_INTERRUPT_VECTOR(INTERRUPT_XUFO, &isr_qr_link); SET_INTERRUPT_PRIORITY(INTERRUPT_XUFO, 21); isr_qr_counter = isr_qr_time = 0; ae0 = ae1 = ae2 = ae3 = 0; ENABLE_INTERRUPT(INTERRUPT_XUFO); /* prepare timer interrupt for 500 Hz engine control (i.e., 5 ms) */ X32_timer_per = 2 * CLOCKS_PER_MS; SET_INTERRUPT_VECTOR(INTERRUPT_TIMER1, &isr_timer); SET_INTERRUPT_PRIORITY(INTERRUPT_TIMER1, 22); ENABLE_INTERRUPT(INTERRUPT_TIMER1); /* prepare button interrupt handler and associated variables */ SET_INTERRUPT_VECTOR(INTERRUPT_BUTTONS, &isr_button); SET_INTERRUPT_PRIORITY(INTERRUPT_BUTTONS, 8); button = 0; ENABLE_INTERRUPT(INTERRUPT_BUTTONS); /* prepare rs232 rx interrupt and getchar handler */ SET_INTERRUPT_VECTOR(INTERRUPT_PRIMARY_RX, &isr_rs232_rx); SET_INTERRUPT_PRIORITY(INTERRUPT_PRIMARY_RX, 20); while (X32_rs232_char) c = X32_rs232_data; // empty buffer ENABLE_INTERRUPT(INTERRUPT_PRIMARY_RX); /* prepare wireless rx interrupt and getchar handler */ SET_INTERRUPT_VECTOR(INTERRUPT_WIRELESS_RX, &isr_wireless_rx); SET_INTERRUPT_PRIORITY(INTERRUPT_WIRELESS_RX, 19); while (X32_wireless_char) c = X32_wireless_data; // empty buffer ENABLE_INTERRUPT(INTERRUPT_WIRELESS_RX); /* initialize remaining stuff */ iptr = optr = 0; // reset FIFO X32_leds = 0; demo_done = 0; /* start the fun */ ENABLE_INTERRUPT(INTERRUPT_GLOBAL); while (! demo_done) { /* process keyboard command (if any) */ c = getchar(); if (c != -1) { process_key(c); } /* display QR data */ print_state(); X32_leds = (X32_leds & 0xFC) | (X32_switches & 0x03 ); if (button == 1){ printf("You have pushed the button!\r\n"); button = 0; } /* don't flood the viewer */ delay_ms(20); } printf("Exit\r\n"); DISABLE_INTERRUPT(INTERRUPT_GLOBAL); return 0; }