Project Badminton Logger / Mbed 2 deprecated BadmintonLogger

Dependencies:   FreescaleIAP MMA8491Q_PG mbed

Fork of LELEC_2811_Accelerometer by LELEC2811 - I&S

main.cpp@4:2de56fc46abb, 2017-11-30 (annotated)

Committer:
ATCuriosity
Date:
Thu Nov 30 14:11:03 2017 +0000
Revision:
4:2de56fc46abb
Parent:
3:d03eae745223
Child:
5:79b8cd191fa8
presque fini

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ATCuriosity 4:2de56fc46abb 1 /*
ATCuriosity 4:2de56fc46abb 2 * LELEC 2811 - BadmintonLogger - Group 5
ATCuriosity 4:2de56fc46abb 3 */
ATCuriosity 4:2de56fc46abb 4
Salamandre 0:a18d6e69c993 5 #include "mbed.h"
Salamandre 0:a18d6e69c993 6 #include "FreescaleIAP.h" // Library for Flash Access
ATCuriosity 2:f146ae6546b5 7 #include "MMA8491Q_PG.h" // Accelerometer
ATCuriosity 4:2de56fc46abb 8 #include <cmath>
Salamandre 0:a18d6e69c993 9
Salamandre 0:a18d6e69c993 10 #define MMA8491_I2C_ADDRESS (0x55<<1)
ATCuriosity 4:2de56fc46abb 11 #define KL25Z_VDD 2.89 // Value of VDD : To be measured on board KL25Z pin P3V3 (calibration)
Salamandre 0:a18d6e69c993 12
Salamandre 0:a18d6e69c993 13 #define LED_ON 0
Salamandre 0:a18d6e69c993 14 #define LED_OFF 1
Salamandre 0:a18d6e69c993 15
ATCuriosity 4:2de56fc46abb 16 #define CONSOLE 0 // print all in console
ATCuriosity 4:2de56fc46abb 17 #define FLASH_MVT 1 // save in flash only mvts
ATCuriosity 4:2de56fc46abb 18 #define FLASH_ALL 2 // save in flash mvtSets
ATCuriosity 4:2de56fc46abb 19
Salamandre 0:a18d6e69c993 20 #define REG_OUT_X_MSB 0x01
Salamandre 0:a18d6e69c993 21 #define REG_OUT_Y_MSB 0x03
Salamandre 0:a18d6e69c993 22 #define REG_OUT_Z_MSB 0x05
ATCuriosity 3:d03eae745223 23
ATCuriosity 4:2de56fc46abb 24 #define SECTOR_SIZE 1024 // Numbers of bits by memory sector
ATCuriosity 4:2de56fc46abb 25 #define RESERVED_SECTOR 32 // 32K reserved for Application Code
ATCuriosity 3:d03eae745223 26
ATCuriosity 3:d03eae745223 27 #define ACQ_TIMER_PERIOD 0.01 // Time between 2 acquisitions (here 10 mSec)
ATCuriosity 3:d03eae745223 28 #define N_PTS 50 // Number of points for each axis used to detect mvt
ATCuriosity 4:2de56fc46abb 29 #define N_MVTS 5 // Number of mvts detected
ATCuriosity 4:2de56fc46abb 30 #define THRESHOLD_MVT 0.5 // threshold to validate a mvt
ATCuriosity 3:d03eae745223 31 #define THRESHOLD_SHOCK 0.5 // threshold to detect shock
Salamandre 0:a18d6e69c993 32
ATCuriosity 3:d03eae745223 33 MMA8491Q my8491(PTE0, PTE1, MMA8491_I2C_ADDRESS); // Setup I2C for MMA8491
Salamandre 0:a18d6e69c993 34
ATCuriosity 3:d03eae745223 35 Serial Host_Comm(USBTX, USBRX); // Set Serial Port
Salamandre 0:a18d6e69c993 36
ATCuriosity 4:2de56fc46abb 37 AnalogIn myPTE20(PTE20); // read Vout_IF
ATCuriosity 4:2de56fc46abb 38 AnalogIn myPTE21(PTE21); // read Vout_FILT
ATCuriosity 4:2de56fc46abb 39 AnalogIn myPTE22(PTE22); // read Vout_GAIN
Salamandre 0:a18d6e69c993 40
ATCuriosity 3:d03eae745223 41 Ticker myTick_Acq; // Periodical timer for Acquisition
ATCuriosity 4:2de56fc46abb 42
ATCuriosity 3:d03eae745223 43 DigitalOut Led_Red(LED1); // Define I/O for LEDs
Salamandre 0:a18d6e69c993 44 DigitalOut Led_Green(LED2);
Salamandre 0:a18d6e69c993 45 DigitalOut Led_Blue(LED3);
ATCuriosity 4:2de56fc46abb 46
Salamandre 0:a18d6e69c993 47 DigitalOut Accel_Enable(PTA13);
Salamandre 0:a18d6e69c993 48
ATCuriosity 4:2de56fc46abb 49 DigitalOut Start_Pulse_Out(PTE4); // Used to enter/exit Acquisition mode
ATCuriosity 4:2de56fc46abb 50 DigitalIn Start_Pulse_In(PTE5); // ShortPin J1_15 and J1_16 to enter in Acq_Mode
Salamandre 0:a18d6e69c993 51
ATCuriosity 3:d03eae745223 52 // --------------------- Structure and Enumeration ---------------------
ATCuriosity 3:d03eae745223 53 struct Data {
ATCuriosity 3:d03eae745223 54 int16_t accX, accY, accZ;
ATCuriosity 4:2de56fc46abb 55 float Vout_IF, Vout_FILT, Vout_GAIN;
ATCuriosity 3:d03eae745223 56 };
ATCuriosity 3:d03eae745223 57
ATCuriosity 4:2de56fc46abb 58 enum Mvt { Undefined = 0, Serve, ClearOverhead, DropOverhead, SmashShot, ClearUnderarm, DropUnderarm };
ATCuriosity 4:2de56fc46abb 59
ATCuriosity 4:2de56fc46abb 60 struct MvtSet {
ATCuriosity 4:2de56fc46abb 61 int16_t inputs [N_PTS*3];
ATCuriosity 4:2de56fc46abb 62 Mvt mvt;
ATCuriosity 3:d03eae745223 63 };
ATCuriosity 3:d03eae745223 64
ATCuriosity 3:d03eae745223 65 // -------------------------- Globale variable --------------------------
ATCuriosity 3:d03eae745223 66 volatile bool bTimer; // 1 means a Timer tick is done
Salamandre 0:a18d6e69c993 67
ATCuriosity 4:2de56fc46abb 68 bool foundError = 0;
ATCuriosity 4:2de56fc46abb 69 int mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 70
ATCuriosity 4:2de56fc46abb 71 /* in flash :
ATCuriosity 4:2de56fc46abb 72 - [ 0x0 ; flash_base_address [ : code
ATCuriosity 4:2de56fc46abb 73 - flash_base_address : flash_next_address (int)
ATCuriosity 4:2de56fc46abb 74 - flash_base_address+4 : mode (int)
ATCuriosity 4:2de56fc46abb 75 - flash_base_address+8 : first address to store data */
Salamandre 0:a18d6e69c993 76 uint32_t KL25_Flash_Size;
ATCuriosity 4:2de56fc46abb 77 int flash_base_address = RESERVED_SECTOR * SECTOR_SIZE ; // Store Flash Base Address
ATCuriosity 4:2de56fc46abb 78 int flash_next_address; // next address for saving data in flash
ATCuriosity 4:2de56fc46abb 79 int flash_base_address_cmd; // base address where the parameters are saved
ATCuriosity 4:2de56fc46abb 80
ATCuriosity 4:2de56fc46abb 81 const float Weights[3*N_PTS*N_MVTS] = {
ATCuriosity 4:2de56fc46abb 82 #include "Weights.txt"
ATCuriosity 4:2de56fc46abb 83 };
ATCuriosity 4:2de56fc46abb 84 const float Biases[N_MVTS] = {
ATCuriosity 4:2de56fc46abb 85 #include "Biases.txt"
ATCuriosity 4:2de56fc46abb 86 };
Salamandre 0:a18d6e69c993 87
ATCuriosity 3:d03eae745223 88 // ------------------------ Function Declaration ------------------------
ATCuriosity 3:d03eae745223 89 void Init(void);
ATCuriosity 4:2de56fc46abb 90 void Clear_Led(void); // switch off led's
ATCuriosity 4:2de56fc46abb 91 bool Check_Jumper(void); // if J1_15 & J1_16 connected together -> return 1
ATCuriosity 4:2de56fc46abb 92 void Check_Console(void); // detect input from user in console
ATCuriosity 4:2de56fc46abb 93 void myTimer_Acq_Task(void); // called by the timer
ATCuriosity 3:d03eae745223 94
ATCuriosity 4:2de56fc46abb 95 void EraseAllSectors(void); // erase all sectors containing data
ATCuriosity 4:2de56fc46abb 96 void EraseSector(int address); // erase one sector
ATCuriosity 4:2de56fc46abb 97 void UpdateParamFlash(void); // update next_address_flash and mode in flash
ATCuriosity 4:2de56fc46abb 98 void WriteFlash(MvtSet mvtSet); // write only mvt or all set depending on 'all'
ATCuriosity 4:2de56fc46abb 99 void ReadFlash(void); // print memory content in console
Salamandre 0:a18d6e69c993 100
ATCuriosity 4:2de56fc46abb 101 Data ReadData(void); // read data from accelerometer and piezo
ATCuriosity 4:2de56fc46abb 102 void Log(void); // read data, detect shock and movement
ATCuriosity 4:2de56fc46abb 103 void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs); // inputs = AccDataLog rotated of amount
ATCuriosity 4:2de56fc46abb 104 void PrintSet(int16_t *inputs); // display set of data
ATCuriosity 4:2de56fc46abb 105 float Sigmoid(float x); // sigmoid function (used by neural network)
ATCuriosity 4:2de56fc46abb 106 Mvt SelectMvt(int16_t *inputs); // compute probabilities for each mvt based on the inputs
Salamandre 0:a18d6e69c993 107
ATCuriosity 4:2de56fc46abb 108 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 109 // -------------------------------------------------------------------------------------------------------
Salamandre 0:a18d6e69c993 110
ATCuriosity 3:d03eae745223 111 // -------------------------------- main --------------------------------
ATCuriosity 3:d03eae745223 112 int main()
ATCuriosity 3:d03eae745223 113 {
ATCuriosity 3:d03eae745223 114 Init ();
ATCuriosity 3:d03eae745223 115
ATCuriosity 4:2de56fc46abb 116 int Count;
Salamandre 0:a18d6e69c993 117
ATCuriosity 4:2de56fc46abb 118 while(!foundError)
Salamandre 0:a18d6e69c993 119 {
ATCuriosity 3:d03eae745223 120 if (Check_Jumper())
Salamandre 0:a18d6e69c993 121 {
Salamandre 0:a18d6e69c993 122 Clear_Led();
Salamandre 0:a18d6e69c993 123 Count = 5;
Salamandre 0:a18d6e69c993 124 while (Count !=0)
Salamandre 0:a18d6e69c993 125 {
ATCuriosity 3:d03eae745223 126 if (Check_Jumper())
Salamandre 0:a18d6e69c993 127 {
ATCuriosity 3:d03eae745223 128 Led_Blue = LED_ON; // Blink to alert user "Enter in Logging mode"
ATCuriosity 4:2de56fc46abb 129 wait_ms(750);
Salamandre 0:a18d6e69c993 130 Led_Blue = LED_OFF;
ATCuriosity 4:2de56fc46abb 131 wait_ms(250);
Salamandre 0:a18d6e69c993 132 Count --;
Salamandre 0:a18d6e69c993 133 if (Count == 0)
ATCuriosity 3:d03eae745223 134 Log();
Salamandre 0:a18d6e69c993 135 }
Salamandre 0:a18d6e69c993 136 else
Salamandre 0:a18d6e69c993 137 Count = 0;
ATCuriosity 3:d03eae745223 138 }
Salamandre 0:a18d6e69c993 139 }
ATCuriosity 4:2de56fc46abb 140 Led_Blue = !Led_Blue;
ATCuriosity 4:2de56fc46abb 141 Check_Console();
ATCuriosity 4:2de56fc46abb 142 wait_ms(100);
Salamandre 0:a18d6e69c993 143 }
ATCuriosity 4:2de56fc46abb 144
ATCuriosity 4:2de56fc46abb 145 Host_Comm.printf("\n\rProgram is exiting due to error...\n\r");
ATCuriosity 4:2de56fc46abb 146 Clear_Led();
ATCuriosity 4:2de56fc46abb 147 Led_Red = LED_ON;
Salamandre 0:a18d6e69c993 148 }
Salamandre 0:a18d6e69c993 149
ATCuriosity 3:d03eae745223 150 // -------------------------------- Init --------------------------------
ATCuriosity 4:2de56fc46abb 151 void Init()
ATCuriosity 3:d03eae745223 152 {
ATCuriosity 3:d03eae745223 153 Start_Pulse_In.mode(PullNone); // Input Pin is programmed as floating
ATCuriosity 4:2de56fc46abb 154 Accel_Enable = 0; // Turn Accel Enable to disabled state
ATCuriosity 3:d03eae745223 155 Clear_Led();
ATCuriosity 3:d03eae745223 156
ATCuriosity 3:d03eae745223 157 myTick_Acq.attach(&myTimer_Acq_Task, ACQ_TIMER_PERIOD); // Timer for acquisition
ATCuriosity 3:d03eae745223 158
ATCuriosity 3:d03eae745223 159 Host_Comm.baud(115200); // Baud rate setting
ATCuriosity 4:2de56fc46abb 160 Host_Comm.printf("\n\rLELEC2811 - Badminton Logger - Group 5\n\r");
ATCuriosity 4:2de56fc46abb 161
ATCuriosity 4:2de56fc46abb 162 KL25_Flash_Size = flash_size(); // Get Size of KL25 Embedded Flash
ATCuriosity 4:2de56fc46abb 163 flash_base_address_cmd = KL25_Flash_Size-SECTOR_SIZE;
ATCuriosity 4:2de56fc46abb 164
ATCuriosity 4:2de56fc46abb 165 int *base_address_ptr = (int*)flash_base_address_cmd;
ATCuriosity 4:2de56fc46abb 166 flash_next_address = base_address_ptr[0];
ATCuriosity 4:2de56fc46abb 167 if (flash_next_address >= flash_base_address_cmd || flash_next_address < flash_base_address)
ATCuriosity 4:2de56fc46abb 168 {
ATCuriosity 4:2de56fc46abb 169 Host_Comm.printf("First run (or error with previous flash_next_address)\n\r");
ATCuriosity 4:2de56fc46abb 170 EraseAllSectors();
ATCuriosity 4:2de56fc46abb 171 flash_next_address = flash_base_address;
ATCuriosity 4:2de56fc46abb 172 mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 173 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 174 }
ATCuriosity 4:2de56fc46abb 175 else {
ATCuriosity 4:2de56fc46abb 176 // add 0xFFFF in flash to tag reset
ATCuriosity 4:2de56fc46abb 177 mode = base_address_ptr[1];
ATCuriosity 4:2de56fc46abb 178 if (mode != CONSOLE && mode != FLASH_MVT && mode != FLASH_ALL) {
ATCuriosity 4:2de56fc46abb 179 mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 180 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 181 }
ATCuriosity 4:2de56fc46abb 182 }
ATCuriosity 4:2de56fc46abb 183
ATCuriosity 4:2de56fc46abb 184 Host_Comm.printf("flash_next_address = %d ; mode = %d\n\r",flash_next_address, mode);
ATCuriosity 4:2de56fc46abb 185 Host_Comm.printf("Memory used = %f %%\n\r",(flash_next_address-flash_base_address)/(flash_base_address_cmd-flash_base_address));
ATCuriosity 4:2de56fc46abb 186
ATCuriosity 4:2de56fc46abb 187 Host_Comm.printf("Initialization done.\n\n\r");
ATCuriosity 4:2de56fc46abb 188
ATCuriosity 4:2de56fc46abb 189 Host_Comm.printf("When the jumper is removed, use the keyboard :\n\r");
ATCuriosity 4:2de56fc46abb 190 Host_Comm.printf("- to erase flash : 'e' = erase flash\n\r");
ATCuriosity 4:2de56fc46abb 191 Host_Comm.printf("- to change mode : 'c' = console mode ; 'm' = write_mvt mode ; 'a' = write_all mode\n\r");
ATCuriosity 4:2de56fc46abb 192 Host_Comm.printf("- to read flash : 'r' = read flash ; 's' = stop reading\n\r");
ATCuriosity 3:d03eae745223 193 }
ATCuriosity 3:d03eae745223 194
ATCuriosity 3:d03eae745223 195 // ----------------------------- Clear_Led ------------------------------
ATCuriosity 4:2de56fc46abb 196 void Clear_Led()
Salamandre 0:a18d6e69c993 197 {
Salamandre 0:a18d6e69c993 198 Led_Red = LED_OFF;
Salamandre 0:a18d6e69c993 199 Led_Green = LED_OFF;
ATCuriosity 4:2de56fc46abb 200 Led_Blue = LED_OFF ;
Salamandre 0:a18d6e69c993 201 }
Salamandre 0:a18d6e69c993 202
ATCuriosity 3:d03eae745223 203 // ---------------------------- Check_Jumper ----------------------------
ATCuriosity 3:d03eae745223 204 bool Check_Jumper()
Salamandre 0:a18d6e69c993 205 {
ATCuriosity 4:2de56fc46abb 206 int i;
Salamandre 0:a18d6e69c993 207 for (i = 0 ; i < 2 ; i ++)
Salamandre 0:a18d6e69c993 208 {
ATCuriosity 3:d03eae745223 209 Start_Pulse_Out = 1;
Salamandre 0:a18d6e69c993 210 wait_ms(1);
ATCuriosity 3:d03eae745223 211 if (Start_Pulse_In != 1)
ATCuriosity 3:d03eae745223 212 return 0;
Salamandre 0:a18d6e69c993 213
ATCuriosity 3:d03eae745223 214 Start_Pulse_Out = 0;
Salamandre 0:a18d6e69c993 215 wait_ms(1);
ATCuriosity 3:d03eae745223 216 if (Start_Pulse_In != 0)
ATCuriosity 3:d03eae745223 217 return 0;
Salamandre 0:a18d6e69c993 218 }
ATCuriosity 3:d03eae745223 219 return 1;
Salamandre 0:a18d6e69c993 220 }
Salamandre 0:a18d6e69c993 221
ATCuriosity 4:2de56fc46abb 222 // --------------------------- Check_Console ----------------------------
ATCuriosity 4:2de56fc46abb 223 void Check_Console()
ATCuriosity 4:2de56fc46abb 224 {
ATCuriosity 4:2de56fc46abb 225 if(Host_Comm.readable())
ATCuriosity 4:2de56fc46abb 226 {
ATCuriosity 4:2de56fc46abb 227 char cmd = Host_Comm.getc();
ATCuriosity 4:2de56fc46abb 228 if ((cmd == 'E') || (cmd == 'e'))
ATCuriosity 4:2de56fc46abb 229 EraseAllSectors();
ATCuriosity 4:2de56fc46abb 230 else if ((cmd == 'C') || (cmd == 'c'))
ATCuriosity 4:2de56fc46abb 231 mode = CONSOLE;
ATCuriosity 4:2de56fc46abb 232 else if ((cmd == 'M') || (cmd == 'm'))
ATCuriosity 4:2de56fc46abb 233 mode = FLASH_MVT;
ATCuriosity 4:2de56fc46abb 234 else if ((cmd == 'A') || (cmd == 'a'))
ATCuriosity 4:2de56fc46abb 235 mode = FLASH_ALL;
ATCuriosity 4:2de56fc46abb 236 else if ((cmd == 'R') || (cmd == 'r'))
ATCuriosity 4:2de56fc46abb 237 ReadFlash();
ATCuriosity 4:2de56fc46abb 238
ATCuriosity 4:2de56fc46abb 239 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 240 }
ATCuriosity 4:2de56fc46abb 241 }
ATCuriosity 4:2de56fc46abb 242
ATCuriosity 3:d03eae745223 243 // -------------------------- myTimer_Acq_Task --------------------------
ATCuriosity 3:d03eae745223 244 void myTimer_Acq_Task() { bTimer = 1; }
ATCuriosity 3:d03eae745223 245
ATCuriosity 4:2de56fc46abb 246 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 247 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 248
ATCuriosity 4:2de56fc46abb 249 // -------------------------- EraseAllSectors ---------------------------
ATCuriosity 4:2de56fc46abb 250 void EraseAllSectors(void)
ATCuriosity 4:2de56fc46abb 251 {
ATCuriosity 4:2de56fc46abb 252 for (int address = flash_base_address ; address < KL25_Flash_Size ; address += SECTOR_SIZE)
ATCuriosity 4:2de56fc46abb 253 {
ATCuriosity 4:2de56fc46abb 254 EraseSector(address);
ATCuriosity 4:2de56fc46abb 255 if(foundError)
ATCuriosity 4:2de56fc46abb 256 return;
ATCuriosity 4:2de56fc46abb 257 }
ATCuriosity 4:2de56fc46abb 258 }
ATCuriosity 4:2de56fc46abb 259
ATCuriosity 4:2de56fc46abb 260 // ---------------------------- EraseSector -----------------------------
ATCuriosity 4:2de56fc46abb 261 void EraseSector(int address)
ATCuriosity 4:2de56fc46abb 262 {
ATCuriosity 4:2de56fc46abb 263 IAPCode status = erase_sector(address);
ATCuriosity 4:2de56fc46abb 264 if (status != Success) {
ATCuriosity 4:2de56fc46abb 265 Host_Comm.printf("\n\rError in EraseSector() : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 266 foundError = 1;
ATCuriosity 4:2de56fc46abb 267 }
ATCuriosity 4:2de56fc46abb 268 }
ATCuriosity 4:2de56fc46abb 269
ATCuriosity 4:2de56fc46abb 270 // -------------------------- UpdateParamFlash --------------------------
ATCuriosity 4:2de56fc46abb 271 void UpdateParamFlash()
ATCuriosity 4:2de56fc46abb 272 {
ATCuriosity 4:2de56fc46abb 273 EraseSector(flash_base_address_cmd);
ATCuriosity 4:2de56fc46abb 274 if(foundError)
ATCuriosity 4:2de56fc46abb 275 return;
ATCuriosity 4:2de56fc46abb 276
ATCuriosity 4:2de56fc46abb 277 int toWrite[2] = {flash_next_address,mode};
ATCuriosity 4:2de56fc46abb 278 IAPCode status = program_flash(flash_base_address_cmd, (char *) &toWrite, 2*sizeof(int));
ATCuriosity 4:2de56fc46abb 279 if (status != Success) {
ATCuriosity 4:2de56fc46abb 280 Host_Comm.printf("\n\rError in UpdateParamFlash() : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 281 foundError = 1;
ATCuriosity 4:2de56fc46abb 282 }
ATCuriosity 4:2de56fc46abb 283 }
ATCuriosity 4:2de56fc46abb 284
ATCuriosity 4:2de56fc46abb 285 // ----------------------------- WriteFlash -----------------------------
ATCuriosity 4:2de56fc46abb 286 void WriteFlash(MvtSet mvtSet)
ATCuriosity 4:2de56fc46abb 287 {
ATCuriosity 4:2de56fc46abb 288 IAPCode status;
ATCuriosity 4:2de56fc46abb 289
ATCuriosity 4:2de56fc46abb 290 if (mode == FLASH_ALL) // inputs (2*3*N_PTS bytes) + mvt (1 byte)
ATCuriosity 4:2de56fc46abb 291 {
ATCuriosity 4:2de56fc46abb 292 // add all bytes one behind the other : 2bytes*3*N_PTS (inputs) + 1byte (mvt)
ATCuriosity 4:2de56fc46abb 293 int remainder = (3*N_PTS) % 2; // modulo 2 because compacting 2bytes into 4bytes words
ATCuriosity 4:2de56fc46abb 294
ATCuriosity 4:2de56fc46abb 295 status = program_flash(flash_next_address, (char*) &(mvtSet.inputs), 2*(3*N_PTS-remainder));
ATCuriosity 4:2de56fc46abb 296 if (status != Success) {
ATCuriosity 4:2de56fc46abb 297 Host_Comm.printf("\n\rError in WriteFlash() (0) : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 298 foundError = 1; return;
ATCuriosity 4:2de56fc46abb 299 }
ATCuriosity 4:2de56fc46abb 300 flash_next_address += 2*(3*N_PTS-remainder);
ATCuriosity 4:2de56fc46abb 301
ATCuriosity 4:2de56fc46abb 302 int16_t toWrite[2] = {0,(int16_t) mvtSet.mvt};
ATCuriosity 4:2de56fc46abb 303 if(remainder)
ATCuriosity 4:2de56fc46abb 304 toWrite[0] = mvtSet.inputs[3*N_PTS-1];
ATCuriosity 4:2de56fc46abb 305 status = program_flash(flash_next_address, (char*) &toWrite, 4);
ATCuriosity 4:2de56fc46abb 306 if (status != Success) {
ATCuriosity 4:2de56fc46abb 307 Host_Comm.printf("\n\rError in WriteFlash() (1) : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 308 foundError = 1; return;
ATCuriosity 4:2de56fc46abb 309 }
ATCuriosity 4:2de56fc46abb 310 }
ATCuriosity 4:2de56fc46abb 311 else // 4 mvts (4 bytes) compacted in one 32bits slot
ATCuriosity 4:2de56fc46abb 312 {
ATCuriosity 4:2de56fc46abb 313 /*
ATCuriosity 4:2de56fc46abb 314 int innerPosition = flash_next_address % 4;
ATCuriosity 4:2de56fc46abb 315 int local_base_address = flash_next_address-innerPosition;
ATCuriosity 4:2de56fc46abb 316 int *address_ptr = (int*)(local_base_address);
ATCuriosity 4:2de56fc46abb 317
ATCuriosity 4:2de56fc46abb 318 int currValue = address_ptr[0];
ATCuriosity 4:2de56fc46abb 319 uint8_t toWrite[4] = {(uint8_t)(currValue>>24),(uint8_t)(currValue>>16),(uint8_t)(currValue>>8),(uint8_t)currValue};
ATCuriosity 4:2de56fc46abb 320 toWrite[innerPosition] = (uint8_t)mvtSet.mvt;
ATCuriosity 4:2de56fc46abb 321
ATCuriosity 4:2de56fc46abb 322 status = program_flash(local_base_address, (char*) &toWrite, 4);
ATCuriosity 4:2de56fc46abb 323 if (status != Success) {
ATCuriosity 4:2de56fc46abb 324 Host_Comm.printf("\n\rError in WriteFlash() (2) : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 325 foundError = 1; return;
ATCuriosity 4:2de56fc46abb 326 }
ATCuriosity 4:2de56fc46abb 327 flash_next_address++;
ATCuriosity 4:2de56fc46abb 328 */
ATCuriosity 4:2de56fc46abb 329 uint8_t toWrite[4] = {0,0,0,mvtSet.mvt};
ATCuriosity 4:2de56fc46abb 330 status = program_flash(flash_next_address, (char*) &toWrite, 4);
ATCuriosity 4:2de56fc46abb 331 if (status != Success) {
ATCuriosity 4:2de56fc46abb 332 Host_Comm.printf("\n\rError in WriteFlash() (2) : status = %d\n\r", status);
ATCuriosity 4:2de56fc46abb 333 foundError = 1; return;
ATCuriosity 4:2de56fc46abb 334 }
ATCuriosity 4:2de56fc46abb 335 }
ATCuriosity 4:2de56fc46abb 336
ATCuriosity 4:2de56fc46abb 337 flash_next_address += 4;
ATCuriosity 4:2de56fc46abb 338 UpdateParamFlash();
ATCuriosity 4:2de56fc46abb 339 }
ATCuriosity 4:2de56fc46abb 340
ATCuriosity 4:2de56fc46abb 341 // ----------------------------- ReadFlash ------------------------------
ATCuriosity 4:2de56fc46abb 342 void ReadFlash()
ATCuriosity 4:2de56fc46abb 343 {
ATCuriosity 4:2de56fc46abb 344 Host_Comm.printf("\n\r------ Begin Read Flash ------\n\r");
ATCuriosity 4:2de56fc46abb 345 Host_Comm.printf("flash_next_address = %d ; mode = %d\n\r",flash_next_address, mode);
ATCuriosity 4:2de56fc46abb 346 Host_Comm.printf("Memory used = %f %%\n\r",(flash_next_address-flash_base_address)/(flash_base_address_cmd-flash_base_address));
ATCuriosity 4:2de56fc46abb 347
ATCuriosity 4:2de56fc46abb 348 char cmd;
ATCuriosity 4:2de56fc46abb 349 int *address_ptr;
ATCuriosity 4:2de56fc46abb 350
ATCuriosity 4:2de56fc46abb 351 int remainder = (flash_next_address) % 4;
ATCuriosity 4:2de56fc46abb 352 int local_base_address = flash_next_address-remainder;
ATCuriosity 4:2de56fc46abb 353
ATCuriosity 4:2de56fc46abb 354 for (int address = flash_base_address+8; address < local_base_address; address+=4)
ATCuriosity 4:2de56fc46abb 355 {
ATCuriosity 4:2de56fc46abb 356 if(Host_Comm.readable())
ATCuriosity 4:2de56fc46abb 357 {
ATCuriosity 4:2de56fc46abb 358 cmd = Host_Comm.getc();
ATCuriosity 4:2de56fc46abb 359 if ((cmd == 'S') || (cmd == 's'))
ATCuriosity 4:2de56fc46abb 360 return;
ATCuriosity 4:2de56fc46abb 361 }
ATCuriosity 4:2de56fc46abb 362
ATCuriosity 4:2de56fc46abb 363 address_ptr = (int*)address;
ATCuriosity 4:2de56fc46abb 364 Host_Comm.printf("%d %d %d %d\n\r",(uint8_t)(address_ptr[0]>>24),(uint8_t)(address_ptr[0]>>16),(uint8_t)(address_ptr[0]>>8),(uint8_t)address_ptr[0]);
ATCuriosity 4:2de56fc46abb 365 }
ATCuriosity 4:2de56fc46abb 366
ATCuriosity 4:2de56fc46abb 367 address_ptr = (int*)local_base_address;
ATCuriosity 4:2de56fc46abb 368 for (int i = 0; i < remainder; i++)
ATCuriosity 4:2de56fc46abb 369 Host_Comm.printf("%d ",(uint8_t)(address_ptr[0]>>(8*(3-i))));
ATCuriosity 4:2de56fc46abb 370
ATCuriosity 4:2de56fc46abb 371 Host_Comm.printf("\n\r------- End Read Flash -------\n\n\r");
ATCuriosity 4:2de56fc46abb 372 }
ATCuriosity 4:2de56fc46abb 373
ATCuriosity 4:2de56fc46abb 374 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 375 // -------------------------------------------------------------------------------------------------------
ATCuriosity 4:2de56fc46abb 376
ATCuriosity 3:d03eae745223 377 // ------------------------------ ReadData ------------------------------
ATCuriosity 3:d03eae745223 378 Data ReadData()
Salamandre 0:a18d6e69c993 379 {
ATCuriosity 3:d03eae745223 380 Data data;
ATCuriosity 3:d03eae745223 381
ATCuriosity 3:d03eae745223 382 // Get Accelerometer data's
ATCuriosity 4:2de56fc46abb 383 Accel_Enable = 1; // Rising Edge -> Start measure
ATCuriosity 3:d03eae745223 384
ATCuriosity 4:2de56fc46abb 385 int ready = 0;
ATCuriosity 3:d03eae745223 386 while((ready && 0x10) == 0) // Wait for accelerometer to have new data's
ATCuriosity 3:d03eae745223 387 ready = my8491.Read_Status();
ATCuriosity 3:d03eae745223 388
ATCuriosity 3:d03eae745223 389 data.accX = my8491.getAccAxis(REG_OUT_X_MSB);
ATCuriosity 3:d03eae745223 390 data.accY = my8491.getAccAxis(REG_OUT_Y_MSB);
ATCuriosity 3:d03eae745223 391 data.accZ = my8491.getAccAxis(REG_OUT_Z_MSB);
Salamandre 0:a18d6e69c993 392
ATCuriosity 4:2de56fc46abb 393 Accel_Enable = 0;
Salamandre 0:a18d6e69c993 394
ATCuriosity 3:d03eae745223 395 // Get Piezo Data's
ATCuriosity 4:2de56fc46abb 396 data.Vout_IF = ((float) myPTE20.read_u16() / 0XFFFF) * KL25Z_VDD; // convert in volt
ATCuriosity 4:2de56fc46abb 397 data.Vout_FILT = ((float) myPTE21.read_u16() / 0XFFFF) * KL25Z_VDD;
ATCuriosity 4:2de56fc46abb 398 data.Vout_GAIN = ((float) myPTE22.read_u16() / 0XFFFF) * KL25Z_VDD;
ATCuriosity 3:d03eae745223 399
ATCuriosity 3:d03eae745223 400 return data;
ATCuriosity 3:d03eae745223 401 }
ATCuriosity 3:d03eae745223 402
ATCuriosity 3:d03eae745223 403 // -------------------------------- Log ---------------------------------
ATCuriosity 3:d03eae745223 404 void Log()
ATCuriosity 3:d03eae745223 405 {
ATCuriosity 3:d03eae745223 406 Data currData;
ATCuriosity 3:d03eae745223 407 int16_t AccDataLog [N_PTS*3] = {}; // array to save latest data read
ATCuriosity 4:2de56fc46abb 408 int index_write = 0; // current position to write data in AccDataLog
ATCuriosity 4:2de56fc46abb 409 bool enoughData = 0;
ATCuriosity 4:2de56fc46abb 410 bool shockDetected = 0; // if shock detected
ATCuriosity 4:2de56fc46abb 411 int n_sinceShock = 0; // number of ReadData() since last chock
ATCuriosity 3:d03eae745223 412
ATCuriosity 4:2de56fc46abb 413 while(Check_Jumper() && !foundError)
Salamandre 0:a18d6e69c993 414 {
ATCuriosity 3:d03eae745223 415 Led_Green != Led_Green; // LED blinks green while logging
ATCuriosity 3:d03eae745223 416
ATCuriosity 3:d03eae745223 417 while (bTimer == 0) {} // Wait Acq Tick Timer
Salamandre 0:a18d6e69c993 418 bTimer = 0;
Salamandre 0:a18d6e69c993 419
ATCuriosity 3:d03eae745223 420 currData = ReadData();
ATCuriosity 4:2de56fc46abb 421 //Host_Comm.printf("%d ; %d ; %d ; %f\n\r", currData.accX, currData.accY, currData.accZ, currData.Vout_FILT);
ATCuriosity 3:d03eae745223 422 AccDataLog[index_write*3] = currData.accX;
ATCuriosity 3:d03eae745223 423 AccDataLog[index_write*3+1] = currData.accY;
ATCuriosity 3:d03eae745223 424 AccDataLog[index_write*3+2] = currData.accZ;
Salamandre 0:a18d6e69c993 425
ATCuriosity 4:2de56fc46abb 426 float amplitude = abs(currData.Vout_FILT - KL25Z_VDD/2.0);
ATCuriosity 4:2de56fc46abb 427 //Host_Comm.printf("amplitude = %f\n\r",amplitude);
ATCuriosity 4:2de56fc46abb 428 if (amplitude >= THRESHOLD_SHOCK && enoughData)
Salamandre 0:a18d6e69c993 429 {
ATCuriosity 3:d03eae745223 430 shockDetected = 1;
ATCuriosity 3:d03eae745223 431 n_sinceShock = 0;
Salamandre 0:a18d6e69c993 432 }
ATCuriosity 3:d03eae745223 433 if (n_sinceShock == N_PTS/2 && shockDetected == 1)
Salamandre 0:a18d6e69c993 434 {
ATCuriosity 4:2de56fc46abb 435 MvtSet mvtSet;
ATCuriosity 4:2de56fc46abb 436 Rotate(AccDataLog, N_PTS-1-index_write, mvtSet.inputs);
ATCuriosity 4:2de56fc46abb 437 mvtSet.mvt = SelectMvt(mvtSet.inputs);
ATCuriosity 4:2de56fc46abb 438 if (mode == CONSOLE)
ATCuriosity 4:2de56fc46abb 439 PrintSet(mvtSet.inputs);
ATCuriosity 4:2de56fc46abb 440 else
ATCuriosity 4:2de56fc46abb 441 WriteFlash(mvtSet);
ATCuriosity 3:d03eae745223 442 shockDetected = 0;
Salamandre 0:a18d6e69c993 443 }
ATCuriosity 4:2de56fc46abb 444
ATCuriosity 3:d03eae745223 445 index_write ++;
ATCuriosity 3:d03eae745223 446 n_sinceShock ++;
ATCuriosity 4:2de56fc46abb 447 if (index_write == N_PTS)
ATCuriosity 4:2de56fc46abb 448 {
ATCuriosity 4:2de56fc46abb 449 enoughData = 1;
ATCuriosity 3:d03eae745223 450 index_write = 0;
ATCuriosity 4:2de56fc46abb 451 }
Salamandre 0:a18d6e69c993 452 }
ATCuriosity 3:d03eae745223 453 Clear_Led();
ATCuriosity 3:d03eae745223 454 }
ATCuriosity 3:d03eae745223 455
ATCuriosity 4:2de56fc46abb 456 // ------------------------------- Rotate -------------------------------
ATCuriosity 4:2de56fc46abb 457 void Rotate(int16_t *AccDataLog, int amount, int16_t *inputs)
ATCuriosity 3:d03eae745223 458 {
ATCuriosity 4:2de56fc46abb 459 for(int i = 0; i < N_PTS; i++)
ATCuriosity 4:2de56fc46abb 460 for(int j = 0; j < 3; j++)
ATCuriosity 4:2de56fc46abb 461 inputs[((i+amount)%N_PTS)*3+j] = AccDataLog[i*3+j];
Salamandre 0:a18d6e69c993 462 }
Salamandre 0:a18d6e69c993 463
ATCuriosity 4:2de56fc46abb 464 // ------------------------------ PrintSet ------------------------------
ATCuriosity 4:2de56fc46abb 465 void PrintSet(int16_t *inputs)
ATCuriosity 3:d03eae745223 466 {
ATCuriosity 4:2de56fc46abb 467 Host_Comm.printf("------ Begin Set ------\n\r");
ATCuriosity 4:2de56fc46abb 468 for(int i = 0; i < N_PTS; i++)
ATCuriosity 4:2de56fc46abb 469 Host_Comm.printf("%d %d %d\n\r", inputs[i*3],inputs[i*3+1],inputs[i*3+2]);
ATCuriosity 4:2de56fc46abb 470 Host_Comm.printf("------- End Set -------\n\n\r");
ATCuriosity 3:d03eae745223 471 }
Salamandre 0:a18d6e69c993 472
ATCuriosity 3:d03eae745223 473 // ------------------------------ Sigmoid -------------------------------
ATCuriosity 4:2de56fc46abb 474 float Sigmoid(float x) { return 1/(1+exp(-x)); }
Salamandre 0:a18d6e69c993 475
ATCuriosity 3:d03eae745223 476 // ----------------------------- SelectMvt ------------------------------
ATCuriosity 4:2de56fc46abb 477 Mvt SelectMvt(int16_t *inputs)
ATCuriosity 3:d03eae745223 478 {
ATCuriosity 3:d03eae745223 479 int i, j;
ATCuriosity 3:d03eae745223 480 float selection [N_MVTS] = {};
ATCuriosity 3:d03eae745223 481 for (j = 0; j < N_MVTS; j++) {
ATCuriosity 3:d03eae745223 482 for (i = 0; i < N_PTS*3; i++)
ATCuriosity 4:2de56fc46abb 483 selection[j] += (float)inputs[i] * Weights[i*N_MVTS+j];
ATCuriosity 4:2de56fc46abb 484 selection[j] = Sigmoid(selection[j] + Biases[j]);
ATCuriosity 3:d03eae745223 485 }
ATCuriosity 3:d03eae745223 486
ATCuriosity 4:2de56fc46abb 487 Mvt mvt = Undefined;
ATCuriosity 4:2de56fc46abb 488 for (i = 0; i < N_MVTS; i++) {
ATCuriosity 3:d03eae745223 489 if (selection[i] > THRESHOLD_MVT)
ATCuriosity 4:2de56fc46abb 490 mvt = static_cast<Mvt>(i+1);
ATCuriosity 4:2de56fc46abb 491 Host_Comm.printf("Proba mvt %d : %f\n\r",i+1,selection[i]);
ATCuriosity 4:2de56fc46abb 492 }
ATCuriosity 3:d03eae745223 493
ATCuriosity 3:d03eae745223 494 return mvt;
Salamandre 0:a18d6e69c993 495 }