Printing.cpp

00001 #include "Printing.h"
00002 #include <iostream>
00003 
00004 namespace Printing {
00005 
00006 #ifdef PRINTINGOFF
00007 void printingloop(void const*){Thread::wait(osWaitForever);}
00008 bool registerID(char, size_t){return true;}
00009 bool unregisterID(char) {return true;}
00010 bool updateval(char, float*, size_t){return true;}
00011 bool updateval(char id, float value){return true;}
00012 #else
00013 
00014 using namespace std;
00015 
00016 size_t idlist[NUMIDS] = {0}; // Stores length of buffer 0 => unassigned
00017 float* buffarr[NUMIDS] = {0};
00018 volatile unsigned int newdataflags = 0;
00019 
00020 bool registerID(char id, size_t length) {   
00021     if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
00022         idlist[id] = length;
00023         buffarr[id] = new float[length];
00024         return true;
00025     } else
00026         return false;
00027 }
00028 bool unregisterID(char id) {
00029     if (id < NUMIDS) {
00030         idlist[id] = 0;
00031         if (buffarr[id])
00032             delete buffarr[id];
00033         return true;
00034     } else
00035         return false;
00036 }
00037 
00038 bool updateval(char id, float* buffer, size_t length) {
00039     //check if the id is registered, and has buffer of correct length
00040     if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) {
00041         for (size_t i = 0; i < length; i++)
00042             buffarr[id][i] = buffer[i];
00043         newdataflags |= (1<<id);
00044         return true;
00045     } else
00046         return false;
00047 }
00048 
00049 bool updateval(char id, float value){
00050     //check if the id is registered, and the old value has been written
00051     if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) {
00052         buffarr[id][0] = value;
00053         newdataflags |= (1<<id);
00054         return true;
00055     } else
00056         return false;
00057 }
00058 
00059 void printingloop(void const*){
00060 
00061     Serial pc(USBTX, USBRX);
00062     pc.baud(115200);
00063     
00064     while(true){
00065     
00066         //send sync symbol
00067         char sync[] = "ABCD";
00068         cout.write(sync, 4);
00069         cout << std::endl;
00070     
00071         // Send number of packets
00072         char numtosend = 0;
00073         for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        
00074         cout.put(numtosend);
00075 
00076         // Send packets
00077         for (char id = 0; id < NUMIDS; id++) {
00078             if (newdataflags & (1<<id)) {
00079                 cout.put(id);
00080                 cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
00081                 newdataflags &= ~(1<<id);
00082             }
00083         }
00084         cout << endl;
00085         Thread::wait(200);
00086     }
00087 }
00088 
00089 
00090 #endif //end PRINTINGOFF
00091 
00092 } //end namespace
00093