Julio Fajardo / Mbed 2 deprecated Freescale_Multi-Sensor_Quadcopter

Freescale Quadcopter with Freedom K64F Board

Dependencies:   FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp

Fork of Freescale_Multi-Sensor_Shield by Shields

Quadcopter based on Freescale FRDM-K64F, Freescale FRDM-FXS-9AXIS, Hobbypower X525 V3 Quadcopter Foldable Kit with 1000KV Motors and SimonK 30A ESC /media/uploads/julioefajardo/1.jpg

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

Committer:
screamer
Date:
Fri Jul 25 12:16:13 2014 +0000
Revision:
0:bfb567985c64
Child:
2:4bc4e25328cc
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 }