File content as of revision 11:c24adeda1b8c:

// Emile Arseneault : arse1503
// Raphael Drouin   : dror2204

// Router Node

#include "mbed.h"
#define FRAMEDELIMITER 0x7E
#define RADTODEG 180/3.14159265358979
#include "rtos.h"
#include "xbee.h"

Mail<char, 250> mymail;
Mail<char[254], 25> receivedFrameResponses;
XBee xbee(p8, p13, p14, &mymail, NULL, NULL);

typedef struct {
  char         messageId; //Frame Id of message and waiting signal
  osThreadId   threadId;  //Thread pointer to wake thread
} waiting_thread_t;
 
Mail<waiting_thread_t, 254> waitingQueue;

Serial pc(USBTX, USBRX);
DigitalOut myled(LED1);
DigitalIn bouton(p30);
I2C accel(p28, p27);

Thread *accelerometre_thread;
Thread *bouton_thread;
//Thread *thread_waker;

void ReadSerial()
{
    // 00 13 A2 00 
    // 40 3E 09 63
    xbee.SendATCommand('S', 'H');
    xbee.SendATCommand('S', 'L');
}

void WaitOnId(char id)
{
    // Envoyer une requete d'attente
    waiting_thread_t *waitTicket = waitingQueue.alloc();
    waitTicket->messageId = id;
    waitTicket->threadId = osThreadGetId(); 
    waitingQueue.put(waitTicket);
    
    // Attendre
    osSignalWait(id, osWaitForever);
}

void InitialisePANID(char PANIDMSB, char PANIDLSB)
{
    char PanID[2] = {PANIDMSB, PANIDLSB};
    char idwait = 0;
    
    idwait = xbee.SendATCommand('I', 'D', &PanID[0], 2);
    //WaitOnId(idwait);
    idwait = xbee.SendATCommand('W', 'R');
    //WaitOnId(idwait);
    idwait = xbee.SendATCommand('A', 'C');
    //WaitOnId(idwait);
}

void xbee_reader()
{
    while(1)
    {
        xbee.InterpretMessage();
        wait(0.001);   
    }
}

void console_printer()
{
    while (true) {
        osEvent evt = mymail.get();
        if (evt.status == osEventMail) 
        {
            char *c = (char*)evt.value.p;
            pc.putc(*c);
            mymail.free(c);
        }
    }
}

void AccelerometreSetup(int addr_accel)
{
    char CTRL_REG1[2] = {0x2A, 0};
    char XYZ_DATA_CFG[2] = {0x0E, 0};
    
    // Read CTRL_REG1 to set it back with bit 1 = 0 to set in stanby mode
    // Register CTRL_REG1 : 0x2A
    accel.write(addr_accel, CTRL_REG1, 1, true);
    accel.read(addr_accel, &CTRL_REG1[1], 1);
    CTRL_REG1[1] = CTRL_REG1[1] & 0b11111110;
    accel.write(addr_accel, CTRL_REG1, 2);
    
    // Set in +-2g with FS bits
    // Register XYZ_DATA_CFG : 0x0E
    int FS_BITS_mask = 0b11111100;
    accel.write(addr_accel, XYZ_DATA_CFG, 1, true);
    accel.read(addr_accel, &XYZ_DATA_CFG[1], 1);
    XYZ_DATA_CFG[1] = XYZ_DATA_CFG[1] & FS_BITS_mask;
    accel.write(addr_accel, XYZ_DATA_CFG, 2);
    
    // Read CTRL_REG1 to set it back with bit 1 to set in active mode
    // Register CTRL_REG1 : 0x2A
    accel.write(addr_accel, CTRL_REG1, 1, true);
    accel.read(addr_accel, &CTRL_REG1[1], 1);
    CTRL_REG1[1] = CTRL_REG1[1] | 1;
    accel.write(addr_accel, CTRL_REG1, 2);
}

void accelerometre_reader()
{
    int addr_accel = 0x1D<<1;
    char OUT[7] = {0x01, 0, 0, 0, 0, 0, 0};
        
    int Z;
    int Angle;
    char OUT_Z_MSB;
    char OUT_Z_LSB;
    
    char dataOut[3];
    
    while(1)
    {
        Thread::signal_wait(1, osWaitForever);
        wait(0.2); // Pour décaller du boutton
        
        accel.write(addr_accel, OUT, 1, true);
        accel.read(addr_accel, &OUT[1], 6);
        
        OUT_Z_MSB = OUT[5];
        OUT_Z_LSB = OUT[6];
        
        Z = ((int16_t)(OUT_Z_MSB << 8) | OUT_Z_LSB) >> 4;
        Angle = (int)(acos(abs(Z)/1024.0)*RADTODEG*100);
            
        // Mettre data en forme
        dataOut[0] = 1;
        dataOut[1] = Angle >> 8;
        dataOut[2] = Angle;
        
        // Envoyer les données
        xbee.ZigBeeTransmit(0x0000, 0x00000000, 0x00000000, &dataOut[0], 3);
    }
}

void bouton_reader()
{
    char dataOut[2];
    
    while(1)
    {
        Thread::signal_wait(1, osWaitForever);
        
        // Mettre data en forme
        dataOut[0] = 2;
        dataOut[1] = (bouton == 1);
        
        // Envoyer les données
        xbee.ZigBeeTransmit(0x0000, 0x00000000, 0x00000000, &dataOut[0], 2);
    }
}

void tick()
{
    myled = !myled;
    
    // Mettre tableau de fonctions ici
    (*accelerometre_thread).signal_set(1);
    (*bouton_thread).signal_set(1);
}

void wake_threads()
{
    // Essaie de thread permettant à un autre thread d'attendre la réception d'un Frame ID
    int smallWait = 5;
    osThreadId inWait[254] = {0};

    while (true) {
        osEvent evt = waitingQueue.get(smallWait);
        if (evt.status == osEventMail) {
            waiting_thread_t *waitingThread = (waiting_thread_t*)evt.value.p;
            printf("Frame Id : %d\n\r", waitingThread->messageId);
            printf("Thread Id : %d\n\r", waitingThread->threadId);
            inWait[waitingThread->messageId] = waitingThread->threadId;
            waitingQueue.free(waitingThread);
        }
        osEvent evt2 = receivedFrameResponses.get(smallWait);
        if (evt2.status == osEventMail) {
            char *receivedResponse = (char*)evt2.value.p;
            printf("Frame Id : %d\n\r", receivedResponse[4]);
            if (inWait[receivedResponse[4]] != 0)
            {
                osSignalSet(inWait[receivedResponse[4]], receivedResponse[4]);
                inWait[receivedResponse[4]] = 0;
            }
            receivedFrameResponses.free((char(*)[254])receivedResponse);
        }
    }
}

int main() 
{
    int addr_accel = 0x1D<<1;
    
    Ticker ticker;
    
    pc.printf("\r\nI am router\r\n");
    
    AccelerometreSetup(addr_accel);
    pc.printf("accelerometre initialiser");
    wait(2);
    Thread thread(xbee_reader);
    Thread thread_printer(console_printer);
    //Thread waking_threads(wake_threads);
    
    InitialisePANID(0x42, 0x69);
    
    Thread thread_accelerometre(accelerometre_reader);
    Thread thread_bouton(bouton_reader);
    
    accelerometre_thread = &thread_accelerometre;
    bouton_thread = &thread_bouton;
    //thread_waker = &waking_threads;

    ReadSerial();
    wait(1);
    xbee.SendATCommand('M', 'Y');
    xbee.SendATCommand('O', 'P');
    wait(1);
    ticker.attach(&tick, 3);
    
    
    while(1) {
        
    }
}