File content as of revision 2:997bfe6f2960:

#include "mbed.h"
#include "MPU9250.h"
#include <math.h> 
#include "SSD1308.h" 
#define cal_num 10 // broj ocitanja analognih ulaza za kalibraciju, za dobivanje srednje vrijednosti
 
//Analogni ulazi 
AnalogIn a1(PA_0);
AnalogIn a2(PA_1);
AnalogIn a3(PA_3);
AnalogIn a4(PA_4);
AnalogIn a5(PA_5);
AnalogIn a6(PA_6);
AnalogIn abat(PA_7);
 
DigitalIn SW1(PA_11);
DigitalIn SW2(PB_5);
DigitalIn SW3(PB_4);
DigitalIn JSW1(PA_12);
DigitalIn JSW2(PB_0);
DigitalIn JSW3(PA_8);
DigitalIn TSW(PB_1);
//Serijska veza sa modulom za radio komunikaciju, to samo prosljeduje poruku
Serial hc(PA_9, PA_10);
Serial pc(PA_2, PA_15);
 
//I2C i2c(PB_7, PB_6);
 
//SSD1308 oled = SSD1308(&i2c, SSD1308_SA0);
 
short TH,PT,RO,YA,TH_def;//Throttle pitch roll yaw i TH_def
float x1_ar,x2_ar,x3_ar,y1_ar,y2_ar,y3_ar; //privremene varijable za zbrajanje ulaza, ocitava se znaci analagni ulaz i zbroji sa prijasnjim pa se na krjau sve podjeli s broje ocitanja 
short mid_point=50; //vrijednost koju smatramo sredinom u nasem slucaju 50 (analogni ulaz ide od 0 do 1.0 pomnozen sa sto od 0 do 100%)
short x1_cor,x2_cor,x3_cor,y1_cor,y2_cor,y3_cor; // korekcijski broj
short low_us=1350; //inace 1000 al da smanjimo osjetljivost joysticka smo stavili 1350 pa ide do 1650
short multiplier_us=3; // s tim mnozimo nas izracunati analogni ulaz u nominalnom stanju 50*3 znaci 150 + 1350 =1500us posto dron prima od 1000 do 2000us
// za ove vrijednosti roll pitch yaw je 1500 nominalno di se ne dogada nista a pomak prema 2000 il 1000 oznacava zakret oko osi 
 
int rcom=0;
 
//spremanje napona baterije
float vbat;
//vrijednost otpornickog djelila znaci ulazni napon  podjeljeno s tim daje napon baterije s tim da ulazni napon tj ocitanje analognog ulaza (0-1)* 3.3 
float bat_divider=0.703313;
char oled_str[30];
void calibration();
void th_calc();
 
 //Senzor
 
float sum = 0;
uint32_t sumCount = 0;
 
MPU9250 mpu9250;
 
Timer t;
 
volatile bool newData = false;
 
InterruptIn isrPin(D12);   //k64 D12  dragon PD_0
 
void mpuisr()
{
    newData=true;
}
 
 
 
