Rio Harris / Mbed 2 deprecated NerfGun_nRF24L01P_TX_9d0f

This code includes FRDM-STBC-AGM01 in order to add 9dof Sensor Fusion to Nerf Gun Demo. 9dof = accelerometer, magnetometer and gyro.

Dependencies:   DebounceIn FXAS21000 FXOS8700Q Madgwick_Algorithm Mahony_Algorithm mbed nRF24L01P

Fork of FRDM-STBC-AGM01 by angus taggart

The goal of this project is to incorporate sensor fusion capability to the already existing Nerf Gun Demo. The demo is controlled using Freescale's FRDM-K64F. I'm looking to add the FRDM-STBC-AGM01 to the project, which not only has FSL's combo accelerometer and magnetometer part FXOS8700Q but also FSL's gyro part FXAS21000. Hopefully the combination of all three sensors for 9dof sensor fusion will improve the mobility and control of the the demo's movement.

Details of the project can be found on the Freescale online community at https://community.freescale.com/groups/demolab/projects/fun-with-sensorfusion-nerf-gun-and-openrov-projects.

main.cpp@0:bfb567985c64, 2014-07-25 (annotated)

Committer:
screamer
Date:
Fri Jul 25 12:16:13 2014 +0000
Revision:
0:bfb567985c64
Child:
2:0cccf85f9b3f
Initial revision

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
screamer 0:bfb567985c64 19 #include "mbed.h"
screamer 0:bfb567985c64 20 #include "MAG3110.h"
screamer 0:bfb567985c64 21 #include "FXOS8700Q.h"
screamer 0:bfb567985c64 22 #include "MMA8652.h"
screamer 0:bfb567985c64 23 #include "FXLS8471Q.h"
screamer 0:bfb567985c64 24 #include "FXAS21000.h"
screamer 0:bfb567985c64 25
screamer 0:bfb567985c64 26 // Requires a Freescale-compatible platform. Comment if you're getting weird results for the alt or any other sensor. Also check the interrupt params below.
screamer 0:bfb567985c64 27 #define FSL_COMPATIBLE
screamer 0:bfb567985c64 28 #if defined(FSL_COMPATIBLE)
screamer 0:bfb567985c64 29 #define MPL3115A2_I2C_ADDRESS (0x60<<1)
screamer 0:bfb567985c64 30 #include "MPL3115A2.h"
screamer 0:bfb567985c64 31
screamer 0:bfb567985c64 32 MPL3115A2 alt(D14, D15, MPL3115A2_I2C_ADDRESS, D4, D3);
screamer 0:bfb567985c64 33 #endif
screamer 0:bfb567985c64 34
screamer 0:bfb567985c64 35 FXLS8471Q acc1(D11, D12, D13, D10);
screamer 0:bfb567985c64 36 MMA8652 acc2(D14, D15);
screamer 0:bfb567985c64 37 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 38 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
screamer 0:bfb567985c64 39 MAG3110 mag2(D14, D15);
screamer 0:bfb567985c64 40 FXAS21000 gyro(D14, D15);
screamer 0:bfb567985c64 41
screamer 0:bfb567985c64 42
screamer 0:bfb567985c64 43 Serial pc(USBTX, USBRX);
screamer 0:bfb567985c64 44
screamer 0:bfb567985c64 45 float print_AltimiterValue( unsigned char *dt)
screamer 0:bfb567985c64 46 {
screamer 0:bfb567985c64 47 unsigned short altm;
screamer 0:bfb567985c64 48 float faltm;
screamer 0:bfb567985c64 49
screamer 0:bfb567985c64 50 /*
screamer 0:bfb567985c64 51 * dt[0] = Bits 12-19 of 20-bit real-time Altitude sample. (b7-b0)
screamer 0:bfb567985c64 52 * dt[1] = Bits 4-11 of 20-bit real-time Altitude sample. (b7-b0)
screamer 0:bfb567985c64 53 * dt[2] = Bits 0-3 of 20-bit real-time Altitude sample (b7-b4)
screamer 0:bfb567985c64 54 */
screamer 0:bfb567985c64 55 altm = (dt[0]<<8) | dt[1];
screamer 0:bfb567985c64 56 //
screamer 0:bfb567985c64 57 if ( dt[0] > 0x7F) {
screamer 0:bfb567985c64 58 altm = ~altm + 1;
screamer 0:bfb567985c64 59 faltm = (float)altm * -1.0f;
screamer 0:bfb567985c64 60 } else {
screamer 0:bfb567985c64 61 faltm = (float)altm * 1.0f;
screamer 0:bfb567985c64 62 }
screamer 0:bfb567985c64 63 //
screamer 0:bfb567985c64 64 faltm = faltm+((float)(dt[2]>>4) * 0.0625f);
screamer 0:bfb567985c64 65 return faltm;
screamer 0:bfb567985c64 66 }
screamer 0:bfb567985c64 67
screamer 0:bfb567985c64 68 float print_TemperatureValue( unsigned char *dt)
screamer 0:bfb567985c64 69 {
screamer 0:bfb567985c64 70 unsigned short temp;
screamer 0:bfb567985c64 71 float ftemp;
screamer 0:bfb567985c64 72
screamer 0:bfb567985c64 73 /*
screamer 0:bfb567985c64 74 * dt[0] = Bits 4-11 of 16-bit real-time temperature sample. (b7-b0)
screamer 0:bfb567985c64 75 * dt[1] = Bits 0-3 of 16-bit real-time temperature sample. (b7-b4)
screamer 0:bfb567985c64 76 */
screamer 0:bfb567985c64 77 temp = dt[0];
screamer 0:bfb567985c64 78 //
screamer 0:bfb567985c64 79 if ( dt[0] > 0x7F) {
screamer 0:bfb567985c64 80 temp = ~temp + 1;
screamer 0:bfb567985c64 81 ftemp = (float)temp * -1.0f;
screamer 0:bfb567985c64 82 } else {
screamer 0:bfb567985c64 83 ftemp = (float)temp * 1.0f;
screamer 0:bfb567985c64 84 }
screamer 0:bfb567985c64 85 //
screamer 0:bfb567985c64 86 ftemp = ftemp+((float)(dt[1]>>4) * 0.0625f);
screamer 0:bfb567985c64 87 return ftemp;
screamer 0:bfb567985c64 88
screamer 0:bfb567985c64 89 }
screamer 0:bfb567985c64 90
screamer 0:bfb567985c64 91
screamer 0:bfb567985c64 92 int main()
screamer 0:bfb567985c64 93 {
screamer 0:bfb567985c64 94 float acc_data[3], gyro_data[3];
screamer 0:bfb567985c64 95 MotionSensorDataUnits adata;
screamer 0:bfb567985c64 96 MotionSensorDataUnits mdata;
screamer 0:bfb567985c64 97 int16_t acc_raw[3];
screamer 0:bfb567985c64 98
screamer 0:bfb567985c64 99 printf("\r\nStarting\r\n\r\n");
screamer 0:bfb567985c64 100
screamer 0:bfb567985c64 101 combo_acc.enable();
screamer 0:bfb567985c64 102 combo_mag.enable();
screamer 0:bfb567985c64 103 mag2.enable();
screamer 0:bfb567985c64 104 printf("FXLS8471 Acc = %X\r\n", acc1.getWhoAmI());
screamer 0:bfb567985c64 105 printf("MMA8652 Acc = %X\r\n", acc2.getWhoAmI());
screamer 0:bfb567985c64 106 printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
screamer 0:bfb567985c64 107 printf("MAG3110 Mag = %X\r\n", mag2.whoAmI());
screamer 0:bfb567985c64 108 printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
screamer 0:bfb567985c64 109
screamer 0:bfb567985c64 110 #if defined(FSL_COMPATIBLE)
screamer 0:bfb567985c64 111 unsigned char alt_data[8];
screamer 0:bfb567985c64 112 alt.Altimeter_Mode();
screamer 0:bfb567985c64 113 printf("MPL3115A2 Alt = %X\r\n", alt.getDeviceID()); // May only be used on a Freescale platform, comment out for others
screamer 0:bfb567985c64 114 #endif
screamer 0:bfb567985c64 115
screamer 0:bfb567985c64 116 wait(3);
screamer 0:bfb567985c64 117
screamer 0:bfb567985c64 118 while(1) {
screamer 0:bfb567985c64 119 acc1.ReadXYZ(acc_data);
screamer 0:bfb567985c64 120 acc1.ReadXYZraw(acc_raw);
screamer 0:bfb567985c64 121 printf("FXLS8471 Acc: X:%6.3f Y:%6.3f Z:%6.3f (Raw X:%4d Y:%4d Z:%4d)\r\n", acc_data[0], acc_data[1], acc_data[2], acc_raw[0], acc_raw[1], acc_raw[2]);
screamer 0:bfb567985c64 122
screamer 0:bfb567985c64 123 acc2.ReadXYZ(acc_data);
screamer 0:bfb567985c64 124 acc2.ReadXYZraw(acc_raw);
screamer 0:bfb567985c64 125 printf("MMA8652 Acc: X:%6.3f Y:%6.3f Z:%6.3f (Raw X:%4d Y:%4d Z:%4d)\r\n", acc_data[0], acc_data[1], acc_data[2], acc_raw[0], acc_raw[1], acc_raw[2]);
screamer 0:bfb567985c64 126
screamer 0:bfb567985c64 127 combo_acc.getAxis(adata);
screamer 0:bfb567985c64 128 printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
screamer 0:bfb567985c64 129
screamer 0:bfb567985c64 130 combo_mag.getAxis(mdata);
screamer 0:bfb567985c64 131 printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
screamer 0:bfb567985c64 132
screamer 0:bfb567985c64 133 mag2.getAxis(mdata);
screamer 0:bfb567985c64 134 printf("MAG3110 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
screamer 0:bfb567985c64 135
screamer 0:bfb567985c64 136 gyro.ReadXYZ(gyro_data);
screamer 0:bfb567985c64 137 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 138
screamer 0:bfb567985c64 139 #if defined(FSL_COMPATIBLE)
screamer 0:bfb567985c64 140 alt.getAllDataRaw(&alt_data[0]); // May only be used on a Freescale platform, comment out for others
screamer 0:bfb567985c64 141 printf("MPL3115A2 Alt: %5.1f\r\n", print_AltimiterValue(&alt_data[0])); // May only be used on a Freescale platform, comment out for others
screamer 0:bfb567985c64 142 printf("MPL3115A2 Temp: %3.1f\r\n", print_TemperatureValue(&alt_data[3])); // May only be used on a Freescale platform, comment out for others
screamer 0:bfb567985c64 143 #endif
screamer 0:bfb567985c64 144
screamer 0:bfb567985c64 145 printf("\r\n");
screamer 0:bfb567985c64 146
screamer 0:bfb567985c64 147 wait(1);
screamer 0:bfb567985c64 148 }
screamer 0:bfb567985c64 149 }