/*
TIM3 overflow interrupt
set next prescaler, output compare and overflow values to
preload registers
*/
@interrupt void ppm_interrupt(void) {
BRES(TIM3_SR1, 0); // erase interrupt flag
if (ppm_channel2) {
if (ppm_channel2 == 2) {
// will be setting channel1 servo, so we are now generating
// SYNC signal in HW
// wakeup CALC task to compute new PPM values
// values for ARR registers will be updated after calculate done
// (and when we are still generating SYNC pulse)
awake(CALC);
}
// set servo channel
TIM3_PSCR = PPM_PSC_SERVO;
TIM3_CCR2H = hi8(PPM_300US_SERVO);
TIM3_CCR2L = lo8(PPM_300US_SERVO);
TIM3_ARRH = ppm_values[ppm_channel2];
ppm_channel2++;
TIM3_ARRL = ppm_values[ppm_channel2];
if (++ppm_channel2 > channels2) {
// next to SYNC pulse
ppm_channel2 = 0;
}
return;
}
// set SYNC signal
TIM3_PSCR = PPM_PSC_SYNC;
TIM3_CCR2H = hi8(PPM_300US_SYNC);
TIM3_CCR2L = lo8(PPM_300US_SYNC);
TIM3_ARRH = ppm_values[0];
TIM3_ARRL = ppm_values[1];
ppm_channel2 = 2; // to first channel (step 2 bytes)
}
void MakeLog(void){
uint16_t crc_main = 0xffff;
if (gf_buffer_indicator == 1){ //if this flag is 1 we must send the g_buffer_uSD_2
for (uint16_t i=0;i<BUFFER_SIZE-2;i++) { //calculating the CRC
crc_main = (uint16_t)(crc_main + g_buffer_uSD_2[i]);
}
g_buffer_uSD_2[BUFFER_SIZE-1] = lo8(crc_main);
g_buffer_uSD_2[BUFFER_SIZE-2] = hi8(crc_main);
g_error = f_lseek(&f_origem, f_size(&f_origem)); //looks for the end of the file
//TODO:: do something with those errors
g_error = f_write(&f_origem, g_buffer_uSD_2, BUFFER_SIZE, &br); //write the buffer on the end of the file
g_error = f_sync(&f_origem); //waits for it to finish
}
else{ //if gf_buffer_indicator is 0 we must send the g_buffer_uSD
for (uint16_t i=0;i<BUFFER_SIZE-2;i++) {//calculating the CRC
crc_main = (uint16_t)(crc_main + g_buffer_uSD[i]);
}
g_buffer_uSD[BUFFER_SIZE-1] = lo8(crc_main);
g_buffer_uSD[BUFFER_SIZE-2] = hi8(crc_main);
g_error = f_lseek(&f_origem, f_size(&f_origem));
//TODO:: do something with this error
g_error = f_write(&f_origem, g_buffer_uSD, BUFFER_SIZE, &br);
g_error = f_sync(&f_origem);
}//close else
}
void trtCreateTask(void (*fun)(void*), uint16_t stacksize, uint32_t release, uint32_t deadline, void *args) {
uint8_t *sp;
struct task *t;
int i;
cli(); // turn off interrupts
++kernel.nbrOfTasks;
sp = kernel.memptr;
kernel.memptr -= stacksize; // decrease free mem ptr
// initialize stack
*sp-- = lo8(fun); // store PC(lo)
*sp-- = hi8(fun); // store PC(hi)
for (i=0; i<25; i++) //WAS -- for (i=0; i<24; i++)
*sp-- = 0x00; // store SREG,r0-r1,r3-r23
// Save args in r24-25 (input arguments stored in these registers)
*sp-- = lo8(args);
*sp-- = hi8(args);
for (i=0; i<6; i++)
*sp-- = 0x00; // store r26-r31
t = &kernel.tasks[kernel.nbrOfTasks];
t->release = release;
t->deadline = deadline;
t->state = TIMEQ;
t->spl = lo8(sp); // store stack pointer
t->sph = hi8(sp);
// call interrupt handler to schedule
TIMER1_COMPA_vect();
}
// set number of channels
void ppm_set_channels(u8 n) {
// disable PPM generation till new values will not be set
ppm_enabled = 0;
BRES(TIM3_CR1, 0); // disable timer
BSET(PD_ODR, 0); // set PPM pin to 1
// start with generating 20ms SYNC signal
TIM3_CCR2H = hi8(PPM_300US_SYNC);
TIM3_CCR2L = hi8(PPM_300US_SYNC);
TIM3_ARRH = hi8(PPM_MUL_SYNC * 20);
TIM3_ARRL = lo8(PPM_MUL_SYNC * 20);
channels = n;
channels2 = (u8)(n << 1); // also 2* value for compare in ppm_interrupt
}
struct k_t *
k_crt_task (void (*pTask) (void), char prio, char *pStk, int stkSize)
{
struct k_t *pT;
int i;
char *s;
if ((k_running) || ((prio <= 0) || (DMY_PRIO < prio))
|| (k_task <= nr_task)) {
goto badexit;
}
pT = task_pool + nr_task; // lets take a task descriptor
pT->nr = nr_task;
nr_task++;
pT->cnt2 = 0; // no time out running on you for the time being
pT->cnt3 = 0; // no time out semaphore
pT->cnt1 = (int) (pStk); // ref to my stack
// stack paint :-)
for (i = 0; i < stkSize; i++) // put hash code on stak to be used by k_unused_stak()
{
pStk[i] = STAK_HASH;
}
s = pStk + stkSize - 1; // now we point on top of stak
*(s--) = 0x00; // 1 byte safety distance :-)
// an interrupt do only push PC on stack by HW - can be 2 or 3 bytes
// depending of 368/.../1280/2560
#ifdef BACKSTOPPER
pT->pt = pTask;
*(s--) = lo8 (jumper); // so top now holds address of function
*(s--) = hi8 (jumper); // which is code body for task
#else
*(s--) = lo8 (pTask); // so top now holds address of function
*(s--) = hi8 (pTask); // which is code body for task
#endif
// NB NB 2560 use 3 byte for call/ret addresses the rest only 2
#if defined (__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
*(s--) = EIND; // best guess : 3 byte addresses !!! or just 0
#endif
// r1 is the socalled zero value register
// see https://gcc.gnu.org/wiki/avr-gcc
// can tmp be non zero (multiplication etc)
*(s--) = 0x00; // r1
*(s--) = 0x00; // r0
*(s--) = 0x00; // sreg
//1280 and 2560 need to save rampz reg just in case
#if defined (__AVR_ATmega2560__) || defined (__AVR_ATmega1280__) || defined (__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__)
*(s--) = RAMPZ; // best guess 0x3b
// obsolete JDN *(s--) = EIND; // best guess
#endif
#if defined (__AVR_ATmega2560__) || defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2561__)
*(s--) = EIND; // best guess 0x3c
#endif
for (i = 0; i < 30; i++) //r2-r31 = 30 regs
{
*(s--) = 0x00;
}
pT->sp_lo = lo8 (s); // now we just need to save stakptr
pT->sp_hi = hi8 (s); // in thread descriptor
// HW DEPENDENT PART - ENDE
pT->prio = prio; // maxv for holding org prio for inheritance
pT->maxv = (int) prio;
prio_enQ (pAQ, pT); // and put task in active Q
return (pT);
badexit:
k_err_cnt++;
return (NULL);
}
struct k_t *
k_crt_task (void (*pTask) (void), char prio, char *pStk, int stkSize)
{
struct k_t *pT;
int i;
char *s;
if (k_running)
return (NULL);
if ((prio <= 0 ) || (DMY_PRIO < prio)) {
pT = NULL;
goto badexit;
}
if (k_task <= nr_task) {
goto badexit;
}
pT = task_pool + nr_task; // lets take a task descriptor
nr_task++;
pT->cnt2 = 0; // no time out running on you for the time being
// HW_DEP_START
// inspiration from http://dev.bertos.org/doxygen/frame_8h_source.html
// and http://www.control.aau.dk/~jdn/kernels/krnl/
// now we are going to precook stak
pT->cnt1 = (int) (pStk);
for (i = 0; i < stkSize; i++) // put hash code on stak to be used by k_unused_stak()
pStk[i] = STAK_HASH;
s = pStk + stkSize - 1; // now we point on top of stak
*(s--) = 0x00; // 1 byte safety distance
*(s--) = lo8 (pTask); // so top now holds address of function
*(s--) = hi8 (pTask); // which is code body for task
// NB NB 2560 use 3 byte for call/ret addresses the rest only 2
#if defined (__AVR_ATmega2560__)
*(s--) = EIND; // best guess : 3 byte addresses !!!
#endif
*(s--) = 0x00; // r1
*(s--) = 0x00; // r0
*(s--) = 0x00; // sreg
//1280 and 2560 need to save rampz reg just in case
#if defined (__AVR_ATmega2560__) || defined (__AVR_ATmega1280__)
*(s--) = RAMPZ; // best guess
*(s--) = EIND; // best guess
#endif
for (i = 0; i < 30; i++) //r2-r31 = 30 regs
*(s--) = 0x00;
pT->sp_lo = lo8 (s); // now we just need to save stakptr
pT->sp_hi = hi8 (s); // in thread descriptor
//HW_DE_ENDE
pT->prio = prio; // maxv for holding org prio for inheritance
pT->maxv = (int) prio;
prio_enQ (pAQ, pT); // and put task in active Q
return (pT); // shall be index to task descriptor
badexit:
k_err_cnt++;
return (NULL);
}