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
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@4:651a5029e003, 2015-08-14 (annotated)
- Committer:
- igalloway
- Date:
- Fri Aug 14 15:15:14 2015 +0000
- Revision:
- 4:651a5029e003
- Parent:
- 3:10f487bae65c
added a comment in main.; Note the FXAS2100 library may have switched back to jim Carvers - may need to get Rio's again.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igalloway | 4:651a5029e003 | 1 | //This is an intern project to port 9DOF sensor fusion to the Nerf gun. |
igalloway | 4:651a5029e003 | 2 | |
screamer | 0:bfb567985c64 | 3 | #include "mbed.h" |
screamer | 0:bfb567985c64 | 4 | #include "FXOS8700Q.h" |
screamer | 0:bfb567985c64 | 5 | #include "FXAS21000.h" |
b50559 | 3:10f487bae65c | 6 | #include "MadgwickAHRS.h" |
b50559 | 3:10f487bae65c | 7 | #include "MahonyAHRS.h" |
b50559 | 3:10f487bae65c | 8 | #include "nRF24L01P.h" |
b50559 | 3:10f487bae65c | 9 | #include "DebounceIn.h" |
screamer | 0:bfb567985c64 | 10 | |
b50559 | 3:10f487bae65c | 11 | #include <iostream> |
b50559 | 3:10f487bae65c | 12 | using namespace std; |
b50559 | 3:10f487bae65c | 13 | |
b50559 | 3:10f487bae65c | 14 | #define TRANSFER_SIZE 9 |
screamer | 0:bfb567985c64 | 15 | |
screamer | 0:bfb567985c64 | 16 | FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0); |
screamer | 0:bfb567985c64 | 17 | FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0); |
screamer | 0:bfb567985c64 | 18 | FXAS21000 gyro(D14, D15); |
b50559 | 3:10f487bae65c | 19 | nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18); // mosi, miso, sck, csn, ce, irq |
screamer | 0:bfb567985c64 | 20 | |
b50559 | 3:10f487bae65c | 21 | DigitalOut myled1(LED1); |
b50559 | 3:10f487bae65c | 22 | DigitalOut myled2(LED2); |
b50559 | 3:10f487bae65c | 23 | DigitalOut myled3(LED3); |
screamer | 0:bfb567985c64 | 24 | |
screamer | 0:bfb567985c64 | 25 | Serial pc(USBTX, USBRX); |
screamer | 0:bfb567985c64 | 26 | |
b50559 | 3:10f487bae65c | 27 | DebounceIn fire_button(PTA4); |
b50559 | 3:10f487bae65c | 28 | DebounceIn cal_button(PTC6); |
b50559 | 3:10f487bae65c | 29 | |
screamer | 0:bfb567985c64 | 30 | int main() |
screamer | 0:bfb567985c64 | 31 | { |
b50559 | 3:10f487bae65c | 32 | |
b50559 | 3:10f487bae65c | 33 | pc.baud(115200); |
b50559 | 3:10f487bae65c | 34 | |
b50559 | 3:10f487bae65c | 35 | MahonyAHRS ahrs(50.0f); |
ajtag | 2:0cccf85f9b3f | 36 | |
b50559 | 3:10f487bae65c | 37 | int16_t roll; |
b50559 | 3:10f487bae65c | 38 | int16_t pitch; |
b50559 | 3:10f487bae65c | 39 | int16_t yaw; |
b50559 | 3:10f487bae65c | 40 | int16_t avg_roll = 0; |
b50559 | 3:10f487bae65c | 41 | int16_t avg_pitch = 0; |
b50559 | 3:10f487bae65c | 42 | int16_t avg_yaw = 0; |
b50559 | 3:10f487bae65c | 43 | int16_t off_yaw = 0; |
b50559 | 3:10f487bae65c | 44 | |
ajtag | 2:0cccf85f9b3f | 45 | float gyro_data[3]; |
screamer | 0:bfb567985c64 | 46 | MotionSensorDataUnits adata; |
screamer | 0:bfb567985c64 | 47 | MotionSensorDataUnits mdata; |
screamer | 0:bfb567985c64 | 48 | |
b50559 | 3:10f487bae65c | 49 | float acc[3]; |
b50559 | 3:10f487bae65c | 50 | float mag[3]; |
b50559 | 3:10f487bae65c | 51 | float gyr[3]; |
b50559 | 3:10f487bae65c | 52 | float avg_acc[3]; |
b50559 | 3:10f487bae65c | 53 | float avg_mag[3]; |
b50559 | 3:10f487bae65c | 54 | float avg_gyr[3]; |
b50559 | 3:10f487bae65c | 55 | |
b50559 | 3:10f487bae65c | 56 | char txData[TRANSFER_SIZE]; |
b50559 | 3:10f487bae65c | 57 | |
b50559 | 3:10f487bae65c | 58 | my_nrf24l01p.setTxAddress(0xDEADBEEF0F); |
b50559 | 3:10f487bae65c | 59 | my_nrf24l01p.powerUp(); |
b50559 | 3:10f487bae65c | 60 | myled1 = 1; |
b50559 | 3:10f487bae65c | 61 | myled2 = 1; |
b50559 | 3:10f487bae65c | 62 | |
b50559 | 3:10f487bae65c | 63 | // Display the setup of the nRF24L01+ chip |
b50559 | 3:10f487bae65c | 64 | |
b50559 | 3:10f487bae65c | 65 | pc.printf( "nRF24L01+ Frequency : %d MHz\r\n", my_nrf24l01p.getRfFrequency() ); |
b50559 | 3:10f487bae65c | 66 | pc.printf( "nRF24L01+ Output power : %d dBm\r\n", my_nrf24l01p.getRfOutputPower() ); |
b50559 | 3:10f487bae65c | 67 | pc.printf( "nRF24L01+ Data Rate : %d kbps\r\n", my_nrf24l01p.getAirDataRate() ); |
b50559 | 3:10f487bae65c | 68 | pc.printf( "nRF24L01+ TX Address : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() ); |
b50559 | 3:10f487bae65c | 69 | pc.printf( "nRF24L01+ RX Address : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() ); |
b50559 | 3:10f487bae65c | 70 | |
screamer | 0:bfb567985c64 | 71 | |
b50559 | 3:10f487bae65c | 72 | my_nrf24l01p.setTransferSize( TRANSFER_SIZE ); |
b50559 | 3:10f487bae65c | 73 | my_nrf24l01p.enable(); |
b50559 | 3:10f487bae65c | 74 | |
b50559 | 3:10f487bae65c | 75 | //============================================================================================= |
screamer | 0:bfb567985c64 | 76 | combo_acc.enable(); |
screamer | 0:bfb567985c64 | 77 | combo_mag.enable(); |
b50559 | 3:10f487bae65c | 78 | |
b50559 | 3:10f487bae65c | 79 | //Calibration |
b50559 | 3:10f487bae65c | 80 | printf("Please Set Device Right Side Up on a Flat Surface\r\n"); |
b50559 | 3:10f487bae65c | 81 | printf("Calibrating...\r\n"); |
b50559 | 3:10f487bae65c | 82 | |
b50559 | 3:10f487bae65c | 83 | int count = 0; |
b50559 | 3:10f487bae65c | 84 | int samples = 100; |
b50559 | 3:10f487bae65c | 85 | int i; |
b50559 | 3:10f487bae65c | 86 | |
b50559 | 3:10f487bae65c | 87 | float cal_adatax = 0; |
b50559 | 3:10f487bae65c | 88 | float cal_adatay = 0; |
b50559 | 3:10f487bae65c | 89 | float cal_adataz = 0; |
b50559 | 3:10f487bae65c | 90 | |
b50559 | 3:10f487bae65c | 91 | float cal_gdatax = 0; |
b50559 | 3:10f487bae65c | 92 | float cal_gdatay = 0; |
b50559 | 3:10f487bae65c | 93 | float cal_gdataz = 0; |
b50559 | 3:10f487bae65c | 94 | |
b50559 | 3:10f487bae65c | 95 | //Transformation matrix obtained via MagMaster 1.0 |
b50559 | 3:10f487bae65c | 96 | float cal_mdatax[3] = {1.012, 0.03, -0.024}; |
b50559 | 3:10f487bae65c | 97 | float cal_mdatay[3] = {-0.021, 0.997, -0.044}; |
b50559 | 3:10f487bae65c | 98 | float cal_mdataz[3] = {0.008, -0.012, 0.993}; |
b50559 | 3:10f487bae65c | 99 | float bias[3] = {0.008,0.018,0.003}; |
ajtag | 2:0cccf85f9b3f | 100 | |
b50559 | 3:10f487bae65c | 101 | while(count < samples) { |
b50559 | 3:10f487bae65c | 102 | combo_acc.getAxis(adata); |
b50559 | 3:10f487bae65c | 103 | gyro.ReadXYZ(gyro_data); |
b50559 | 3:10f487bae65c | 104 | |
b50559 | 3:10f487bae65c | 105 | cal_adatax += adata.x; |
b50559 | 3:10f487bae65c | 106 | cal_adatay += adata.y; |
b50559 | 3:10f487bae65c | 107 | cal_adataz += adata.z; |
b50559 | 3:10f487bae65c | 108 | cal_gdatax += gyro_data[0]; |
b50559 | 3:10f487bae65c | 109 | cal_gdatay += gyro_data[1]; |
b50559 | 3:10f487bae65c | 110 | cal_gdataz += gyro_data[2]; |
b50559 | 3:10f487bae65c | 111 | |
b50559 | 3:10f487bae65c | 112 | count++; |
b50559 | 3:10f487bae65c | 113 | wait(.1); |
b50559 | 3:10f487bae65c | 114 | } |
b50559 | 3:10f487bae65c | 115 | |
b50559 | 3:10f487bae65c | 116 | cal_adatax = -cal_adatax / samples; |
b50559 | 3:10f487bae65c | 117 | cal_adatay = -cal_adatay / samples; |
b50559 | 3:10f487bae65c | 118 | cal_adataz = -cal_adataz / samples + 1; |
b50559 | 3:10f487bae65c | 119 | cal_gdatax = -cal_gdatax / samples; |
b50559 | 3:10f487bae65c | 120 | cal_gdatay = -cal_gdatay / samples; |
b50559 | 3:10f487bae65c | 121 | cal_gdataz = -cal_gdataz / samples; |
b50559 | 3:10f487bae65c | 122 | |
b50559 | 3:10f487bae65c | 123 | printf("\r\n"); |
screamer | 0:bfb567985c64 | 124 | |
b50559 | 3:10f487bae65c | 125 | //============================================================================================== |
b50559 | 3:10f487bae65c | 126 | |
b50559 | 3:10f487bae65c | 127 | count = 0; |
screamer | 0:bfb567985c64 | 128 | |
screamer | 0:bfb567985c64 | 129 | while(1) { |
b50559 | 3:10f487bae65c | 130 | |
b50559 | 3:10f487bae65c | 131 | myled1 = 0; |
screamer | 0:bfb567985c64 | 132 | |
b50559 | 3:10f487bae65c | 133 | i = 0; |
b50559 | 3:10f487bae65c | 134 | while(i < samples){ |
b50559 | 3:10f487bae65c | 135 | |
b50559 | 3:10f487bae65c | 136 | //Read sensors |
b50559 | 3:10f487bae65c | 137 | combo_acc.getAxis(adata); |
b50559 | 3:10f487bae65c | 138 | combo_mag.getAxis(mdata); |
b50559 | 3:10f487bae65c | 139 | gyro.ReadXYZ(gyro_data); |
b50559 | 3:10f487bae65c | 140 | avg_acc[0] = avg_gyr[0] + gyr[0]; |
b50559 | 3:10f487bae65c | 141 | avg_acc[1] = avg_acc[1] + acc[1]; |
b50559 | 3:10f487bae65c | 142 | avg_acc[2] = avg_acc[2] + acc[2]; |
b50559 | 3:10f487bae65c | 143 | avg_mag[0] = avg_mag[0] + mag[0]; |
b50559 | 3:10f487bae65c | 144 | avg_mag[1] = avg_mag[1] + mag[1]; |
b50559 | 3:10f487bae65c | 145 | avg_mag[2] = avg_mag[2] + mag[2]; |
b50559 | 3:10f487bae65c | 146 | avg_gyr[0] = avg_gyr[0] + gyr[0]; |
b50559 | 3:10f487bae65c | 147 | avg_gyr[1] = avg_gyr[1] + gyr[1]; |
b50559 | 3:10f487bae65c | 148 | avg_gyr[2] = avg_gyr[2] + gyr[2]; |
b50559 | 3:10f487bae65c | 149 | i++; |
b50559 | 3:10f487bae65c | 150 | } |
b50559 | 3:10f487bae65c | 151 | |
b50559 | 3:10f487bae65c | 152 | acc[0] = avg_acc[0]/samples; |
b50559 | 3:10f487bae65c | 153 | acc[1] = avg_acc[1]/samples; |
b50559 | 3:10f487bae65c | 154 | acc[2] = avg_acc[2]/samples; |
b50559 | 3:10f487bae65c | 155 | mag[0] = avg_mag[0]/samples; |
b50559 | 3:10f487bae65c | 156 | mag[1] = avg_mag[1]/samples; |
b50559 | 3:10f487bae65c | 157 | mag[2] = avg_mag[2]/samples; |
b50559 | 3:10f487bae65c | 158 | gyr[0] = avg_gyr[0]/samples; |
b50559 | 3:10f487bae65c | 159 | gyr[1] = avg_gyr[1]/samples; |
b50559 | 3:10f487bae65c | 160 | gyr[2] = avg_gyr[2]/samples; |
b50559 | 3:10f487bae65c | 161 | |
b50559 | 3:10f487bae65c | 162 | //Apply accelerometer zero offset calibration |
b50559 | 3:10f487bae65c | 163 | acc[0] = adata.x + cal_adatax; |
b50559 | 3:10f487bae65c | 164 | acc[1] = adata.y + cal_adatay; |
b50559 | 3:10f487bae65c | 165 | acc[2] = adata.z + cal_adataz; |
b50559 | 3:10f487bae65c | 166 | |
b50559 | 3:10f487bae65c | 167 | //Apply gyro zero offset calibration |
b50559 | 3:10f487bae65c | 168 | gyr[0] = gyro_data[0] + cal_gdatax; |
b50559 | 3:10f487bae65c | 169 | gyr[1] = gyro_data[1] + cal_gdatay; |
b50559 | 3:10f487bae65c | 170 | gyr[2] = gyro_data[2] + cal_gdataz; |
screamer | 0:bfb567985c64 | 171 | |
b50559 | 3:10f487bae65c | 172 | //Apply magnetometer soft-iron calibration |
b50559 | 3:10f487bae65c | 173 | mag[0] = (mdata.x-bias[0])*cal_mdatax[0] + (mdata.y-bias[1])*cal_mdatax[1] + (mdata.z-bias[2])*cal_mdatax[2]; |
b50559 | 3:10f487bae65c | 174 | mag[1] = (mdata.x-bias[0])*cal_mdatay[0] + (mdata.y-bias[1])*cal_mdatay[1] + (mdata.z-bias[2])*cal_mdatay[2]; |
b50559 | 3:10f487bae65c | 175 | mag[2] = (mdata.x-bias[0])*cal_mdataz[0] + (mdata.y-bias[1])*cal_mdataz[1] + (mdata.z-bias[2])*cal_mdataz[2]; |
b50559 | 3:10f487bae65c | 176 | |
b50559 | 3:10f487bae65c | 177 | |
b50559 | 3:10f487bae65c | 178 | ahrs.update(gyr[0], gyr[1], gyr[2], acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]); |
b50559 | 3:10f487bae65c | 179 | ahrs.getEuler(); |
b50559 | 3:10f487bae65c | 180 | |
b50559 | 3:10f487bae65c | 181 | roll = ahrs.getRoll() * 8; |
b50559 | 3:10f487bae65c | 182 | pitch = ahrs.getPitch() * 8; |
b50559 | 3:10f487bae65c | 183 | yaw = (ahrs.getYaw() - off_yaw) * 8; |
b50559 | 3:10f487bae65c | 184 | |
b50559 | 3:10f487bae65c | 185 | count ++; |
b50559 | 3:10f487bae65c | 186 | if(count == 1){ |
b50559 | 3:10f487bae65c | 187 | off_yaw = yaw; |
b50559 | 3:10f487bae65c | 188 | yaw = yaw - off_yaw; |
b50559 | 3:10f487bae65c | 189 | } |
screamer | 0:bfb567985c64 | 190 | |
b50559 | 3:10f487bae65c | 191 | //printf("Roll: %d\tPitch: %d\tYaw: %d\r\n", roll, pitch, yaw); |
b50559 | 3:10f487bae65c | 192 | //printf("Acc: %6.3f, %6.3f, %6.3f Mag: %6.3f, %6.3f, %6.3f Gyro: %6.3f, %6.3f, %6.3f\r\n", acc[0], acc[1], acc[2], mag[0], mag[1], mag[2], gyr[0], gyr[1], gyr[2]); |
screamer | 0:bfb567985c64 | 193 | |
b50559 | 3:10f487bae65c | 194 | txData[0] = (pitch) & 0xff; |
b50559 | 3:10f487bae65c | 195 | txData[1] = (pitch>>8) & 0xff; |
b50559 | 3:10f487bae65c | 196 | txData[2] = (pitch>>16) & 0xff; |
b50559 | 3:10f487bae65c | 197 | txData[3] = (pitch>>24) & 0xff; |
b50559 | 3:10f487bae65c | 198 | |
b50559 | 3:10f487bae65c | 199 | txData[4] = (yaw) & 0xff; |
b50559 | 3:10f487bae65c | 200 | txData[5] = (yaw>>8) & 0xff; |
b50559 | 3:10f487bae65c | 201 | txData[6] = (yaw>>16) & 0xff; |
b50559 | 3:10f487bae65c | 202 | txData[7] = (yaw>>24) & 0xff; |
b50559 | 3:10f487bae65c | 203 | |
b50559 | 3:10f487bae65c | 204 | txData[8] = (char)(fire_button.read()); |
b50559 | 3:10f487bae65c | 205 | |
b50559 | 3:10f487bae65c | 206 | myled1 = fire_button.read(); |
b50559 | 3:10f487bae65c | 207 | myled3 = !myled1; |
ajtag | 2:0cccf85f9b3f | 208 | |
b50559 | 3:10f487bae65c | 209 | my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) ); |
b50559 | 3:10f487bae65c | 210 | |
b50559 | 3:10f487bae65c | 211 | wait(.001); |
b50559 | 3:10f487bae65c | 212 | |
b50559 | 3:10f487bae65c | 213 | |
screamer | 0:bfb567985c64 | 214 | } |
screamer | 0:bfb567985c64 | 215 | } |