File content as of revision 17:ca53e6d36163:

/* BRIEF:
 * Connects to wifi and listen on ip given by dhcp on port 2000
 *
 * Commands are send in for #X where X is from {S,E,T,R}, all commands are
 * confirmed by mbed by sending #A
 *
 * Use #S to start saving data, #E to stop saving. This can be done repeatedly,
 * mbed saves the data into separated files and generates new swimmer id
 * At the end, use #T to start data transfer, the format of transmitted data is:
 *
 * Header for each swimmer:
 * (int) -1 (int) -1 (int) swimmer_id
 * Data itself
 * (char) 0xAA (int) count 3*(short) acc 3*(float) gyro
 * End of swimmer block
 * (int) -1 (int) -1  
 *
 * where count is number starting on 0, incremented on every packet
 * 0xAA is a sync byte used to avoid confusion when some bytes are lost during
 * transmission (UART on faster reates seems to be unstable)
 * 
 * CHANGELOG 22/10/2015
 * - Refactored code
 * - Now using USB flash instead of SD card 
 * - SIZE of packets to be saved at once changed to 32 (from 1000, saving took too much time, effectively loosing measurements)
 * - Using fwrite to write 32 packets at once (packet size = 16 bytes -> 32*16 = 512 bytes -> USB flash sector size is 512 bytes, thus maximizing fwrite effectivity
 * - Saving of 32 packets takes about 3,5 ms
 * KNOWN ISSUES:
 * - Opening USB disk takes some time, couple first measurements not valid (perhaps open next file right when saving stops ?)
 */

// Includes ====================================================================

#include "mbed.h"
#include "PowerControl.h"
#include "EthernetPowerControl.h"
#include "L3GD20_init.h"
#include "math.h"
#include "stdint.h"
#include "DirHandle.h"
#include "MSCFileSystem.h"
#include "L3GD20.h"
#include "adc.h"
#include "utils.h"
#include "wifi.h"
#include <stdio.h>

// Defines =====================================================================

/* credentials for wifi */
#define SSID                "BRNET"
#define PASS                "Fuhu27che9"
#define SECURITY            WPA

/* ADC & data acquisition related stuff */
#define SAMPLE_RATE         700000                                              // Sample rate for ADC conversion
#define PERIOD              0.01                                                // Setting the period of measuring, 0.01 menas 100 times per second
#define SIZE                32                                                  // Setting number of instance in array - for saving to SD card
#define BUFFER_SIZE        (sizeof(int) + sizeof(short)*6)                      //size of one data block

/* mass storage disk name */
#define FSNAME "usb"

/* Commands mbed understands */
#define SAVING_START        'S'
#define SAVING_STOP         'E'
#define TRANSFER_START      'T'
#define CHECK_READY         'R'

/* Function like macros for leds (bleh) */
#define leds_off() { led_measuring = led_sending = led_working = led_saving = 0; }
#define leds_error() { led_measuring = led_sending = led_working = led_saving = 1; }

// Watchdog class ==============================================================

class Watchdog {
public:
    /* Kick the watchdog with time period setting */
    void kick(float s) {
        LPC_WDT->WDCLKSEL = 0x1;                                                // Set CLK src to PCLK
        uint32_t clk = SystemCoreClock / 16;                                    // WD has a fixed /4 prescaler, PCLK default is /4
        LPC_WDT->WDTC = s * (float)clk;
        LPC_WDT->WDMOD = 0x3;                                                   // Enabled and Reset
        kick();
    }

    /* Kick the watchdog */
    void kick() {
        LPC_WDT->WDFEED = 0xAA;
        LPC_WDT->WDFEED = 0x55;
    }
};

// Objects & vars ==============================================================

Watchdog w;                                                                     // Initialize watchdogs
Ticker Saving_Ticker;                                                           // Tickers for interrupts on measuring
ADC adc(SAMPLE_RATE, 1);                                                        // Initialize ADC to maximum SAMPLE_RATE and cclk divide set to 1   originally MMA7361L
Wifi wifi(p13, p14, p11, p12, p23);                                             // Wifly module
I2C i2c(p28, p27);                                                              // NEW function GYRO connection originally L3G4200D

/* LED indicators */
DigitalOut led_measuring(LED1);
DigitalOut led_sending(LED2);
DigitalOut led_working(LED3);
DigitalOut led_saving(LED4);

// Added for test purposes
DigitalOut flagSetNotify(p9);
DigitalOut measureNotify(p10);

/* USB & saving related stuffs */
FILE *gfp;                                                                      // Global file pointer
MSCFileSystem logger(FSNAME);                                                   // Initialize USB flash disk
char fname[40];                                                                 // Variable for name of Text file saved on USB flash disk
static volatile uint64_t absolute = 0;                                          // Variable for numbering saved data
volatile int relative = 0;
volatile short swimmer_id = 0;
volatile short last_send = 0;
volatile bool measuringFlag = 0;                                                // Flag that is set with interrupt

