Test whole program
Dependencies: X_NUCLEO_IKS01A1 mbed
Fork of Sensors_Reader_JACKLENZ by
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?
User | Revision | Line number | New 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>© 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 | } |