File content as of revision 4:651a5029e003:

//This is an intern project to port 9DOF sensor fusion to the Nerf gun.

#include "mbed.h"
#include "FXOS8700Q.h"
#include "FXAS21000.h"
#include "MadgwickAHRS.h"
#include "MahonyAHRS.h"
#include "nRF24L01P.h"
#include "DebounceIn.h"

#include <iostream>
using namespace std;

#define TRANSFER_SIZE   9

FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
FXAS21000 gyro(D14, D15);
nRF24L01P my_nrf24l01p(PTD6, PTD7, PTD5, PTD4, PTC12, PTC18);    // mosi, miso, sck, csn, ce, irq

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);

Serial pc(USBTX, USBRX);

DebounceIn fire_button(PTA4);
DebounceIn cal_button(PTC6);

int main()
{

    pc.baud(115200);

    MahonyAHRS ahrs(50.0f);
    
    int16_t roll;
    int16_t pitch;
    int16_t yaw;
    int16_t avg_roll = 0;
    int16_t avg_pitch = 0;
    int16_t avg_yaw = 0;
    int16_t off_yaw = 0;

    float gyro_data[3];
    MotionSensorDataUnits adata;
    MotionSensorDataUnits mdata;

    float acc[3];
    float mag[3];
    float gyr[3];
    float avg_acc[3];
    float avg_mag[3];
    float avg_gyr[3];
    
    char txData[TRANSFER_SIZE];
    
    my_nrf24l01p.setTxAddress(0xDEADBEEF0F);
    my_nrf24l01p.powerUp();
    myled1 = 1;
    myled2 = 1;
    
    // Display the setup of the nRF24L01+ chip
    
    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
    pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
    

    my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
    my_nrf24l01p.enable();

    //=============================================================================================
    combo_acc.enable();
    combo_mag.enable();

    //Calibration
    printf("Please Set Device Right Side Up on a Flat Surface\r\n");
    printf("Calibrating...\r\n");

    int count = 0;
    int samples = 100;
    int i;

    float cal_adatax = 0;
    float cal_adatay = 0;
    float cal_adataz = 0;

    float cal_gdatax = 0;
    float cal_gdatay = 0;
    float cal_gdataz = 0;

    //Transformation matrix obtained via MagMaster 1.0
    float cal_mdatax[3] = {1.012, 0.03, -0.024};
    float cal_mdatay[3] = {-0.021, 0.997, -0.044};
    float cal_mdataz[3] = {0.008, -0.012, 0.993};
    float bias[3] = {0.008,0.018,0.003};

    while(count < samples) {
        combo_acc.getAxis(adata);
        gyro.ReadXYZ(gyro_data);

        cal_adatax += adata.x;
        cal_adatay += adata.y;
        cal_adataz += adata.z;
        cal_gdatax += gyro_data[0];
        cal_gdatay += gyro_data[1];
        cal_gdataz += gyro_data[2];

        count++;
        wait(.1);
    }

    cal_adatax = -cal_adatax / samples;
    cal_adatay = -cal_adatay / samples;
    cal_adataz = -cal_adataz / samples + 1;
    cal_gdatax = -cal_gdatax / samples;
    cal_gdatay = -cal_gdatay / samples;
    cal_gdataz = -cal_gdataz / samples;

    printf("\r\n");
    
    //==============================================================================================

      count = 0;
    
    while(1) {

        myled1 = 0;
        
        i = 0;
        while(i < samples){
            
            //Read sensors
            combo_acc.getAxis(adata);
            combo_mag.getAxis(mdata);
            gyro.ReadXYZ(gyro_data);
            avg_acc[0] = avg_gyr[0] + gyr[0];
            avg_acc[1] = avg_acc[1] + acc[1];
            avg_acc[2] = avg_acc[2] + acc[2];
            avg_mag[0] = avg_mag[0] + mag[0];
            avg_mag[1] = avg_mag[1] + mag[1];
            avg_mag[2] = avg_mag[2] + mag[2];
            avg_gyr[0] = avg_gyr[0] + gyr[0];
            avg_gyr[1] = avg_gyr[1] + gyr[1];
            avg_gyr[2] = avg_gyr[2] + gyr[2];
            i++;
            }
            
            acc[0] = avg_acc[0]/samples;
            acc[1] = avg_acc[1]/samples;
            acc[2] = avg_acc[2]/samples;
            mag[0] = avg_mag[0]/samples;
            mag[1] = avg_mag[1]/samples;
            mag[2] = avg_mag[2]/samples;
            gyr[0] = avg_gyr[0]/samples;
            gyr[1] = avg_gyr[1]/samples;
            gyr[2] = avg_gyr[2]/samples;

        //Apply accelerometer zero offset calibration
        acc[0] = adata.x + cal_adatax;
        acc[1] = adata.y + cal_adatay;
        acc[2] = adata.z + cal_adataz;

        //Apply gyro zero offset calibration
        gyr[0] = gyro_data[0] + cal_gdatax;
        gyr[1] = gyro_data[1] + cal_gdatay;
        gyr[2] = gyro_data[2] + cal_gdataz;

        //Apply magnetometer soft-iron calibration
        mag[0] = (mdata.x-bias[0])*cal_mdatax[0] + (mdata.y-bias[1])*cal_mdatax[1] + (mdata.z-bias[2])*cal_mdatax[2];
        mag[1] = (mdata.x-bias[0])*cal_mdatay[0] + (mdata.y-bias[1])*cal_mdatay[1] + (mdata.z-bias[2])*cal_mdatay[2];
        mag[2] = (mdata.x-bias[0])*cal_mdataz[0] + (mdata.y-bias[1])*cal_mdataz[1] + (mdata.z-bias[2])*cal_mdataz[2];


        ahrs.update(gyr[0], gyr[1], gyr[2], acc[0], acc[1], acc[2], mag[0], mag[1], mag[2]);
        ahrs.getEuler();
        
        roll = ahrs.getRoll() * 8;
        pitch = ahrs.getPitch() * 8;
        yaw = (ahrs.getYaw() - off_yaw) * 8;
        
        count ++;
        if(count == 1){
            off_yaw = yaw;
            yaw = yaw - off_yaw;
            }
        
        //printf("Roll: %d\tPitch: %d\tYaw: %d\r\n", roll, pitch, yaw);
        //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]);
        
        txData[0] = (pitch) & 0xff;
        txData[1] = (pitch>>8)  & 0xff;
        txData[2] = (pitch>>16)  & 0xff;
        txData[3] = (pitch>>24)  & 0xff;
    
        txData[4] = (yaw) & 0xff;
        txData[5] = (yaw>>8)  & 0xff;
        txData[6] = (yaw>>16)  & 0xff;
        txData[7] = (yaw>>24)  & 0xff;
            
        txData[8] = (char)(fire_button.read());
                
        myled1 = fire_button.read();
        myled3 = !myled1;
        
        my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, sizeof( txData ) );
               
        wait(.001);
        
                
    }
}