/* Accelerometer buffers */
short int acc_x[SIZE];
short int acc_y[SIZE];
short int acc_z[SIZE];

/* Gyroscope buffers */
short int gyro_x[SIZE];
short int gyro_y[SIZE];
short int gyro_z[SIZE];

// Functions -> I2C ============================================================

/* Reads byte from given address */
char readByte(char address, char reg)
{
    char result;                                                                // Buffer for returning byte
    i2c.start();                                                                // Start i2c transfer
    i2c.write(address);                                                         // Slave address with direction=write
    i2c.write(reg);                                                             // Register address
    i2c.start();                                                                // Break transmission to change bus direction
    i2c.write(address + 1);                                                     // Slave address with direction=read [bit0=1]
    result = i2c.read(0);                                                       // Byte we want to read
    i2c.stop();                                                                 // Stop i2c operation
    return (result);                                                            // Return byte
}

/* Sends 1 byte to an I2C address */
void writeByte(char address, char reg,char value)
{
    i2c.start();                                                                // Start i2c transfer
    i2c.write(address);                                                         // Slave address
    i2c.write(reg);                                                             // Register address
    i2c.write(value);                                                           // Value to be written
    i2c.stop();                                                                 // Stop i2c operation
}

// Functions -> Init ===========================================================

/* Prepares board for operation (adc's, gyroscope etc.) */
void boardInit (void)
{
    /* L3GD20 Gyroscope init */
    writeByte(L3GD20_ADDR,gCTRL_REG1,0x0F);                                     // Set ODR to 95Hz, BW to 12.5Hz, enable axes and turn on device
    writeByte(L3GD20_ADDR,gCTRL_REG4,0x10);                                     // Full scale selected at 500dps (degrees per second)
    wait(1);                                                                    // Wait for settings to stabilize

    /* Accelerometer init */
    adc.setup(p18,1);                                                           // Set up ADC on pin 18
    adc.setup(p19,1);                                                           // Set up ADC on pin 19
    adc.setup(p20,1);                                                           // Set up ADC on pin 20

    /* Rest of the init */
    PHY_PowerDown();                                                            // Disable ETH PHY to conserve power (about 40mA)
    leds_off();                                                                 // Turn off all leds
}

// Functions -> Measuring ======================================================

/* Main measuring function */
void measuring(void)
{
    /* Vars */
    char xL, xH, yL, yH, zL, zH;                                                // Gyro high/low bytes
    char buffer[SIZE][BUFFER_SIZE];                                             // Buffer
    int pos = 0;                                                                // Position

    /* Read ACC_X */
    adc.start();                                                                // Starting ADC conversion
    adc.select(p18);                                                            // Measure pin 18
    while(!adc.done(p18));                                                      // Wait for it to complete
    acc_x[relative] = (short) adc.read(p18);                                    // Read ACC_X value
    /* Read ACC_Y */
    adc.start();                                                                // Again
    adc.select(p19);                                                            // Measure pin 19
    while(!adc.done(p19));                                                      // Wait for it to complete
    acc_y[relative] = (short) adc.read(p19);                                    // Read ACC_Y value
    /* Read ACC_Z */
    adc.start();                                                                // Again
    adc.select(p20);                                                            // Measure pin 20
    while(!adc.done(p20));                                                      // Wait for it to complete
    acc_z[relative] = (short) adc.read(p20);                                    // Read ACC_Z value

    /* Read Gyro_X */
    xL=readByte(L3GD20_ADDR,gOUT_X_L);                                          // Read GYRO_X Low byte
    xH=readByte(L3GD20_ADDR,gOUT_X_H);                                          // Read GYRO_X High byte
    /* Read Gyro_X */
    yL=readByte(L3GD20_ADDR,gOUT_Y_L);                                          // Read GYRO_Y Low byte
    yH=readByte(L3GD20_ADDR,gOUT_Y_H);                                          // Read GYRO_Y High byte
    /* Read Gyro_X */
    zL=readByte(L3GD20_ADDR,gOUT_Z_L);                                          // Read GYRO_Z Low byte
    zH=readByte(L3GD20_ADDR,gOUT_Z_H);                                          // Read GYRO_Z High byte
    /* Add Gyroscope values to global buffer */
    gyro_x[relative] = (xH<<8) | (xL);                                          // 16-bit 2's complement GYRO_X
    gyro_y[relative] = (yH<<8) | (yL);                                          // 16-bit 2's complement GYRO_Y
    gyro_z[relative] = (zH<<8) | (zL);                                          // 16-bit 2's complement GYRO_Z

    /* Increment counters */
    absolute++;                                                                 // Increment absolute counter
    relative++;                                                                 // Increment relative counter

    /* After our buffers are full save the data */
    if ( absolute % SIZE == 0 )
    {
        /* Flip led_saving state */
        led_saving = !led_saving;

        /* Handle file error */
        if (gfp == NULL) {
            printf("\r\nUnable to append data to file %s", fname);              // Notify user
            return;                                                             // Return
        }

        /* Iterate all buffer elements */
        for (int k = 0; k < SIZE; k++)
        {
            pos = absolute-SIZE+k;                                              // Increment position
            toBytes(buffer[k], &pos, sizeof(int));                              // Append data to buffer in byte form
            toBytes(buffer[k]+sizeof(int), &acc_x[k], sizeof(short));
            toBytes(buffer[k]+sizeof(int) + sizeof(short), &acc_y[k], sizeof(short));
            toBytes(buffer[k]+sizeof(int) + sizeof(short)*2, &acc_z[k], sizeof(short));
            toBytes(buffer[k]+sizeof(int) + sizeof(short)*3, &gyro_x[k], sizeof(short));
            toBytes(buffer[k]+sizeof(int) + sizeof(short)*4, &gyro_y[k], sizeof(short));
            toBytes(buffer[k]+sizeof(int) + sizeof(short)*5, &gyro_z[k], sizeof(short));                              
        }
        
        //measureNotify = 1;                                                    // Used for saving debug
        /* Write 32 buffer lines (16 bytes) to file (512 bytes total -> one block) */
        fwrite(buffer, SIZE, BUFFER_SIZE, gfp);                                 
        //measureNotify = 0;                                                    // Used for saving debug
    
        /* Reset relative counter */
        relative = 0;
    }
}

