void WasteOfTime(uint32_t waittime)
{
uint32_t oldTime,newTime;
OS_GetTicks(&oldTime); // get time
do
{
OS_GetTicks(&newTime); // get time
}
while (newTime - oldTime < waittime);
}
void TaskTWI(void)
{
OS_WaitTicks(OSALM_TWIWAIT,10); // BMP085 needs 10ms in advance.
TWI_init();
bmp085_init(TaskTWIWait1ms);
lsm303_config_accel(TaskTWIWait1ms);
lsm303_config_magnet(TaskTWIWait1ms);
// init ISR
INTC_register_interrupt(&int_handler_ACC, AVR32_EIC_IRQ_0, AVR32_INTC_INTLEVEL_INT2);
// activate and enable EIC pins:
eic_enable_channel(0); // enable rising edge isr.
TWI_RegisterReadyHandler(TWIreadyhandler);
lsm303_TWI_trig_read_accel(); // restart interrupt
TWIstate = etwi_readACC;
lastACCSample = OS_GetTicks();
while(1)
{
if (OS_GetTicks()-lastACCSample > 400)
{
lsm303_TWI_trig_read_accel(); // restart interrupt
TWIstate = etwi_readACC;
}
uint8_t ret = OS_WaitEventTimeout(OSEVT_TWIRDY,OSALM_TWITIMEOUT,200);
if((ret & OSEVT_TWIRDY) == 0)
{
// timeout
//asm("breakpoint"); fixme wtf why and why
//emstop(5); // will hang up while flashing, if we stop here!
}
else
{
long bar = bmp085_calc_pressure(bmp085raw); // just read stuff from rx buffers out of ISR!!! (large calculation!)
int32_t h_mm = bmp085_calcHeight_mm(bar);
s_nHeight_mm = h_mm; // update global
#if SIMULATION == 1
// do not update z position, this is done in TaskNavi to make it even more complicated. ;)
#else
NAV_UpdatePosition_z_m((float)h_mm*0.001);
#endif
}
}
}
void TWIreadyhandler(void)
{
static uint8_t tictoc=0;
/*
Procedure:
Acc via EIC triggers next conversion (1344Hz);
all due conversions of other sensors (mag (200Hz), press) will be chained via TWI_ready handler.
*/
switch(TWIstate)
{
case etwi_readACC:
lastACCSample = OS_GetTicks();
acc_res[acc_wr].state = lsm303_get_acc_results(&acc_res[acc_wr].v);
acc_wr++;
if(acc_wr == ACC_QSIZE) acc_wr = 0;
tictoc ++;
if(tictoc == 4)
{
TWIstate = etwi_readMAG;
lsm303_TWI_trig_read_magnet();
}
else if(tictoc == 5) // 1344/6 = 224Hz (Mag runs at 220)
{
tictoc = 0;
if(BaroMachine()==1)
TWIstate = etwi_BAR;
else
TWIstate = etwi_Idle;
}
else
{
TWIstate = etwi_Idle;
}
break;
case etwi_readMAG:
// do not set state!!
mag_res[mag_wr].state = lsm303_get_mag_results(&mag_res[mag_wr].v);
mag_wr++;
if(mag_wr == MAG_QSIZE)
mag_wr = 0;
TWIstate = etwi_Idle;
break;
case etwi_BAR:
if(BaroMachine()==1)
TWIstate = etwi_BAR; // should NEVER happen !
else
TWIstate = etwi_Idle;
break;
default:
asm("breakpoint"); // wtf why do we land here
break;
}
}
uint32_t BaroMachine(void) // return 1 if it has triggered TWI
{
// if nothing is running, start a conversion
// if conversion was started and conversion time is elapsed, start getting results
uint32_t now = OS_GetTicks();
uint32_t elapsed = now - StartTime;
switch(BMPState)
{
case eBMP_Idle:
if(TempDecrementer-- == 0)
{
TempDecrementer = GETTEMPEVERY;
BMPState = eBMP_waitTemp;
WaitTime = bmp085_TWI_trig_temp_meas(); // takes up to 5 ms
StartTime = now;
return 1; // TWI write triggered
}
else
{
BMPState = eBMP_waitBaro;
WaitTime = bmp085_TWI_trig_press_meas(); // takes up to 25.5 ms
StartTime = now;
return 1; // TWI write triggered
}
break;
case eBMP_waitTemp:
if (elapsed > WaitTime)
{
BMPState = eBMP_readTemp;
bmp085_TWI_trig_temp_read();
return 1; // TWI read triggered
}
break;
case eBMP_waitBaro:
if (elapsed > WaitTime+10)
{
BMPState = eBMP_readBaro;
bmp085_TWI_trig_press_read();
return 1; // TWI read triggered
}
break;
case eBMP_readTemp:
BMPState = eBMP_Idle;
bmp085_temp = bmp085_get_temperature(); // just read stuff from rx buffers
break;
case eBMP_readBaro:
BMPState = eBMP_Idle;
bmp085raw = bmp085_read_up();
OS_SetEventFromISR(OSTSK_TWI,OSEVT_TWIRDY);
break;
default:
break;
}
return 0; // did not trigger TWI
}
/*
Called periodically (˜1ms) by timer scheduler for RX and TX
@param tnow timestamp in uSecond
*/
void hott_serial_scheduler( uint32_t tnow )
{
static uint32_t _hott_serial_timer;
_hott_check_serial_data( tnow ); // Check for data request
if(_hott_msg_ptr == 0) return; //no data to send
if(_hott_telemetry_is_sending)
{
//we are sending already, wait for a delay of 2ms between data bytes
if(tnow - _hott_serial_timer < 3000) //delay ca. 3,5 mS. 19200 baud = 520uS / Byte + 3ms required delay
// Graupner Specs claims here 2ms but in reality it's 3ms, they using 3ms too...
return;
}
else
{
//new data request
tnow = OS_GetTicks()*1000; //correct the 5ms delay in _hott_check_serial_data()...
}
_hott_send_telemetry_data();
_hott_serial_timer = tnow;
}
unsigned long long time_in_microseconds(void)
{
return ((unsigned long long)OS_GetTicks() * 1000LL);
}
// This is the coordinator task, the main beat for the tests comes from here.
void TestTask0(void)
{
uint8_t ret,i;
uint32_t ti,tj;
uint16_t t;
while(1)
{
switch(testcase)
{
case 0:
// Task Priority
assert(testvar ==0);
testvar++;
break;
case 1:
// Timer / Wait
OS_WaitTicks(OSALMTest0, 3);
t=10;
OS_GetTicks(&ti); // get time
OS_WaitTicks(OSALMTest0, t);
OS_GetTicks(&tj); // get time
assert(tj-ti == t); // waited time was correct
break;
case 2:
// WaitEvent
// SetEvent
// wait for A, then A occurs
testvar =0;
OS_WaitTicks(OSALMTest0, 10);
assert(testvar==1);
OS_SetEvent(OSTestTskID1,1<<0);
OS_WaitTicks(OSALMTest0, 2);
assert(testvar==2);
// A occurs, then wait for A
testvar =3;
OS_SetEvent(OSTestTskID1,1<<0);
assert(testvar==3);
OS_WaitTicks(OSALMTest0, 100); // other task wastes time...
assert(testvar==4);
// wait for more events, one occurs, then the other
OS_SetEvent(OSTestTskID1,1<<2);
assert(testvar==4);
OS_WaitTicks(OSALMTest0, 10);
assert(testvar==6);
OS_SetEvent(OSTestTskID1,1<<1);
assert(testvar==6);
OS_WaitTicks(OSALMTest0, 10);
assert(testvar==8);
break;
case 3:
// MutexGet
// MutexRelease
break;
case 4:
// QueueIn
// QueueOut
OS_WaitTicks(OSALMTest0, 50);
for (i=0;i<20;i++) // forward
{
ret = OS_QueueIn(&TestQ,&i);
assert(ret==0);
}
OS_SetEvent(OSTestTskID1,1<<6);
OS_WaitTicks(OSALMTest0, 50);
for (i=20;i>0;i--) // backward
{
ret = OS_QueueIn(&TestQ,&i);
assert(ret==0);
}
OS_SetEvent(OSTestTskID1,1<<6);
OS_WaitTicks(OSALMTest0, 50);
// check, if q is empty
t = OS_GetQueueSpace(&TestQ);
assert(t==64);
for (i=0;i<64;i++) // overload the Q
{
ret = OS_QueueIn(&TestQ,&i);
assert(ret==0);
}
// check, if none left
t = OS_GetQueueSpace(&TestQ);
assert(t==0);
// now one too much:
ret = OS_QueueIn(&TestQ,&i);
assert(ret==1);
break;
case 5:
// SetAlarm
// WaitAlarm
OS_SetAlarm(OSALMTest1,53);
OS_SetAlarm(OSALMTest2,44);
break;
case 6:
// Event with timeout
OS_SetEvent(OSTestTskID1,1<<3);
OS_WaitTicks(OSALMTest0, 50);
// do not set the second one...
break;
case 7:
//queue long test
ret = OS_QueueOut(&TestQLong,(uint8_t*)&ti); // Q empty
assert(ret == 1);
for (i=0;i<10;i++)
{
ret = OS_QueueIn(&TestQLong,(uint8_t*)&longQtest[i]);
assert (ret==0);
}
ret = OS_QueueIn(&TestQLong,(uint8_t*)&longQtest[9]);
assert (ret==1); // Q full
for (i=0;i<10;i++)
{
ret = OS_QueueOut(&TestQLong,(uint8_t*)&ti);
assert(ret==0);
assert(ti == longQtest[i]);
}
ret = OS_QueueOut(&TestQLong,(uint8_t*)&ti);
assert(ret==1); // q empty
break;
case 8:
// Nested Mutexes
OS_MutexGet(0);
WasteOfTime(1);
OS_MutexGet(1);
WasteOfTime(1);
OS_MutexRelease(0);
WasteOfTime(1);
OS_MutexRelease(1);
assert(1==1); // if it doesnt work, we do not come to here,
break;
case 9:
// test of OS_GetUnusedStack (uint8_t TaskID)
t=OS_GetUnusedStack(OSTestTskID0); // task0
assert(t>80 && t<150);
t=OS_GetUnusedStack(OSTestTskID1); // task1
assert(t>80 && t<150);
t=OS_GetUnusedStack(OSTestTskID2); // task2
assert(t>80 && t<150);
t=OS_GetUnusedStack(5); // idle
assert(t>2000 && t<2600);
break;
case 10:
OS_GetTicks(&ti);
OS_SetAlarm(OSALMTest0,39);
OS_SetAlarm(OSALMTest3,64);
OS_WaitAlarm(OSALMTest0);
OS_GetTicks(&tj);
assert(tj-ti == 39);
OS_WaitAlarm(OSALMTest3);
OS_GetTicks(&tj);
assert(tj-ti == 64);
break;
case 11:
assert(0); // just for proofing that it works!
break;
default:
// The END
while(1)
{
// sit here and wait for someone picking up the results out of "TestResults". 0 = OK.
// "TestProcessed" shows the number of processed assertions.
asm("break");
}
break;
}
OS_WaitTicks(OSALMTest0, 500); // wait for tests to be processed...
testcase++;
OS_SetEvent(OSTestTskID1,1<<7); // tick other tasks in sync.
OS_SetEvent(OSTestTskID2,1<<7);
}//while(1)
}
void TestTask2(void)
{
uint32_t ts,te;
uint32_t i,j;
uint16_t t;
while(1)
{
switch(testcase)
{
case 0:
// Task Priority
assert(testvar ==2);
testvar++;
break;
case 1:
// Timer / Wait
OS_WaitTicks(OSALMTest2, 2);
t=50;
OS_GetTicks(&i); // get time
OS_WaitTicks(OSALMTest2, t);
OS_GetTicks(&j); // get time
assert(j-i == t); // waited time was correct
break;
case 2:
// WaitEvent
// SetEvent
break;
case 3:
// MutexGet
// MutexRelease
// two cases:
// 1:mutex is occupied by higher prio
WasteOfTime(10);
OS_MutexGet(0);
testvar = 7;
WasteOfTime(50);
assert(testvar == 7);
OS_MutexRelease(0);
OS_SetEvent(OSTestTskID1,1<<1);
// 2:mutex is occupied
OS_MutexGet(0);
testvar = 4;
WasteOfTime(50);
assert(testvar == 4);
OS_MutexRelease(0);
break;
case 4:
// QueueIn
// QueueOut
break;
case 5:
// SetAlarm
// WaitAlarm
OS_GetTicks(&ts);
OS_WaitAlarm(OSALMTest2);
OS_GetTicks(&te);
assert(te-ts == 44);
break;
case 6:
// Event with timeout
break;
case 7:
// long queue
break;
default:
break;
}
OS_WaitEvent(1<<7);
}// while(1)
}
void TestTask1(void)
{
uint8_t ret,i;
uint32_t ts,te;
uint8_t v;
uint16_t t=25;
uint32_t ti,tj;
while(1)
{
switch(testcase)
{
case 0:
// Task Priority
assert(testvar ==1);
testvar++;
break;
case 1:
OS_WaitTicks(OSALMTest1, 1);
// Timer / Wait
OS_GetTicks(&ti); // get time
OS_WaitTicks(OSALMTest1, t);
OS_GetTicks(&tj); // get time
assert(tj-ti == t); // waited time was correct
break;
case 2:
// WaitEvent
// SetEvent
// wait for A, then A occurs
testvar=1;
OS_WaitEvent(1<<0);
testvar=2;
// A occurs, then wait for A
WasteOfTime(50);
assert(testvar==3);
OS_WaitEvent(1<<0);
testvar=4;
// wait for more events, one occurs, then the other
ret = OS_WaitEvent(1<<1 | 1<<2);
testvar=5;
assert(ret == (1<<2));
testvar=6;
ret = OS_WaitEvent(1<<1 | 1<<2);
testvar=7;
assert(ret == (1<<1));
testvar=8;
// wait for less events
break;
case 3:
// MutexGet
// MutexRelease
// two cases:
// 1:mutex is free
OS_MutexGet(0);
WasteOfTime(50);
testvar = 1;
OS_WaitTicks(OSALMTest1, 50);
assert(testvar == 1);
OS_MutexRelease(0);
OS_WaitEvent(1<<1);
// 2:mutex is occupied by lower prio
OS_WaitTicks(OSALMTest1, 50);
OS_MutexGet(0);
testvar = 3;
WasteOfTime(50);
assert(testvar == 3);
OS_MutexRelease(0);
break;
case 4:
// QueueIn
// QueueOut
TestQ.read = 13; // tweak the queue to a "used" state
TestQ.write =13;
ret = OS_QueueOut(&TestQ,&v); // q empty at start
assert(ret == 1);
OS_WaitEvent(1<<6);
for (i=0;i<20;i++) // forward
{
ret = OS_QueueOut(&TestQ,&v);
assert(v==i);
assert(ret ==0);
}
// Q empty
ret = OS_QueueOut(&TestQ,&v);
//assert(v==i);
assert(ret ==1);
OS_WaitEvent(1<<6);
for (i=20;i>0;i--) // backward
{
ret = OS_QueueOut(&TestQ,&v);
assert(v==i);
assert(ret ==0);
}
// Q empty
ret = OS_QueueOut(&TestQ,&v);
assert(ret ==1);
//OS_WaitEvent(1<<6);
break;
case 5:
// SetAlarm
// WaitAlarm
OS_GetTicks(&ts);
OS_WaitAlarm(OSALMTest1);
OS_GetTicks(&te);
assert(te-ts == 53);
break;
case 6:
// Event with timeout
ret = OS_WaitEventTimeout(1<<3,OSALMTest1, 30);
assert(ret == 1<<3); // no timeout, it was the event.
OS_GetTicks(&ti); // get time
ret = OS_WaitEventTimeout(1<<3,OSALMTest1, t); // wait for the event, which never comes...
assert(ret == 0); // timeout recoginzed
OS_GetTicks(&tj); // get time
assert(tj-ti == t); // waited time was correct
break;
case 7:
// long queue
break;
default:
break;
}
OS_WaitEvent(1<<7);
}// while(1)
}
uint32_t LED_GetLastFlightstateChanged(void)
{
return OS_GetTicks() - lastStateChangeTime;
}
// interfacing functions
void LED_SetFlightstate(FlightState_t fs) // includes remembering last state change time?
{
lastStateChangeTime = OS_GetTicks();
myFlightstate = fs;
}