Elecia White / Mbed 2 deprecated freedom_inertial_hello_world

A hello world for inertial sensors

Dependencies:   FXAS21000 FXOS8700Q mbed

Fork of FRDM-STBC-AGM01 by angus taggart

main.cpp@4:5ab2bb2f062b, 2015-06-30 (annotated)

Committer:
Elecia
Date:
Tue Jun 30 16:23:32 2015 +0000
Revision:
4:5ab2bb2f062b
Parent:
3:123b546e4a5c 
Added element14 link

Who changed what in which revision?

UserRevisionLine numberNew contents of line
screamer 0:bfb567985c64 1 /* Copyright (c) 2010-2011 mbed.org, MIT License
screamer 0:bfb567985c64 2 *
screamer 0:bfb567985c64 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
screamer 0:bfb567985c64 4 * and associated documentation files (the "Software"), to deal in the Software without
screamer 0:bfb567985c64 5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
screamer 0:bfb567985c64 6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
screamer 0:bfb567985c64 7 * Software is furnished to do so, subject to the following conditions:
screamer 0:bfb567985c64 8 *
screamer 0:bfb567985c64 9 * The above copyright notice and this permission notice shall be included in all copies or
screamer 0:bfb567985c64 10 * substantial portions of the Software.
screamer 0:bfb567985c64 11 *
screamer 0:bfb567985c64 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
screamer 0:bfb567985c64 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
screamer 0:bfb567985c64 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
screamer 0:bfb567985c64 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
screamer 0:bfb567985c64 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
screamer 0:bfb567985c64 17 */
screamer 0:bfb567985c64 18
Elecia 3:123b546e4a5c 19 /* This project was created by Elecia White of Embedded.fm and Logical Elegance using hardware
Elecia 3:123b546e4a5c 20 * supplied by element14. It uses
Elecia 3:123b546e4a5c 21 * * a FRDM-KL05Z board (http://www.newark.com/webapp/wcs/stores/servlet/ProductDisplay?catalogId=15003&langId=-1&urlRequestType=Base&partNumber=53W2646&storeId=10194)
Elecia 3:123b546e4a5c 22 * * an IMU sensor toolbox shield (http://www.newark.com/webapp/wcs/stores/servlet/ProductDisplay?catalogId=15003&langId=-1&urlRequestType=Base&partNumber=30Y6365&storeId=10194)
Elecia 3:123b546e4a5c 23 * * a momentary button connected to A5 and ground
Elecia 3:123b546e4a5c 24 * Together, these create a sort of "hello world" for the accelerometer, gyro, and magnetometer so
Elecia 3:123b546e4a5c 25 * people can see what each sensor does.
Elecia 3:123b546e4a5c 26 *
Elecia 3:123b546e4a5c 27 * On boot, the system will have no LED running though it will be outputting IMU
Elecia 3:123b546e4a5c 28 * data on the serial port available from USB (115200, 8, None, 1).
Elecia 3:123b546e4a5c 29 *
Elecia 3:123b546e4a5c 30 * Press the button to go into accelerometer demo mode. The LED will come on and
Elecia 3:123b546e4a5c 31 * tell you upon which axis it feels gravity (red = X, green = Y, blue = Z).
Elecia 3:123b546e4a5c 32 * As you move it, the LED will change. If you jiggle it, you can see the light
Elecia 3:123b546e4a5c 33 * change in intensity. A gentle toss up might lead to a dark sensor at the top
Elecia 3:123b546e4a5c 34 * of the arc (free fall!). And if you tap on the device, you will likely see a
Elecia 3:123b546e4a5c 35 * brighter flare of the LED: accelerometers do really well with tap detection.
Elecia 3:123b546e4a5c 36 *
Elecia 3:123b546e4a5c 37 * Press the button again to get into gyro demo mode. The LED will go off until
Elecia 3:123b546e4a5c 38 * you turn the unit. Spinning it around one axis is the obvious mechanism. But
Elecia 3:123b546e4a5c 39 * try swinging it back and forth in different direction. Look at the joints on
Elecia 3:123b546e4a5c 40 * your arm, what colors do you see when you move (and how consistently)?
Elecia 3:123b546e4a5c 41 *
Elecia 3:123b546e4a5c 42 * Press the button again to go into the North Star mode. Remember, it might light
Elecia 3:123b546e4a5c 43 * your way home, but only if you and it are pointed in the same direction.
Elecia 3:123b546e4a5c 44 *
Elecia 4:5ab2bb2f062b 45 * For more information, see the element14 post: http://www.element14.com/community/community/designcenter/kinetis_kl2_freedom_board/blog/2015/06/30/brave-new-hello-world
Elecia 3:123b546e4a5c 46 *
Elecia 3:123b546e4a5c 47 */
screamer 0:bfb567985c64 48 #include "mbed.h"
screamer 0:bfb567985c64 49 #include "FXOS8700Q.h"
screamer 0:bfb567985c64 50 #include "FXAS21000.h"
Elecia 3:123b546e4a5c 51 #include "led.h"
Elecia 3:123b546e4a5c 52 #include "Adafruit_9DOF.h"
Elecia 3:123b546e4a5c 53 #include "northStarFuncs.h"
Elecia 3:123b546e4a5c 54 #include "button.h"
screamer 0:bfb567985c64 55
Elecia 3:123b546e4a5c 56 #define ACCEL_FLOAT_TO_MG_INT (1000.0F) // G to mG
Elecia 3:123b546e4a5c 57 #define PI_F 3.14159265
Elecia 3:123b546e4a5c 58 #define DEGREES_TO_MRADS (1000.0*(2.0*PI_F/360.0))
screamer 0:bfb567985c64 59
screamer 0:bfb567985c64 60 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 61 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 62 FXAS21000 gyro(D14, D15);
screamer 0:bfb567985c64 63
Elecia 3:123b546e4a5c 64 Serial pc(USBTX, USBRX);
Elecia 3:123b546e4a5c 65 SMART_LED smartLed(LED1, LED2, LED3);
Elecia 3:123b546e4a5c 66 DigitalIn gButton(A5, PullUp);
Elecia 3:123b546e4a5c 67
Elecia 3:123b546e4a5c 68 typedef struct {
Elecia 3:123b546e4a5c 69 int16_t a[NUM_AXIS]; // 3D accelerometer data in 0.001 G (milliGs)
Elecia 3:123b546e4a5c 70 int16_t m[NUM_AXIS]; // 3D magnetometer data in 0.001 gauss (milliGauss)
Elecia 3:123b546e4a5c 71 int16_t g[NUM_AXIS]; // 3D angular rate darta in degrees per second
Elecia 3:123b546e4a5c 72 } tImuData;
Elecia 3:123b546e4a5c 73
Elecia 3:123b546e4a5c 74
Elecia 3:123b546e4a5c 75 // UpdateLeds uses the inertial data to set the LED (according to the current mode).
Elecia 3:123b546e4a5c 76 // It alsu updates the current mode when the button is pressed
Elecia 3:123b546e4a5c 77 int UpdateLed(tImuData *imuData)
Elecia 3:123b546e4a5c 78 {
Elecia 3:123b546e4a5c 79 static eModes gMode = OFF;
Elecia 3:123b546e4a5c 80 uint8_t r = 0, b = 0, g = 0;
Elecia 3:123b546e4a5c 81 int delayMs = 10;
screamer 0:bfb567985c64 82
Elecia 3:123b546e4a5c 83 switch (gMode) {
Elecia 3:123b546e4a5c 84 case ACCEL:
Elecia 3:123b546e4a5c 85 // the color brightness is the abs value of the direction,
Elecia 3:123b546e4a5c 86 // scaled to one G
Elecia 3:123b546e4a5c 87 r = conditionAccels(imuData->a[X]);
Elecia 3:123b546e4a5c 88 g = conditionAccels(imuData->a[Y]);
Elecia 3:123b546e4a5c 89 b = conditionAccels(imuData->a[Z]);
Elecia 3:123b546e4a5c 90 delayMs = 10; // 100 Hz readings on the accel
Elecia 3:123b546e4a5c 91 break;
Elecia 3:123b546e4a5c 92 case MAG:
Elecia 3:123b546e4a5c 93 tOrientation orientation;
Elecia 3:123b546e4a5c 94 if (fusionGetOrientation(imuData->a, imuData->m, &orientation)) {
Elecia 3:123b546e4a5c 95 // N = white, E = green, W = red, S = dark
Elecia 3:123b546e4a5c 96 r = conditionMags(orientation.heading, -45.0, 120.0);
Elecia 3:123b546e4a5c 97 g = conditionMags(orientation.heading, 45.0, 120.0);
Elecia 3:123b546e4a5c 98 b = conditionMags(orientation.heading, 0.0, 120.0);
Elecia 3:123b546e4a5c 99 } else{
Elecia 3:123b546e4a5c 100 printf("orientation failed\r\n");
Elecia 3:123b546e4a5c 101 }
Elecia 3:123b546e4a5c 102 delayMs = 20; // 50 Hz readings on the mag
Elecia 3:123b546e4a5c 103 break;
Elecia 3:123b546e4a5c 104 case GYRO:
Elecia 3:123b546e4a5c 105 // scale the brightness to the motion being
Elecia 3:123b546e4a5c 106 // experienced, where X = R, Y = B, Z = G
Elecia 3:123b546e4a5c 107 r = conditionGyros(imuData->g[X]);
Elecia 3:123b546e4a5c 108 g = conditionGyros(imuData->g[Y]);
Elecia 3:123b546e4a5c 109 b = conditionGyros(imuData->g[Z]);
Elecia 3:123b546e4a5c 110 delayMs = 10;
Elecia 3:123b546e4a5c 111 break;
Elecia 3:123b546e4a5c 112 default:
Elecia 3:123b546e4a5c 113 r = g = b = 0;
Elecia 3:123b546e4a5c 114 delayMs = 10;
Elecia 3:123b546e4a5c 115 break;
Elecia 3:123b546e4a5c 116 }
Elecia 3:123b546e4a5c 117 if (debounceButtonPress(gButton)) {
Elecia 3:123b546e4a5c 118 printf("Button pressed from %d", gMode);
Elecia 3:123b546e4a5c 119 gMode = changeMode(gMode);
Elecia 3:123b546e4a5c 120 printf(" to new mode %d\r\n", gMode);
Elecia 3:123b546e4a5c 121 }
Elecia 3:123b546e4a5c 122 smartLed.set(r, g, b);
Elecia 3:123b546e4a5c 123 return (delayMs);
Elecia 3:123b546e4a5c 124 }
screamer 0:bfb567985c64 125
screamer 0:bfb567985c64 126 int main()
Elecia 3:123b546e4a5c 127 {
ajtag 2:0cccf85f9b3f 128 pc.baud(115200);
ajtag 2:0cccf85f9b3f 129
ajtag 2:0cccf85f9b3f 130 float gyro_data[3];
screamer 0:bfb567985c64 131 MotionSensorDataUnits adata;
screamer 0:bfb567985c64 132 MotionSensorDataUnits mdata;
screamer 0:bfb567985c64 133 printf("\r\nStarting\r\n\r\n");
screamer 0:bfb567985c64 134
screamer 0:bfb567985c64 135 combo_acc.enable();
screamer 0:bfb567985c64 136 combo_mag.enable();
ajtag 2:0cccf85f9b3f 137 printf("FXOS8700 Combo mag = %X\r\n", combo_mag.whoAmI());
ajtag 2:0cccf85f9b3f 138 printf("FXOS8700 Combo acc = %X\r\n", combo_acc.whoAmI());
screamer 0:bfb567985c64 139 printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
Elecia 3:123b546e4a5c 140
screamer 0:bfb567985c64 141 while(1) {
Elecia 3:123b546e4a5c 142 tImuData imuData;
screamer 0:bfb567985c64 143 combo_acc.getAxis(adata);
Elecia 3:123b546e4a5c 144 imuData.a[X] = adata.x * ACCEL_FLOAT_TO_MG_INT;
Elecia 3:123b546e4a5c 145 imuData.a[Y] = adata.y * ACCEL_FLOAT_TO_MG_INT;
Elecia 3:123b546e4a5c 146 imuData.a[Z] = adata.z * ACCEL_FLOAT_TO_MG_INT;
screamer 0:bfb567985c64 147 printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
screamer 0:bfb567985c64 148
Elecia 3:123b546e4a5c 149 combo_mag.getAxis(mdata); // data in mGauss already
Elecia 3:123b546e4a5c 150 imuData.m[X] = mdata.x;
Elecia 3:123b546e4a5c 151 imuData.m[Y] = mdata.y;
Elecia 3:123b546e4a5c 152 imuData.m[Z] = mdata.z;
screamer 0:bfb567985c64 153 printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
Elecia 3:123b546e4a5c 154
screamer 0:bfb567985c64 155 gyro.ReadXYZ(gyro_data);
Elecia 3:123b546e4a5c 156 imuData.g[X] = gyro_data[0] * DEGREES_TO_MRADS;
Elecia 3:123b546e4a5c 157 imuData.g[Y] = gyro_data[1] * DEGREES_TO_MRADS;
Elecia 3:123b546e4a5c 158 imuData.g[Z] = gyro_data[2] * DEGREES_TO_MRADS;
Elecia 3:123b546e4a5c 159 printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
screamer 0:bfb567985c64 160 printf("\r\n");
screamer 0:bfb567985c64 161
Elecia 3:123b546e4a5c 162 int delayMs = UpdateLed(&imuData);
Elecia 3:123b546e4a5c 163 wait_ms(delayMs);
screamer 0:bfb567985c64 164 }
screamer 0:bfb567985c64 165 }