/* Uninitialise the serial connection on the QR-side
* Author: Maurijn Neumann
*/
void serial_uninit() {
//Disable interrupts
DISABLE_INTERRUPT(INTERRUPT_PRIMARY_RX);
DISABLE_INTERRUPT(INTERRUPT_PRIMARY_TX);
//Communication is off
X32_leds &= ~COMM_LED;
}
int main()
{
uint16_t i;
register uint8_t tmp = MCUCR;
//! set interrupt vectors to bootloader
MCUCR = tmp | (1 << IVCE);
MCUCR = tmp | (1 << IVSEL);
DISABLE_INTERRUPT();
//! setup led...
led_init();
//! turn off Radio
sbi(DDRD, 4);
sbi(DDRD, 6);
sbi(DDRD, 7);
cc1k_write(0x0B, 0);
cc1k_write(0, (1 << 5) | (1<<4) | (1<<3) | (1<<2) | (1<<1) | (1<<0));
//! setup triggler pins
sbi(DDRC, 1);
//led_red_on();
ENABLE_INTERRUPT();
//! initialize data
//FlashBufferLoad(100, 0);
for(i = 0; i < 256; i++) {
somedata[i] = i;
}
timer_init(128, 3);
for(;;){ }
}
/* Write a byte of data to the serial link or set it up to be sent later (QR-side)
* Returns 1 if data was written directly, 0 if placed in buffer, -1 otherwise
* Author: Maurijn Neumann
*/
int serial_write(unsigned char c) {
//Check if data can be written immediately
if ((serial_buffer_send_top == serial_buffer_send_base) &&
(X32_serial_status & 0x01)) {
//Write to buffer, and done
// X32_display = c;
X32_serial_data = c;
return 1;
} else {
//Place data in buffer, update, *critical section due to top/base comparison*
DISABLE_INTERRUPT(INTERRUPT_GLOBAL);
serial_buffer_send[serial_buffer_send_top] = c;
serial_buffer_send_top++;
if (serial_buffer_send_top >= SERIAL_BUFFER_SIZE)
serial_buffer_send_top = 0;
if (serial_buffer_send_top == serial_buffer_send_base) {
log_msg("Serial output buffer full; discarding");
log_int(serial_buffer_send_top);
log_data(LOG_COMM_DISCARD_BUFFER, serial_buffer_send, SERIAL_BUFFER_SIZE);
return -1;
}
ENABLE_INTERRUPT(INTERRUPT_GLOBAL);
return 0;
}
}
/**
initialize timer.
- timer utility init
- timer register setting
*/
void timer_init()
{
DISABLE_INTERRUPT();
for (int i=0; i<MAX_SYS_TIMER; i++)
{
timer_list[i].running = 0;
timer_list[i].value = 0;
}
#ifdef USE_TIMER3
// 1ms.
// TCCR0 : FOC0, WGM00, COM01, COM00, WGM01, CS02, CS01, CS00
// CS12,CS11,CS10 : 1=clk, 2=clk/8, 3=clk/32, 4=clk/64, 5=clk/128, 6=clk/256, 7=clk/1024.
// WGM01,WGM00 : 0x10=CTC(Clear Timer on Compare match) mode; top = OCR0,
// timer control : CLK_io = system clock = 8 MHz.
TCNT3H = 0x00;
TCNT3L = 0x00;
TCCR3A = 0x40; // Normal mode. WGMn[3-0] = b0100.
// COM1A1,A0 = b00, normal port operation.(CTC mode)
TCCR3B = 0x0C; // CSn2/1/0 = b100, clk_io/256, CTC mode.
TCCR3C = 0x00;
OCR3AH = 0x00;
OCR3AL = TIMER_TCNT; //value=31 for 8Mhz; // 1ms
ETIMSK |=0x10; // enable OCIE3A, don't touch other flags.
#endif
ENABLE_INTERRUPT();
}
void isr_buttons(void)
{
char i;
DISABLE_INTERRUPT(INTERRUPT_BUTTONS);
for(i=0; i < 10; i++, catnap(500))
X32_LEDS = ALL_ON, catnap(500), X32_LEDS = ALL_OFF;
stop();
}
/* |------------------+------------------------------------------------------------------| */
EXPORT void ShutdownOS( StatusType Error )
{
DISABLE_INTERRUPT();
#if (cfgOS_SHUTDOWNHOOK == 1)
ShutdownHook(Error);
#endif
/* @req OS425: If ShutdownOS() is called and ShutdownHook() returns then the operating
system shall disable all interrupts and enter an endless loop. */
for ( ; ; )
{
/* Dead lopp here */
}
}
void isr_sensors(void)
{
DISABLE_INTERRUPT(INTERRUPT_GLOBAL);
qr.sax = X32_QR_S1 - qr.off_ax;
qr.say = X32_QR_S2 - qr.off_ay;
qr.saz = X32_QR_S3 - qr.off_az;
qr.sp = X32_QR_S4 - qr.off_p;
qr.sq = X32_QR_S5 - qr.off_q;
qr.sr = X32_QR_S6 - qr.off_r;
mafilter();
//butter_second();
ENABLE_INTERRUPT(INTERRUPT_GLOBAL);
}
EXPORT void LeaveISR(void)
{
DISABLE_INTERRUPT();
if(knl_taskindp > 0)
{
knl_taskindp--;
}
if((0 == knl_taskindp) && (!knl_dispatch_disabled))
{
if(knl_curtsk != knl_schedtsk)
{
devTrace(tlPort,"OS:Dispatch in ISR has been requested!\n");
knl_isr_dispatch();
}
}
ENABLE_INTERRUPT();
}
/* |------------------+------------------------------------------------------| */
EXPORT void StartOS ( AppModeType AppMode )
{
DISABLE_INTERRUPT();
knl_dispatch_disabled = 1; /* Dispatch disable */
#if defined( OpenOSEKStartupHook )
OpenOSEKStartupHook();
#endif
knl_appmode = AppMode;
knl_task_init();
#if(cfgOS_ALARM_NUM > 0)
knl_alarm_counter_init();
#endif
knl_resource_init();
#if(cfgOS_STARTUPHOOK == 1)
StartupHook();
#endif
knl_force_dispatch();
}
/* Author: Henrique Dantas */
void yaw_mode_ctrl()
{
//yaw control
ENABLE_INTERRUPT(INTERRUPT_OVERFLOW);
butterworth_filter();
yaw_error = p_yaw * ((yaw*SCALE_YAW) - s5_filtered);
//we set roll and pitch to zero for a better control of joystick
roll=0;
pitch=0;
oo1 = ((SCALE_LIFT*lift) - (yaw_error/SCALE_YAW_ERROR)) >> 2;
oo2 = ((SCALE_LIFT*lift) + (yaw_error/SCALE_YAW_ERROR)) >> 2;
oo3 = ((SCALE_LIFT*lift) - (yaw_error/SCALE_YAW_ERROR)) >> 2;
oo4 = ((SCALE_LIFT*lift) + (yaw_error/SCALE_YAW_ERROR)) >> 2;
DISABLE_INTERRUPT(INTERRUPT_OVERFLOW);
}
void isr_timer(void)
{
if( qr.link_down )
{
unsigned char i;
qr.current_mode = SAFE_MODE;
stop_motors();
qr.exit = 1;
qr.flag_mode = 1;
for(i=0; i < 5; i++, catnap(500))
X32_LEDS = ALL_ON, catnap(500), X32_LEDS = ALL_OFF;
#ifdef PERIPHERAL_DISPLAY
X32_DISPLAY = 0xf1fa;
#endif
DISABLE_INTERRUPT(INTERRUPT_TIMER1);
}
qr.link_down = 1;
}
/* main entry point */
int main() {
int time, iterations;
/* make the code somewhat faster when running in the sim */
if (STATE_SIMULATOR) {
/* set timer 1 period to 5 seconds */
peripherals[PERIPHERAL_TIMER1_PERIOD] = 100*CLOCKS_PER_MS;
/* set timer 2 period to 7 seconds */
peripherals[PERIPHERAL_TIMER2_PERIOD] = 140*CLOCKS_PER_MS;
printf("Running in simulation mode!\n");
} else {
/* set timer 1 period to 5 seconds */
peripherals[PERIPHERAL_TIMER1_PERIOD] = 5000*CLOCKS_PER_MS;
/* set timer 2 period to 7 seconds */
peripherals[PERIPHERAL_TIMER2_PERIOD] = 7000*CLOCKS_PER_MS;
}
/* set interrupt addresses & priorities */
/* note: the priority MUST be > 1, larger means higher prio */
SET_INTERRUPT_VECTOR(INTERRUPT_TIMER1, &int_timer1);
SET_INTERRUPT_PRIORITY(INTERRUPT_TIMER1, 50);
SET_INTERRUPT_VECTOR(INTERRUPT_TIMER2, &int_timer2);
SET_INTERRUPT_PRIORITY(INTERRUPT_TIMER2, 40);
SET_INTERRUPT_VECTOR(INTERRUPT_PRIMARY_RX, &int_com1_in);
SET_INTERRUPT_PRIORITY(INTERRUPT_PRIMARY_RX, 30);
SET_INTERRUPT_VECTOR(INTERRUPT_PRIMARY_TX, &int_com1_out);
SET_INTERRUPT_PRIORITY(INTERRUPT_PRIMARY_TX, 20);
#ifdef INTERRUPT_SECONDARY_RX
SET_INTERRUPT_VECTOR(INTERRUPT_SECONDARY_RX, &int_com2_in);
SET_INTERRUPT_PRIORITY(INTERRUPT_SECONDARY_RX, 30);
#endif
#ifdef INTERRUPT_SECONDARY_TX
SET_INTERRUPT_VECTOR(INTERRUPT_SECONDARY_TX, &int_com2_out);
SET_INTERRUPT_PRIORITY(INTERRUPT_SECONDARY_TX, 20);
#endif
SET_INTERRUPT_VECTOR(INTERRUPT_BUTTONS, &int_buttons);
SET_INTERRUPT_PRIORITY(INTERRUPT_BUTTONS, 10);
SET_INTERRUPT_VECTOR(INTERRUPT_SWITCHES, &int_switches);
SET_INTERRUPT_PRIORITY(INTERRUPT_SWITCHES, 10);
/* enable interrupts */
ENABLE_INTERRUPT(INTERRUPT_TIMER1);
ENABLE_INTERRUPT(INTERRUPT_TIMER2);
ENABLE_INTERRUPT(INTERRUPT_PRIMARY_RX);
//ENABLE_INTERRUPT(INTERRUPT_PRIMARY_TX);
#ifdef INTERRUPT_SECONDARY_RX
ENABLE_INTERRUPT(INTERRUPT_SECONDARY_RX);
#endif
#ifdef INTERRUPT_SECONDARY_TX
//ENABLE_INTERRUPT(INTERRUPT_SECONDARY_TX);
#endif
ENABLE_INTERRUPT(INTERRUPT_BUTTONS);
ENABLE_INTERRUPT(INTERRUPT_SWITCHES);
/* this is the global 'turn all interrupts on/off interrupt',
when it's off, no interrupt will ever trigger, when it's
on, all enabled interrupts may trigger */
ENABLE_INTERRUPT(INTERRUPT_GLOBAL);
/* run idle loop, print "IDLE LOOP" each 1000 iterations */
time = iterations = 0;
while(!quit) {
if ((STATE_SIMULATOR && X32_MS_CLOCK - time > 50) || X32_MS_CLOCK - time > 1000) {
printf("IDLE LOOP: %d iterations/sec\r\n", iterations);
iterations = 0;
time = X32_MS_CLOCK;
}
iterations++;
}
/* disable interrupts */
DISABLE_INTERRUPT(INTERRUPT_GLOBAL);
DISABLE_INTERRUPT(INTERRUPT_TIMER1);
DISABLE_INTERRUPT(INTERRUPT_TIMER2);
DISABLE_INTERRUPT(INTERRUPT_PRIMARY_RX);
DISABLE_INTERRUPT(INTERRUPT_PRIMARY_TX);
#ifdef INTERRUPT_SECONDARY_RX
DISABLE_INTERRUPT(INTERRUPT_SECONDARY_RX);
#endif
#ifdef INTERRUPT_SECONDARY_TX
DISABLE_INTERRUPT(INTERRUPT_SECONDARY_TX);
#endif
DISABLE_INTERRUPT(INTERRUPT_BUTTONS);
DISABLE_INTERRUPT(INTERRUPT_SWITCHES);
return 0;
}