int main(){
    
    SW1.mode(PullDown);
    SW2.mode(PullDown);
    SW3.mode(PullDown);
    JSW1.mode(PullDown);
    JSW2.mode(PullUp);
    JSW3.mode(PullUp);
    //switch izmedu 2 joysticka
    TSW.mode(PullDown);
    //kalibraciju vrsim nakon sto ih sve pull up/downam
    calibration();
    //funkcije za display
    //oled.setDisplayFlip(false,false);
    //oled.setContrastControl(0xFF);
    
 
    //Senzor
    
        //pc.baud(9600);
 
    i2c.frequency(400000);  // use fast (400 kHz) I2C
    t.start();
    isrPin.rise(&mpuisr);
 
    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
    
 
    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
        pc.printf("MPU9250 is online...\n\r");
        wait(1);
 
 
        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
      
        wait(2);
        mpu9250.initMPU9250();
        mpu9250.initAK8963(magCalibration);
        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
        wait(2);
    } else {
        pc.printf("Could not connect to MPU9250: \n\r");
        pc.printf("%#x \n",  whoami);
 
 
        while(1) ; // Loop forever if communication doesn't happen
    }
 
    mpu9250.getAres(); // Get accelerometer sensitivity
    mpu9250.getGres(); // Get gyro sensitivity
    mpu9250.getMres(); // Get magnetometer sensitivity
    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
   
    
    
    while(1){
        //display sve
        //oled.clearDisplay();
        //sprintf(oled_str, "%d", TH_def);
        //oled.writeString(0, 0, oled_str);
        
        if(TSW.read()){
            
            TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
            th_calc();
            
            PT =3000-(low_us+((a3.read()*100)+y1_cor)*multiplier_us); 
            YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
            RO =3000-(low_us+((a4.read()*100)+y2_cor)*multiplier_us);
            
            rcom=0;
            //switch desno u sredini sluzi za gps
            if(SW2.read()==1){
                rcom=2;
            }
            //switch skroz dolje desno sluzi za Arming drona 
            if(SW3.read()==1){
                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1000,1000,1000,2000,0,0,0);
            }
            
            else{
                hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
                pc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
               
            }
           
            
            vbat=(abat.read()*3.3f)/bat_divider;
            pc.printf("%0.2f\n",vbat);
            wait_ms(10);//
        
        }
        
        else{
            
            TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
            th_calc();
            
                   static int readycnt=0;
        // If intPin goes high, all data registers have new data
 
#if USE_ISR
        if(newData) {
            newData=false;
            mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS);  //? need this with ISR
#else
        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
#endif
            readycnt++;
            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
            // Now we'll calculate the accleration value into actual g's
            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
            ay = (float)accelCount[1]*aRes - accelBias[1];
            az = (float)accelCount[2]*aRes - accelBias[2];
 
            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
            // Calculate the gyro value into actual degrees per second
            gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
            gy = (float)gyroCount[1]*gRes - gyroBias[1];
            gz = (float)gyroCount[2]*gRes - gyroBias[2];
 
            mpu9250.readMagData(magCount);  // Read the x/y/z adc values
            // Calculate the magnetometer values in milliGauss
            // Include factory calibration per data sheet and user environmental corrections
            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
        }
 
        Now = t.read_us();
        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
        lastUpdate = Now;
 
        sum += deltat;
        sumCount++;
 
 
        
        uint32_t us = t.read_us();
        mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
        us = t.read_us()-us;
        int delt_t = 0; // used to control display output rate
        int count = 0;  // used to control display output rate
        
        delt_t = t.read_ms() - count;
            
            readycnt=0;
 
            tempCount = mpu9250.readTempData();  // Read the adc values
            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
            
            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
            pitch *= 180.0f / PI;
            yaw   *= 180.0f / PI;
            yaw   += 3.95f;   // Godine 2018 magnetna deklinacija je +3* 57’ za Zagreb
            roll  *= 180.0f / PI;
            
            if ( pitch < 10 && pitch > -20) {
                PT = 1500;
            }
            else {
        
                if ( pitch > 10 ) {
                    PT = 1500 + ((pitch - 10 )*3.75);
                    }
                else if (pitch < -20 ){
                    PT = 1500 + (( pitch + 20)*3.75);
                    }
                  }
            if(pitch < -60)PT=1350;
            if(pitch > 50)PT=1650;
            
            if ( abs(roll)<10) {
                RO = 1500;
                }
            else {
        
                if ( roll > 10 ) {
                    RO = 1500 + ((roll - 10 )*3.75);
                    }
                else if (roll < -10 ){
                    RO = 1500 + (( roll + 10)*3.75);
                    }
                  }
            if(roll < -50)RO=1350;
            if(roll > 50)RO=1650;
            
            YA =3000-(low_us+((a2.read()*100)+x2_cor)*multiplier_us);
            
            
            hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",TH_def,RO,PT,YA,1000,0,0,rcom);
            //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
         
           
            count = t.read_ms();
            sum = 0;
            sumCount = 0;   
        
        
        
        }
    }
}
 
void calibration(){
    
    x1_ar=0;
    x2_ar=0;
    x3_ar=0;
    y1_ar=0;
    y2_ar=0;
    y3_ar=0;
    
    for(int a=0;a<cal_num;a++){
        x1_ar+=a1.read();
        x2_ar+=a2.read();
        x3_ar+=a5.read(); 
        y1_ar+=a3.read();
        y2_ar+=a4.read();
        y3_ar+=a6.read();       
    }
    x1_cor=mid_point-((x1_ar/cal_num)*100);
    x2_cor=mid_point-((x2_ar/cal_num)*100);
    x3_cor=mid_point-((x3_ar/cal_num)*100);
    y1_cor=mid_point-((y1_ar/cal_num)*100);
    y2_cor=mid_point-((y2_ar/cal_num)*100);
    y3_cor=mid_point-((y3_ar/cal_num)*100);
    
}
 
void th_calc(){
    
    if(abs(TH)<10){
        TH_def+=0;
    }
    else{
    
        if(TH>0){
            TH_def+=10;
        }
        else{
            TH_def-=10;
        }
    }
    
    if(TH_def<1000)TH_def=1000;
    if(TH_def>2000)TH_def=2000;
    if(JSW2.read()==0)TH_def=1000;
}