/* Sets the measuring flag */
void setMeasuringFlag(void)
{
    //flagSetNotify = 1;                                                        // Used for saving debug
    measuringFlag = 1;                                                          // Set measuring flag
    //flagSetNotify = 0;                                                        // Used for saving debug
}

/* Sees whether giver file exists on flash disk */
bool doesFileExist(const char *filename)
{
    /* Dir related structs & vars */
    DIR *dir;                                                                   // Prepare directory handle
    struct dirent *file;                                                        // Directory entry structure
    dir = opendir("/usb");                                                      // Open root of usb flash disk                                                      
    
    /* Iterate all available files in root directory */
    while((file = readdir(dir)) != NULL)                                        // Incrementally points to next files, until NULL is reached
    {
        /* Check if it's file we are looking for */
        if(strcmp(filename+5, file->d_name) == 0)                               // +5 moves us beyond /usb/, not cool I know...
        {
            closedir(dir);                                                      // Close dir & return true
            return true;   
        }
    }
    
    /* If file is not present */
    closedir(dir);                                                              // Close dir & return false
    return false;                                                               // If not, return false
} 

// Functions -> Commands =======================================================

/* Starts to acquire data from sensors */
void startSaving(void)
{
    /* Open global file with current swimmer ID */
    gfp = fopen(fname, "wb");                                                   // Try to open that file
    if(gfp == NULL) {
        leds_error();                                                           // Turn on all leds
        printf("\r\nUnable to open file %s for writing", fname);                // Notify user
        return;                                                                 // Exit function
    }

    /* Prepare file & vars for data acquisition */
    fprintf(gfp, "%c%c%c", -1, -1, swimmer_id);                                 // Append header to current swimmer output file

    /* Reset counters */
    absolute = 0;                                                               // Reset absolute counter
    relative = 0;                                                               // Reset relative counter

    /* Set timer */
    Saving_Ticker.attach(&setMeasuringFlag, PERIOD);                            // Attach function that periodically sets the flag
}

