main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-April-2015
00007  * @brief   Example application for using the X_NUCLEO_IKS01A1 
00008  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037 */ 
00038 
00039 /**
00040  * @mainpage X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Sensor Nucleo Expansion Board Firmware Package
00041  *
00042  * <b>Introduction</b>
00043  *
00044  * This firmware package includes Components Device Drivers, Board Support Package
00045  * and example application for STMicroelectronics X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Nucleo
00046  * Expansion Board
00047  * 
00048  * <b>Example Application</b>
00049  *
00050  */
00051 
00052 
00053 /*** Includes ----------------------------------------------------------------- ***/
00054 #include "mbed.h"
00055 #include "assert.h"
00056 #include "x_nucleo_iks01a1.h"
00057 #include "Kalman.h"
00058 #include <math.h>
00059 
00060 #include <Ticker.h>
00061 
00062 
00063 /*** Constants ---------------------------------------------------------------- ***/
00064 namespace {
00065     const int MS_INTERVALS = 5;
00066     const double RAD_TO_DEG = 57.2957786;
00067     const double PI = 3.14159265;
00068 }
00069 
00070 
00071 /*** Macros ------------------------------------------------------------------- ***/
00072 #define APP_LOOP_PERIOD 3000 // in ms
00073 
00074 #if defined(TARGET_STM)
00075 #define LED_OFF (0)
00076 #else
00077 #define LED_OFF (1)
00078 #endif
00079 #define LED_ON  (!LED_OFF)
00080 
00081 #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead
00082 #define DECLINATION 2.23 
00083 
00084 Kalman kalmanX; // Create the Kalman instances
00085 Kalman kalmanY;
00086 Kalman kalmanZ;
00087 
00088 double gyroXangle, gyroYangle; // Angle calculate using the gyro only
00089 double compAngleX, compAngleY; // Calculated angle using a complementary filter
00090 double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter
00091 
00092 Timer t;
00093 uint32_t timer;
00094 
00095 /*** Typedefs ----------------------------------------------------------------- ***/
00096 typedef struct {
00097     int32_t AXIS_X;
00098     int32_t AXIS_Y;
00099     int32_t AXIS_Z;
00100 } AxesRaw_TypeDef;
00101 
00102 /*** Serial declaration --------------------------------------------------------- ***/
00103 Serial ser(USBTX,USBRX,115200);
00104 
00105 /*** Static variables --------------------------------------------------------- ***/
00106 #ifdef DBG_MCU
00107 /* betzw: enable debugging while using sleep modes */
00108 #include "DbgMCU.h"
00109 static DbgMCU enable_dbg;
00110 #endif // DBG_MCU
00111 
00112 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance();
00113 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
00114 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
00115 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
00116 
00117 static Ticker ticker;
00118 static DigitalOut myled(LED1, LED_OFF);
00119 
00120 static volatile bool timer_irq_triggered = false;
00121 static volatile bool ff_irq_triggered = false;
00122 
00123 
00124 /*** Helper Functions (1/2) ------------------------------------------------------------ ***/
00125 
00126 
00127 /*** Interrupt Handler Top-Halves ------------------------------------------------------ ***/
00128 /* Called in interrupt context, therefore just set a trigger variable */
00129 static void timer_irq(void) {
00130     timer_irq_triggered = true;
00131 }
00132 
00133 /* Called in interrupt context, therefore just set a trigger variable */
00134 static void ff_irq(void) {
00135     ff_irq_triggered = true;
00136 
00137     /* Disable IRQ until handled */
00138     mems_expansion_board->gyro_lsm6ds3->Disable_Free_Fall_Detection_IRQ();
00139 }
00140 
00141 
00142 /*** Interrupt Handler Bottom-Halves ------------------------------------------------- ***/
00143 /* Handle Free Fall Interrupt
00144    (here we are in "normal" context, i.e. not in IRQ context)
00145 */
00146 static void handle_ff_irq(void) {
00147     printf("\nFree Fall Detected!\n\n");
00148 
00149     /* Re-enable IRQ */
00150     mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection_IRQ();
00151 }
00152 
00153 
00154 /*** Helper Functions (2/2) ------------------------------------------------------------ ***/
00155 /* Initialization function */
00156 static void init(void) {
00157     t.start();
00158     uint8_t id1, id2;
00159     
00160     /* Determine ID of Gyro & Motion Sensor */
00161     assert((mems_expansion_board->gyro_lsm6ds0 == NULL) ||
00162            (mems_expansion_board->gyro_lsm6ds3 == NULL));
00163     CALL_METH(gyroscope, read_id, &id1, 0x0);
00164     CALL_METH(accelerometer, read_id, &id2, 0x0);
00165         printf("Gyroscope | Motion Sensor ID      = %s (0x%x | 0x%x)\n", 
00166            ((id1 == I_AM_LSM6DS3_XG) ? "LSM6DS3" : 
00167         ((id1 == I_AM_LSM6DS0_XG) ? "LSM6DS0" : "UNKNOWN")),
00168            id1, id2
00169            );
00170     assert(id1 == id2);
00171 
00172     /* Register Free Fall Detection IRQ Handler & Enable Detection */
00173     if(mems_expansion_board->gyro_lsm6ds3 != NULL) {
00174         mems_expansion_board->gyro_lsm6ds3->Attach_Free_Fall_Detection_IRQ(ff_irq);
00175         mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection();
00176     }
00177     
00178     AxesRaw_TypeDef MAG_Value;
00179     AxesRaw_TypeDef ACC_Value;
00180     AxesRaw_TypeDef GYR_Value;
00181     unsigned int ret = 0;
00182     
00183     /* Switch LED On */
00184     myled = LED_ON;
00185     //printf("===\n");
00186 
00187     /* Determine Environmental Values */
00188     ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
00189     ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
00190     ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
00191     
00192     /* IMU Data */
00193     double accX, accY, accZ;
00194     double gyroX, gyroY, gyroZ;
00195 
00196     accX = ACC_Value.AXIS_X;
00197     accY = ACC_Value.AXIS_Y;
00198     accZ = ACC_Value.AXIS_Z;
00199     /*
00200     **
00201     gyroX = GYR_Value.AXIS_X;
00202     gyroY = GYR_Value.AXIS_Y;
00203     gyroZ = GYR_Value.AXIS_Z;
00204     **
00205     */
00206     
00207     #ifdef RESTRICT_PITCH // Eq. 25 and 26
00208     double roll  = atan2(accY, accZ) * RAD_TO_DEG;
00209     double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
00210     #else // Eq. 28 and 29
00211     double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
00212     double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
00213     #endif
00214     //double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
00215     kalmanX.setAngle(roll); // Set starting angle
00216     kalmanY.setAngle(pitch);
00217     gyroXangle = roll;
00218     gyroYangle = pitch;
00219     compAngleX = roll;
00220     compAngleY = pitch;
00221     
00222     timer = t.read_us();
00223 }
00224 
00225 /* Main cycle function */
00226 static void main_cycle(void) {
00227     AxesRaw_TypeDef MAG_Value;
00228     AxesRaw_TypeDef ACC_Value;
00229     AxesRaw_TypeDef GYR_Value;
00230     unsigned int ret = 0;
00231     
00232     /* Switch LED On */
00233     myled = LED_ON;
00234     //printf("===\n");
00235 
00236     /* Determine Environmental Values */
00237     ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
00238     ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
00239     ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
00240 
00241     /* Print Values Out */
00242         //printf("I2C [errors]: 0x%.2x    X         Y         Z\n", ret); 
00243         /*
00244         **
00245         printf("%9ld:%9ld:%9ld:", ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
00246         printf("%9ld:%9ld:%9ld:", GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
00247         printf("%9ld:%9ld:%9ld\n", MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
00248         **
00249         */
00250         
00251     /* IMU Data */
00252     double accX, accY, accZ;
00253     double gyroX, gyroY, gyroZ;
00254     double magX, magY, magZ;
00255     
00256     accX = ACC_Value.AXIS_X;
00257     accY = ACC_Value.AXIS_Y;
00258     accZ = ACC_Value.AXIS_Z;
00259     gyroX = GYR_Value.AXIS_X;
00260     gyroY = GYR_Value.AXIS_Y;
00261     gyroZ = GYR_Value.AXIS_Z;
00262     
00263     double dt = (double)(t.read_us() - timer) / 1000000; // Calculate delta time
00264     timer = t.read_us();
00265     
00266     #ifdef RESTRICT_PITCH // Eq. 25 and 26
00267     double roll  = atan2(accY, accZ) * RAD_TO_DEG;
00268     double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
00269     #else // Eq. 28 and 29
00270     double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
00271     double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
00272     #endif
00273     
00274     double gyroXrate = gyroX / 131.0; // Convert to deg/s
00275     double gyroYrate = gyroY / 131.0; // Convert to deg/s
00276     
00277     #ifdef RESTRICT_PITCH
00278     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
00279     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
00280         kalmanX.setAngle(roll);
00281         compAngleX = roll;
00282         kalAngleX = roll;
00283         gyroXangle = roll;
00284     }
00285     else
00286     kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
00287     
00288     if (abs(kalAngleX) > 90)
00289     gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
00290     kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
00291     #else
00292     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
00293     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
00294         kalmanY.setAngle(pitch);
00295         compAngleY = pitch;
00296         kalAngleY = pitch;
00297         gyroYangle = pitch;
00298     }
00299     else
00300     kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
00301     
00302     if (abs(kalAngleY) > 90)
00303     gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
00304     kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
00305     #endif
00306     
00307     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
00308     gyroYangle += gyroYrate * dt;
00309     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
00310     //gyroYangle += kalmanY.getRate() * dt;
00311     
00312     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
00313     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
00314     
00315     // Reset the gyro angle when it has drifted too much
00316     if (gyroXangle < -180 || gyroXangle > 180)
00317     gyroXangle = kalAngleX;
00318     if (gyroYangle < -180 || gyroYangle > 180)
00319     gyroYangle = kalAngleY;
00320     
00321     // Compute the Heading
00322     
00323     magX = MAG_Value.AXIS_X;
00324     magY = MAG_Value.AXIS_Y;
00325     magZ = MAG_Value.AXIS_Z;
00326     
00327     float heading;
00328     if (magY== 0)
00329     heading = (magX < 0) ? 180.0 : 0;
00330     else
00331     heading = atan2(magX , magY);
00332     //arctan(imu.mx / sqrt(imu.mz*imu.mz + imu.my*imu.my)):
00333     heading -= DECLINATION * PI / 180;
00334     
00335     if (heading > PI)
00336     heading -= (2 * PI);
00337     else if (heading < -PI || heading < 0)
00338     heading += (2 * PI);
00339     heading *= 180.0 / PI;
00340   
00341     printf("%lf:%1f:%1f\n", kalAngleY, kalAngleX, heading);
00342     
00343     
00344     /* Switch LED Off */
00345     myled = LED_OFF;
00346 }
00347 
00348 
00349 /*** Main function ------------------------------------------------------------- ***/
00350 /* Generic main function/loop for enabling WFE in case of 
00351    interrupt based cyclic execution
00352 */
00353 int main()
00354 {
00355     /* Start & initialize */
00356     //printf("\n--- Starting new run ---\n");
00357     init();
00358 
00359     /* Start timer irq */
00360     ticker.attach_us(timer_irq, MS_INTERVALS * APP_LOOP_PERIOD);
00361 
00362     while (true) {
00363         if(timer_irq_triggered) {
00364             timer_irq_triggered = false;
00365             main_cycle();
00366         } else if(ff_irq_triggered) {
00367             ff_irq_triggered = false;
00368             handle_ff_irq();
00369         } else {
00370             __WFE(); /* it is recommended that SEVONPEND in the 
00371                     System Control Register is NOT set */
00372         }
00373     }
00374     t.stop();
00375 }