File content as of revision 22:167dacfe0b14:

#include "Printing.h"
#include <iostream>

namespace Printing {

#ifdef PRINTINGOFF
void printingloop(void const*){Thread::wait(osWaitForever);}
bool registerID(char, size_t){return true;}
bool unregisterID(char) {return true;}
bool updateval(char, float*, size_t){return true;}
bool updateval(char id, float value){return true;}
#else

using namespace std;

size_t idlist[NUMIDS] = {0}; // Stores length of buffer 0 => unassigned
float* buffarr[NUMIDS] = {0};
volatile unsigned int newdataflags = 0;

bool registerID(char id, size_t length) {   
    if (id < NUMIDS && !idlist[id]) {//check if the id is already taken
        idlist[id] = length;
        buffarr[id] = new float[length];
        return true;
    } else
        return false;
}
bool unregisterID(char id) {
    if (id < NUMIDS) {
        idlist[id] = 0;
        if (buffarr[id])
            delete buffarr[id];
        return true;
    } else
        return false;
}

bool updateval(char id, float* buffer, size_t length) {
    //check if the id is registered, and has buffer of correct length
    if (id < NUMIDS && idlist[id] == length && buffarr[id] && !(newdataflags & (1<<id))) {
        for (size_t i = 0; i < length; i++)
            buffarr[id][i] = buffer[i];
        newdataflags |= (1<<id);
        return true;
    } else
        return false;
}

bool updateval(char id, float value){
    //check if the id is registered, and the old value has been written
    if (id < NUMIDS && idlist[id] == 1 && buffarr[id] && !(newdataflags & (1<<id))) {
        buffarr[id][0] = value;
        newdataflags |= (1<<id);
        return true;
    } else
        return false;
}

void printingloop(void const*){

    Serial pc(USBTX, USBRX);
    pc.baud(115200);
    
    char sync[] = "ABCD";
    cout.write(sync, 4);
    cout << std::endl;
    
    while(true){
        // Send number of packets
        char numtosend = 0;
        for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        
        cout.put(numtosend);

        // Send packets
        for (char id = 0; id < NUMIDS; id++) {
            if (newdataflags & (1<<id)) {
                cout.put(id);
                cout.write((char*)buffarr[id], idlist[id] * sizeof(float));
                newdataflags &= ~(1<<id);
            }
        }
        cout << endl;
        Thread::wait(200);
    }
}


#endif //end PRINTINGOFF

} //end namespace