/* Sends acquired data to USB flash */
void stopSaving(void)
{
    /* Stop saving */
    Saving_Ticker.detach();                                                     // Stop setting measuring flag

    /* Try to append data left in buffers */
    if(gfp == NULL) {
        leds_error();                                                           // Turn on all leds
        printf("\r\nUnable to open file %s before sending data", fname);        // Notify user
        return;                                                                 // Exit function
    }

    /* Append data left to buffer */
    int base = ((absolute - (absolute%SIZE)) < 0) ? 0 : absolute - (absolute%SIZE);
    char buffer[relative][BUFFER_SIZE];                                         // packet array
    
    /* Iterate all array elements */
    for (int k = 0; k < relative; k++) {
        int pos = base+k;
        toBytes(buffer[k], &pos, sizeof(int));
        toBytes(buffer[k]+sizeof(int), &acc_x[k], sizeof(short));
        toBytes(buffer[k]+sizeof(int) + sizeof(short), &acc_y[k], sizeof(short));
        toBytes(buffer[k]+sizeof(int) + sizeof(short)*2, &acc_z[k], sizeof(short));
        toBytes(buffer[k]+sizeof(int) + sizeof(short)*3, &gyro_x[k], sizeof(short));
        toBytes(buffer[k]+sizeof(int) + sizeof(short)*4, &gyro_y[k], sizeof(short));
        toBytes(buffer[k]+sizeof(int) + sizeof(short)*5, &gyro_z[k], sizeof(short));
    }
    
    /* Write what's left */
    fwrite(buffer, relative, BUFFER_SIZE, gfp);                                 // Write all data at once

    /* Append EOF and close file */
    fprintf(gfp, "%c%c", -1,-1);                                                // Write EOF characters
    fclose(gfp);                                                                // Close file

    /* Prepare for next swimmer */
    swimmer_id++;                                                               // Increment swimmer ID
    sprintf(fname, "/usb/swimmer%d.txt", swimmer_id);                           // Update current file name
    remove(fname);                                                              // If file with such ID exists remove it to avoid reading old data
}

/* Sends acquired data via Wi-Fi */
void transfer(void)
{
     /* Vars */
    char name[30];                                                              // File name buffer  
    
    /* File transfer prerequisites */
    fclose(gfp);                                                                // Close curretly opened file
    int requested_id = wifi.readByte();                                         // Get byte containing requested file ID
    sprintf(name, "/usb/swimmer%d.txt", requested_id);                          // Create file name based on current swimmer ID
    
    /* Handle transfer */
    if(doesFileExist(name))                                                     // At first check whether file exists (fopen used to freeze mbed)
    {
        /* Send ACK (should make this more abstract) */
        wifi.sendByte('#');
        wifi.sendByte('A');
        wait_ms(50);                                                            // Timeout is used to make sure C# gets ready (sockets are messy, flushing is pain)
        /* Actually try to send file */                                         
        printf("\r\nSending %s", name);                                         // Notify user which user is being sent
        if (wifi.sendFile(name, requested_id))                                  // Send !
        {
            leds_error();                                                       // Handle error
            printf("\r\nUnable to send data");                                  // Notify user via text also
        }
        else
        {
            printf("\r\nSwimmer %d finished", requested_id);                    // Otherwise all is AOK
        }  
    }
    else
    {
        /* In case file doest not exist send NACK */
        wifi.sendByte('#');                                                     
        wifi.sendByte('F');
        printf("\r\nRequest non-existing file...");
    }
}

// Main ========================================================================

int main()
{
    /* Begin by initialization */
    printf("\r\nBoot...");                                                      // Notify user that board is booting
    boardInit();                                                                     // Initialize board
    printf("\r\nReady...");                                                     // Notify user that board is ready

    /* Set zeroth swimmer name */
    sprintf(fname, "/usb/swimmer%d.txt", swimmer_id);                           // Prepare zeroth swimmer filename

    /* Main while */
    while (1)
    {
        /* Read command from wifly */
        char cmd = wifi.getCmd();
        
        /* Master switch */
        switch (cmd) {
            /* start measuring data periodically and save them info file */
            case SAVING_START:
            {
                leds_off();                                                     // Turn off all leds
                startSaving();                                                  // Actually start saving
                printf("\r\nSaving swimmer %d...", swimmer_id);                 // Notify user
                led_measuring = 1;                                              // Turn on led_measuring
                break;                                                          // Break from switch
            }

            /* stop saving data */
            case SAVING_STOP:
            {
                leds_off();                                                     // Turn off all leds
                stopSaving();                                                   // Actually stop saving
                printf("\r\nStopped...");                                       // Notify user
                break;                                                          // Break from switch
            }

            /* Send all data */
            case TRANSFER_START:
            {
                leds_off();                                                     // Turn off all leds
                led_sending = 1;                                                // Turn on led_sending
                printf("\r\nSending data...");                                  // Notify user that data is being sent
                transfer();                                                     // Actually transfer data
                printf("\r\nSending finished...");                              // Notify user that data was sent successfully
                leds_off();                                                     // Turn off all leds again
                break;                                                          // Break from switch
            }

            /* Ready || Nothing on serial */
            case 'R':
            case 'N':
            {
                /* Handle data measuring */
                if(measuringFlag)                                               // Wait for timer to set the flag again and again
                {
                    measuring();                                                // Measure !
                    measuringFlag = 0;                                          // Reset flag
                }
                break;                                                          // Break from switch
            }

            /* Everything else is ballast */
            default :
            {
                leds_error();                                                   // Turn on all leds
                printf("\r\nCommand %c is unknown!", cmd);                      // Notify user
                break;                                                          // Break from switch
            }
        }
    }
}