void run() {
suspendCallerUntil(1*SECONDS);
protector.enter();
xprintf("hier %s\n", name);
xprintf("at %ld,%09ld: thread started ", SPLIT_SEC_NS(NOW()) ) ;
xprintf("- scheduleCounter= %ld\n", (long) Scheduler::getScheduleCounter());
protector.leave();
for(long i=0; i < max; i++){
protector.enter();
counter++;
protector.leave();
}
protector.enter();
xprintf("hier %s\n", name);
xprintf("at %ld,%09ld:", SPLIT_SEC_NS(NOW()));
xprintf("counter= %ld ", counter);
xprintf("- scheduleCounter= %ld\n", (long) Scheduler::getScheduleCounter());
protector.leave();
while(1) {
xprintf(" %x, = %s beendet semaphore daten: \n", (int)this, name);
//xprintf(" owenr %x, entercnt =%d\n", (int)protector.owner, (int)protector.ownerEnterCnt); // variables are private
suspendCallerUntil();
}
}
void run() {
suspendCallerUntil(3*SECONDS);
xprintf("\nPriority ceiler 122 times normal\n");
for (long long cnt = 0; cnt < 1220000LL; cnt++) {
if(cnt % 10000 == 0) {
xprintf("+"); FFLUSH();
}
}
xprintf("\nPriority ceiler 122 tmes prio ceiling\n");
PRIORITY_CEILING {
for (long long cnt = 0; cnt < 1220000LL; cnt++) {
if (cnt % 10000 == 0) {
xprintf("+"); FFLUSH();
}
}
}
xprintf("\nPriority ceiler 122 times normal\n");
for (long long cnt = 0; cnt < 1220000LL; cnt++) {
if(cnt % 10000 == 0) {
xprintf("+"); FFLUSH();
}
}
xprintf("\nPriority ceiler terminates\n");
while(1) {
suspendCallerUntil();
}
}
void Camera::run(){
cameraInitFinished = true;
while(1){
if (processData) {
processData = false;
uint8_t buffer[16];
PictureProperties p;
p.type = GRAY;
p.Height = HEIGHT;
p.Width = WIDTH;
if (sendPic) { // If picture was requested, send
state = sendingPicture;
if(!propertiesSent){
PRINTF("CAMERA_TX_START;%03d;%03d;%01d;PROPS;", p.Height, p.Width, p.type);
while (!TeleUART.isWriteFinished()) {
}
propertiesSent = true;
}
uint8_t linecount = 0;
int32_t length = 0;
for(int i = 0; i < IMAGESIZE; i ++){
PRINTF("%03u", DCMI_Buffer[i]);
while (!TeleUART.isWriteFinished()) {
}
suspendCallerUntil(NOW() + 100*MICROSECONDS);
}
suspendCallerUntil(NOW() + 1*MILLISECONDS);
TeleUART.write(";$\n", 4);
if(!continous){
sendPic = false;
}else{
takePicture();
}
state = regularMode;
}
suspendCallerUntil(NOW()+100*MILLISECONDS); // Could run even faster but 200ms is sufficient for mission mode
}
}
}
void RotationControl::run(){
IMU_DATA_RAW raw;
int cnt = 0;
while(1){
if(!isActive()) suspendCallerUntil(END_OF_TIME);
imuData.get(raw);
controlOut = PI(desSpeed, raw.ANGULAR_RAW_Z);
//
// err = desSpeed - (raw.ANGULAR_RAW_Z);
period = SECONDS_NOW() - lastTime;
//
// if(!(cnt % 100)) PRINTF("dps error: %f, des: %f, current: %f\n",err,desSpeed,raw.ANGULAR_RAW_Z);
//
// if((err > 0.1) || (err < -0.1)){
// i += (err * period);
// }
//
// dt = (err - lastError) / period;
//
// pPart = err * pGain;
// iPart = i * iGain;
// dPart = dt * dGain;
//
//
// controlOut = pPart + iPart + dPart;
//
// // control output deckeln
// // if(controlOut > 1000) controlOut = 1000;
// //Saturation filter
// if (controlOut > MAX) {
// controlOut = MAX;
// }
// else if (controlOut < MIN) {
// controlOut = MIN;
// }
//
// if(!(cnt % 100)) PRINTF("control output: %f, pPart %f, iPart %f, dPart %f\n",controlOut,pPart,iPart,dPart);
// if(!(cnt%100)) PRINTF("p: %f, i: %f, d: %f\n",pGain,iGain,dGain);
// cnt++;
motor.setspeed(controlOut);
lastTime = SECONDS_NOW();
// lastError = err;
suspendCallerUntil(NOW()+IMU_SAMPLERATE*MILLISECONDS);
}
}
void run() {
suspendCallerUntil(3*SECONDS);
int64_t aproxLoopsFor3Seconds = getSpeedKiloLoopsPerSecond() * 1500LL;
int64_t aproxLoopsForPrint = aproxLoopsFor3Seconds / 80;
xprintf("\nPriority ceiler 122 times normal\n");
for (int64_t cnt = 0; cnt < aproxLoopsFor3Seconds; cnt++) {
if(cnt % aproxLoopsForPrint == 0) {
xprintf("-"); FFLUSH();
}
}
xprintf("\nPriority ceiler 122 tmes prio ceiling\n");
PRIORITY_CEILING {
for (int64_t cnt = 0; cnt < aproxLoopsFor3Seconds; cnt++) {
if (cnt % aproxLoopsForPrint == 0) {
xprintf("-"); FFLUSH();
}
}
}
xprintf("\nPriority ceiler 122 times normal\n");
for (int64_t cnt = 0; cnt < aproxLoopsFor3Seconds; cnt++) {
if(cnt % aproxLoopsForPrint == 0) {
xprintf("-"); FFLUSH();
}
}
xprintf("\nPriority ceiler 122 tmes atomarLock\n");
globalAtomarLock();
for (int64_t cnt = 0; cnt < aproxLoopsFor3Seconds; cnt++) {
if (cnt % aproxLoopsForPrint == 0) {
xprintf("-"); FFLUSH();
}
}
globalAtomarUnlock();
xprintf("\nPriority ceiler 122 times normal\n");
for (int64_t cnt = 0; cnt < aproxLoopsFor3Seconds; cnt++) {
if(cnt % aproxLoopsForPrint == 0) {
xprintf("-"); FFLUSH();
}
}
xprintf("\nPriority ceiler terminates\n");
while(1) {
suspendCallerUntil();
}
}
void run() {
while(1) {
xprintf("*");
FFLUSH();
suspendCallerUntil();
}
}
void Camera::run() {
while (1) {
if (processData) {
processData = false; // Wait till the next frame (interrup) fires processing
DetectSatellite(); // Perform detection algorithm
if (sendPic) { // If picture was requested, send
tm.turnOff();
char tmpVal[4];
tmUart.write("CAMERA", 6);
for (int i = 0; i < IMAGESIZE; i += 2) {
sprintf(tmpVal, "%03u", DCMI_Buffer[i]);
tmUart.write(tmpVal, 4);
while (!tmUart.isWriteFinished()) {
}
}
tmUart.write("CAMEND", 6);
sendPic = false;
}
if (active) { // Continue captureing/processing if cam is still active
Capture();
}
suspendCallerUntil(NOW()+200*MILLISECONDS); // Could run even faster but 200ms is suficient for mission mode
}
}
}
void run() {
uint8_t rxBuf[6]={0};
uint8_t txBuf[3]={0};
int32_t err[10] = {0};
while (1) {
/** check out L3G4200D (extern) **/
#if L3G4200D_TEST
memset(rxBuf,0,sizeof(rxBuf));
memset(txBuf,0,sizeof(txBuf));
txBuf[0] = 0x28 | 0x80; // start reading with x-low register, read multiple register
err[0] = i2c1.writeRead(L3G4200D_GYR_ADDR,txBuf,1,rxBuf,6);
txBuf[0] = 0x0f; // WHO_AM_I Register
err[1] = i2c1.writeRead(L3G4200D_GYR_ADDR,txBuf,1,&rxBuf[6],1);
if (printError("L3G4200D GYRO", err, 2) > 0){
PRINTF("Init I2C and all slaves ...\n\n");
init(); // init i2c1/i2c2 and all slaves
} else {
PRINTF("L3G4200D \"Who am I\" reg (0xD3):0x%x\n",rxBuf[6]);
//PRINTF("L3G4200D GYRO x,y,z: %d, %d, %d\n",rxBuf[0]|(rxBuf[1]<<8), rxBuf[2]|(rxBuf[3]<<8), rxBuf[4]|(rxBuf[5]<<8)); // LSB first
PRINTF("L3G4200D GYRO x,y,z: %d, %d, %d\n\n",rxBuf[1]|(rxBuf[0]<<8), rxBuf[3]|(rxBuf[2]<<8), rxBuf[5]|(rxBuf[4]<<8)); // MSB first
}
#endif
/** check out LSM303 (extern) **/
#if LSM303_ACC_TEST
// ACC
memset(rxBuf,0,sizeof(rxBuf));
memset(txBuf,0,sizeof(txBuf));
txBuf[0] = 0x28 | 0x80; // start reading with x-low register, read multiple register
err[0] = i2c1.writeRead(LSM303DLH_ACC_ADDR,txBuf,1,rxBuf,6);
if (printError("LSM303 ACC", err, 1) > 0){
PRINTF("Init I2C and all slaves ...\n\n");
init(); // init i2c1/i2c2 and all slaves
}else{
//PRINTF("LSM303 ACC x,y,z: %d, %d, %d\n\n",rxBuf[0]|(rxBuf[1]<<8), rxBuf[2]|(rxBuf[3]<<8), rxBuf[4]|(rxBuf[5]<<8)); // LSB first
PRINTF("LSM303 ACC x,y,z: %d, %d, %d\n\n",rxBuf[1]|(rxBuf[0]<<8), rxBuf[3]|(rxBuf[2]<<8), rxBuf[5]|(rxBuf[4]<<8)); // MSB first
}
#endif
// MAG
#if LSM303_MAG_TEST
memset(rxBuf,0,sizeof(rxBuf));
memset(txBuf,0,sizeof(txBuf));
txBuf[0] = 0x03; // start reading with x-high register, read multiple register
err[0] = i2c1.writeRead(LSM303DLH_MAG_ADDR,txBuf,1,rxBuf,6);
if (printError("LSM303 MAG", err, 1) > 0){
PRINTF("Init I2C and all slaves ...\n\n");
init(); // init i2c1/i2c2 and all slaves
}else{
PRINTF("LSM303 MAG x,y,z: %d, %d, %d\n\n",rxBuf[1]|rxBuf[0]<<8,rxBuf[3]|rxBuf[2]<<8,rxBuf[5]|rxBuf[4]<<8); // MSB first
}
#endif
suspendCallerUntil(NOW()+1*SECONDS);
}
}
void run(){
int cnt=0;
while(1){
cnt++;
suspendCallerUntil(NOW() + 2*SECONDS);
PRINTF("After 2 Seconds : %3.9f %d\n", SECONDS_NOW(), cnt);
PRINTF("Timenow = %lld\n", NOW());
}
}
void run() {
//long long nextTime = NOW();
char* buffer = "Hallo Welt";
lcd.write(buffer,10,0);
suspendCallerUntil();
while (1) {
//nextTime += periode;
//suspendCallerUntil(nextTime);
}
}
void run () {
int cnt;
xprintf("receiver\n");
while(1) {
bool ok = fifo.get(cnt);
if (ok) {
PRINTF("reading %d\n", cnt);
} else {
suspendCallerUntil(NOW() + 1*SECONDS);
}
}
}
void ThCamera::run()
{
suspendCallerUntil(NOW() + 5*SECONDS);
cam.init();
while(1)
{
//Suspend until request received
suspendCallerUntil();
cam.takePicture();
#ifdef PROTOCOL_BINARY
char buff[8];
//Header for image packet
buff[0] = 0xAA;
buff[1] = 0xAA;
buff[2] = 0xAA;
buff[3] = 0x02;
SerializationUtil::WriteInt((int16_t)HEIGHT,buff,4);
SerializationUtil::WriteInt((int16_t)WIDTH,buff,6);
//Lock bluetooth
BT_Semaphore.enter();
uart_stdout.write(buff, 8);
#else
PRINTF("I");
#endif
writeToBT((char *)DCMI_Buffer, IMAGESIZE);
//Release bluetooth
BT_Semaphore.leave();
}
}
void run () {
int cnt = 0;
xprintf("sender\n");
while(1) {
cnt++;
bool ok = fifo.put(cnt);
if (ok) {
PRINTF("Sending %d\n", cnt);
} else {
PRINTF("Fifo full\n");
}
if ((cnt % 15) == 0) {
PRINTF("Wainting 3 seconds\n");
suspendCallerUntil(NOW() + 3*SECONDS);
}
}
}
void run() {
int k = 0;
AT(100*MILLISECONDS);
while (1) {
k++;
if (k>20) k=1;
for (int i=0;i<k;i++){
uart_stdout.write("X",1);
}
uint64_t startTime = NOW();
uart_stdout.suspendUntilWriteFinished();
uint64_t suspendTime = NOW()-startTime;
PRINTF("\nsuspend time: %d µs\n",(int)(suspendTime/1000));
suspendCallerUntil(NOW()+1000*MILLISECONDS);
}
}
void run() {
int i = 0;
PRINTF("PWM Test started!\nPress +/- to decrease/increase duty cycle\n");
while(1) {
int input = UART_DEBUG.getcharNoWait();
if (input != 0){
switch (input){
case '+':
i+=10;
if (i>512) i=512;
pwm3.write(i);
PRINTF("%d\n",i);
break;
case '-':
i-=10;
if (i<0) i=0;
pwm3.write(i);
PRINTF("%d\n",i);
break;
}
}
suspendCallerUntil(NOW() + 100*MILLISECONDS);
}
}
void run() {
//we prefer to have pointers so we create these:
FileSystem* fileSystem = &fs_;
File* file = &f_;
Directory* d = &d_;
xprintf("\n////////////////////////////\n"
"main > TESTING INIT AND MOUNT\n"
"//////////////////////\n");
fileSystem->enableDebugOutput(true);
//disk must be mounted before anything else can be done
fileSystem->mountDisk(0);
xprintf("main > mount done\n");
//format will create a new fat file system
//this can be omitted if there already is a fs on the disk
fileSystem->formatDisk(0);
xprintf("main > format done\n");
//setting time changes RODOS system time
fileSystem->setTime(2012, 03, 03, 07, 31, 12);
xprintf("\n////////////////////////////\n"
"main > TESTING DIRECTORY FUNTIONS\n"
"///////////////////////////\n");
file->enableDebugOutput(true);
file->open("/test.txt", file->O_WRITE);
file->putString("This is my first file!\n");
file->close();
xprintf("main > created '/test.txt'\n");
//this will not work because it does not create intermediate directories
xprintf("main > create dir '/FirstDir/ScndDir'\n");
fileSystem->createDirectory("/FirstDir/ScndDir");
bool isDir;
bool exists = fileSystem->fileExists("/FirstDir/ScndDir", &isDir);
if (!exists) {
xprintf("main > directory '/FirstDir/ScndDir' does not exist -> correct\n");
} else {
xprintf("main > directory '/FirstDir/ScndDir' exists -> NOT CORRECT\n");
}
//if we create directories one after the other everything will work fine
xprintf("main > create dir '/FirstDir'\n");
fileSystem->createDirectory("/FirstDir");
xprintf("main > create dir '/FirstDir/ScndDir'\n");
fileSystem->createDirectory("/FirstDir/ScndDir");
exists = fileSystem->fileExists("/FirstDir/ScndDir", &isDir);
if (!exists) {
xprintf("main > directory '/FirstDir/ScndDir' does not exist -> NOT CORRECT\n");
} else {
xprintf("main > directory '/FirstDir/ScndDir' exists -> correct\n");
}
//fill directory with files
fileSystem->move("/test.txt", "/FirstDir/test.txt");
file->open("FirstDir/file2.txt", file->O_WRITE);
file->putString("This is file2, dude.");
file->close();
file->open("FirstDir/file3.txt", file->O_WRITE);
file->putString("This is file3, bro.");
file->close();
//rename entire directory
fileSystem->move("FirstDir", "MyDir");
//open and interate through directory
d->open("/MyDir");
d->resetEntryEnumeration();
xprintf("main > reset entry numeration on '/MyDir/'\n");
char nextFileName[100];
long size;
d->readNextEntry(nextFileName, 100, &size);
while (nextFileName[0] != '\0') {
xprintf("main > next entry in '/MyDir/' is %s\nsize: %ld\n", nextFileName, size);
char path[200];
path[0] = 0;
xsprintf(path, "/MyDir/%s", nextFileName);
exists = fileSystem->fileExists(path, &isDir);
//read content if file
if (isDir == false) {
file->enableDebugOutput(true);
file->open(path, file->O_READ);
char readStr[200];
readStr[0] = 0;
file->getString(readStr, 200);
xprintf("main > read file > %s\n%s\n", path, readStr);
file->close();
}
nextFileName[0] = 0;
d->readNextEntry(nextFileName, 100, &size);
}
//directories don't need to be closed after use. you can just free them
//this will remove the disk
fileSystem->unmountDisk(0);
while(1) {
suspendCallerUntil();
}
}
void run() {
uint32_t pageIdx = 1;
xprintf("**************** init SPI interface ****************\n");
#define SPI_BR 10000000
int retVal = spi1.init(SPI_BR);
if (retVal >= 0){
xprintf("SPI interface successfully initialized with baudrate: %d Hz\n",SPI_BR);
}else{
xprintf("SPI interface init failed with error: %d\n",retVal);
}
xprintf("\n\n");
xprintf("**************** init SPI flash ****************\n");
if (spiFlash1.init() != 0) {
xprintf("flash init failed! break ... \n");
suspendCallerUntil();
}else{
xprintf("SPI flash successfully initialized\n");
}
xprintf("\n\n");
xprintf("**************** read chip IDs ****************\n");
xprintf("manufacturer ID (expected: 0x1F): %x\n",
spiFlash1.getManufacturerID());
xprintf("device ID (expected: 0x2701): %x\n",
spiFlash1.getDeviceID());
xprintf("\n\n");
xprintf("**************** page erase ****************\n");
uint32_t errCnt = 0;
if (spiFlash1.erasePage(pageIdx) == 0) {
memset(tmpBuffer, 0, sizeof(tmpBuffer));
spiFlash1.readPage(pageIdx, tmpBuffer, sizeof(tmpBuffer));
for (uint32_t i; i < SPIFlash_AT45DBxxx::PAGE_TOTAL_SIZE; i++) {
if (tmpBuffer[i] != 0xff)
errCnt++;
}
xprintf("page %d erased with %d errors\n", pageIdx, errCnt);
} else {
xprintf("error in erase command\n", pageIdx);
}
xprintf("\n\n");
xprintf("**************** page write ****************\n");
int32_t wrCnt = spiFlash1.writePage(pageIdx, testBuffer,
sizeof(testBuffer));
xprintf("%d bytes of testBuffer written to page %d:\n", wrCnt, pageIdx);
xprintf("size of testBuffer: %d\n", sizeof(testBuffer));
xprintf("\n\n");
xprintf("**************** page read ****************\n");
memset(tmpBuffer, 0, sizeof(tmpBuffer));
uint32_t rdCnt = spiFlash1.readPage(pageIdx, tmpBuffer, sizeof(tmpBuffer));
xprintf("%d bytes of page %d read.\n", rdCnt, pageIdx);
for (uint32_t i; i < rdCnt; i++) {
if (tmpBuffer[i] != i)
errCnt++;
}
xprintf("page %d read with %d errors\n", pageIdx, errCnt);
xprintf("\n\n");
xprintf("********* page write/read offset *********\n");
errCnt = 0;
uint8_t wrVal = 0;
uint8_t rdVal = 0;
if ( (spiFlash1.erasePage(pageIdx) == Flash::Success) && (spiFlash1.blankCheck(pageIdx) == Flash::Success) ) {
for(uint32_t i = 0;i<spiFlash1.getPageSize(pageIdx);i++){
rdVal = 0;
spiFlash1.writePageOffset(pageIdx, i, &wrVal, sizeof(wrVal));
spiFlash1.readPageOffset(pageIdx, i, &rdVal, sizeof(rdVal));
if(rdVal != wrVal){
errCnt++;
//PRINTF("compare error in offset test\n")
}
wrVal++;
if( wrVal == 0xFF) wrVal++; // bad test values
if (wrVal == 0) wrVal++; // bad test values
}
xprintf("page %d read with %d errors\n", pageIdx, errCnt);
xprintf("\n\n");
}else{
xprintf("page %d erase failed\n", pageIdx);
}
xprintf("**************** page erase ****************\n");
errCnt = 0;
if (spiFlash1.erasePage(pageIdx) == 0) {
memset(tmpBuffer, 0, sizeof(tmpBuffer));
spiFlash1.readPage(pageIdx, tmpBuffer, sizeof(tmpBuffer));
for (uint32_t i; i < SPIFlash_AT45DBxxx::PAGE_TOTAL_SIZE; i++) {
if (tmpBuffer[i] != 0xff)
errCnt++;
}
xprintf("page %d erased with %d errors\n", pageIdx, errCnt);
} else {
xprintf("error in erase command\n");
}
xprintf("\n\n");
suspendCallerUntil();
}
