/* Initialise the serial connection on the QR-side
* Author: Maurijn Neumann
*/
int serial_init() {
//Declare variables
unsigned char c;
//Setup interrupts (recv)
SET_INTERRUPT_VECTOR(INTERRUPT_PRIMARY_RX, &isr_serial_rx);
SET_INTERRUPT_PRIORITY(INTERRUPT_PRIMARY_RX, 20);
//There may yet be data in the internal buffer, empty to prevent unscheduled interrupts
while (X32_serial_status & 0x02)
//Emtpy through reading
c = X32_serial_data;
//Setup interrupts (send)
SET_INTERRUPT_VECTOR(INTERRUPT_PRIMARY_TX, &isr_serial_tx);
SET_INTERRUPT_PRIORITY(INTERRUPT_PRIMARY_TX, 15);
//Clear buffers
serial_buffer_recv_base = 0;
serial_buffer_recv_top = 0;
serial_buffer_send_base = 0;
serial_buffer_send_top = 0;
//Enable interrupts
ENABLE_INTERRUPT(INTERRUPT_PRIMARY_RX);
ENABLE_INTERRUPT(INTERRUPT_PRIMARY_TX);
//Communication is setup correctly
X32_leds |= COMM_LED;
return 0;
}
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();
}
EXPORT void EnterISR(void)
{
if(knl_taskindp < 0xFF)
{
knl_taskindp++; /* Enter Task Independent Part */
}
ENABLE_INTERRUPT();
}
//when task start to run
EXPORT void portActivateR(void)
{
/* This is the most easiest Way to get Internal Resourse and
* to make a task non-preemtable I think */
GetInternalResource();
ENABLE_INTERRUPT();
knl_tcb_pc[knl_curtsk]();
}
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();
}
/* 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 irInit()
{
IO_INPUT_PP(IR); IO_HIGH(IR);
// TIM3_CH2
RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
TIM3->PSC = F_CPU / 1000000;
TIM3->ARR = 32000;
TIM3->EGR = TIM_EGR_UG;
TIM3->CNT = 0;
TIM3->CCMR1 = TIM_CCMR1_CC2S_0;
TIM3->CCER = TIM_CCER_CC2P | TIM_CCER_CC2E;
TIM3->DIER = TIM_DIER_CC2IE;
ENABLE_INTERRUPT(TIM3_IRQn);
ir_lastChangeTime = 0;
irparams.rawlen = 0;
TIM3->CR1 = TIM_CR1_CEN;
irparams.rcvstate = STATE_STOP;
}
/* 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;
}
void main()
{
SCB->VTOR = 0x08000000;
// set clock source as HSI / 2 * (PLL) 4
RCC->CFGR |= RCC_CFGR_PLLMULL8 | RCC_CFGR_PLLXTPRE_HSE_Div2 /*| RCC_CFGR_PLLSRC*/;
RCC->CR |= RCC_CR_PLLON;
while (!(RCC->CR & RCC_CR_PLLRDY));
RCC->CFGR |= RCC_CFGR_SW_PLL;
while (!(RCC->CFGR & RCC_CFGR_SWS_PLL));
// enable peripherals
RCC->APB1ENR = RCC_APB1ENR_TIM2EN;
RCC->APB2ENR = RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPCEN | RCC_APB2ENR_ADC1EN | RCC_APB2ENR_USART1EN | RCC_APB2ENR_AFIOEN;
AFIO->MAPR = AFIO_MAPR_SWJ_CFG_JTAGDISABLE;
// configure USART
IO_ALT_PUSH_PULL(UART_TX);
USART1->BRR = USART_BRR(230400);
USART1->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE | USART_CR1_RXNEIE;
ENABLE_INTERRUPT(USART1_IRQn);
// enable systick
SysTick->LOAD = SysTick->VAL = (F_CPU / 1000) / 8;
SysTick->CTRL = /*SysTick_CTRL_CLKSOURCE |*/ SysTick_CTRL_ENABLE | SysTick_CTRL_TICKINT;
_delay_init();
IO_PUSH_PULL(LED);
IO_HIGH(LED);
#ifndef ETHERNET_MODULE
OW_UART_init();
tempInit();
ioInit();
irInit();
#endif
#ifdef ETHERNET
ethInit();
#endif
uint8_t b;
uint32_t lastCheck = 0;
for (;;)
{
// if (IO_IS_LOW(IN1)) dodump = 1;
// else dodump = 0;
// dodump=1;
#ifndef ETHERNET_MODULE
ioProcess();
tempProcess();
irProcess();
#endif
provTmr();
#ifdef ETHERNET
ethProcess();
#endif
}
}
