Los Putacos / Mbed OS WearableDevice_Nucleo_New

Dependencies:   MPU9250_SPI

Fork of WearableDevice_Nucleo by Los Putacos

Committer:
Muglug
Date:
Sun Nov 12 14:54:49 2017 +0000
Revision:
27:c4b2ce6fa5b8
Parent:
26:4bc56ce08d15
Child:
28:c7e977a19564
Optimization Changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gusteibolt 1:a6dc717fb060 1 /*
gusteibolt 1:a6dc717fb060 2 * Los Putacos
gusteibolt 1:a6dc717fb060 3 * Copyright (C) 2017, All rights reserved.
Muglug 18:7f9c2b8541e1 4 * ________________________________________
gusteibolt 1:a6dc717fb060 5 *
gusteibolt 1:a6dc717fb060 6 * Created by: Gustavo Campana, Michael Schmidt, Miguel Lopez
gusteibolt 1:a6dc717fb060 7 * Date: 11-Oct-2017
gusteibolt 1:a6dc717fb060 8 * Version: V0.1
gusteibolt 1:a6dc717fb060 9 */
Muglug 23:aad5fd1b3ef9 10 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 11
gusteibolt 1:a6dc717fb060 12 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 13 // Board: NUCLEO - F401RE
gusteibolt 1:a6dc717fb060 14 // Version: MR1136 rev C
gusteibolt 1:a6dc717fb060 15 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 16
gusteibolt 1:a6dc717fb060 17 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 18 // Includes
Muglug 25:86137c182a17 19 #include "mbed.h"
Muglug 23:aad5fd1b3ef9 20 #include "configuration.h"
gusteibolt 3:26aeff25f610 21
Muglug 17:f98597cd2efc 22 #include "XBeeLib.h"
Muglug 19:0a3ae902722e 23 #include "SDFileSystem.h"
Muglug 24:eed68c95160c 24 #include "MPU9250.h"
Muglug 24:eed68c95160c 25
Muglug 24:eed68c95160c 26 #include "event.h"
gusteibolt 1:a6dc717fb060 27 //-----------------------------------------------------------------
gusteibolt 0:73cd0cb02330 28
gusteibolt 1:a6dc717fb060 29 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 30 // Declarations
Muglug 25:86137c182a17 31 FILE *fp; // FILE Type for SD-Card
Muglug 26:4bc56ce08d15 32 Timer TimeStamp; // Timer µS time-stamp
Muglug 26:4bc56ce08d15 33 Serial PC(USBTX, USBRX); // Create an Serial PC Object - USBTX, USBRX
Muglug 26:4bc56ce08d15 34 SPI spi(MPU_MOSI, MPU_MISO, MPU_SCK); // Create an SPI Object for MPU9250 - MOSI, MISO, SCK
Muglug 26:4bc56ce08d15 35 mpu9250_spi imu(spi, MPU_CS); // Create an MPU9250 Object - SPI Object, CS
Muglug 26:4bc56ce08d15 36 SDFileSystem sd(SD_MOSI, SD_MISO, SD_CLK, SD_CS, "sd", SD_CD, SDFileSystem::SWITCH_NEG_NO, 25000000); // Create an SDFileSystem Object - MOSI, MISO, CLK, CS
Muglug 26:4bc56ce08d15 37 XBeeLib::XBeeZB XBee = XBeeLib::XBeeZB(RADIO_TX, RADIO_RX, RADIO_RESET, NC, NC, 230400); // Create an XBee Object - TX, RX, RESET, NC, NC, BaudRate
Muglug 26:4bc56ce08d15 38 XBeeLib::TxStatus txStatus; // XBee Status Variable
Muglug 25:86137c182a17 39
Muglug 25:86137c182a17 40 // Events
Muglug 25:86137c182a17 41 EventQueue queue(32 * EVENTS_EVENT_SIZE); // Event Setup
Muglug 25:86137c182a17 42
Muglug 25:86137c182a17 43 // Tickers
Muglug 26:4bc56ce08d15 44 Ticker Ticker_IMU;
Muglug 26:4bc56ce08d15 45 Ticker Ticker_ReceiveXBee;
Muglug 25:86137c182a17 46
Muglug 25:86137c182a17 47 // Threads
Muglug 26:4bc56ce08d15 48 Thread Thread_IMU(osPriorityRealtime);
Muglug 26:4bc56ce08d15 49 Thread Thread_ReceiveXBee(osPriorityNormal);
gusteibolt 3:26aeff25f610 50
Muglug 26:4bc56ce08d15 51 // Global Variables
Muglug 26:4bc56ce08d15 52 uint16_t Time_Data = 0;
Muglug 27:c4b2ce6fa5b8 53 uint32_t Data_Size = 0;
Muglug 27:c4b2ce6fa5b8 54 uint16_t LineCount = 0;
Muglug 26:4bc56ce08d15 55 volatile uint16_t readPointer = 0;
Muglug 26:4bc56ce08d15 56 volatile uint16_t readPointer_MIC = 0;
Muglug 26:4bc56ce08d15 57 volatile uint16_t writePointer = 0;
Muglug 26:4bc56ce08d15 58 volatile uint16_t writePointer_MIC = 0;
Muglug 24:eed68c95160c 59
Muglug 27:c4b2ce6fa5b8 60 uint8_t ReadIMUDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 61 uint8_t WriteSDDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 62 uint8_t SendXBeeDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 63
Muglug 26:4bc56ce08d15 64 uint8_t Requested_Time = 0;
Muglug 26:4bc56ce08d15 65 uint8_t Current_Position = 0;
gusteibolt 3:26aeff25f610 66
Muglug 26:4bc56ce08d15 67 char Time_Buffer[2];
Muglug 27:c4b2ce6fa5b8 68 char Data_Buffer[255];
Muglug 26:4bc56ce08d15 69 const char DeviceNr[6] = "DEV01";
Muglug 26:4bc56ce08d15 70 int16_t Data_Storage[BufferSize]; // BufferSize defined in "event.h"
Muglug 26:4bc56ce08d15 71 uint16_t Data_Storage_MIC[BufferSize_MIC]; // BufferSize_MIC defined in "event.h"
Muglug 23:aad5fd1b3ef9 72
gusteibolt 1:a6dc717fb060 73 //-----------------------------------------------------------------
gusteibolt 1:a6dc717fb060 74
gusteibolt 1:a6dc717fb060 75 //-----------------------------------------------------------------
Muglug 23:aad5fd1b3ef9 76 /* Callback function, invoked at packet reception */
Muglug 23:aad5fd1b3ef9 77 static void receive_cb(const XBeeLib::RemoteXBeeZB& remote, bool broadcast, const uint8_t *const data, uint16_t len)
Muglug 23:aad5fd1b3ef9 78 {
Muglug 26:4bc56ce08d15 79 PC.printf("Package Length: %i \r\n", len); // Display Package Length
Muglug 23:aad5fd1b3ef9 80 for (int i = 0; i < len; i++) {
Muglug 23:aad5fd1b3ef9 81 Data_Buffer[i] = char(data[i]);
Muglug 26:4bc56ce08d15 82 PC.printf("%c", Data_Buffer[i]); // Display Received Package
Muglug 23:aad5fd1b3ef9 83 }
Muglug 24:eed68c95160c 84
Muglug 26:4bc56ce08d15 85 if (len == 9) { // Check Package Length (correct: len = 9)
Muglug 26:4bc56ce08d15 86 if ((char(Data_Buffer[0]) == '#') && (char(Data_Buffer[6]) == ',')) { // Check for "#" and "," at specific Positions
Muglug 23:aad5fd1b3ef9 87 PC.printf(" - Command Detected!\r\n");
Muglug 24:eed68c95160c 88
Muglug 26:4bc56ce08d15 89 if (strstr(Data_Buffer, DeviceNr) != NULL) { // Check for valid DeviceNr in Package
Muglug 26:4bc56ce08d15 90 PC.printf("Correct Package ID!");
Muglug 26:4bc56ce08d15 91
Muglug 26:4bc56ce08d15 92 Current_Position = (uint8_t)(strchr(Data_Buffer, ',') - Data_Buffer);
Muglug 26:4bc56ce08d15 93 Time_Buffer[0] = Data_Buffer[Current_Position + 1];
Muglug 26:4bc56ce08d15 94 Time_Buffer[1] = Data_Buffer[Current_Position + 2];
Muglug 26:4bc56ce08d15 95 Requested_Time = atoi(Time_Buffer); // Requested Time by received Package
Muglug 27:c4b2ce6fa5b8 96 memset(Data_Buffer, 0, sizeof(Data_Buffer)); // Clear Data_Buffer
Muglug 27:c4b2ce6fa5b8 97
Muglug 26:4bc56ce08d15 98 PC.printf(" - Time: %d\n", Requested_Time); // Display Requested Time
Muglug 26:4bc56ce08d15 99 PC.printf("------------------------------------\r\n");
Muglug 26:4bc56ce08d15 100 Ticker_ReceiveXBee.detach();
Muglug 26:4bc56ce08d15 101
Muglug 26:4bc56ce08d15 102 PC.printf("Measuring ...");
Muglug 26:4bc56ce08d15 103 TimeStamp.reset(); // Reset TimeStamp
Muglug 26:4bc56ce08d15 104 TimeStamp.start(); // Start TimeStamp
Muglug 26:4bc56ce08d15 105 Thread_IMU.start(callback(&queue, &EventQueue::dispatch_forever)); // Start Measurement Thread
Muglug 26:4bc56ce08d15 106 Ticker_IMU.attach_us(queue.event(&ReadIMU), 1000); // Attach 1 ms Ticker to Measurement "ReadIMU"
Muglug 26:4bc56ce08d15 107 } else
Muglug 23:aad5fd1b3ef9 108 PC.printf("Wrong Package ID!\r\n");
Muglug 24:eed68c95160c 109 } else
Muglug 23:aad5fd1b3ef9 110 PC.printf(" - Non-valid Command Delimiter!\r\n");
Muglug 24:eed68c95160c 111 } else
Muglug 23:aad5fd1b3ef9 112 PC.printf(" - Non-valid Package Length!\r\n");
gusteibolt 8:ba93a973f967 113 }
gusteibolt 8:ba93a973f967 114 //-----------------------------------------------------------------
gusteibolt 8:ba93a973f967 115
gusteibolt 8:ba93a973f967 116 //-----------------------------------------------------------------
Muglug 18:7f9c2b8541e1 117 void Setup()
Muglug 18:7f9c2b8541e1 118 {
Muglug 26:4bc56ce08d15 119 PC.baud(230400); // Initialize PC Serial Connection
Muglug 24:eed68c95160c 120 PC.printf("\r\n------------- Booting! -------------\r\n");
Muglug 23:aad5fd1b3ef9 121 PC.printf("CPU SystemCoreClock is %d Hz", SystemCoreClock);
Muglug 26:4bc56ce08d15 122 wait(0.5);
Muglug 18:7f9c2b8541e1 123
Muglug 26:4bc56ce08d15 124 XBee.init(); // Initialize XBee Serial Connection
Muglug 26:4bc56ce08d15 125 XBee.register_receive_cb(&receive_cb); // Register Callbacks
Muglug 26:4bc56ce08d15 126 wait(0.5);
Muglug 18:7f9c2b8541e1 127
Muglug 25:86137c182a17 128 // Initialize IMU SPI Connection
Muglug 26:4bc56ce08d15 129 if(imu.init(1, BITS_DLPF_CFG_188HZ)) // Initialize the MPU9250
Muglug 24:eed68c95160c 130 PC.printf("\nCouldn't initialize MPU9250 via SPI!");
Muglug 26:4bc56ce08d15 131 PC.printf("\nWHOAMI = 0x%2x", imu.whoami()); // Output I2C Address to check SPI (correct: 104 - 0x68)
Muglug 25:86137c182a17 132 PC.printf("\nAcc_Scale = %u\n", imu.set_acc_scale(BITS_FS_16G)); // Set Full Range for Acc.
Muglug 26:4bc56ce08d15 133 imu.calib_acc(); // Calibrate Acceleration Sensor
Muglug 26:4bc56ce08d15 134 wait(0.5);
Muglug 24:eed68c95160c 135
Muglug 25:86137c182a17 136 // Initialize SD-Card SPI Connection
Muglug 26:4bc56ce08d15 137 mkdir("/sd/Log", 0777); // Create 'Log' Directory
Muglug 26:4bc56ce08d15 138 fp = fopen("/sd/Log/Data_Log.txt", "w"); // Create & Open "Data_Log.txt" File
Mister_Lopez 4:56e903769e94 139
Muglug 26:4bc56ce08d15 140 if(fp == NULL)
Muglug 23:aad5fd1b3ef9 141 error("File Writing Failed!\n");
Muglug 26:4bc56ce08d15 142 else {
Muglug 26:4bc56ce08d15 143 PC.printf("\nSD-Card Initialized!\n");
Muglug 23:aad5fd1b3ef9 144 }
Muglug 24:eed68c95160c 145 fclose(fp);
Muglug 24:eed68c95160c 146
Muglug 26:4bc56ce08d15 147 // Display Card Type
Muglug 26:4bc56ce08d15 148 printf("Card type: ");
Muglug 26:4bc56ce08d15 149 SDFileSystem::CardType cardType = sd.card_type();
Muglug 26:4bc56ce08d15 150 if (cardType == SDFileSystem::CARD_NONE)
Muglug 26:4bc56ce08d15 151 printf("None\n");
Muglug 26:4bc56ce08d15 152 else if (cardType == SDFileSystem::CARD_MMC)
Muglug 26:4bc56ce08d15 153 printf("MMC\n");
Muglug 26:4bc56ce08d15 154 else if (cardType == SDFileSystem::CARD_SD)
Muglug 26:4bc56ce08d15 155 printf("SD\n");
Muglug 26:4bc56ce08d15 156 else if (cardType == SDFileSystem::CARD_SDHC)
Muglug 26:4bc56ce08d15 157 printf("SDHC\n");
Muglug 26:4bc56ce08d15 158 else
Muglug 26:4bc56ce08d15 159 printf("Unknown\n");
Muglug 24:eed68c95160c 160
Muglug 26:4bc56ce08d15 161 // Display Card Capacity
Muglug 26:4bc56ce08d15 162 printf("Sectors: %u\n", sd.disk_sectors());
Muglug 26:4bc56ce08d15 163 printf("Capacity: %.1fMB\n", sd.disk_sectors() / 2048.0);
Muglug 24:eed68c95160c 164
Muglug 26:4bc56ce08d15 165 TimeStamp.reset(); // Reset Timer TimeStamp
Muglug 26:4bc56ce08d15 166 led1 = 1; // Turn ON LED to display 'Ready!'
Muglug 26:4bc56ce08d15 167 PC.printf("\r\n------------- Ready! ---------------\r\n");
Muglug 24:eed68c95160c 168
Muglug 26:4bc56ce08d15 169 Thread_ReceiveXBee.start(callback(&queue, &EventQueue::dispatch_forever)); // Start XBee-Receiving Thread
Muglug 26:4bc56ce08d15 170 Ticker_ReceiveXBee.attach(queue.event(&ReceiveXBee), 0.5); // Attach 500 ms Ticker to "ReceiveXBee"
Muglug 23:aad5fd1b3ef9 171 }
Muglug 23:aad5fd1b3ef9 172 //-----------------------------------------------------------------
Muglug 18:7f9c2b8541e1 173
Mister_Lopez 4:56e903769e94 174 //-----------------------------------------------------------------
Muglug 27:c4b2ce6fa5b8 175 void Reset() // Reset All Variables
Muglug 27:c4b2ce6fa5b8 176 {
Muglug 27:c4b2ce6fa5b8 177 Time_Data = 0;
Muglug 27:c4b2ce6fa5b8 178 Data_Size = 0;
Muglug 27:c4b2ce6fa5b8 179 LineCount = 0;
Muglug 27:c4b2ce6fa5b8 180
Muglug 27:c4b2ce6fa5b8 181 readPointer = 0;
Muglug 27:c4b2ce6fa5b8 182 readPointer_MIC = 0;
Muglug 27:c4b2ce6fa5b8 183 writePointer = 0;
Muglug 27:c4b2ce6fa5b8 184 writePointer_MIC = 0;
Muglug 27:c4b2ce6fa5b8 185
Muglug 27:c4b2ce6fa5b8 186 ReadIMUDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 187 WriteSDDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 188 SendXBeeDone_Flag = 0;
Muglug 27:c4b2ce6fa5b8 189
Muglug 27:c4b2ce6fa5b8 190 Requested_Time = 0;
Muglug 27:c4b2ce6fa5b8 191 Current_Position = 0;
Muglug 27:c4b2ce6fa5b8 192
Muglug 27:c4b2ce6fa5b8 193 memset(Time_Buffer, 0, sizeof(Time_Buffer));
Muglug 27:c4b2ce6fa5b8 194 memset(Data_Buffer, 0, sizeof(Data_Buffer));
Muglug 27:c4b2ce6fa5b8 195 memset(Data_Storage, 0, sizeof(Data_Storage));
Muglug 27:c4b2ce6fa5b8 196 memset(Data_Storage_MIC, 0, sizeof(Data_Storage_MIC));
Muglug 27:c4b2ce6fa5b8 197 }
Muglug 27:c4b2ce6fa5b8 198 //-----------------------------------------------------------------
Muglug 27:c4b2ce6fa5b8 199
Muglug 27:c4b2ce6fa5b8 200 //-----------------------------------------------------------------
gusteibolt 0:73cd0cb02330 201 int main()
gusteibolt 0:73cd0cb02330 202 {
Muglug 18:7f9c2b8541e1 203 Setup(); // Initial Setups
Muglug 18:7f9c2b8541e1 204
Muglug 26:4bc56ce08d15 205 while (true) {
Muglug 27:c4b2ce6fa5b8 206 while ((writePointer != readPointer) && (writePointer_MIC != readPointer_MIC) && (ReadIMUDone_Flag == 1)) {
Muglug 27:c4b2ce6fa5b8 207 if (((Requested_Time * 1000) - LineCount) >= 8) {
Muglug 27:c4b2ce6fa5b8 208 for (int i = 0; i < 8; i++) {
Muglug 27:c4b2ce6fa5b8 209 Data_Size += sprintf(&Data_Buffer[Data_Size], "%d;%d;%d;%d;%d#\n", Data_Storage[readPointer++], Data_Storage_MIC[readPointer_MIC++], Data_Storage[readPointer++], Data_Storage[readPointer++], Data_Storage[readPointer++]);
Muglug 27:c4b2ce6fa5b8 210 LineCount++;
Muglug 27:c4b2ce6fa5b8 211 }
Muglug 27:c4b2ce6fa5b8 212
Muglug 27:c4b2ce6fa5b8 213 txStatus = XBee.send_data_to_coordinator((const uint8_t *) Data_Buffer, Data_Size);
Muglug 27:c4b2ce6fa5b8 214 if (txStatus != XBeeLib::TxStatusSuccess)
Muglug 27:c4b2ce6fa5b8 215 PC.printf("send_data_to_coordinator() failed with error %d\n", (int) txStatus);
Muglug 27:c4b2ce6fa5b8 216 } else {
Muglug 27:c4b2ce6fa5b8 217 for (int i = 0; i < ((Requested_Time * 1000) - LineCount); i++) {
Muglug 27:c4b2ce6fa5b8 218 Data_Size += sprintf(&Data_Buffer[Data_Size], "%d;%d;%d;%d;%d#\n", Data_Storage[readPointer++], Data_Storage_MIC[readPointer_MIC++], Data_Storage[readPointer++], Data_Storage[readPointer++], Data_Storage[readPointer++]);
Muglug 27:c4b2ce6fa5b8 219 LineCount++;
Muglug 27:c4b2ce6fa5b8 220 }
Muglug 27:c4b2ce6fa5b8 221
Muglug 27:c4b2ce6fa5b8 222 txStatus = XBee.send_data_to_coordinator((const uint8_t *) Data_Buffer, Data_Size);
Muglug 27:c4b2ce6fa5b8 223 if (txStatus != XBeeLib::TxStatusSuccess)
Muglug 27:c4b2ce6fa5b8 224 PC.printf("send_data_to_coordinator() failed with error %d\n", (int) txStatus);
Muglug 27:c4b2ce6fa5b8 225 }
Muglug 27:c4b2ce6fa5b8 226
Muglug 27:c4b2ce6fa5b8 227 Data_Size = 0;
Muglug 27:c4b2ce6fa5b8 228 memset(Data_Buffer, 0, sizeof(Data_Buffer)); // Clear Data_Buffer
Muglug 26:4bc56ce08d15 229 }
Muglug 26:4bc56ce08d15 230
Muglug 27:c4b2ce6fa5b8 231 if ((LineCount >= (Requested_Time * 1000)) && (ReadIMUDone_Flag == 1)) {
Muglug 27:c4b2ce6fa5b8 232 PC.printf(" Done! - Time taken: %d ms\n", TimeStamp.read_ms());
Muglug 27:c4b2ce6fa5b8 233 TimeStamp.stop();
Muglug 25:86137c182a17 234
Muglug 26:4bc56ce08d15 235 readPointer = 0;
Muglug 26:4bc56ce08d15 236 readPointer_MIC = 0;
Muglug 27:c4b2ce6fa5b8 237 SendXBeeDone_Flag = 1;
Muglug 26:4bc56ce08d15 238
Muglug 27:c4b2ce6fa5b8 239 PC.printf("Writing SD-Card ...");
Muglug 27:c4b2ce6fa5b8 240 fp = fopen("/sd/Log/Data_Log.txt", "w"); // Create & Open "Data_Log.txt" File
Muglug 27:c4b2ce6fa5b8 241 setvbuf(fp, NULL, _IONBF, 0); // Set SD-Buffer to "0"
Muglug 27:c4b2ce6fa5b8 242 fprintf(fp, "Time; Mic; AccX; AccY; AccZ#\n"); // Write Header into File
Muglug 27:c4b2ce6fa5b8 243 TimeStamp.reset();
Muglug 26:4bc56ce08d15 244 TimeStamp.start();
Muglug 26:4bc56ce08d15 245 }
Muglug 26:4bc56ce08d15 246
Muglug 27:c4b2ce6fa5b8 247 while ((writePointer != readPointer) && (writePointer_MIC != readPointer_MIC) && (SendXBeeDone_Flag == 1)) {
Muglug 27:c4b2ce6fa5b8 248 fprintf(fp, "%d;%d;%d;%d;%d#\n", Data_Storage[readPointer++], Data_Storage_MIC[readPointer_MIC++], Data_Storage[readPointer++], Data_Storage[readPointer++], Data_Storage[readPointer++]);
Muglug 27:c4b2ce6fa5b8 249 WriteSDDone_Flag = 1;
Muglug 27:c4b2ce6fa5b8 250 }
Muglug 25:86137c182a17 251
Muglug 27:c4b2ce6fa5b8 252 if ((readPointer == writePointer) && (writePointer_MIC == readPointer_MIC) && (WriteSDDone_Flag == 1)) {
Muglug 27:c4b2ce6fa5b8 253 fclose(fp);
Muglug 27:c4b2ce6fa5b8 254 PC.printf(" Done! - Time taken: %d ms\n", TimeStamp.read_ms());
Muglug 27:c4b2ce6fa5b8 255 PC.printf("------------------------------------\r\n");
Muglug 27:c4b2ce6fa5b8 256 TimeStamp.stop();
Muglug 27:c4b2ce6fa5b8 257
Muglug 27:c4b2ce6fa5b8 258 Reset(); // Reset All Variables
Muglug 27:c4b2ce6fa5b8 259 Ticker_ReceiveXBee.attach(queue.event(&ReceiveXBee), 0.5);
Muglug 26:4bc56ce08d15 260 }
Muglug 19:0a3ae902722e 261 }
gusteibolt 0:73cd0cb02330 262 }
Muglug 18:7f9c2b8541e1 263 //-----------------------------------------------------------------