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@3:10f487bae65c, 2015-08-13 (annotated)

Committer:
b50559
Date:
Thu Aug 13 22:17:28 2015 +0000
Revision:
3:10f487bae65c
Parent:
2:0cccf85f9b3f
Child:
4:651a5029e003
initial commit.  not functional. yaw effected by both pitch and roll.

Who changed what in which revision?

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