openXsensor/oXs_imu.cpp

   1 /* =============================================================================
   2 This part is based on Nav6 project (source code is placed under the MIT license)
   3
   4 Copyright (c) 2013 Kauai Labs
   5
   6 Portions of this work are based upon the I2C Dev Library by Jeff Rowberg
   7 (www.i2cdevlib.com) which is open-source licensed under the MIT
   8 License.
   9
  10 This work also incorporates modifications on the official Invensense Motion
  11 Driver v. 5.1 library in order to reduce the size of flash memory.
  12 =============================================================================
  13 */
  14
  15
  16 #define EMPL_TARGET_ATMEGA328
  17 //#include <Wire.h>
  18 //#include "I2Cdev.h"
  19 #include "helper_3dmath.h"
  20 extern "C" {
  21   #include "inv_mpu.h"
  22   #include "inv_mpu_dmp_motion_driver.h"
  23 }
  24
  25 #include "oXs_imu.h"
  26
  27 #ifdef USE_6050
  28
  29 #ifdef DEBUG
  30 //      #define DEBUG_MPU
  31 #endif
  32
  33 #if ! ( defined (PIN_INT_6050) && ( ( PIN_INT_6050 == 2 ) || ( PIN_INT_6050 == 3 ) ) )
  34 #error Error in oXs_config.h : PIN_INT_6050 must be 2 or 3 when a IMU 6050 is used
  35 #endif
  36
  37
  38 //float mpuDeltaVit = 0;
  39 //static float mpuVit = 0 ;
  40 float linear_acceleration_x;
  41 float linear_acceleration_y;
  42 float linear_acceleration_z;
  43 float world_linear_acceleration_z ;
  44 bool newImuAvailable = false ;
  45
  46 /*
  47 // ***************************************
  48 // Invensense Hardware Abstracation Layer
  49 // ***************************************
  50 struct hal_s {
  51 //    unsigned char sensors;
  52 //    unsigned char dmp_on;
  53 //    unsigned char wait_for_tap;
  54     volatile unsigned char new_gyro;
  55 //    unsigned short report;
  56 //    unsigned short dmp_features;
  57 //    unsigned char motion_int_mode;
  58 };
  59 //static struct hal_s hal ; //= {0};
  60 */
  61
  62 volatile unsigned char new_mpu_data ;
  63
  64 // ******************* ISR for INT0 ******************************************************
  65 volatile uint32_t lastImuInterruptMillis ;
  66 // this ISR handles interrupt 0 on rising edge ; when it occurs, it means that DMP has filled the fifo
  67 #if PIN_INT_6050 == 2                //  Pin2 uses INT0
  68 ISR(INT0_vect, ISR_NOBLOCK) { // allows other interrupts to be served when this one is activated
  69 #else                            // //  Pin3 uses INT1
  70 ISR(INT1_vect, ISR_NOBLOCK) { // allows other interrupts to be served when this one is activated
  71 #endif
  72  new_mpu_data = 1;
  73  //lastImuInterruptMillis = millis();
  74 }
  75
  76
  77 // ****************************  set up the imu 6050 (including the dmp)
  78 void setupImu() {
  79     // MPU-6050 Initialization
  80     // Gyro sensitivity:      2000 degrees/sec
  81     // Accel sensitivity:     2 g
  82     // Gyro Low-pass filter:  98Hz
  83     // DMP Update rate:       50Hz
  84 #ifdef DEBUG_MPU
  85     Serial.print(F("Initializing MPU..."));
  86 #endif
  87     boolean mpu_initialized = false;
  88     while ( !mpu_initialized ) {
  89       if ( initialize_mpu() ) {                       // enable imu
  90         mpu_initialized = true;
  91 #ifdef DEBUG_MPU
  92         Serial.print(F("Success"));
  93         dump_all() ;
  94 #endif
  95
  96 //        boolean gyro_ok, accel_ok;
  97 //        run_mpu_self_test(gyro_ok,accel_ok);
  98 //        enable_mpu();  // replaced by next instruction = mpu_set_dmp_state(1);
  99         mpu_set_dmp_state_on();                     //enable dmp
 100       }
 101       else {
 102 #ifdef DEBUG_MPU
 103         Serial.print(F("Failed"));
 104 #endif
 105  //       mpu_force_reset();
 106         delay(100);
 107 #ifdef DEBUG_MPU
 108         Serial.println(F("Re-initializing"));
 109 #endif
 110       }
 111     }
 112 #ifdef DEBUG_MPU
 113     Serial.println(F("Initialization Complete"));
 114     dump_all() ;
 115 #endif
 116 }  // ***************** End of setupIMU
 117
 118 #ifdef DEBUG_MPU
 119 // used for debug in order to get all register and all memory
 120 void dump_all(){
 121     static uint8_t dump_count ;
 122     for( unsigned char reg = 0 ; reg < 118 ; reg++){
 123       unsigned char data ;
 124       mpu_read_reg(reg, &data) ;
 125       Serial.print("reg") ;Serial.print(dump_count) ;Serial.print(", ") ; Serial.print(reg,HEX);Serial.print(",");Serial.println(data,HEX);
 126       //delay(10);
 127     }
 128     delay(1000);
 129     unsigned char dataAdr[1] ;
 130     for( int iadr = 0 ; iadr < 3062 ; iadr++){
 131       mpu_read_mem(iadr,1,dataAdr) ;
 132       Serial.print("mem") ;Serial.print(dump_count) ;Serial.print(", ") ; Serial.print(iadr,HEX);Serial.print(",");Serial.println(dataAdr[0],HEX);
 133       //delay(10);
 134     }
 135     dump_count++ ;
 136 }
 137 #endif
 138
 139 /*****************************************
 140 * Conversion Factors
 141 *****************************************/
 142 // angle in radians = angle in degrees * Pi / 180
 143 const float degrees_to_radians = M_PI / 180.0;
 144 // angle in degrees = angle in radians * 180 / Pi
 145 const float radians_to_degrees = 180.0 / M_PI;
 146
 147 /*****************************************
 148 * MPU Calibration
 149 *****************************************/
 150 #define STARTUP_CALIBRATION_DELAY_MS        19000
 151 #define CALIBRATED_OFFSET_AVERAGE_PERIOD_MS  1000
 152
 153 #define NAV6_CALIBRATION_STATE_WAIT 0 // Waiting for MPU to complete internal calibration
 154 #define NAV6_CALIBRATION_STATE_ACCUMULATE  1 // Accumulate Yaw/Pitch/Roll offsets
 155 #define NAV6_CALIBRATION_STATE_COMPLETE    2 // Normal Operation
 156
 157 int calibration_state = NAV6_CALIBRATION_STATE_WAIT;
 158 int calibration_accumulator_count = 0;
 159 float yaw_accumulator = 0.0;
 160 float quaternion_accumulator[4] = { 0.0, 0.0, 0.0, 0.0 };
 161 float calibrated_yaw_offset = 0.0;
 162 float calibrated_quaternion_offset[4] = { 0.0, 0.0, 0.0, 0.0 };
 163
 164 /****************************************
 165 * Gyro/Accel/DMP State
 166 ****************************************/
 167 //float temp_centigrade = 0.0;  // Gyro/Accel die temperature
 168 float ypr[3] = { 0, 0, 0 };
 169 struct ONE_MEASUREMENT yaw ; // in 1/10 of degree
 170 struct ONE_MEASUREMENT pitch ; // in 1/10 of degree
 171 struct ONE_MEASUREMENT roll ; // in 1/10 of degree
 172 //long curr_mpu_temp;
 173 unsigned long sensor_timestamp;
 174
 175 struct FloatVectorStruct {
 176   float x;
 177   float y;
 178   float z;
 179 };
 180
 181 struct FloatVectorStruct gravity;
 182
 183 #define ACCEL_ON        (0x01)
 184 #define GYRO_ON         (0x02)
 185 /* Starting sampling rate. */
 186 #define DEFAULT_MPU_HZ    (200) //modified : was originally 100
 187 #define MAX_NAV6_MPU_RATE (200)// modified : was originally 100
 188 #define MIN_NAV6_MPU_RATE (4)
 189 /* Data requested by client. */
 190 #define PRINT_ACCEL     (0x01)
 191 #define PRINT_GYRO      (0x02)
 192 #define PRINT_QUAT      (0x04)
 193
 194 /****************************************
 195 * Gyro/Accel/DMP Configuration
 196 ****************************************/
 197 unsigned char accel_fsr = 2;  // accelerometer full-scale rate, in +/- Gs (possible values are 2, 4, 8 or 16).  Default:  2 ; when changed, it must be changed in inv_mpu too
 198 unsigned short dmp_update_rate; // update rate, in hZ (possible values are between 4 and 1000).  Default:  200
 199 unsigned short gyro_fsr = 2000;  // Gyro full-scale_rate, in +/- degrees/sec, possible values are 250, 500, 1000 or 2000.  Default:  2000 ;  ; when changed, it must be changed in inv_mpu too
 200
 201 /* The mounting matrix below tells the MPL how to rotate the raw  // code in order to get this orientation has already been set in the firmware
 202  * data from the driver(s). The matrix below reflects the axis
 203  * orientation of the MPU-6050 on the nav6 circuit board.
 204  */
 205 /*
 206 static signed char gyro_orientation[9] = { 1, 0, 0,
 207                                            0, 1, 0,
 208                                            0, 0, 1};
 209 */
 210
 211 // ******************************************************************************************
 212 //                              Read imu 6050 sensor
 213 // ******************************************************************************************
 214
 215 bool read6050 () {
 216   // If the MPU Interrupt occurred, read the fifo and process the data
 217 //  if (hal.new_gyro && hal.dmp_on) {
 218     bool newAccelerationAvailable = false;
 219     if (new_mpu_data) {  // new_mpu_data is set in a call back function in an ISR attached to interrupt INT0 (reading arduino pin 2 (or 3) which is connected to INT from mpu6050 )
 220
 221         short gyro[3], accel[3], sensors;         // To do : gyro and sensors are not used anymore and could be removed from here
 222         unsigned char more = 0;
 223         long quat[4];
 224                                                   // To do : in quaternion, only 16 bits from the 32 are used; it should be possible to save time and memory filling only 16 bits the dmp_read_fifo()
 225                                                   //         in this function, remove giro and sensors parameters.
 226                                                   // To do : Some temporary variable could be removed
 227                                                   // To do : kalman filter takes about 4 msec to execute; it could be splitted in e.g. 4 sections in order to keep loop executing time at about 1 msec
 228                                                   // To do : there should be some easy way to calibrate the mpu
 229                                                   // To do : there should be some general function to perform all calculations based on raw sensor values (in order to keep the sensor as short as possible)
 230                                                   // To do : add some code to detect errors on I2C (there are a few write without checks)
 231                                                   // To do : at start up, send a few I2C SCL pulse to clear pending bytes on I2C
 232                                                   // To do : activate/ or not the check on quaternion validity read from fifo (to detect wrong fifo read
 233                                                   // To do : Initialisation of mpu can probably be simplified again (no need to write in pwm_mgnt_2 probably (it stays on 0)
 234                                                   //         accel_cfg is probably already default filled with 0 (2 g) so no need to overwrite
 235                                                   //         dmp_enable_feature() do nothing more except setting the size of fifo
 236                                                   //         max fifo size could be reduced (save memory)
 237                                                   // To do : remove perhaps code for accel_half detection (is not used currently ; previously was used with get and set function)
 238                                                   //         in case of error on I2c during set up, check that imu is reset (and not only fifo reset (because reset of fifo is different before or after dmp is enabled)
 239                                                   // To do : use hysteresis for Vspeed with imu
 240                                                   // To do : detect when acc is not as usual and then tranmit Vspeed based only on baro
 241
 242         /* This function gets new data from the FIFO when the DMP is in
 243          * use. The FIFO can contain any combination of gyro, accel,
 244          * quaternion, and gesture data. The sensors parameter tells the
 245          * caller which data fields were actually populated with new data.
 246          * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
 247          * the FIFO isn't being filled with accel data.
 248          * The driver parses the gesture data to determine if a gesture
 249          * event has occurred; on an event, the application will be notified
 250          * via a callback (assuming that a callback function was properly
 251          * registered). The more parameter is non-zero if there are
 252          * leftover packets in the FIFO.
 253          */
 254         int success = dmp_read_fifo(gyro, accel, quat, &sensors, &more);  // 0 = OK
 255 //        Serial.print("fifo "); Serial.println(mpu_getfifo_count_debug()) ;
 256 //        Serial.print("success ") ; Serial.println(success ) ;
 257         if (!more)                  // if no more data
 258             new_mpu_data = 0;       // reset the indicator saying that data are available in the FIFO, so it will be updated by the callback function on the interrupt.
 259         if (  success == 0 ) {
 260 #ifdef DEBUG_MPUxxxx         // this part allows to check the delay between 2 kalman filter. It is hardcoded (#define with frequency) and set on 20msec
 261                static unsigned long prevSensorTimeStamp;
 262                unsigned long sensor_timestamp = millis() ;
 263               if (prevSensorTimeStamp == 0) prevSensorTimeStamp = sensor_timestamp ;
 264               uint16_t mpuDeltaTime =  sensor_timestamp - prevSensorTimeStamp ;
 265               prevSensorTimeStamp = sensor_timestamp ;
 266               Serial.print("dt ") ; Serial.println(mpuDeltaTime ) ;
 267 #endif
 268 #ifdef DISPLAY_ACC_OFFSET
 269             static int offsetCount;
 270             static int32_t accelOffsetSumX ;
 271             static int32_t accelOffsetSumY ;
 272             static int32_t accelOffsetSumZ;
 273             offsetCount++ ;
 274             accelOffsetSumX += accel[0] ;
 275             accelOffsetSumY += accel[1] ;
 276             accelOffsetSumZ+= accel[2] ;
 277             if( offsetCount >= 100 ){
 278               Serial.print("acc "); Serial.print(accelOffsetSumX / offsetCount ) ;Serial.print(" "); Serial.print(accelOffsetSumY / offsetCount  ) ; Serial.print(" "); Serial.println(accelOffsetSumZ / offsetCount  ) ;
 279               offsetCount = 0 ;
 280               accelOffsetSumX  = 0 ;
 281               accelOffsetSumY = 0 ;
 282               accelOffsetSumZ = 0 ;
 283             }
 284 #endif
 285               accel[0] -= ACC_OFFSET_X ; //-160 ;  //Offset for X ; value is measured on a table when sensor is flat
 286               accel[1] -= ACC_OFFSET_Y ; //-341 ;  //Offset for X ; value is measured on a table when sensor is flat
 287               accel[2] -= ACC_OFFSET_Z ; //-854 ;  //Offset for X ; value is measured on a table when sensor is flat
 288
 289               Quaternion q( (float)(quat[0] >> 16) / 16384.0f,
 290                             (float)(quat[1] >> 16) / 16384.0f,
 291                             (float)(quat[2] >> 16) / 16384.0f,
 292                             (float)(quat[3] >> 16) / 16384.0f);
 293
 294              // Calculate Yaw/Pitch/Roll
 295              // Update client with yaw/pitch/roll and tilt-compensated magnetometer data
 296              getGravity(&gravity, &q);
 297
 298 //             dmpGetYawPitchRoll(ypr, &q, &gravity);
 299                static uint8_t yawCount ;
 300                yawCount++ ;
 301                if (yawCount >= 20 ) {
 302                   yaw.value = 10*atan2(2 * q.x* q.y - 2 * q.w * q.z, 2* q.w * q.w + 2 * q.x * q.x - 1) * radians_to_degrees ; // yaw in 1/10 of degree
 303                   pitch.value = 10*atan(gravity.x / sqrt(gravity.y*gravity.y + gravity.z*gravity.z)) * radians_to_degrees; // Pitch in 1/10 of degree
 304                   roll.value = 10*atan(gravity.y / sqrt(gravity.x*gravity.x + gravity.z*gravity.z)) * radians_to_degrees; // Roll in 1/10 of degree
 305                   yaw.available = true ;
 306                   pitch.available = true ;
 307                   roll.available = true ;
 308                }
 309
 310               float q1[4];
 311               float q2[4];
 312               float q_product[4];
 313               float q_conjugate[4];
 314               float q_final[4];
 315
 316               // calculate linear acceleration by
 317               // removing the gravity component from raw acceleration values
 318
 319               linear_acceleration_x = ((((float)accel[0]) / 16384.0f ) - gravity.x )  ; // converted in 1 g unit
 320               linear_acceleration_y = ((((float)accel[1]) / 16384.0f ) - gravity.y )  ;
 321               linear_acceleration_z = ((((float)accel[2]) / 16384.0f ) - gravity.z )  ;
 322
 323               // Calculate world-frame acceleration
 324
 325               q1[0] = q.w ;   // division factor added by Ms in order to get a result around 1 for 1G
 326               q1[1] = q.x ;
 327               q1[2] = q.y ;
 328               q1[3] = q.z ;
 329
 330               q2[0] = 0;
 331               q2[1] = linear_acceleration_x;
 332               q2[2] = linear_acceleration_y;
 333               q2[3] = linear_acceleration_z;
 334
 335               // Rotate linear acceleration so that it's relative to the world reference frame
 336
 337               // http://www.cprogramming.com/tutorial/3d/quaternions.html
 338               // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
 339               // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
 340               // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
 341
 342               // P_out = q * P_in * conj(q)
 343               // - P_out is the output vector
 344               // - q is the orientation quaternion
 345               // - P_in is the input vector (a*aReal)
 346               // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
 347
 348               // calculate quaternion product
 349               // Quaternion multiplication is defined by:
 350               //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
 351               //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
 352               //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
 353               //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
 354
 355               q_product[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];  // new w
 356               q_product[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];  // new x
 357               q_product[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];  // new y
 358               q_product[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];  // new z
 359
 360               q_conjugate[0] = q1[0];
 361               q_conjugate[1] = -q1[1];
 362               q_conjugate[2] = -q1[2];
 363               q_conjugate[3] = -q1[3];
 364
 365  //             q_final[0] = q_product[0]*q_conjugate[0] - q_product[1]*q_conjugate[1] - q_product[2]*q_conjugate[2] - q_product[3]*q_conjugate[3];  // new w
 366  //             q_final[1] = q_product[0]*q_conjugate[1] + q_product[1]*q_conjugate[0] + q_product[2]*q_conjugate[3] - q_product[3]*q_conjugate[2];  // new x
 367  //             q_final[2] = q_product[0]*q_conjugate[2] - q_product[1]*q_conjugate[3] + q_product[2]*q_conjugate[0] + q_product[3]*q_conjugate[1];  // new y
 368               q_final[3] = q_product[0]*q_conjugate[3] + q_product[1]*q_conjugate[2] - q_product[2]*q_conjugate[1] + q_product[3]*q_conjugate[0];  // new z
 369
 370 //              world_linear_acceleration_x = q_final[1];
 371 //              world_linear_acceleration_y = q_final[2];
 372               world_linear_acceleration_z = q_final[3] * 981.0f  ;  // conversion from g to cm/sec2 => * 981 cm/sec2
 373               newAccelerationAvailable = true;
 374 #ifdef DEBUG_MPU
 375 //             Serial.print("acc "); Serial.print(accel[0] ) ;Serial.print(" "); Serial.print(accel[1] ) ; Serial.print(" "); Serial.println(accel[2] ) ;
 376 //               Serial.print("q "); Serial.print(q.w ) ;Serial.print(" "); Serial.print(q.x ) ;Serial.print(" "); Serial.print(q.y ) ; Serial.print(" "); Serial.println(q.z ) ;
 377 //               Serial.print("grav "); Serial.print(gravity.x ) ;Serial.print(" "); Serial.print(gravity.y ) ; Serial.print(" "); Serial.println(gravity.z ) ;
 378 //               Serial.print("liear Acc "); Serial.print(linear_acceleration_x); Serial.print(" "); Serial.print(linear_acceleration_y ); Serial.print(" "); Serial.println(linear_acceleration_z );
 379
 380                //Serial.print(mpuDeltaTime );Serial.print(",") ;
 381                //Serial.println((int) mpuVit ) ;
 382 //                Serial.println(world_linear_acceleration_z );
 383                   Serial.print(millis()) ;Serial.print(","); Serial.print((int) (accel[2] / 16384.0 * 981.0) ) ; Serial.print(","); Serial.print((int) (gravity.z * 981.0 )) ;
 384                   Serial.print(","); Serial.print((int) linear_acceleration_z * 981 );Serial.print(",");Serial.println((int)world_linear_acceleration_z );
 385 #endif
 386
 387       }  // end success
 388     }   // end  new_mpu_data
 389     return newAccelerationAvailable ;
 390 }
 391 /***************************************
 392 * nav6 Protocol Configuration/State
 393 ***************************************/
 394
 395 //boolean update_type = MSGID_YPR_UPDATE;
 396 //char protocol_buffer[64];
 397 /*
 398 void loop_xxxxxxxx() {
 399
 400   // If the MPU Interrupt occurred, read the fifo and process the data
 401
 402   if (hal.new_gyro && hal.dmp_on) {
 403
 404     short gyro[3], accel[3], sensors;
 405     unsigned char more = 0;
 406     long quat[4];
 407     //float euler[3];
 408     // This function gets new data from the FIFO when the DMP is in
 409     // use. The FIFO can contain any combination of gyro, accel,
 410     // quaternion, and gesture data. The sensors parameter tells the
 411     // caller which data fields were actually populated with new data.
 412     // For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
 413     // the FIFO isn't being filled with accel data.
 414     // The driver parses the gesture data to determine if a gesture
 415     // event has occurred; on an event, the application will be notified
 416     // via a callback (assuming that a callback function was properly
 417     // * registered). The more parameter is non-zero if there are
 418     // leftover packets in the FIFO.
 419
 420     int success = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
 421         &more);
 422     if (!more)
 423         hal.new_gyro = 0;
 424
 425     if ( ( success == 0 ) && ( (sensors & INV_XYZ_ACCEL) != 0 ) && ( (sensors & INV_WXYZ_QUAT) != 0 ) ) {
 426
 427       Quaternion q( (float)(quat[0] >> 16) / 16384.0f,
 428                     (float)(quat[1] >> 16) / 16384.0f,
 429                     (float)(quat[2] >> 16) / 16384.0f,
 430                     (float)(quat[3] >> 16) / 16384.0f);
 431
 432       // Calculate Yaw/Pitch/Roll
 433       // Update client with yaw/pitch/roll and tilt-compensated magnetometer data
 434
 435       getGravity(&gravity, &q);
 436       dmpGetYawPitchRoll(ypr, &q, &gravity);
 437
 438       boolean accumulate = false;
 439       if ( calibration_state == NAV6_CALIBRATION_STATE_WAIT ) {
 440
 441         if ( millis() >= STARTUP_CALIBRATION_DELAY_MS ) {
 442
 443           calibration_state = NAV6_CALIBRATION_STATE_ACCUMULATE;
 444         }
 445
 446       }
 447       if ( calibration_state == NAV6_CALIBRATION_STATE_ACCUMULATE ) {
 448
 449         accumulate = true;
 450         if ( millis() >= (STARTUP_CALIBRATION_DELAY_MS + CALIBRATED_OFFSET_AVERAGE_PERIOD_MS) ) {
 451
 452           accumulate = false;
 453           calibrated_yaw_offset = yaw_accumulator / calibration_accumulator_count;
 454           calibrated_quaternion_offset[0] = quaternion_accumulator[0] / calibration_accumulator_count;
 455           calibrated_quaternion_offset[1] = quaternion_accumulator[1] / calibration_accumulator_count;
 456           calibrated_quaternion_offset[2] = quaternion_accumulator[2] / calibration_accumulator_count;
 457           calibrated_quaternion_offset[3] = quaternion_accumulator[3] / calibration_accumulator_count;
 458           calibration_state = NAV6_CALIBRATION_STATE_COMPLETE;
 459
 460           // Since calibration data has likely changed, send an update
 461
 462           int num_bytes = IMUProtocol::encodeStreamResponse(  protocol_buffer, update_type,
 463                                                                 gyro_fsr, accel_fsr, dmp_update_rate, calibrated_yaw_offset,
 464                                                                 (uint16_t)(calibrated_quaternion_offset[0] * 16384),
 465                                                                 (uint16_t)(calibrated_quaternion_offset[1] * 16384),
 466                                                                 (uint16_t)(calibrated_quaternion_offset[2] * 16384),
 467                                                                 (uint16_t)(calibrated_quaternion_offset[3] * 16384),
 468                                                                 calibration_state );
 469           Serial.write((unsigned char *)protocol_buffer, num_bytes);
 470         }
 471         else {
 472
 473           calibration_accumulator_count++;
 474
 475         }
 476       }
 477
 478       float x = ypr[0] * radians_to_degrees;
 479       float y = ypr[1] * radians_to_degrees;
 480       float z = ypr[2] * radians_to_degrees;
 481
 482       if ( accumulate ) {
 483
 484         yaw_accumulator += x;
 485         quaternion_accumulator[0] += q.w;
 486         quaternion_accumulator[1] += q.x;
 487         quaternion_accumulator[2] += q.y;
 488         quaternion_accumulator[3] += q.z;
 489
 490       }
 491       if ( update_type == MSGID_QUATERNION_UPDATE ) {
 492
 493         // Update client with quaternions and some raw sensor data
 494
 495         int num_bytes = IMUProtocol::encodeQuaternionUpdate(  protocol_buffer,
 496                                                                quat[0] >> 16, quat[1] >> 16, quat[2] >> 16, quat[3] >> 16,
 497                                                                accel[0], accel[1], accel[2],
 498                                                                mag_x, mag_y, mag_z,
 499                                                                temp_centigrade);
 500         Serial.write((unsigned char *)protocol_buffer, num_bytes);
 501       }
 502       else if ( update_type == MSGID_GYRO_UPDATE ) {
 503
 504         // Update client with raw sensor data only
 505
 506         int num_bytes = IMUProtocol::encodeGyroUpdate(  protocol_buffer,
 507                                                        gyro[0], gyro[1], gyro[2],
 508                                                        accel[0], accel[1], accel[2],
 509                                                        mag_x, mag_y, mag_z,
 510                                                        temp_centigrade);
 511         Serial.write((unsigned char *)protocol_buffer, num_bytes);
 512
 513       }
 514       else {
 515
 516         // Send a Yaw/Pitch/Roll/Heading update
 517
 518         x -= calibrated_yaw_offset;
 519
 520         if ( x < -180 ) x += 360;
 521         if ( x > 180 ) x -= 360;
 522         if ( y < -180 ) y += 360;
 523         if ( y > 180 ) y -= 360;
 524         if ( z < -180 ) z += 360;
 525         if ( z > 180 ) z -= 360;
 526
 527         float linear_acceleration_x;
 528         float linear_acceleration_y;
 529         float linear_acceleration_z;
 530         float q1[4];
 531         float q2[4];
 532         float q_product[4];
 533         float q_conjugate[4];
 534         float q_final[4];
 535         float world_linear_acceleration_x;
 536         float world_linear_acceleration_y;
 537         float world_linear_acceleration_z;
 538
 539         // calculate linear acceleration by
 540         // removing the gravity component from raw acceleration values
 541
 542         linear_acceleration_x = (((float)accel[0]) / (32768.0 / accel_fsr)) - gravity.x;
 543         linear_acceleration_y = (((float)accel[1]) / (32768.0 / accel_fsr)) - gravity.y;
 544         linear_acceleration_z = (((float)accel[2]) / (32768.0 / accel_fsr)) - gravity.z;
 545
 546         // Calculate world-frame acceleration
 547
 548         q1[0] = quat[0] >> 16;
 549         q1[1] = quat[1] >> 16;
 550         q1[2] = quat[2] >> 16;
 551         q1[3] = quat[3] >> 16;
 552
 553         q2[0] = 0;
 554         q2[1] = linear_acceleration_x;
 555         q2[2] = linear_acceleration_y;
 556         q2[3] = linear_acceleration_z;
 557
 558         // Rotate linear acceleration so that it's relative to the world reference frame
 559
 560         // http://www.cprogramming.com/tutorial/3d/quaternions.html
 561         // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
 562         // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
 563         // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
 564
 565         // P_out = q * P_in * conj(q)
 566         // - P_out is the output vector
 567         // - q is the orientation quaternion
 568         // - P_in is the input vector (a*aReal)
 569         // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
 570
 571         // calculate quaternion product
 572         // Quaternion multiplication is defined by:
 573         //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
 574         //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
 575         //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
 576         //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
 577
 578         q_product[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];  // new w
 579         q_product[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];  // new x
 580         q_product[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];  // new y
 581         q_product[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];  // new z
 582
 583         q_conjugate[0] = q1[0];
 584         q_conjugate[1] = -q1[1];
 585         q_conjugate[2] = -q1[2];
 586         q_conjugate[3] = -q1[3];
 587
 588         q_final[0] = q_product[0]*q_conjugate[0] - q_product[1]*q_conjugate[1] - q_product[2]*q_conjugate[2] - q_product[3]*q_conjugate[3];  // new w
 589         q_final[1] = q_product[0]*q_conjugate[1] + q_product[1]*q_conjugate[0] + q_product[2]*q_conjugate[3] - q_product[3]*q_conjugate[2];  // new x
 590         q_final[2] = q_product[0]*q_conjugate[2] - q_product[1]*q_conjugate[3] + q_product[2]*q_conjugate[0] + q_product[3]*q_conjugate[1];  // new y
 591         q_final[3] = q_product[0]*q_conjugate[3] + q_product[1]*q_conjugate[2] - q_product[2]*q_conjugate[1] + q_product[3]*q_conjugate[0];  // new z
 592
 593         world_linear_acceleration_x = q_final[1];
 594         world_linear_acceleration_y = q_final[2];
 595         world_linear_acceleration_z = q_final[3];
 596
 597         int num_bytes = IMUProtocol::encodeYPRUpdate(protocol_buffer, x, y, z,compass_heading_degrees);
 598         Serial.write((unsigned char *)protocol_buffer, num_bytes);
 599       }
 600     }
 601     else {
 602
 603     }
 604   }
 605
 606   // If any serial bytes are received, scan to see if a start
 607   // of message has been received.  Remove any bytes that precede
 608   // the start of a message.
 609
 610   bool found_start_of_message = false;
 611   while ( Serial.available() > 0 ) {
 612     char rcv_byte = Serial.peek();
 613     if ( rcv_byte != PACKET_START_CHAR ) {
 614       Serial.read();
 615     }
 616     else {
 617       delay(2);
 618       found_start_of_message = true;
 619       break;
 620     }
 621   }
 622
 623   // If sufficient bytes have been received, process the data and
 624   // if a valid message is received, handle it.
 625
 626   boolean send_stream_response = false;
 627   if( found_start_of_message && ( Serial.available() >= STREAM_CMD_MESSAGE_LENGTH ) ) {
 628     int bytes_read = 0;
 629     while ( Serial.available() ) {
 630       if ( bytes_read >= sizeof(protocol_buffer) ) {
 631         break;
 632       }
 633       protocol_buffer[bytes_read++] = Serial.read();
 634     }
 635     int i = 0;
 636     // Scan the buffer looking for valid packets
 637     while ( i < bytes_read )
 638     {
 639       int bytes_remaining = bytes_read - i;
 640       char stream_type;
 641       unsigned char update_rate_hz;
 642       int packet_length = IMUProtocol::decodeStreamCommand( &protocol_buffer[i], bytes_remaining, stream_type, update_rate_hz );
 643       if ( packet_length > 0 )
 644       {
 645         send_stream_response = true;
 646         update_type = stream_type;
 647
 648         // if update rate has changed, reconfigure the MPU w/the new rate.
 649
 650         if ( update_rate_hz != dmp_update_rate ) {
 651           if ( update_rate_hz > MAX_NAV6_MPU_RATE ) {
 652             update_rate_hz = MAX_NAV6_MPU_RATE;
 653           }
 654           else if ( update_rate_hz < MIN_NAV6_MPU_RATE ) {
 655             update_rate_hz = MIN_NAV6_MPU_RATE;
 656           }
 657           disable_mpu();
 658           mpu_set_sample_rate(update_rate_hz);
 659           dmp_set_fifo_rate(update_rate_hz);
 660           // Read back configuration in case it was set improperly.
 661           mpu_get_sample_rate(&dmp_update_rate);
 662           enable_mpu();
 663         }
 664
 665         i += packet_length;
 666       }
 667       else // current index is not the start of a valid packet; increment
 668       {
 669         i++;
 670       }
 671     }
 672
 673   }
 674   if ( send_stream_response ) {
 675         int num_bytes = IMUProtocol::encodeStreamResponse(  protocol_buffer, update_type,
 676                                                               gyro_fsr, accel_fsr, dmp_update_rate, calibrated_yaw_offset,
 677                                                               (uint16_t)(calibrated_quaternion_offset[0] * 16384),
 678                                                               (uint16_t)(calibrated_quaternion_offset[1] * 16384),
 679                                                               (uint16_t)(calibrated_quaternion_offset[2] * 16384),
 680                                                               (uint16_t)(calibrated_quaternion_offset[3] * 16384),
 681                                                               calibration_state );
 682       Serial.write((unsigned char *)protocol_buffer, num_bytes);
 683   }
 684 }
 685 */ // end of loopxxxxxx
 686
 687 /* Every time new gyro data is available, this function is called in an
 688  * ISR context. In this example, it sets a flag saying the FIFO can be read
 689  */
 690
 691 void gyro_data_ready_cb(void) {
 692     new_mpu_data = 1;
 693 }
 694
 695 /* These next two functions converts the orientation matrix (see
 696  * gyro_orientation) to a scalar representation for use by the DMP.
 697  * NOTE: These functions are borrowed from Invensense's MPL.
 698  */
 699 unsigned short inv_row_2_scale(const signed char *row) {
 700     unsigned short b;
 701     if (row[0] > 0)
 702         b = 0;
 703     else if (row[0] < 0)
 704         b = 4;
 705     else if (row[1] > 0)
 706         b = 1;
 707     else if (row[1] < 0)
 708         b = 5;
 709     else if (row[2] > 0)
 710         b = 2;
 711     else if (row[2] < 0)
 712         b = 6;
 713     else
 714         b = 7;      // error
 715     return b;
 716 }
 717
 718
 719 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx) {
 720     unsigned short scalar; // scalar becomes one of the following values when only positive values are used in gyro orientation; values can be different when -1 is used instead of 1
 721     /*
 722        XYZ  010_001_000 Identity Matrix => when matrix is usual (and positive)  it returns 0x88 = 010_001_000
 723        XZY  001_010_000
 724        YXZ  010_000_001
 725        YZX  000_010_001
 726        ZXY  001_000_010
 727        ZYX  000_001_010
 728      */
 729
 730     scalar = inv_row_2_scale(mtx);
 731     scalar |= inv_row_2_scale(mtx + 3) << 3;
 732     scalar |= inv_row_2_scale(mtx + 6) << 6;
 733
 734
 735     return scalar;
 736 }
 737
 738 boolean initialize_mpu() {
 739
 740     int8_t result;
 741 //    struct int_param_s int_param;   // contains the call back function used by interrupt on dmp and the pin number of interrupt
 742
 743     /* Set up gyro.
 744      * Every function preceded by mpu_ is a driver function and can be found
 745      * in inv_mpu.h.
 746      */
 747 //    int_param.cb = gyro_data_ready_cb;           // name of the function being used when DMP interrupt occurs
 748 //    int_param.pin = 0;                           // use interrupt 0 to detect when mpu call an interrupt
 749 //    result = mpu_init(&int_param);               // initialize MPU with default values
 750     result = mpu_init();               // initialize MPU with default values
 751     if ( result != 0 ) {
 752 #ifdef DEBUG_MPU
 753       Serial.print("mpu_init failed!");
 754 #endif
 755       return false;
 756     }
 757                                                  // initialize the interrupt INT0 (= on arduino pin 2) or INT1 (on pin 3)
 758 #if PIN_INT_6050 == 2                            // INT0 is used
 759   #define IMU_INT_EDGE      0x03 // rising edge on pin2 (INT0)
 760   #define IMU_PIN_HEX       0x02
 761   #define IMU_INT_BIT       0x01
 762 #else                                            // INT1 is used
 763   #define IMU_INT_EDGE      0x0C  // rising edge on pin 3 (INT1)
 764   #define IMU_PIN_HEX       0x04
 765   #define IMU_INT_BIT       0x02
 766 #endif
 767   PORTD |= IMU_PIN_HEX ;  // Pullup resistor
 768   DDRD &= ~IMU_PIN_HEX ;  // set pin as Input
 769   EICRA |= IMU_INT_EDGE ;    // Interrupt on rising edge
 770   EIFR =   IMU_INT_BIT ;     // Clear interrupt flag on INT0 or INT1 writing a 1
 771   EIMSK |= IMU_INT_BIT ;    // Enable interrupt INT0 or INT1
 772
 773     /* Get/set hardware configuration. Start gyro. */
 774     /* Wake up all sensors. */
 775 //    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);                                      // to check if it can be removed because dmp is based on dmp feature
 776     mpu_enable_pwm_mgnt() ;                                                               // this is the same as set sensor : pw_mgnt_1 (6B) = INV_CLK_PLL (= 01) and pw_mgnt_2 (6C) = XYZG = 07
 777
 778     /* Push both gyro and accel data into the FIFO. */
 779 //    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);                                  ////// to check if this can be removed (could be that fifo is filled automatically based on dmp feature settings)
 780 //    mpu_set_sample_rate(DEFAULT_MPU_HZ);                                               ////// This is probably needed but when done here, no need to do it again in dmp init
 781       mpu_set_sample_rate200hz();                     // this function is made by Ms and is hardcoded to work for 200 hz. Still it is also called when dmp is enable (so no need to use it twice
 782     /* Read back configuration in case it was set improperly. */
 783 //    mpu_get_sample_rate(&dmp_update_rate);                                             ////// to check if this can be removed
 784 //    mpu_get_gyro_fsr(&gyro_fsr);
 785 //    mpu_get_accel_fsr(&accel_fsr);
 786
 787     /* Initialize HAL state variables. */                                              // this is already done by compiler
 788 //    memset(&hal, 0, sizeof(hal));
 789 //    hal.sensors = ACCEL_ON | GYRO_ON;
 790 #ifdef DEBUG_PMU
 791     dump_all();
 792 #endif
 793     /* To initialize the DMP:
 794      * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
 795      *    inv_mpu_dmp_motion_driver.h into the MPU memory.
 796      * 2. Push the gyro and accel orientation matrix to the DMP.
 797      * 3. Register gesture callbacks. Don't worry, these callbacks won't be
 798      *    executed unless the corresponding feature is enabled.
 799      * 4. Call dmp_enable_feature(mask) to enable different features.
 800      * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
 801      * 6. Call any feature-specific control functions.
 802      *
 803      * To enable the DMP, just call mpu_set_dmp_state(1). This function can
 804      * be called repeatedly to enable and disable the DMP at runtime.
 805      *
 806      * The following is a short summary of the features supported in the DMP
 807      * image provided in inv_mpu_dmp_motion_driver.c:
 808      * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
 809      * 200Hz. Integrating the gyro data at higher rates reduces numerical
 810      * errors (compared to integration on the MCU at a lower sampling rate).
 811      * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
 812      * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
 813      * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
 814      * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
 815      * an event at the four orientations where the screen should rotate.
 816      * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
 817      * no motion.
 818      * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
 819      * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
 820      * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
 821      * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
 822      */
 823     result = dmp_load_motion_driver_firmware();                           // load the firmware
 824     if ( result != 0 ) {
 825 #ifdef DEBUG_MPU
 826       Serial.print("Firmware Load ERROR ");
 827       Serial.println(result);
 828 #endif
 829       return false;
 830     }
 831 //    dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation));
 832 #ifdef DEBUG_MPU
 833 //      Serial.print("scalar  ");
 834 //      Serial.println(inv_orientation_matrix_to_scalar(gyro_orientation),HEX);
 835 #endif
 836
 837 //    unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |  DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
 838 //    unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |  DMP_FEATURE_GYRO_CAL;
 839     dmp_enable_feature( (unsigned short )DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |  DMP_FEATURE_GYRO_CAL );
 840 //    dmp_set_fifo_rate(50);// test with 20 hz (was originally on 100) and it works (interrupt is activated only once every 20 msec)
 841 //                          // this is not used because it is already changed in the firmware.
 842
 843 #ifdef DEBUG_MPUxxxxxxx
 844       unsigned char dataToCheck[16] ;
 845       uint16_t addToCheck = 0x0A98 ;
 846       mpu_read_mem(addToCheck , 16 , dataToCheck) ;
 847       Serial.print("Firmw.adr ");Serial.print(addToCheck);
 848       for (uint8_t iCheck = 0 ; iCheck <16 ; iCheck++) {
 849          Serial.print(" "); Serial.print(dataToCheck[iCheck], HEX );
 850       }
 851       Serial.println(" ");
 852 #endif
 853
 854
 855     return true;
 856 }
 857
 858 /*
 859 void disable_mpu() {                                // is not used by oXs
 860     mpu_set_dmp_state(0);
 861     hal.dmp_on = 0;
 862 }
 863 */
 864 /*
 865 void enable_mpu() {
 866
 867     mpu_set_dmp_state_on();  // This enables the DMP; at this point, interrupts should commence
 868 //    hal.dmp_on = 1;
 869 }
 870 */
 871 /*
 872 boolean run_mpu_self_test(boolean& gyro_ok, boolean& accel_ok) {                               // saving : is currently not used by oXs
 873
 874     int result;
 875     long gyro[3], accel[3];
 876     boolean success = false;
 877
 878     gyro_ok = false;
 879     accel_ok = false;
 880     result = mpu_run_self_test(gyro, accel);
 881
 882     if ( ( result & 0x1 ) != 0 ) {
 883       // Gyro passed self test
 884       gyro_ok = true;
 885       float sens;
 886       mpu_get_gyro_sens(&sens);
 887       gyro[0] = (long)(gyro[0] * sens);
 888       gyro[1] = (long)(gyro[1] * sens);
 889       gyro[2] = (long)(gyro[2] * sens);
 890       dmp_set_gyro_bias(gyro);
 891     }
 892     if ( ( ( result & 0x2 ) != 0 ) || true)  { // || true has been added to force the test on accel which in my case returned false
 893       // Accelerometer passed self test
 894       accel_ok = true;
 895       unsigned short accel_sens;
 896       mpu_get_accel_sens(&accel_sens);
 897 #ifdef DEBUG_MPU
 898         Serial.print(F("testAccX "));Serial.print( accel[0] ); Serial.print(F(",")); Serial.print( accel[1] ); Serial.print(F(","));;Serial.print( accel[2] );Serial.print(F(","));Serial.println( accel_sens );
 899 #endif
 900       accel[0] *= accel_sens;
 901       accel[1] *= accel_sens;
 902       accel[2] *= accel_sens;
 903       dmp_set_accel_bias(accel);
 904     }
 905     success = gyro_ok && accel_ok;
 906     return success;
 907 }
 908 */
 909
 910 /*
 911 void getEuler(float *data, Quaternion *q) {
 912     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
 913     data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                                          // theta
 914     data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi????
 915 }
 916 */
 917
 918 void getGravity(struct FloatVectorStruct *v, Quaternion *q) {
 919     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
 920     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
 921     v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
 922 }
 923
 924 void dmpGetYawPitchRoll(float *data, Quaternion *q, struct FloatVectorStruct *gravity) {
 925     // yaw: (about Z axis)
 926     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
 927     // pitch: (nose up/down, about Y axis)
 928     data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
 929     // roll: (tilt left/right, about X axis)
 930     data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
 931 }
 932
 933 /*
 934 // The following code calculates the amount of free memory.
 935 extern unsigned int __heap_start;
 936 extern void *__brkval;
 937
 938  // The free list structure as maintained by the
 939  // avr-libc memory allocation routines.
 940 struct __freelist {
 941    size_t sz;
 942    struct __freelist *nx;
 943 };
 944
 945 // The head of the free list structure //
 946 extern struct __freelist *__flp;
 947
 948 // Calculates the size of the free list //
 949 int freeListSize() {
 950
 951    struct __freelist* current;
 952    int total = 0;
 953
 954   for (current = __flp; current; current = current->nx) {
 955      total += 2; // Add two bytes for the memory block's header  //
 956      total += (int) current->sz;
 957    }
 958
 959   return total;
 960 }
 961
 962 int freeMemory()  {                                                    // is currently not used by oXs (there is already such a function in debug mode)
 963    int free_memory;
 964    if ((int)__brkval == 0) {
 965      free_memory = ((int)&free_memory) - ((int)&__heap_start);
 966    } else {
 967      free_memory = ((int)&free_memory) - ((int)__brkval);
 968      free_memory += freeListSize();
 969    }
 970    return free_memory;
 971 }
 972 */
 973 #endif // end USE_6050