Ahmad Kobeissi / Mbed 2 deprecated Controllify

Test whole program

Dependencies:   X_NUCLEO_IKS01A1 mbed

Fork of Sensors_Reader_JACKLENZ by Giacomo Lanza

main.cpp@73:2e4b834c065d, 2017-11-13 (annotated)

Committer:
ahmad47
Date:
Mon Nov 13 12:58:06 2017 +0000
Revision:
73:2e4b834c065d
Parent:
71:a6a052fd3d22 
Test 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wolfgang Betz 0:3e4f610a0689 1 /**
Wolfgang Betz 0:3e4f610a0689 2 ******************************************************************************
Wolfgang Betz 0:3e4f610a0689 3 * @file main.cpp
Wolfgang Betz 0:3e4f610a0689 4 * @author AST / EST
Wolfgang Betz 0:3e4f610a0689 5 * @version V0.0.1
Wolfgang Betz 0:3e4f610a0689 6 * @date 14-April-2015
Wolfgang Betz 0:3e4f610a0689 7 * @brief Example application for using the X_NUCLEO_IKS01A1
Wolfgang Betz 5:a1de9dcb0f41 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
Wolfgang Betz 0:3e4f610a0689 9 ******************************************************************************
Wolfgang Betz 0:3e4f610a0689 10 * @attention
Wolfgang Betz 0:3e4f610a0689 11 *
Wolfgang Betz 0:3e4f610a0689 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Wolfgang Betz 0:3e4f610a0689 13 *
Wolfgang Betz 0:3e4f610a0689 14 * Redistribution and use in source and binary forms, with or without modification,
Wolfgang Betz 0:3e4f610a0689 15 * are permitted provided that the following conditions are met:
Wolfgang Betz 0:3e4f610a0689 16 * 1. Redistributions of source code must retain the above copyright notice,
Wolfgang Betz 0:3e4f610a0689 17 * this list of conditions and the following disclaimer.
Wolfgang Betz 0:3e4f610a0689 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Wolfgang Betz 0:3e4f610a0689 19 * this list of conditions and the following disclaimer in the documentation
Wolfgang Betz 0:3e4f610a0689 20 * and/or other materials provided with the distribution.
Wolfgang Betz 0:3e4f610a0689 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Wolfgang Betz 0:3e4f610a0689 22 * may be used to endorse or promote products derived from this software
Wolfgang Betz 0:3e4f610a0689 23 * without specific prior written permission.
Wolfgang Betz 0:3e4f610a0689 24 *
Wolfgang Betz 0:3e4f610a0689 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Wolfgang Betz 0:3e4f610a0689 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Wolfgang Betz 0:3e4f610a0689 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Wolfgang Betz 0:3e4f610a0689 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Wolfgang Betz 0:3e4f610a0689 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Wolfgang Betz 0:3e4f610a0689 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Wolfgang Betz 0:3e4f610a0689 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Wolfgang Betz 0:3e4f610a0689 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Wolfgang Betz 0:3e4f610a0689 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Wolfgang Betz 0:3e4f610a0689 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Wolfgang Betz 0:3e4f610a0689 35 *
Wolfgang Betz 0:3e4f610a0689 36 ******************************************************************************
Wolfgang Betz 0:3e4f610a0689 37 */
Wolfgang Betz 0:3e4f610a0689 38
Wolfgang Betz 0:3e4f610a0689 39 /**
Wolfgang Betz 5:a1de9dcb0f41 40 * @mainpage X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Sensor Nucleo Expansion Board Firmware Package
Wolfgang Betz 0:3e4f610a0689 41 *
Wolfgang Betz 0:3e4f610a0689 42 * <b>Introduction</b>
Wolfgang Betz 0:3e4f610a0689 43 *
Wolfgang Betz 0:3e4f610a0689 44 * This firmware package includes Components Device Drivers, Board Support Package
Wolfgang Betz 0:3e4f610a0689 45 * and example application for STMicroelectronics X_NUCLEO_IKS01A1 MEMS Inertial & Environmental Nucleo
Wolfgang Betz 0:3e4f610a0689 46 * Expansion Board
Wolfgang Betz 0:3e4f610a0689 47 *
Wolfgang Betz 0:3e4f610a0689 48 * <b>Example Application</b>
Wolfgang Betz 0:3e4f610a0689 49 *
Wolfgang Betz 0:3e4f610a0689 50 */
Wolfgang Betz 0:3e4f610a0689 51
Wolfgang Betz 0:3e4f610a0689 52
Wolfgang Betz 0:3e4f610a0689 53 /*** Includes ----------------------------------------------------------------- ***/
Wolfgang Betz 0:3e4f610a0689 54 #include "mbed.h"
Wolfgang Betz 33:7ba7fbf0503a 55 #include "assert.h"
Wolfgang Betz 0:3e4f610a0689 56 #include "x_nucleo_iks01a1.h"
ahmad47 71:a6a052fd3d22 57 #include "Kalman.h"
JackLenz 70:c6b61c5cadf4 58 #include <math.h>
Wolfgang Betz 0:3e4f610a0689 59
Wolfgang Betz 0:3e4f610a0689 60 #include <Ticker.h>
Wolfgang Betz 0:3e4f610a0689 61
Wolfgang Betz 0:3e4f610a0689 62
Wolfgang Betz 0:3e4f610a0689 63 /*** Constants ---------------------------------------------------------------- ***/
Wolfgang Betz 0:3e4f610a0689 64 namespace {
ahmad47 71:a6a052fd3d22 65 const int MS_INTERVALS = 5;
JackLenz 70:c6b61c5cadf4 66 const double RAD_TO_DEG = 57.2957786;
JackLenz 70:c6b61c5cadf4 67 const double PI = 3.14159265;
Wolfgang Betz 0:3e4f610a0689 68 }
Wolfgang Betz 0:3e4f610a0689 69
Wolfgang Betz 0:3e4f610a0689 70
Wolfgang Betz 0:3e4f610a0689 71 /*** Macros ------------------------------------------------------------------- ***/
Wolfgang Betz 40:f567538352e1 72 #define APP_LOOP_PERIOD 3000 // in ms
Wolfgang Betz 0:3e4f610a0689 73
Wolfgang Betz 4:81037ace7f27 74 #if defined(TARGET_STM)
Wolfgang Betz 4:81037ace7f27 75 #define LED_OFF (0)
Wolfgang Betz 4:81037ace7f27 76 #else
Wolfgang Betz 4:81037ace7f27 77 #define LED_OFF (1)
Wolfgang Betz 4:81037ace7f27 78 #endif
Wolfgang Betz 4:81037ace7f27 79 #define LED_ON (!LED_OFF)
Wolfgang Betz 4:81037ace7f27 80
ahmad47 71:a6a052fd3d22 81 #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead
ahmad47 71:a6a052fd3d22 82 #define DECLINATION 2.23
ahmad47 71:a6a052fd3d22 83
ahmad47 71:a6a052fd3d22 84 Kalman kalmanX; // Create the Kalman instances
ahmad47 71:a6a052fd3d22 85 Kalman kalmanY;
ahmad47 71:a6a052fd3d22 86 Kalman kalmanZ;
ahmad47 71:a6a052fd3d22 87
ahmad47 71:a6a052fd3d22 88 double gyroXangle, gyroYangle; // Angle calculate using the gyro only
ahmad47 71:a6a052fd3d22 89 double compAngleX, compAngleY; // Calculated angle using a complementary filter
ahmad47 71:a6a052fd3d22 90 double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter
ahmad47 71:a6a052fd3d22 91
ahmad47 71:a6a052fd3d22 92 Timer t;
ahmad47 71:a6a052fd3d22 93 uint32_t timer;
Wolfgang Betz 0:3e4f610a0689 94
Wolfgang Betz 1:9458657e49ee 95 /*** Typedefs ----------------------------------------------------------------- ***/
Wolfgang Betz 1:9458657e49ee 96 typedef struct {
Wolfgang Betz 1:9458657e49ee 97 int32_t AXIS_X;
Wolfgang Betz 1:9458657e49ee 98 int32_t AXIS_Y;
Wolfgang Betz 1:9458657e49ee 99 int32_t AXIS_Z;
Wolfgang Betz 1:9458657e49ee 100 } AxesRaw_TypeDef;
Wolfgang Betz 1:9458657e49ee 101
JackLenz 70:c6b61c5cadf4 102 /*** Serial declaration --------------------------------------------------------- ***/
JackLenz 70:c6b61c5cadf4 103 Serial ser(USBTX,USBRX,115200);
Wolfgang Betz 1:9458657e49ee 104
Wolfgang Betz 0:3e4f610a0689 105 /*** Static variables --------------------------------------------------------- ***/
Wolfgang Betz 0:3e4f610a0689 106 #ifdef DBG_MCU
Wolfgang Betz 0:3e4f610a0689 107 /* betzw: enable debugging while using sleep modes */
Wolfgang Betz 0:3e4f610a0689 108 #include "DbgMCU.h"
Wolfgang Betz 0:3e4f610a0689 109 static DbgMCU enable_dbg;
Wolfgang Betz 0:3e4f610a0689 110 #endif // DBG_MCU
Wolfgang Betz 0:3e4f610a0689 111
Wolfgang Betz 1:9458657e49ee 112 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance();
Wolfgang Betz 33:7ba7fbf0503a 113 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Wolfgang Betz 33:7ba7fbf0503a 114 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Wolfgang Betz 32:97bff5dadafd 115 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
Wolfgang Betz 18:645c96f209c7 116
Wolfgang Betz 0:3e4f610a0689 117 static Ticker ticker;
Wolfgang Betz 40:f567538352e1 118 static DigitalOut myled(LED1, LED_OFF);
Wolfgang Betz 40:f567538352e1 119
Wolfgang Betz 0:3e4f610a0689 120 static volatile bool timer_irq_triggered = false;
Wolfgang Betz 40:f567538352e1 121 static volatile bool ff_irq_triggered = false;
Wolfgang Betz 4:81037ace7f27 122
Wolfgang Betz 0:3e4f610a0689 123
Wolfgang Betz 0:3e4f610a0689 124 /*** Helper Functions (1/2) ------------------------------------------------------------ ***/
Wolfgang Betz 0:3e4f610a0689 125
Wolfgang Betz 0:3e4f610a0689 126
Wolfgang Betz 0:3e4f610a0689 127 /*** Interrupt Handler Top-Halves ------------------------------------------------------ ***/
Wolfgang Betz 0:3e4f610a0689 128 /* Called in interrupt context, therefore just set a trigger variable */
Wolfgang Betz 0:3e4f610a0689 129 static void timer_irq(void) {
Wolfgang Betz 0:3e4f610a0689 130 timer_irq_triggered = true;
Wolfgang Betz 0:3e4f610a0689 131 }
Wolfgang Betz 0:3e4f610a0689 132
Wolfgang Betz 40:f567538352e1 133 /* Called in interrupt context, therefore just set a trigger variable */
Wolfgang Betz 40:f567538352e1 134 static void ff_irq(void) {
Wolfgang Betz 40:f567538352e1 135 ff_irq_triggered = true;
Wolfgang Betz 40:f567538352e1 136
Wolfgang Betz 40:f567538352e1 137 /* Disable IRQ until handled */
Wolfgang Betz 40:f567538352e1 138 mems_expansion_board->gyro_lsm6ds3->Disable_Free_Fall_Detection_IRQ();
Wolfgang Betz 40:f567538352e1 139 }
Wolfgang Betz 40:f567538352e1 140
Wolfgang Betz 0:3e4f610a0689 141
Wolfgang Betz 0:3e4f610a0689 142 /*** Interrupt Handler Bottom-Halves ------------------------------------------------- ***/
Wolfgang Betz 40:f567538352e1 143 /* Handle Free Fall Interrupt
Wolfgang Betz 40:f567538352e1 144 (here we are in "normal" context, i.e. not in IRQ context)
Wolfgang Betz 40:f567538352e1 145 */
Wolfgang Betz 40:f567538352e1 146 static void handle_ff_irq(void) {
Wolfgang Betz 40:f567538352e1 147 printf("\nFree Fall Detected!\n\n");
Wolfgang Betz 40:f567538352e1 148
Wolfgang Betz 40:f567538352e1 149 /* Re-enable IRQ */
Wolfgang Betz 40:f567538352e1 150 mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection_IRQ();
Wolfgang Betz 40:f567538352e1 151 }
Wolfgang Betz 0:3e4f610a0689 152
Wolfgang Betz 0:3e4f610a0689 153
Wolfgang Betz 0:3e4f610a0689 154 /*** Helper Functions (2/2) ------------------------------------------------------------ ***/
Wolfgang Betz 0:3e4f610a0689 155 /* Initialization function */
Wolfgang Betz 0:3e4f610a0689 156 static void init(void) {
ahmad47 71:a6a052fd3d22 157 t.start();
Wolfgang Betz 33:7ba7fbf0503a 158 uint8_t id1, id2;
JackLenz 70:c6b61c5cadf4 159
Wolfgang Betz 33:7ba7fbf0503a 160 /* Determine ID of Gyro & Motion Sensor */
Wolfgang Betz 33:7ba7fbf0503a 161 assert((mems_expansion_board->gyro_lsm6ds0 == NULL) ||
Wolfgang Betz 33:7ba7fbf0503a 162 (mems_expansion_board->gyro_lsm6ds3 == NULL));
Wolfgang Betz 69:12b1170b510a 163 CALL_METH(gyroscope, read_id, &id1, 0x0);
Wolfgang Betz 69:12b1170b510a 164 CALL_METH(accelerometer, read_id, &id2, 0x0);
Wolfgang Betz 33:7ba7fbf0503a 165 printf("Gyroscope | Motion Sensor ID = %s (0x%x | 0x%x)\n",
Wolfgang Betz 33:7ba7fbf0503a 166 ((id1 == I_AM_LSM6DS3_XG) ? "LSM6DS3" :
Wolfgang Betz 33:7ba7fbf0503a 167 ((id1 == I_AM_LSM6DS0_XG) ? "LSM6DS0" : "UNKNOWN")),
Wolfgang Betz 33:7ba7fbf0503a 168 id1, id2
Wolfgang Betz 33:7ba7fbf0503a 169 );
Wolfgang Betz 33:7ba7fbf0503a 170 assert(id1 == id2);
Wolfgang Betz 33:7ba7fbf0503a 171
Wolfgang Betz 40:f567538352e1 172 /* Register Free Fall Detection IRQ Handler & Enable Detection */
Wolfgang Betz 40:f567538352e1 173 if(mems_expansion_board->gyro_lsm6ds3 != NULL) {
Wolfgang Betz 40:f567538352e1 174 mems_expansion_board->gyro_lsm6ds3->Attach_Free_Fall_Detection_IRQ(ff_irq);
Wolfgang Betz 40:f567538352e1 175 mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection();
Wolfgang Betz 40:f567538352e1 176 }
ahmad47 71:a6a052fd3d22 177
Wolfgang Betz 1:9458657e49ee 178 AxesRaw_TypeDef MAG_Value;
Wolfgang Betz 1:9458657e49ee 179 AxesRaw_TypeDef ACC_Value;
Wolfgang Betz 1:9458657e49ee 180 AxesRaw_TypeDef GYR_Value;
Wolfgang Betz 50:4a902230142d 181 unsigned int ret = 0;
Wolfgang Betz 50:4a902230142d 182
Wolfgang Betz 1:9458657e49ee 183 /* Switch LED On */
Wolfgang Betz 4:81037ace7f27 184 myled = LED_ON;
ahmad47 71:a6a052fd3d22 185 //printf("===\n");
Wolfgang Betz 1:9458657e49ee 186
Wolfgang Betz 1:9458657e49ee 187 /* Determine Environmental Values */
Wolfgang Betz 69:12b1170b510a 188 ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
Wolfgang Betz 69:12b1170b510a 189 ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
Wolfgang Betz 69:12b1170b510a 190 ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
ahmad47 71:a6a052fd3d22 191
ahmad47 71:a6a052fd3d22 192 /* IMU Data */
ahmad47 71:a6a052fd3d22 193 double accX, accY, accZ;
ahmad47 71:a6a052fd3d22 194 double gyroX, gyroY, gyroZ;
Wolfgang Betz 1:9458657e49ee 195
JackLenz 70:c6b61c5cadf4 196 accX = ACC_Value.AXIS_X;
JackLenz 70:c6b61c5cadf4 197 accY = ACC_Value.AXIS_Y;
JackLenz 70:c6b61c5cadf4 198 accZ = ACC_Value.AXIS_Z;
ahmad47 71:a6a052fd3d22 199 /*
ahmad47 71:a6a052fd3d22 200 **
JackLenz 70:c6b61c5cadf4 201 gyroX = GYR_Value.AXIS_X;
JackLenz 70:c6b61c5cadf4 202 gyroY = GYR_Value.AXIS_Y;
JackLenz 70:c6b61c5cadf4 203 gyroZ = GYR_Value.AXIS_Z;
ahmad47 71:a6a052fd3d22 204 **
ahmad47 71:a6a052fd3d22 205 */
JackLenz 70:c6b61c5cadf4 206
JackLenz 70:c6b61c5cadf4 207 #ifdef RESTRICT_PITCH // Eq. 25 and 26
JackLenz 70:c6b61c5cadf4 208 double roll = atan2(accY, accZ) * RAD_TO_DEG;
JackLenz 70:c6b61c5cadf4 209 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
JackLenz 70:c6b61c5cadf4 210 #else // Eq. 28 and 29
JackLenz 70:c6b61c5cadf4 211 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
JackLenz 70:c6b61c5cadf4 212 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
JackLenz 70:c6b61c5cadf4 213 #endif
ahmad47 71:a6a052fd3d22 214 //double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
ahmad47 71:a6a052fd3d22 215 kalmanX.setAngle(roll); // Set starting angle
ahmad47 71:a6a052fd3d22 216 kalmanY.setAngle(pitch);
ahmad47 71:a6a052fd3d22 217 gyroXangle = roll;
ahmad47 71:a6a052fd3d22 218 gyroYangle = pitch;
ahmad47 71:a6a052fd3d22 219 compAngleX = roll;
ahmad47 71:a6a052fd3d22 220 compAngleY = pitch;
ahmad47 71:a6a052fd3d22 221
ahmad47 71:a6a052fd3d22 222 timer = t.read_us();
ahmad47 71:a6a052fd3d22 223 }
ahmad47 71:a6a052fd3d22 224
ahmad47 71:a6a052fd3d22 225 /* Main cycle function */
ahmad47 71:a6a052fd3d22 226 static void main_cycle(void) {
ahmad47 71:a6a052fd3d22 227 AxesRaw_TypeDef MAG_Value;
ahmad47 71:a6a052fd3d22 228 AxesRaw_TypeDef ACC_Value;
ahmad47 71:a6a052fd3d22 229 AxesRaw_TypeDef GYR_Value;
ahmad47 71:a6a052fd3d22 230 unsigned int ret = 0;
JackLenz 70:c6b61c5cadf4 231
ahmad47 71:a6a052fd3d22 232 /* Switch LED On */
ahmad47 71:a6a052fd3d22 233 myled = LED_ON;
ahmad47 71:a6a052fd3d22 234 //printf("===\n");
ahmad47 71:a6a052fd3d22 235
ahmad47 71:a6a052fd3d22 236 /* Determine Environmental Values */
ahmad47 71:a6a052fd3d22 237 ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
ahmad47 71:a6a052fd3d22 238 ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
ahmad47 71:a6a052fd3d22 239 ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
ahmad47 71:a6a052fd3d22 240
ahmad47 71:a6a052fd3d22 241 /* Print Values Out */
ahmad47 71:a6a052fd3d22 242 //printf("I2C [errors]: 0x%.2x X Y Z\n", ret);
ahmad47 71:a6a052fd3d22 243 /*
ahmad47 71:a6a052fd3d22 244 **
ahmad47 71:a6a052fd3d22 245 printf("%9ld:%9ld:%9ld:", ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
ahmad47 71:a6a052fd3d22 246 printf("%9ld:%9ld:%9ld:", GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
ahmad47 71:a6a052fd3d22 247 printf("%9ld:%9ld:%9ld\n", MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
ahmad47 71:a6a052fd3d22 248 **
ahmad47 71:a6a052fd3d22 249 */
ahmad47 71:a6a052fd3d22 250
ahmad47 71:a6a052fd3d22 251 /* IMU Data */
ahmad47 71:a6a052fd3d22 252 double accX, accY, accZ;
ahmad47 71:a6a052fd3d22 253 double gyroX, gyroY, gyroZ;
ahmad47 71:a6a052fd3d22 254 double magX, magY, magZ;
ahmad47 71:a6a052fd3d22 255
ahmad47 71:a6a052fd3d22 256 accX = ACC_Value.AXIS_X;
ahmad47 71:a6a052fd3d22 257 accY = ACC_Value.AXIS_Y;
ahmad47 71:a6a052fd3d22 258 accZ = ACC_Value.AXIS_Z;
ahmad47 71:a6a052fd3d22 259 gyroX = GYR_Value.AXIS_X;
ahmad47 71:a6a052fd3d22 260 gyroY = GYR_Value.AXIS_Y;
ahmad47 71:a6a052fd3d22 261 gyroZ = GYR_Value.AXIS_Z;
ahmad47 71:a6a052fd3d22 262
ahmad47 71:a6a052fd3d22 263 double dt = (double)(t.read_us() - timer) / 1000000; // Calculate delta time
ahmad47 71:a6a052fd3d22 264 timer = t.read_us();
ahmad47 71:a6a052fd3d22 265
ahmad47 71:a6a052fd3d22 266 #ifdef RESTRICT_PITCH // Eq. 25 and 26
ahmad47 71:a6a052fd3d22 267 double roll = atan2(accY, accZ) * RAD_TO_DEG;
ahmad47 71:a6a052fd3d22 268 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
ahmad47 71:a6a052fd3d22 269 #else // Eq. 28 and 29
ahmad47 71:a6a052fd3d22 270 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
ahmad47 71:a6a052fd3d22 271 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
ahmad47 71:a6a052fd3d22 272 #endif
ahmad47 71:a6a052fd3d22 273
ahmad47 71:a6a052fd3d22 274 double gyroXrate = gyroX / 131.0; // Convert to deg/s
ahmad47 71:a6a052fd3d22 275 double gyroYrate = gyroY / 131.0; // Convert to deg/s
ahmad47 71:a6a052fd3d22 276
ahmad47 71:a6a052fd3d22 277 #ifdef RESTRICT_PITCH
ahmad47 71:a6a052fd3d22 278 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
ahmad47 71:a6a052fd3d22 279 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
ahmad47 71:a6a052fd3d22 280 kalmanX.setAngle(roll);
ahmad47 71:a6a052fd3d22 281 compAngleX = roll;
ahmad47 71:a6a052fd3d22 282 kalAngleX = roll;
ahmad47 71:a6a052fd3d22 283 gyroXangle = roll;
ahmad47 71:a6a052fd3d22 284 }
ahmad47 71:a6a052fd3d22 285 else
ahmad47 71:a6a052fd3d22 286 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
ahmad47 71:a6a052fd3d22 287
ahmad47 71:a6a052fd3d22 288 if (abs(kalAngleX) > 90)
ahmad47 71:a6a052fd3d22 289 gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
ahmad47 71:a6a052fd3d22 290 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
ahmad47 71:a6a052fd3d22 291 #else
ahmad47 71:a6a052fd3d22 292 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
ahmad47 71:a6a052fd3d22 293 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
ahmad47 71:a6a052fd3d22 294 kalmanY.setAngle(pitch);
ahmad47 71:a6a052fd3d22 295 compAngleY = pitch;
ahmad47 71:a6a052fd3d22 296 kalAngleY = pitch;
ahmad47 71:a6a052fd3d22 297 gyroYangle = pitch;
ahmad47 71:a6a052fd3d22 298 }
ahmad47 71:a6a052fd3d22 299 else
ahmad47 71:a6a052fd3d22 300 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
ahmad47 71:a6a052fd3d22 301
ahmad47 71:a6a052fd3d22 302 if (abs(kalAngleY) > 90)
ahmad47 71:a6a052fd3d22 303 gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
ahmad47 71:a6a052fd3d22 304 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
ahmad47 71:a6a052fd3d22 305 #endif
ahmad47 71:a6a052fd3d22 306
ahmad47 71:a6a052fd3d22 307 gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
ahmad47 71:a6a052fd3d22 308 gyroYangle += gyroYrate * dt;
ahmad47 71:a6a052fd3d22 309 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
ahmad47 71:a6a052fd3d22 310 //gyroYangle += kalmanY.getRate() * dt;
ahmad47 71:a6a052fd3d22 311
ahmad47 71:a6a052fd3d22 312 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
ahmad47 71:a6a052fd3d22 313 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
ahmad47 71:a6a052fd3d22 314
ahmad47 71:a6a052fd3d22 315 // Reset the gyro angle when it has drifted too much
ahmad47 71:a6a052fd3d22 316 if (gyroXangle < -180 || gyroXangle > 180)
ahmad47 71:a6a052fd3d22 317 gyroXangle = kalAngleX;
ahmad47 71:a6a052fd3d22 318 if (gyroYangle < -180 || gyroYangle > 180)
ahmad47 71:a6a052fd3d22 319 gyroYangle = kalAngleY;
ahmad47 71:a6a052fd3d22 320
ahmad47 71:a6a052fd3d22 321 // Compute the Heading
ahmad47 71:a6a052fd3d22 322
ahmad47 71:a6a052fd3d22 323 magX = MAG_Value.AXIS_X;
ahmad47 71:a6a052fd3d22 324 magY = MAG_Value.AXIS_Y;
ahmad47 71:a6a052fd3d22 325 magZ = MAG_Value.AXIS_Z;
ahmad47 71:a6a052fd3d22 326
ahmad47 71:a6a052fd3d22 327 float heading;
ahmad47 71:a6a052fd3d22 328 if (magY== 0)
ahmad47 71:a6a052fd3d22 329 heading = (magX < 0) ? 180.0 : 0;
ahmad47 71:a6a052fd3d22 330 else
ahmad47 71:a6a052fd3d22 331 heading = atan2(magX , magY);
ahmad47 71:a6a052fd3d22 332 //arctan(imu.mx / sqrt(imu.mz*imu.mz + imu.my*imu.my)):
ahmad47 71:a6a052fd3d22 333 heading -= DECLINATION * PI / 180;
ahmad47 71:a6a052fd3d22 334
ahmad47 71:a6a052fd3d22 335 if (heading > PI)
ahmad47 71:a6a052fd3d22 336 heading -= (2 * PI);
ahmad47 71:a6a052fd3d22 337 else if (heading < -PI || heading < 0)
ahmad47 71:a6a052fd3d22 338 heading += (2 * PI);
ahmad47 71:a6a052fd3d22 339 heading *= 180.0 / PI;
ahmad47 71:a6a052fd3d22 340
ahmad47 71:a6a052fd3d22 341 printf("%lf:%1f:%1f\n", kalAngleY, kalAngleX, heading);
JackLenz 70:c6b61c5cadf4 342
Wolfgang Betz 1:9458657e49ee 343
Wolfgang Betz 1:9458657e49ee 344 /* Switch LED Off */
Wolfgang Betz 4:81037ace7f27 345 myled = LED_OFF;
Wolfgang Betz 0:3e4f610a0689 346 }
Wolfgang Betz 0:3e4f610a0689 347
Wolfgang Betz 0:3e4f610a0689 348
Wolfgang Betz 0:3e4f610a0689 349 /*** Main function ------------------------------------------------------------- ***/
Wolfgang Betz 51:5ce8202680b4 350 /* Generic main function/loop for enabling WFE in case of
Wolfgang Betz 0:3e4f610a0689 351 interrupt based cyclic execution
Wolfgang Betz 0:3e4f610a0689 352 */
Wolfgang Betz 0:3e4f610a0689 353 int main()
Wolfgang Betz 0:3e4f610a0689 354 {
Wolfgang Betz 0:3e4f610a0689 355 /* Start & initialize */
ahmad47 71:a6a052fd3d22 356 //printf("\n--- Starting new run ---\n");
Wolfgang Betz 0:3e4f610a0689 357 init();
Wolfgang Betz 0:3e4f610a0689 358
Wolfgang Betz 0:3e4f610a0689 359 /* Start timer irq */
Wolfgang Betz 0:3e4f610a0689 360 ticker.attach_us(timer_irq, MS_INTERVALS * APP_LOOP_PERIOD);
Wolfgang Betz 0:3e4f610a0689 361
Wolfgang Betz 47:6a63161486cf 362 while (true) {
Wolfgang Betz 47:6a63161486cf 363 if(timer_irq_triggered) {
Wolfgang Betz 47:6a63161486cf 364 timer_irq_triggered = false;
Wolfgang Betz 47:6a63161486cf 365 main_cycle();
Wolfgang Betz 47:6a63161486cf 366 } else if(ff_irq_triggered) {
Wolfgang Betz 47:6a63161486cf 367 ff_irq_triggered = false;
Wolfgang Betz 47:6a63161486cf 368 handle_ff_irq();
Wolfgang Betz 47:6a63161486cf 369 } else {
Wolfgang Betz 49:0223aee4b17a 370 __WFE(); /* it is recommended that SEVONPEND in the
Wolfgang Betz 49:0223aee4b17a 371 System Control Register is NOT set */
Wolfgang Betz 47:6a63161486cf 372 }
Wolfgang Betz 47:6a63161486cf 373 }
ahmad47 71:a6a052fd3d22 374 t.stop();
Wolfgang Betz 0:3e4f610a0689 375 }