main.cpp

00001 /* BRIEF:
00002  * Connects to wifi and listen on ip given by dhcp on port 2000
00003  *
00004  * Commands are send in for #X where X is from {S,E,T,B,R}, all commands are
00005  * confirmed by mbed by sending #A
00006  *
00007  * Use #S to start saving data, #E to stop saving. This can be done repeatedly,
00008  * mbed saves the data into separated files and generates new swimmer id
00009  * At the end, use #T to start data transfer, the format of transmitted data is:
00010  *
00011  * Header for each swimmer:
00012  * (int) -1 (int) -1 (int) swimmer_id
00013  * Data itself
00014  * (char) 0xAA (int) count 3*(short) acc 3*(float) gyro
00015  * End of swimmer block
00016  * (int) -1 (int) -1  
00017  *
00018  * where count is number starting on 0, incremented on every packet
00019  * 0xAA is a sync byte used to avoid confusion when some bytes are lost during
00020  * transmission (UART on faster reates seems to be unstable)
00021  * 
00022  * CHANGELOG 22/10/2015
00023  * - Refactored code
00024  * - Now using USB flash instead of SD card 
00025  * - SIZE of packets to be saved at once changed to 32 (from 1000, saving took too much time, effectively loosing measurements)
00026  * - 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
00027  * - Saving of 32 packets takes about 3,5 ms
00028  * KNOWN ISSUES:
00029  * - Opening USB disk takes some time, couple first measurements not valid (perhaps open next file right when saving stops ?)
00030  *
00031  * CHANGELOG 13/11/2015
00032  * - Focused on data acquisition
00033  * - #T commands now requires one byte after ACK to pick which swimmer data to send
00034  * - When board resets, it deletes all files on root, to make sure old data won't get send
00035  * - Added new command #B which represents begining of unified scope of data and should be sent right after connecting (clears all files and resets swimmerid) 
00036  *
00037  * CHANGELOG 7/1/2016
00038  * - Integrated MPU6000 IMU library
00039  * - Rewritten data acquisition to support new IMU
00040  * - Edited usbhost library to support sandisk flash disk
00041  *
00042  * CHANGELOG 12/1/2016
00043  * - Added delay after chipselect deselection between device bursts (solved gyro data inconsistency)
00044  *
00045  * CHANGELOG 6/5/2016
00046  * - Moved to first real HW (rev 1.0)
00047  * - Added multipurpose button functionality
00048  *
00049  * CHANGELOG 19/10/2016
00050  * - Added status message
00051  * - Changed button behaviour
00052  * - Added background battery monitoring
00053  * - Added background filesystem monitoring
00054  *
00055  * CHANGELOG 22/10/2016
00056  * - Slightly adjusted wifly communication (added timeouts)
00057  * - Disabled TCP autoretransmission on wifly
00058  * - Added send log feature
00059  *
00060  * CHANGELOG 05/03/2017
00061  * - Added define switch for new revision of hardware
00062  * - HW v1r2 only supports single MPU
00063  *
00064  * CHANGELOG 10/03/2017
00065  * - Added swimmer index to the status message.
00066  * - Added record file for records, function appendRecord, sendRecords
00067  * - Added clear root command
00068  *
00069  * CHANGELOG 13/03/2017
00070  * - Removed clearRoot() call from init.
00071  * - Change representation of the records file to binary.
00072  * - Add Prepare command - first send prepare, wait for a file open and then send Start.
00073  *
00074  * CHANGELOG 20/05/2017
00075  * - Changed communication protocol - each message have header with its size.
00076  */
00077 
00078 // Includes ====================================================================
00079 
00080 #include "mbed.h"
00081 #include "EthernetPowerControl.h"
00082 #include "MSCFileSystem.h"
00083 #include "PowerControl.h"
00084 #include "DirHandle.h"
00085 #include "mpu6000.h"
00086 #include "stdint.h"
00087 #include "stdio.h"
00088 #include "utils.h"
00089 #include "wifi.h"
00090 #include "math.h"
00091 
00092 // Revision ====================================================================
00093 
00094 // Revision define, it's important to select proper version! (pinmuxes differ)
00095 #define REVISION 1
00096 
00097 // Defines =====================================================================
00098 
00099 /* Data acquisition related stuff */
00100 //#define SAMPLE_RATE         700000                                              // Sample rate for ADC conversion
00101 #define SAMPLE_RATE         10000
00102 #define SAMPLE_PERIOD       0.01                                                // Setting the period of measuring, 0.01 means 100 times per second
00103 #define SIZE                32                                                  // Setting number of instance in array - for saving to SD card
00104 #define BUFFER_SIZE        (sizeof(int) + sizeof(short)*6)                      //size of one data block
00105 
00106 /* Battery related defines */
00107 #define UVLO_THRESHOLD 140U                                                     // Battery low threshold
00108 #define BATTERY_PERIOD 30U                                                      // How often to measure voltage in seconds
00109 
00110 /* Button press defines */
00111 #define PRESS_PERIOD_SHUTDOWN   2000U
00112 #define PRESS_PERIOD_LOW        250U
00113 #define PRESS_PERIOD_HIGH       450U
00114 
00115 /* Mass storage realted stuff */
00116 #define FSNAME "usb"
00117 #define LOG_FILE "/usb/log.txt"
00118 #define RECORDS_FILE "/usb/records.txt"                                         
00119 #define SPACE_LOW_THRESHOLD 100                                                 // USB Flash disk free threshold 
00120 #define SPACE_PERIOD 300U                                                       // How often to measure free space in seconds
00121 
00122 /* Commands swimfly understands */
00123 #define PREPARE_SAVING      'P'
00124 #define SAVING_START        'S'
00125 #define SAVING_STOP         'E'
00126 #define TRANSFER_REQUEST    'T'
00127 #define SCOPE_BEGIN         'B'
00128 #define CHECK_READY         'R'
00129 #define SEND_LOG            'L'
00130 #define SEND_RECORDS        'D'
00131 #define HALT                'H'
00132 
00133 /* Comm stuff */
00134 #define SOH 0x01
00135 
00136 /* Test mode define (undefine to disable) */
00137 //#define LOG_TEST_MODE
00138 
00139 /* Function like macros for leds (bleh) */
00140 #define leds_off() { led_measuring = led_sending = led_working = led_saving = 0; }
00141 #define leds_error() { led_measuring = led_sending = led_working = led_saving = 1; }
00142 
00143 // Function to power down magic USB interface chip with new firmware
00144 #define USR_POWERDOWN    (0x104)
00145 int semihost_powerdown() {
00146     uint32_t arg;
00147     return __semihost(USR_POWERDOWN, &arg);
00148 }
00149 
00150 // Function prototypes =========================================================
00151 
00152 /* File handling */
00153 void clearRoot(void);
00154 void appendLog(char textToAppend[]);
00155 void appendRecord(void);
00156 bool doesFileExist(const char *filename);
00157 void spaceMeas(void);
00158 void readSwimmerID(void);
00159 
00160 /* Button handlers */
00161 void btnPressCallback(void);
00162 void btnReleaseCallback(void);
00163 
00164 /* Power management */
00165 void pllInit(void);
00166 void battMeas(void);
00167 void shutdownHandler(void);
00168 
00169 /* Init */
00170 void boardInit(void);
00171 
00172 /* Measurement */
00173 void measure(void);
00174 void setMeasuringFlag(void);
00175 
00176 /* Command functions */
00177 void prepareSaving(void);
00178 void startSaving(void);
00179 void stopSaving(void);
00180 void transfer(void);
00181 void sendFile(char* name);
00182 void halt(void);
00183 void beginScope(void);
00184 void sendStatus(void);
00185 
00186 /* Generic functions */
00187 void blink(void);
00188 
00189 // Watchdog class ==============================================================
00190 
00191 class Watchdog {
00192 public:
00193     /* Kick the watchdog with time period setting */
00194     void setKick(float s) {
00195         LPC_WDT->WDCLKSEL = 0x1;                                                // Set CLK src to PCLK
00196         uint32_t clk = SystemCoreClock / 16;                                    // WD has a fixed /4 prescaler, PCLK default is /4
00197         LPC_WDT->WDTC = s * (float)clk;
00198         LPC_WDT->WDMOD = 0x3;                                                   // Enabled and Reset
00199         kick();
00200     }
00201 
00202     /* Kick the watchdog */
00203     void kick() {
00204         LPC_WDT->WDFEED = 0xAA;
00205         LPC_WDT->WDFEED = 0x55;
00206     }
00207 };
00208 
00209 // Objects & vars ==============================================================
00210 
00211 /* Objects */                                                                  
00212 Watchdog wdt;                                                                   // Initialize watchdog
00213 Ticker Saving_Ticker;                                                           // Ticker for interrupts on measuring
00214 Ticker Battery_Ticker;                                                          // Ticker for voltage measurement
00215 Ticker Space_Ticker;                                                            // Ticker for free space measurement
00216 Wifi wifi(p13, p14, p12, p23);                                                  // Wifly module
00217 SPI spi(p5, p6, p7);                                                            // NEW MPU6000 SPI connection
00218 MPU6000 mpu(spi, p8);                                                           // NEW MPU6000
00219 Timer button_timer;                                                             // Button press time counter
00220 Serial pc(USBTX, USBRX);                                                        // USB Serial port
00221 
00222 /* LED indicators */
00223 #if REVISION == 1
00224     DigitalOut led_system(p30);
00225 #elif REVISION == 2
00226     DigitalOut led_system(p27);
00227     DigitalOut cs_2(p26);
00228 #endif
00229 DigitalOut led_measuring(LED1);
00230 DigitalOut led_sending(LED2);
00231 DigitalOut led_working(LED3);
00232 DigitalOut led_saving(LED4);
00233 
00234 /* System LED notification patterns */
00235 //typedef enum {USER_BOOT, UVLO, DISK_FULL, AOK} blink_t;
00236 
00237 /* Power related stuff */
00238 DigitalOut wifly_enable_n(p9);
00239 DigitalOut flash_enable_n(p10);
00240 DigitalOut btn_latch_n(p28);
00241 InterruptIn btn_input(p29);
00242 AnalogIn batt_vol(p15);
00243 volatile uint8_t systemRequest      = 0;
00244 volatile bool userShutdownFlag      = 0;
00245 volatile bool UVLOShutdownFlag      = 0;
00246 volatile uint8_t battLevel          = 150U;                                     // Batt level between 136U/3.4816V and 165U/4.2240V (High byte of 16bit voltage measurement)
00247 volatile uint32_t curTime           = 0;
00248 volatile uint32_t oldTime           = 0;
00249 
00250 /* Added for test purposes */
00251 #ifdef LOG_TEST_MODE
00252 DigitalOut flagSetNotify(p19);
00253 DigitalOut measureNotify(p20);
00254 #endif
00255 
00256 /* USB && saving related stuffs */
00257 FILE *gfp;                                                                      // Global file pointer
00258 MSCFileSystem logger(FSNAME);                                                   // Initialize USB flash disk
00259 char fname[40];                                                                 // Variable for name of Text file saved on USB flash disk
00260 uint8_t timestamp[8];                                                           // Variable for name of Text file saved on USB flash disk
00261 static volatile uint64_t absolute   = 0;                                        // Variable for numbering saved data
00262 volatile int relative               = 0;                                        // Relative packtets saved number
00263 volatile uint8_t swimmer_id         = 0;                                        // Global swimmer ID                      
00264 volatile bool measuringFlag         = 0;                                        // Flag that is set with interrupt and indicates measurement is to be done
00265 volatile bool savingFlag            = 0;                                        // Flag that is set and indicates data is being saved periodically
00266 volatile bool lowSpaceFlag          = 0;                                        // Flag that is set when below SPACE_THRESHOLD MB's remain free
00267 volatile uint16_t mbFree            = 0;                                        // Variable to store remaining space on USB flash disk
00268 
00269 /* Accelerometer buffers */
00270 short int acc_x[SIZE];
00271 short int acc_y[SIZE];
00272 short int acc_z[SIZE];
00273 
00274 /* Gyroscope buffers */
00275 short int gyro_x[SIZE];
00276 short int gyro_y[SIZE];
00277 short int gyro_z[SIZE];
00278 
00279 /* Command buffers */
00280 char cmd_header[FRAME_HEADER_LEN];
00281 char cmd_data[256];
00282 int cmd_len;
00283 
00284 // Functions -> File handling ==================================================
00285 
00286 /* Clear all files in root directory */
00287 void clearRoot(void)
00288 {
00289     char buf[40];                                                               // Buffer for string building
00290     
00291     /* Dir related structs & vars */
00292     DIR *dir;                                                                   // Prepare directory handle
00293     struct dirent *file;                                                        // Directory entry structure
00294     dir = opendir("/usb");                                                      // Open root of usb flash disk                                                      
00295                                                                 
00296     /* Iterate all files beside log file in root directory and delete them */
00297     while((file = readdir(dir)) != NULL)                                        // Incrementally points to next files, until NULL is reached
00298     {
00299         if (strcmp(file->d_name, "log.txt") != 0)                               // Only dele non-log files
00300         {
00301             sprintf(buf, "/usb/");
00302             sprintf(buf+5, file->d_name);
00303             remove(buf);                                                        // Remove file that is currently being pointed to
00304         }
00305     }
00306       
00307     /* If file is not present */
00308     closedir(dir);                                                              // Close dir & return false    
00309     
00310     FILE *records;
00311     records = fopen(RECORDS_FILE, "w");                                         // Create new file for records
00312     fclose(records);  
00313 }
00314 
00315 /* Append line to log file */
00316 void appendLog(char textToAppend[])
00317 {   
00318     printf("Writing to log file...\r\n");
00319 
00320     /* Append line to log file */
00321     FILE *log;
00322     log = fopen(LOG_FILE, "a");                                                 // Open file with log name
00323     fprintf(log, "%s\r\n", textToAppend);                                       // Append line
00324     fclose(log);                                                                // Close file
00325 }
00326 
00327 /* Append record to records file */
00328 void appendRecord(void)
00329 {   
00330     printf("Writing to record file...\r\n");
00331 
00332     /* Append line to records file */
00333     FILE *records;
00334     records = fopen(RECORDS_FILE, "a");                                         // Open file with records name
00335     
00336     uint8_t recordbuffer[9];
00337     
00338     recordbuffer[0] = swimmer_id;
00339     
00340     for(int i = 0; i < 8; i++)
00341     {
00342         recordbuffer[i+1] = timestamp[i];
00343     }
00344     
00345     fwrite(recordbuffer, 1, 9, records);                                        // Write 9 bytes of record
00346     fflush(records); 
00347     fclose(records);                                                            // Close file
00348 }
00349 
00350 /* Read last swimmer ID */
00351 void readSwimmerID(void)
00352 {   
00353     size_t fileSize;
00354     FILE *records;
00355             
00356     if(false == doesFileExist(RECORDS_FILE))
00357     {
00358         swimmer_id = 0;
00359         printf("File with records does not exists, so ID set to 0...\r\n");
00360         swimmer_id = 0;
00361         printf("New records file created...\r\n");
00362         records = fopen(RECORDS_FILE, "a");
00363     }
00364     else
00365     {
00366         printf("Reading last swimmer ID from records file...\r\n");
00367 
00368         /* Append line to records file */
00369         records = fopen(RECORDS_FILE, "r");                                     // Open file with records name
00370         
00371         fseek(records, 0L, SEEK_END);
00372         fileSize = ftell(records);
00373         fseek(records, 0L, SEEK_SET);
00374      
00375         if(true == ferror(records) || fileSize < 9)
00376         {
00377             swimmer_id = 0;
00378         }
00379         else
00380         {
00381             fseek(records, -9, SEEK_END);
00382             swimmer_id = (uint8_t)fgetc(records) + 1;
00383         }
00384     }
00385     
00386     printf("Actual index is %d...\r\n", swimmer_id);
00387     fclose(records);                                                       
00388 }
00389 
00390 /* Sees whether giver file exists on flash disk */
00391 bool doesFileExist(const char *filename)
00392 {
00393     /* Dir related structs & vars */
00394     DIR *dir;                                                                   // Prepare directory handle
00395     struct dirent *file;                                                        // Directory entry structure
00396     dir = opendir("/usb");                                                      // Open root of usb flash disk                                                      
00397     
00398     /* Iterate all available files in root directory */
00399     while((file = readdir(dir)) != NULL)                                        // Incrementally points to next files, until NULL is reached
00400     {
00401         /* Check if it's file we are looking for */
00402         if(strcmp(filename+5, file->d_name) == 0)                               // +5 moves us beyond /usb/, not cool I know...
00403         {
00404             closedir(dir);                                                      // Close dir & return true
00405             return true;   
00406         }
00407     }
00408     
00409     /* If file is not present */
00410     closedir(dir);                                                              // Close dir & return false
00411     return false;                                                               // If not, return false
00412 }
00413 
00414 /* Measures space left and updates global variable value */
00415 void spaceMeas(void)
00416 {
00417     FATFS* fs;
00418     DWORD fre_clust;
00419     f_getfree("0:",&fre_clust,&fs);
00420     mbFree = (uint16_t)((fs->csize * fs->free_clust) >> 11);    
00421 }
00422 
00423 // Functions -> Power management ===============================================
00424 
00425 void pllInit(void)
00426 {
00427     LPC_SC->PLL0CON   = 0x00;             /* PLL0 Disable                    */
00428     LPC_SC->PLL0FEED  = 0xAA;
00429     LPC_SC->PLL0FEED  = 0x55;
00430  
00431     LPC_SC->CCLKCFG   = 0x00000005;       /* Select Clock Divisor = 6        */
00432     LPC_SC->PLL0CFG   = 0x0000000B;       /* configure PLL0                  */
00433     LPC_SC->PLL0FEED  = 0xAA;             /* divide by 1 then multiply by 12 */
00434     LPC_SC->PLL0FEED  = 0x55;             /* PLL0 frequency = 288,000,000    */
00435  
00436     LPC_SC->PLL0CON   = 0x01;             /* PLL0 Enable                     */
00437     LPC_SC->PLL0FEED  = 0xAA;
00438     LPC_SC->PLL0FEED  = 0x55;
00439     while (!(LPC_SC->PLL0STAT & (1<<26)));/* Wait for PLOCK0                 */
00440  
00441     LPC_SC->PLL0CON   = 0x03;             /* PLL0 Enable & Connect           */
00442     LPC_SC->PLL0FEED  = 0xAA;
00443     LPC_SC->PLL0FEED  = 0x55;
00444     while (!(LPC_SC->PLL0STAT & ((1<<25) | (1<<24))));/* Wait for PLLC0_STAT & PLLE0_STAT */
00445  
00446     SystemCoreClockUpdate();            /* Should now be at 48,000,000    */
00447     SystemCoreClock = (12000000 * 2 /
00448                       (((LPC_SC->PLL0STAT >> 16) & 0xFF) + 1) *
00449                       ((LPC_SC->PLL0STAT & 0x7FFF) + 1)  /
00450                       ((LPC_SC->CCLKCFG & 0xFF)+ 1));   
00451 }
00452 
00453 /* Callback for button press */
00454 void btnPressCallback(void)
00455 {
00456     /* Resets and starts timer */
00457     button_timer.reset();
00458     button_timer.start();
00459 }
00460 
00461 /* Callback for button release */
00462 void btnReleaseCallback(void)
00463 {
00464     /* Stop timer */
00465     button_timer.stop();
00466     /* See whether user is trying to shutdown */
00467     uint32_t buf = button_timer.read_ms();
00468     if((buf < PRESS_PERIOD_HIGH) && (buf > PRESS_PERIOD_LOW))
00469     {
00470         systemRequest += 1;
00471     }
00472     else
00473     {
00474         led_system = !led_system;
00475         systemRequest = 0;   
00476     }
00477 }
00478 
00479 /* Does single battery voltage measurement, and sets UVLO flag if voltage too low */
00480 void battMeas(void)
00481 {
00482     battLevel = batt_vol.read_u16() >> 8;                                       // Store High byte of voltage measurement to global variable 
00483     if(battLevel < UVLO_THRESHOLD)                                              // Determine whether we have enough juice
00484     {
00485         UVLOShutdownFlag = 1;                                                   // Raise Under Voltage Lock Out flag if necessary
00486     }
00487 }
00488 
00489 /* Checkes whether UVLO flag is set and halts board if necessary */
00490 void shutdownHandler(void)
00491 {
00492      /* Button shutdown handler */
00493      if(3 == systemRequest)
00494         {
00495             halt();                                                             // Halt       
00496         }
00497      /* UVLO shutdown handler */   
00498      if(1 == UVLOShutdownFlag)                                                  // Control of flag that is being set asynchronously if battLevel drops below threshold   
00499      {
00500         if (1 == savingFlag)                                                    // If saving's in progress stop it, to make sure file system won't get corrupted
00501         {
00502             stopSaving();    
00503         }
00504         UVLOShutdownFlag = 0;                                                           
00505         blink();
00506         blink();
00507         appendLog("Battery low halting");
00508         printf("Battery low, halting.\r\n");
00509         btn_latch_n = 1;                                                        // Halt
00510      }
00511 }
00512 
00513 // Functions -> Init ===========================================================
00514 
00515 /* Prepares board for operation (adc's, gyroscope etc.) */
00516 void boardInit (void)
00517 {   
00518     //pllInit();
00519     pc.baud(9600);           
00520     pc.printf("\r\nBooting...\r\n");                                            // Notify user that board is booting
00521     pc.printf("SystemCoreClock = %d Hz\r\n", SystemCoreClock);
00522     
00523     /* Watchdog */
00524     if ((LPC_WDT->WDMOD >> 2) & 1)
00525     {
00526         led_system = 1;
00527         wait(1);
00528         led_system = 0;
00529     }
00530     wdt.setKick(15.0);
00531     
00532     /* Power related stuff */
00533     btn_latch_n = 0;                                                            // This latch holds power P-MOS opened in order to provide power to whole board
00534     wait_ms(20);
00535     flash_enable_n = 0;
00536     wait_ms(20);
00537     wifly_enable_n = 0;
00538     wifi.init();                                                                // Initialize wifly
00539     led_measuring = 1;                                                          // Use incremental LED's to notify user about boot stages
00540     #if REVISION == 2
00541         cs_2 = 1;
00542     #endif
00543     /* IMU init */
00544     uint8_t name = mpu.init();                                                  // Initializes both gyro and accelerometer
00545     printf("0x%02X MPUID\r\n", name);
00546     wait_ms(40);                                                                // Wait for settings to stabilize
00547     led_sending = 1;                                                            // Light up second LED to notify mpu init ok
00548     
00549     /* Power saving features */
00550     PHY_PowerDown();                                                            // Disable ETH PHY to conserve power (about 40mA)
00551     semihost_powerdown();                                                       // Disable USB Host to conserve power (about 30mA)
00552     wait_ms(40);                                                                // Wait a bit
00553     led_working = 1;                                                            // Light up third LED
00554     
00555     /* Measure voltage */
00556     battMeas();
00557     printf("%d mV\r\n", (battLevel << 8) / 10);
00558     spaceMeas();
00559     printf("%d MB\r\n", mbFree);
00560     shutdownHandler();
00561     wait_ms(40);  
00562     led_saving = 1;
00563     
00564     /* Timers & Tickers init */
00565     Battery_Ticker.attach(&battMeas, BATTERY_PERIOD);
00566     Space_Ticker.attach(&spaceMeas, SPACE_PERIOD);                                                            
00567     button_timer.start();                                                       // Start timer that counts lengths of button presses
00568     btn_input.fall(&btnPressCallback);                                          // Attach button press callback
00569     btn_input.rise(&btnReleaseCallback);                                        // Attach button release callback
00570     wait_ms(40);
00571     
00572     readSwimmerID();
00573     
00574     /* End of init */
00575     appendLog("Booted");                                                        // Append line to log file
00576     leds_off();                                                                 // Turn all LEDs off
00577     blink();                                                                    // Blink system led to notify user
00578     wdt.setKick(5);
00579     pc.printf("Ready...\r\n");                                                  // Notify user of end init
00580 }
00581 
00582 // Functions -> Measuring ======================================================
00583 
00584 /* Main measuring function */
00585 void measure(void)
00586 {
00587     /* Vars */
00588     char buffer[SIZE][BUFFER_SIZE];                                             // Buffer
00589     int pos = 0;                                                                // Position
00590 
00591     /* Read IMU values */
00592     mpu.valRead(ACCEL, acc_x[relative], acc_y[relative], acc_z[relative]);      // Read Accelerometer values
00593     mpu.valRead(GYRO, gyro_x[relative], gyro_y[relative],gyro_z[relative]);     // Read Gyroscope values
00594     
00595     /* Increment counters */
00596     absolute++;                                                                 // Increment absolute counter
00597     relative++;                                                                 // Increment relative counter
00598 
00599     /* After our buffers are full save the data */
00600     if ( absolute % SIZE == 0 )
00601     {
00602         /* Flip led_saving state */
00603         led_saving = !led_saving;
00604 
00605         /* Handle file error */
00606         if (gfp == NULL) {
00607             printf("Unable to append data to file %s\r\n", fname);              // Notify user
00608             return;                                                             // Return
00609         }
00610 
00611         /* Iterate all buffer elements */
00612         for (int k = 0; k < SIZE; k++)
00613         {
00614             pos = absolute-SIZE+k;                                              // Increment position
00615             toBytes(buffer[k], &pos, sizeof(int));                              // Append data to buffer in byte form
00616             toBytes(buffer[k]+sizeof(int), &acc_x[k], sizeof(short));
00617             toBytes(buffer[k]+sizeof(int) + sizeof(short), &acc_y[k], sizeof(short));
00618             toBytes(buffer[k]+sizeof(int) + sizeof(short)*2, &acc_z[k], sizeof(short));
00619             toBytes(buffer[k]+sizeof(int) + sizeof(short)*3, &gyro_x[k], sizeof(short));
00620             toBytes(buffer[k]+sizeof(int) + sizeof(short)*4, &gyro_y[k], sizeof(short));
00621             toBytes(buffer[k]+sizeof(int) + sizeof(short)*5, &gyro_z[k], sizeof(short));                              
00622         }
00623         
00624         /* Write */
00625         #ifdef LOG_TEST_MODE
00626         measureNotify = 1;                                                      // Used for saving debug
00627         #endif
00628         fwrite(buffer, SIZE, BUFFER_SIZE, gfp);                                 // Write 32 buffer lines (16 bytes) to file (512 bytes total -> one block)
00629         fflush(gfp);                                                            // Flush to make sure nothing is left in buffer
00630         #ifdef LOG_TEST_MODE
00631         measureNotify = 0;                                                      // Used for saving debug
00632         #endif
00633     
00634         /* Reset relative counter */
00635         relative = 0;
00636     }
00637 }
00638 
00639 /* Sets the measuring flag */
00640 void setMeasuringFlag(void)
00641 {
00642     #ifdef LOG_TEST_MODE
00643     flagSetNotify = 1;                                                          // Used for saving debug
00644     #endif
00645     measuringFlag = 1;                                                          // Set measuring flag
00646     #ifdef LOG_TEST_MODE
00647     flagSetNotify = 0;                                                          // Used for saving debug
00648     #endif
00649 }
00650 
00651 // Functions -> Commands =======================================================
00652 
00653 /* Prepares all to acquire data from sensors */
00654 void prepareSaving(void)
00655 {
00656     for(int i = 0; i < 8; i++)
00657     {
00658         timestamp[i] = cmd_data[i];
00659     }
00660     
00661     savingFlag = 1;                                                             // Set saving flag
00662     /* Open global file with current swimmer ID */
00663     gfp = fopen(fname, "wb");                                                   // Try to open that file
00664     if(gfp == NULL) {
00665         leds_error();                                                           // Turn on all leds
00666         printf("Unable to open file %s for writing\r\n", fname);                // Notify user
00667         return;                                                                 // Exit function
00668     }
00669 
00670     /* Prepare file & vars for data acquisition */
00671     fprintf(gfp, "%c%c%c", -1, -1, swimmer_id);                                 // Append header to current swimmer output file
00672     fflush(gfp);
00673 
00674     /* Reset counters */
00675     absolute = 0;                                                               // Reset absolute counter
00676     relative = 0;                                                               // Reset relative counter
00677 }
00678 
00679 /* Starts to acquire data from sensors */
00680 void startSaving(void)
00681 {
00682     Saving_Ticker.attach(&setMeasuringFlag, SAMPLE_PERIOD);                     // Attach function that periodically sets the flag
00683 }
00684 
00685 /* Stops acquiring data, and save data left in buffers to USB */
00686 void stopSaving(void)
00687 {
00688     savingFlag = 0;                                                             // Clear saving flag
00689     /* Stop saving */
00690     Saving_Ticker.detach();                                                     // Stop setting measuring flag
00691 
00692     /* Try to append data left in buffers */
00693     if(gfp == NULL) {
00694         leds_error();                                                           // Turn on all leds
00695         printf("Unable to open file %s before sending data\r\n", fname);        // Notify user
00696         return;                                                                 // Exit function
00697     }
00698 
00699     /* Append data left to buffer */
00700     int base = ((absolute - (absolute%SIZE)) < 0) ? 0 : absolute - (absolute%SIZE);
00701     char buffer[relative][BUFFER_SIZE];                                         // packet array
00702     
00703     /* Iterate all array elements */
00704     for (int k = 0; k < relative; k++) {
00705         int pos = base+k;
00706         toBytes(buffer[k], &pos, sizeof(int));
00707         toBytes(buffer[k]+sizeof(int), &acc_x[k], sizeof(short));
00708         toBytes(buffer[k]+sizeof(int) + sizeof(short), &acc_y[k], sizeof(short));
00709         toBytes(buffer[k]+sizeof(int) + sizeof(short)*2, &acc_z[k], sizeof(short));
00710         toBytes(buffer[k]+sizeof(int) + sizeof(short)*3, &gyro_x[k], sizeof(short));
00711         toBytes(buffer[k]+sizeof(int) + sizeof(short)*4, &gyro_y[k], sizeof(short));
00712         toBytes(buffer[k]+sizeof(int) + sizeof(short)*5, &gyro_z[k], sizeof(short));
00713     }
00714     
00715     /* Write what's left */
00716     fwrite(buffer, relative, BUFFER_SIZE, gfp);                                 // Write all data at once
00717     fflush(gfp);
00718 
00719     /* Append EOF and close file */
00720     fprintf(gfp, "%c%c", -1,-1);                                                // Write EOF characters
00721     fflush(gfp);
00722     fclose(gfp);                                                                // Close file
00723     
00724     /* Append this file to the records file */
00725     appendRecord();
00726     
00727     /* Prepare for next swimmer */
00728     swimmer_id++;                                                               // Increment swimmer ID
00729     sprintf(fname, "/usb/swimmer%d.txt", swimmer_id);                           // Update current file name
00730     remove(fname);                                                              // If file with such ID exists remove it to avoid reading old data
00731 }
00732 
00733 /* Sends acquired data via Wi-Fi */
00734 void transfer(void)
00735 {
00736      /* Vars */
00737     char name[30];                                                              // File name buffer  
00738     
00739     /* File transfer prerequisites */
00740     printf("closing");
00741     if (gfp != NULL) {
00742         fclose(gfp);
00743     }
00744     printf(" file %d", cmd_data[0]);
00745     sprintf(name, "/usb/swimmer%d.txt", cmd_data[0]);                           // Create file name based on current swimmer ID
00746     printf(name);
00747     sendFile(name);    
00748 }
00749 
00750 /* Halts board */
00751 void halt(void)
00752 {
00753     if (1 == savingFlag)                                                        // If saving's in progress stop it, to make sure file system won't get corrupted
00754     {
00755         stopSaving();    
00756     }
00757     systemRequest = 0;
00758     blink();
00759     btn_latch_n = 1;  
00760 }
00761 
00762 /* Begins new scope (clears all data && rests swimmer_id counder) */
00763 void beginScope(void)
00764 {
00765     /* Clear all files and reset swimmer_id only when not after reset */
00766     if(swimmer_id != 0)                                                         // Redundancy check
00767     {
00768         appendLog("New scope");
00769         clearRoot();                                                            // Clears all files on root of USB flash disk
00770         swimmer_id = 0;                                                         // Resets simmer_id counter
00771         sprintf(fname, "/usb/swimmer%d.txt", swimmer_id);                       // Update current file name
00772     }
00773 }
00774 
00775 // Functions -> Generic functions ==============================================
00776 
00777 /* Sends acquired data via Wi-Fi */
00778 void sendFile(char* name)
00779 {
00780     /* Handle transfer */
00781     if(doesFileExist(name))                                             // At first check whether file exists (fopen used to freeze mbed)
00782     {                                                                   // Timeout is used to make sure C# gets ready (sockets are messy, flushing is pain)
00783         /* Actually try to send file */                                         
00784         printf("Sending %s...\r\n", name);                              // Notify user which user is being sent
00785         if (wifi.sendFile(name) != 0)                                   // Send !
00786         {
00787             leds_error();                                               // Handle error
00788             wifi.sendFail();
00789             printf("Unable to send file.\r\n");                         // Notify user via text also
00790         }
00791         else
00792         {
00793             printf("File transfer finished.\r\n");                     // Otherwise all is AOK
00794         }  
00795     }
00796     else
00797     {
00798         /* In case file doest not exist send NACK */
00799         wifi.sendNack();
00800         printf("Requested non-existing file.\r\n");
00801     }     
00802 }
00803 
00804 /* Sends status */
00805 void sendStatus(void)
00806 {
00807     uint8_t checksum = SOH + savingFlag + battLevel + swimmer_id;               // Calculate checksum
00808     wifi.sendByte('#');                                                         // #
00809     wifi.sendByte('A');                                                         // A
00810     wifi.sendByte(5);                                                           // Send count of the data
00811     wifi.sendByte(0);
00812     wifi.sendByte(0);
00813     wifi.sendByte(0);
00814     wifi.sendByte(SOH);                                                         // Send start of heading
00815     wifi.sendByte(savingFlag);                                                  // Send state of saving
00816     wifi.sendByte(battLevel);                                                   // Sends current battery level
00817     wifi.sendByte(swimmer_id);                                                  // Sends current swimmer id index
00818     wifi.sendByte(checksum);                                                    // Send checksum
00819     
00820     wifi.waitForAck();
00821 }
00822 
00823 void blink(void)
00824 {
00825     if(led_system)
00826     {
00827         led_system = 0;    
00828     }
00829     wait_ms(200);
00830     led_system = 1;
00831     wait_ms(200);
00832     led_system = 0;    
00833 }
00834 
00835 // Main ========================================================================
00836 
00837 int main()
00838 {
00839     /* Begin by initialization */                                               
00840     boardInit();                                                                // Initialize board
00841     
00842     /* Set zeroth swimmer name */
00843     sprintf(fname, "/usb/swimmer%d.txt", swimmer_id);                           // Prepare zeroth swimmer filename
00844     
00845     /* Main while */
00846     while (1)
00847     {
00848         /* Shutdown handler */
00849         shutdownHandler();
00850         /* Read command from wifly */
00851         cmd_len = wifi.getCmd(cmd_header, cmd_data);
00852         
00853         if(cmd_header[1] != 'N')
00854         {
00855             printf(">> #%c and %d bytes\r\n", cmd_header[1], cmd_len);
00856         }
00857         
00858         /* Master switch */
00859         switch (cmd_header[1]) {
00860             /* start measuring data periodically and save them info file */
00861             case PREPARE_SAVING:
00862             {
00863                 wifi.sendAck();
00864                 leds_off();                                                     // Turn off all leds
00865                 prepareSaving();                                                // Prepares all to saving
00866                 printf("Opening file for swimmer %d...\r\n", swimmer_id);       // Notify user
00867                 break;                                                          // Break from switch
00868             }
00869             
00870             /* start measuring data periodically and save them info file */
00871             case SAVING_START:
00872             {
00873                 wifi.sendAck();
00874                 leds_off();                                                     // Turn off all leds
00875                 startSaving();                                                  // Actually start saving
00876                 printf("Saving swimmer %d...\r\n", swimmer_id);                 // Notify user
00877                 led_measuring = 1;                                              // Turn on led_measuring
00878                 break;                                                          // Break from switch
00879             }
00880 
00881             /* stop saving data */
00882             case SAVING_STOP:
00883             {
00884                 leds_off();                                                     // Turn off all leds
00885                 stopSaving();                                                   // Actually stop saving
00886                 wifi.sendAck();
00887                 printf("Stopped...\r\n");                                       // Notify user
00888                 break;                                                          // Break from switch
00889             }
00890 
00891             /* Send all data */
00892             case TRANSFER_REQUEST:
00893             {
00894                 leds_off();                                                     // Turn off all leds
00895                 led_sending = 1;                                                // Turn on led_sending
00896                 printf("Sending data...\r\n");                                  // Notify user that data is being sent
00897                 transfer();                                                     // Actually transfer data
00898                 printf("Sending finished...\r\n");                              // Notify user that data was sent successfully
00899                 leds_off();                                                     // Turn off all leds again
00900                 break;                                                          // Break from switch
00901             }
00902 
00903             /* Begin new scope */
00904             case SCOPE_BEGIN:
00905             {
00906                 wifi.sendAck();
00907                 leds_off();                                                     // Turn off all leds
00908                 led_measuring = led_saving = 1;                                 // Turn on two leds to depict special behaviour
00909                 printf("Starting new scope...\r\n");                            // Notify user that new scope is beginning
00910                 beginScope();                                                   // Begin scope ! (clear all data && reset swimmer_id counter)
00911                 leds_off();                                                     // Turn off all leds
00912                 break;                                                          // Break
00913             }
00914             
00915             /* Send log file */
00916             case SEND_LOG:
00917             {
00918                 wifi.sendAck();
00919                 leds_off();                                                     // Turn off all leds
00920                 led_measuring = led_saving = 1;                                 // Turn on two leds to depict special behaviour
00921                 printf("Sending log...\r\n");                                   // Notify user that new scope is beginning
00922                 sendFile(LOG_FILE);                                             // Send log file
00923                 leds_off();                                                     // Turn off all leds
00924                 break;                                                          // Break
00925             }
00926             
00927             /* Send records file */
00928             case SEND_RECORDS:
00929             {
00930                 leds_off();                                                     // Turn off all leds
00931                 led_measuring = led_saving = 1;                                 // Turn on two leds to depict special behaviour
00932                 printf("Sending records...\r\n");                               // Notify user that new scope is beginning
00933                 sendFile(RECORDS_FILE);                                         // Send records file
00934                 leds_off();                                                     // Turn off all leds
00935                 break;                                                          // Break
00936             }
00937             
00938             /* Halt swimfly */
00939             case HALT:
00940             {
00941                 wifi.sendAck();
00942                 leds_off();                                                     // Turn off all leds
00943                 led_measuring = led_saving = 1;                                 // Turn on two leds to depict special behaviour
00944                 halt();                                                         // Halt swimfly
00945                 leds_off();                                                     // Turn off all leds
00946                 break;                                                          // Break
00947             }
00948             
00949             /* Ready */
00950             case CHECK_READY:
00951             {
00952                 printf("Sending status...\r\n");                                //
00953                 leds_off();                                                     // Turn off all leds
00954                 sendStatus();                                                   // Sends status message
00955                 break;                                                          // Break
00956             }
00957             
00958             /* Nothing on serial */
00959             case 'N':
00960             {
00961                 /* Handle data measuring */
00962                 if(measuringFlag)                                               // Wait for timer to set the flag again and again
00963                 {
00964                     measure();                                                  // Measure !
00965                     measuringFlag = 0;                                          // Reset flag
00966                 }
00967                 break;                                                          // Break from switch
00968             }
00969 
00970             /* Fail (should be handled in some inner function */
00971             case 'F':
00972             {
00973                 break;                                                          // Break from switch
00974             }
00975             
00976             /* Nothing on serial */
00977             case 'A':
00978             {
00979                 break;                                                          // Break from switch
00980             }
00981             
00982             /* Everything else is ballast */
00983             default :
00984             {
00985                 wifi.sendNack();
00986                 leds_error();                                                   // Turn on all leds
00987                 printf("Command %c is unknown!\r\n", cmd_header[1]);            // Notify user
00988                 break;                                                          // Break from switch
00989             }
00990         }
00991         wdt.kick();                                                             // Kick watchdog each iteration
00992     }
00993 }