Bob Giesberts / Mbed 2 deprecated Inductive_Sensor_3

Version 3.0: Switching to newer LDC1614 which is placed on the same PCB.

Dependencies:   Bob DS1825 LDC1614 LDC1101 SDFileSystem mbed

Fork of Inductive_Sensor by Bob Giesberts

main.cpp@13:2caedc56b863, 2016-08-23 (annotated)

Committer:
bobgiesberts
Date:
Tue Aug 23 08:26:14 2016 +0000
Revision:
13:2caedc56b863
Parent:
12:cceece4f3afb
Child:
15:5de8394fdb3c
Final version before switching to the LDC1614

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bobgiesberts 2:1a203732fc95 1 /**
bobgiesberts 2:1a203732fc95 2 * @file main.cpp
bobgiesberts 2:1a203732fc95 3 * @brief This file programs the processor for the inductive force sensor
bobgiesberts 2:1a203732fc95 4 * using the library LDC1101.h and LDC1101.cpp.
bobgiesberts 12:cceece4f3afb 5 * - Led: processing communication with LDC1101
bobgiesberts 10:3cab80866536 6 *
bobgiesberts 4:ae441c5727b9 7 * Log protocol:
bobgiesberts 13:2caedc56b863 8 * - 0:30 minutes at 20 Hz
bobgiesberts 13:2caedc56b863 9 * - 14:30 minutes rest
bobgiesberts 2:1a203732fc95 10 *
bobgiesberts 2:1a203732fc95 11 * @author Bob Giesberts
bobgiesberts 2:1a203732fc95 12 *
bobgiesberts 2:1a203732fc95 13 * @date 2015-12-17
bobgiesberts 2:1a203732fc95 14 */
bobgiesberts 10:3cab80866536 15 #include "mbed.h"
bobgiesberts 11:599ca9982e45 16 #include "LDC1101.h" // inductive force sensor
bobgiesberts 11:599ca9982e45 17 #include "SDFileSystem.h" // control the SD card
bobgiesberts 11:599ca9982e45 18 #include "Bob.h" // processorboard
bobgiesberts 11:599ca9982e45 19 #include "DS1825.h" // thermometer
bobgiesberts 10:3cab80866536 20
bobgiesberts 10:3cab80866536 21 #include <iostream>
bobgiesberts 10:3cab80866536 22 #include <vector>
bobgiesberts 10:3cab80866536 23 #include <string>
bobgiesberts 10:3cab80866536 24 using namespace std;
bobgiesberts 10:3cab80866536 25
bobgiesberts 5:736a81a59f3c 26 // SETTINGS
bobgiesberts 12:cceece4f3afb 27 bool DEBUG = false;
bobgiesberts 12:cceece4f3afb 28 float C = 120E-12; // Capacitor in F
bobgiesberts 13:2caedc56b863 29 uint16_t INTERVAL_FIRST = 3600; // First sampling time in seconds. 60:00 minutes = 3600 sec
bobgiesberts 12:cceece4f3afb 30 uint16_t INTERVAL_OFF = 870; // Waiting interval in seconds. 14:30 minutes = 870 sec (14*60+30)
bobgiesberts 12:cceece4f3afb 31 uint16_t INTERVAL_ON = 30; // Sampling interval in seconds. 0:30 minutes = 30 sec
bobgiesberts 12:cceece4f3afb 32
bobgiesberts 12:cceece4f3afb 33 // Correct pinnames
bobgiesberts 12:cceece4f3afb 34 // PinName _LEDpin = PTB0; // For the first sensor (a bright red led)
bobgiesberts 12:cceece4f3afb 35 // PinName _Tpin = PTB1; // For the first sensor
bobgiesberts 12:cceece4f3afb 36 PinName _LEDpin = PTB1; // For all other sensors (a faint green led)
bobgiesberts 12:cceece4f3afb 37 PinName _Tpin = PTB0; // For all other sensors
bobgiesberts 12:cceece4f3afb 38
bobgiesberts 0:e81b68888268 39
bobgiesberts 5:736a81a59f3c 40 // load libraries
bobgiesberts 12:cceece4f3afb 41 Bob bob( _LEDpin, // LED
bobgiesberts 12:cceece4f3afb 42 PTC3, // sd_enable
bobgiesberts 12:cceece4f3afb 43 PTE0, // sd_present
bobgiesberts 12:cceece4f3afb 44 PTC2, // battery
bobgiesberts 12:cceece4f3afb 45 PTE30 // 3V3_DET TODO: can we use this to detect if we're charging the battery? If so, how should we respond?
bobgiesberts 12:cceece4f3afb 46 );
bobgiesberts 12:cceece4f3afb 47 // Serial pc(USBTX, USBRX);
bobgiesberts 4:ae441c5727b9 48
bobgiesberts 4:ae441c5727b9 49 // timer variables
bobgiesberts 6:ff39d60061ca 50 uint32_t now = 0, next = 0, prev = 0;
bobgiesberts 8:8cc1960467ae 51 uint8_t t_high = 0;
bobgiesberts 11:599ca9982e45 52 uint32_t t_lost = 0, lost_prev = 0;
bobgiesberts 8:8cc1960467ae 53 uint8_t lost_high = 0;
bobgiesberts 11:599ca9982e45 54 uint32_t t_sleep = 0;
bobgiesberts 12:cceece4f3afb 55 uint32_t t;
bobgiesberts 4:ae441c5727b9 56
bobgiesberts 4:ae441c5727b9 57 // file variables
bobgiesberts 4:ae441c5727b9 58 FILE *fp;
bobgiesberts 4:ae441c5727b9 59 string filename = "/sd/data00.txt";
bobgiesberts 4:ae441c5727b9 60 const char *fn;
bobgiesberts 4:ae441c5727b9 61
bobgiesberts 11:599ca9982e45 62 // temperature variables
bobgiesberts 11:599ca9982e45 63 uint8_t Tdata[9];
bobgiesberts 11:599ca9982e45 64 int16_t T_bin;
bobgiesberts 11:599ca9982e45 65 float T;
bobgiesberts 11:599ca9982e45 66
bobgiesberts 5:736a81a59f3c 67 // temporal storage for data samples
bobgiesberts 12:cceece4f3afb 68 const uint8_t package_size = 60; // ±12-13 Hz, so 60 would take about 5 sec.
bobgiesberts 12:cceece4f3afb 69 struct mydata {
bobgiesberts 12:cceece4f3afb 70 uint32_t t[package_size]; // time (s)
bobgiesberts 12:cceece4f3afb 71 uint32_t L[package_size]; // LHR_DATA
bobgiesberts 12:cceece4f3afb 72 } collected;
bobgiesberts 12:cceece4f3afb 73 uint8_t counter = 0;
bobgiesberts 12:cceece4f3afb 74
bobgiesberts 12:cceece4f3afb 75 // function to write all data to the SD card
bobgiesberts 12:cceece4f3afb 76 void storeit( float T )
bobgiesberts 12:cceece4f3afb 77 {
bobgiesberts 13:2caedc56b863 78 // TODO: writing to SD card more efficient!
bobgiesberts 13:2caedc56b863 79 // t 32-bit | 0.00 2000000.00 3-10x8-bit | FFFFFFFF 8x8
bobgiesberts 13:2caedc56b863 80 // L 24-bit | 0 1677216 6- 7x8-bit | FFFFFF 6x8
bobgiesberts 13:2caedc56b863 81 // V 8-bit | 3.10 4.20 4x8-bit | FF 2x8
bobgiesberts 13:2caedc56b863 82 // T 16-bit | -20.0000 85.0000 7x8-bit | FFFF 4x8
bobgiesberts 13:2caedc56b863 83
bobgiesberts 13:2caedc56b863 84
bobgiesberts 13:2caedc56b863 85 // construct data
bobgiesberts 13:2caedc56b863 86 /*
bobgiesberts 13:2caedc56b863 87 bob.ledon();
bobgiesberts 13:2caedc56b863 88 // fp = fopen( fn, "a" ); // open file (append)
bobgiesberts 13:2caedc56b863 89 char buffer[16]; memset(buffer, 0, sizeof(buffer));
bobgiesberts 13:2caedc56b863 90 char towrite[64]; memset(towrite, 0, sizeof(towrite));
bobgiesberts 13:2caedc56b863 91 int n = 0, m = 0;
bobgiesberts 13:2caedc56b863 92 for( int i = 0; i < counter; i++ )
bobgiesberts 13:2caedc56b863 93 {
bobgiesberts 13:2caedc56b863 94 for( int j = 0; j < 4; j++ )
bobgiesberts 13:2caedc56b863 95 {
bobgiesberts 13:2caedc56b863 96 // build up next 'word'
bobgiesberts 13:2caedc56b863 97 switch(j)
bobgiesberts 13:2caedc56b863 98 {
bobgiesberts 13:2caedc56b863 99 case 0: n = sprintf(buffer, "%.2f;", (float) collected.t[i]/100.0); break;
bobgiesberts 13:2caedc56b863 100 case 1: n = sprintf(buffer, "%d;", collected.L[i]); break;
bobgiesberts 13:2caedc56b863 101 case 2: n = sprintf(buffer, "%.2f;", bob.battery()); break;
bobgiesberts 13:2caedc56b863 102 case 3: n = sprintf(buffer, "%.4f\r\n", T); break;
bobgiesberts 13:2caedc56b863 103 }
bobgiesberts 13:2caedc56b863 104
bobgiesberts 13:2caedc56b863 105 // calculate if there is still room left for that word
bobgiesberts 13:2caedc56b863 106 if( strlen(towrite) + n > 64 )
bobgiesberts 13:2caedc56b863 107 {
bobgiesberts 13:2caedc56b863 108
bobgiesberts 13:2caedc56b863 109 // pc.printf( "%s", towrite);
bobgiesberts 13:2caedc56b863 110 fp = fopen( fn, "a" ); // open file (append)
bobgiesberts 13:2caedc56b863 111 fprintf( fp, "%s", towrite);
bobgiesberts 13:2caedc56b863 112 fclose( fp ); // close file
bobgiesberts 13:2caedc56b863 113
bobgiesberts 13:2caedc56b863 114 memset(towrite, 0, sizeof(towrite));
bobgiesberts 13:2caedc56b863 115 }
bobgiesberts 13:2caedc56b863 116
bobgiesberts 13:2caedc56b863 117 // build up letter with the next word
bobgiesberts 13:2caedc56b863 118 m = sprintf(towrite, "%s%s", towrite, buffer);
bobgiesberts 13:2caedc56b863 119 }
bobgiesberts 13:2caedc56b863 120 }
bobgiesberts 13:2caedc56b863 121 if(m > 0)
bobgiesberts 13:2caedc56b863 122 {
bobgiesberts 13:2caedc56b863 123 fp = fopen( fn, "a" ); // open file (append)
bobgiesberts 13:2caedc56b863 124 fprintf( fp, "%s", towrite);
bobgiesberts 13:2caedc56b863 125 fclose( fp ); // close file
bobgiesberts 13:2caedc56b863 126 //pc.printf( "%s", towrite);
bobgiesberts 13:2caedc56b863 127 }
bobgiesberts 13:2caedc56b863 128 //fclose( fp ); // close file
bobgiesberts 13:2caedc56b863 129 bob.ledoff();
bobgiesberts 13:2caedc56b863 130 */
bobgiesberts 13:2caedc56b863 131
bobgiesberts 13:2caedc56b863 132
bobgiesberts 13:2caedc56b863 133
bobgiesberts 13:2caedc56b863 134
bobgiesberts 13:2caedc56b863 135
bobgiesberts 12:cceece4f3afb 136 // write data to SD card
bobgiesberts 12:cceece4f3afb 137 bob.ledon();
bobgiesberts 12:cceece4f3afb 138 fp = fopen( fn, "a" ); // open file (append)
bobgiesberts 12:cceece4f3afb 139 for( int i = 0; i < counter; i++ )
bobgiesberts 13:2caedc56b863 140 fprintf( fp, "%.2f;%d;%.2f;%.4f\r\n", (float) collected.t[i]/100.0, collected.L[i], bob.battery(), T ); // write to file
bobgiesberts 12:cceece4f3afb 141 fclose( fp ); // close file
bobgiesberts 13:2caedc56b863 142
bobgiesberts 13:2caedc56b863 143 // write to screen
bobgiesberts 13:2caedc56b863 144 //pc.printf( "%.2f;%d;%.2f;%.4f\r\n", (float) collected.t[counter-1]/100.0, collected.L[counter-1], bob.battery(), T ); // write to screen
bobgiesberts 13:2caedc56b863 145
bobgiesberts 12:cceece4f3afb 146 bob.ledoff();
bobgiesberts 13:2caedc56b863 147
bobgiesberts 13:2caedc56b863 148
bobgiesberts 13:2caedc56b863 149
bobgiesberts 12:cceece4f3afb 150
bobgiesberts 12:cceece4f3afb 151 // Reset data
bobgiesberts 12:cceece4f3afb 152 memset(collected.t, 0, counter);
bobgiesberts 12:cceece4f3afb 153 memset(collected.L, 0, counter);
bobgiesberts 12:cceece4f3afb 154 counter = 0;
bobgiesberts 12:cceece4f3afb 155 }
bobgiesberts 5:736a81a59f3c 156
bobgiesberts 10:3cab80866536 157 int main(void)
bobgiesberts 10:3cab80866536 158 {
bobgiesberts 13:2caedc56b863 159
bobgiesberts 13:2caedc56b863 160 // TODO:
bobgiesberts 13:2caedc56b863 161 // - implement this data conversion in writing to SD!!
bobgiesberts 13:2caedc56b863 162 // - use T as uint16_t, not float!
bobgiesberts 13:2caedc56b863 163 // - use V as uint16_t, not float!
bobgiesberts 13:2caedc56b863 164 // - write blocks of 512 bit (4x 32 + 32 + 16 + 16)
bobgiesberts 13:2caedc56b863 165 //
bobgiesberts 13:2caedc56b863 166 // float temp_a = 127.9375;
bobgiesberts 13:2caedc56b863 167 // pc.printf( "A: %.4f (%08x); \r\n", temp_a, * (uint32_t *) &temp_a);
bobgiesberts 13:2caedc56b863 168
bobgiesberts 13:2caedc56b863 169
bobgiesberts 13:2caedc56b863 170
bobgiesberts 13:2caedc56b863 171
bobgiesberts 6:ff39d60061ca 172 // Load SD File system
bobgiesberts 12:cceece4f3afb 173 // - the Raspberry Pie SD cards give a warning here that might just be ignored:
bobgiesberts 12:cceece4f3afb 174 // "Not in idle state after sending CMD8 (not an SD card?)
bobgiesberts 12:cceece4f3afb 175 // Didn't get a response from the disk
bobgiesberts 12:cceece4f3afb 176 // Set 512-byte block timed out"
bobgiesberts 12:cceece4f3afb 177 // TODO: buy 'better' SD cards from IAPC / the Stores (Kingston ...)
bobgiesberts 11:599ca9982e45 178 bob.wakeup();
bobgiesberts 12:cceece4f3afb 179 SDFileSystem *sd = new SDFileSystem(PTD6, // mosi
bobgiesberts 12:cceece4f3afb 180 PTD7, // miso
bobgiesberts 12:cceece4f3afb 181 PTD5, // sclk
bobgiesberts 12:cceece4f3afb 182 PTD4, // cs
bobgiesberts 12:cceece4f3afb 183 "sd"
bobgiesberts 12:cceece4f3afb 184 );
bobgiesberts 10:3cab80866536 185
bobgiesberts 6:ff39d60061ca 186 // Create a new data file (data00.txt)
bobgiesberts 10:3cab80866536 187 mkdir("/sd", 0777);
bobgiesberts 11:599ca9982e45 188 for(uint8_t i = 0; i < 100; i++)
bobgiesberts 11:599ca9982e45 189 {
bobgiesberts 6:ff39d60061ca 190 filename[8] = i/10 + '0';
bobgiesberts 4:ae441c5727b9 191 filename[9] = i%10 + '0';
bobgiesberts 4:ae441c5727b9 192 fp = fopen( filename.c_str() , "r" );
bobgiesberts 11:599ca9982e45 193 if( fp == NULL ) // read failed so file does not exist
bobgiesberts 11:599ca9982e45 194 {
bobgiesberts 4:ae441c5727b9 195 fp = fopen( filename.c_str(), "w" ); // create it
bobgiesberts 11:599ca9982e45 196 if( fp != NULL )
bobgiesberts 11:599ca9982e45 197 {
bobgiesberts 4:ae441c5727b9 198 fn = filename.c_str();
bobgiesberts 6:ff39d60061ca 199 fclose( fp );
bobgiesberts 4:ae441c5727b9 200 break;
bobgiesberts 10:3cab80866536 201 } else {
bobgiesberts 12:cceece4f3afb 202 bob.ledon(); // error: unable to create new file
bobgiesberts 4:ae441c5727b9 203 }
bobgiesberts 10:3cab80866536 204 } else { // file already exists
bobgiesberts 4:ae441c5727b9 205 fclose( fp );
bobgiesberts 4:ae441c5727b9 206 }
bobgiesberts 1:22c272515015 207 }
bobgiesberts 11:599ca9982e45 208 if( fp == NULL )
bobgiesberts 11:599ca9982e45 209 {
bobgiesberts 12:cceece4f3afb 210 bob.ledon(); // error: file 00 - 99 already exists
bobgiesberts 12:cceece4f3afb 211 // pc.printf( "/sd/data00.txt t/m /sd/data99.txt already exist!\n" );
bobgiesberts 11:599ca9982e45 212 }
bobgiesberts 10:3cab80866536 213
bobgiesberts 6:ff39d60061ca 214 // Unload SD File system
bobgiesberts 10:3cab80866536 215 delete sd;
bobgiesberts 10:3cab80866536 216 sd = NULL;
bobgiesberts 12:cceece4f3afb 217
bobgiesberts 11:599ca9982e45 218 while(1)
bobgiesberts 11:599ca9982e45 219 {
bobgiesberts 11:599ca9982e45 220 // Wakeup: SD + LDC1101 on
bobgiesberts 11:599ca9982e45 221 bob.wakeup();
bobgiesberts 10:3cab80866536 222
bobgiesberts 11:599ca9982e45 223 // what time is it now?
bobgiesberts 8:8cc1960467ae 224 prev = now; // 0 -> 429 496 --> (4 294,96 s) (71 min)
bobgiesberts 8:8cc1960467ae 225 now = (uint32_t) clock(); // 0 -> 429 496 --> (4 294,96 s) (71 min)
bobgiesberts 11:599ca9982e45 226 if( now < prev )
bobgiesberts 11:599ca9982e45 227 t_high++; // 0 -> 255 --> (255*4 294,96 s) (12 days)
bobgiesberts 11:599ca9982e45 228 t = now + 429496.7296*t_high + t_lost + 429496.7296*lost_high; // 0 -> 219 901 952 --> (2 199 019,52 s) (25 days)
bobgiesberts 11:599ca9982e45 229
bobgiesberts 11:599ca9982e45 230 // load libraries to take control over the communication
bobgiesberts 12:cceece4f3afb 231 LDC1101 *ldc = new LDC1101( PTC6, // mosi
bobgiesberts 12:cceece4f3afb 232 PTC7, // miso
bobgiesberts 12:cceece4f3afb 233 PTC5, // sck
bobgiesberts 12:cceece4f3afb 234 PTC4, // cs
bobgiesberts 12:cceece4f3afb 235 C, // capacitor
bobgiesberts 12:cceece4f3afb 236 16E6 // f_CLKIN
bobgiesberts 12:cceece4f3afb 237 ); // load sensor (LDC1101) library (this takes 0.81 s)
bobgiesberts 11:599ca9982e45 238 DS1825 *thermometer = new DS1825( _Tpin ); // load thermometer (DS1825) library
bobgiesberts 12:cceece4f3afb 239 SDFileSystem *sd = new SDFileSystem(PTD6, // mosi
bobgiesberts 12:cceece4f3afb 240 PTD7, // miso
bobgiesberts 12:cceece4f3afb 241 PTD5, // sclk
bobgiesberts 12:cceece4f3afb 242 PTD4, // cs
bobgiesberts 12:cceece4f3afb 243 "sd"
bobgiesberts 12:cceece4f3afb 244 ); // load SD system on
bobgiesberts 11:599ca9982e45 245 mkdir("/sd", 0777); // select folder
bobgiesberts 10:3cab80866536 246
bobgiesberts 12:cceece4f3afb 247
bobgiesberts 12:cceece4f3afb 248 // How long should we takes samples?
bobgiesberts 11:599ca9982e45 249 if( next == 0 )
bobgiesberts 11:599ca9982e45 250 {
bobgiesberts 11:599ca9982e45 251 next = t + INTERVAL_FIRST*100;
bobgiesberts 10:3cab80866536 252 } else {
bobgiesberts 11:599ca9982e45 253 next = t + INTERVAL_ON*100; // 0 -> 219 904 952 --> (2 199 049,52 s) (25 days)
bobgiesberts 8:8cc1960467ae 254 }
bobgiesberts 10:3cab80866536 255
bobgiesberts 6:ff39d60061ca 256 // Take samples for INTERVAL_ON seconds
bobgiesberts 11:599ca9982e45 257 while( t < next )
bobgiesberts 11:599ca9982e45 258 {
bobgiesberts 12:cceece4f3afb 259 // wait for new data to be ready
bobgiesberts 12:cceece4f3afb 260 while( !ldc->is_New_LHR_data() ) { }
bobgiesberts 10:3cab80866536 261
bobgiesberts 12:cceece4f3afb 262 // what time is it now?
bobgiesberts 12:cceece4f3afb 263 prev = now;
bobgiesberts 12:cceece4f3afb 264 now = (uint32_t) clock();
bobgiesberts 12:cceece4f3afb 265 if( now < prev )
bobgiesberts 12:cceece4f3afb 266 t_high++;
bobgiesberts 12:cceece4f3afb 267 t = now + 429496.7296*t_high + t_lost + 429496.7296*lost_high;
bobgiesberts 10:3cab80866536 268
bobgiesberts 12:cceece4f3afb 269 if( !ldc->is_Oscillation_Error() ) // sensor overloaded, this happends when the target is too close to the coil
bobgiesberts 12:cceece4f3afb 270 {
bobgiesberts 12:cceece4f3afb 271 // Store data in temporal memory
bobgiesberts 12:cceece4f3afb 272 collected.t[counter] = t;
bobgiesberts 12:cceece4f3afb 273 collected.L[counter] = ldc->get_LHR_Data();
bobgiesberts 12:cceece4f3afb 274 counter++;
bobgiesberts 6:ff39d60061ca 275 }
bobgiesberts 11:599ca9982e45 276
bobgiesberts 12:cceece4f3afb 277 // Write a full package of data points to the SD card
bobgiesberts 12:cceece4f3afb 278 if( counter >= package_size-1 )
bobgiesberts 12:cceece4f3afb 279 storeit( thermometer->getTemperature() );
bobgiesberts 10:3cab80866536 280 }
bobgiesberts 12:cceece4f3afb 281
bobgiesberts 12:cceece4f3afb 282 // Write remaining data to the SD card
bobgiesberts 12:cceece4f3afb 283 if( counter > 0 )
bobgiesberts 12:cceece4f3afb 284 storeit( thermometer->getTemperature() );
bobgiesberts 10:3cab80866536 285
bobgiesberts 11:599ca9982e45 286 // prepare for sleep, power down the SD and the LDC1101
bobgiesberts 12:cceece4f3afb 287 delete sd; sd = NULL; // unload library to be able to power down completely
bobgiesberts 12:cceece4f3afb 288 delete ldc; ldc = NULL; // unload library to be able to power down completely
bobgiesberts 12:cceece4f3afb 289 delete thermometer; thermometer = NULL; // unload library to be able to power down completely
bobgiesberts 11:599ca9982e45 290 bob.beforesleep();
bobgiesberts 12:cceece4f3afb 291
bobgiesberts 12:cceece4f3afb 292 // if the battery is almost empty (Vbatt < 3.10 V), the termometer stops
bobgiesberts 12:cceece4f3afb 293 // working well and L can not be corrected. So just shut down...
bobgiesberts 12:cceece4f3afb 294 if( bob.battery() < 3.10f )
bobgiesberts 12:cceece4f3afb 295 {
bobgiesberts 12:cceece4f3afb 296 bob.ledon(); // error: battery empty
bobgiesberts 12:cceece4f3afb 297 // pc.printf( "Battery empty (%.1f V), shutting down.\n", bob.battery() );
bobgiesberts 12:cceece4f3afb 298 exit(0);
bobgiesberts 12:cceece4f3afb 299 }
bobgiesberts 10:3cab80866536 300
bobgiesberts 11:599ca9982e45 301 // what time is it now?
bobgiesberts 11:599ca9982e45 302 prev = now;
bobgiesberts 11:599ca9982e45 303 now = (uint32_t) clock();
bobgiesberts 11:599ca9982e45 304 if( now < prev )
bobgiesberts 11:599ca9982e45 305 t_high++;
bobgiesberts 11:599ca9982e45 306 t = now + 429496.7296*t_high + t_lost + 429496.7296*lost_high;
bobgiesberts 10:3cab80866536 307
bobgiesberts 8:8cc1960467ae 308 // Calculate sleeping time (correction to INTERVAL_OFF)
bobgiesberts 12:cceece4f3afb 309 // t has passed the limit of next. Those few ms are substracted from INTERVAL_OFF.
bobgiesberts 11:599ca9982e45 310 t_sleep = INTERVAL_OFF*100 - (t - next);
bobgiesberts 8:8cc1960467ae 311
bobgiesberts 12:cceece4f3afb 312 // Calculate time that will be lost during sleep
bobgiesberts 11:599ca9982e45 313 lost_prev = t_lost;
bobgiesberts 11:599ca9982e45 314 t_lost += t_sleep;
bobgiesberts 11:599ca9982e45 315 if( t_lost < lost_prev )
bobgiesberts 11:599ca9982e45 316 lost_high++;
bobgiesberts 13:2caedc56b863 317
bobgiesberts 11:599ca9982e45 318 // Sleep now (ms), enter low power mode
bobgiesberts 11:599ca9982e45 319 bob.sleep( t_sleep * 10 );
bobgiesberts 10:3cab80866536 320
bobgiesberts 0:e81b68888268 321 }
bobgiesberts 6:ff39d60061ca 322
bobgiesberts 11:599ca9982e45 323
bobgiesberts 11:599ca9982e45 324
bobgiesberts 11:599ca9982e45 325
bobgiesberts 12:cceece4f3afb 326 }
bobgiesberts 12:cceece4f3afb 327