File content as of revision 2:0cf2ef636323:

#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 koda koristi print na PC i tera term
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 s tim da
// 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();

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);
    while(1){
        //display sve
        //oled.clearDisplay();
        //sprintf(oled_str, "%d", TH_def);
        //oled.writeString(0, 0, oled_str);
        
        if(TSW.read()){
            //ulaz znaci npr 0.54 default *100 =54- korekcija izracunata u kalibraciji pa je blizu 50 nekad 49 nekad 51 ali blize nego bez korekcije, *5=250
            //-250=0 za default nema spustanja il dizanja TH, recimo da dizes sto je u nasem slucaju zbog naopacke zalemljenog joysticka ustvari prema dolje (zato je -1 tu)
            //ond aje npr 0.74*100 -korekcija =70*5=350 -250=100 pozitivan broj ali *-1 posto smo ustvari joystick pomakli prema dolje i logicno je da je to spustanje gasa
            // pa u funkciji tj_calc vidimo d aje vrijednost negativna i dok god je negativna svakim prolazom kroz ovu petlju spustamo za 10, da je joystick prema gore bila bi pozitivna i dizali bi z a10
            TH =(-1)*((((a1.read()*100)+x1_cor)*5)-250);
            th_calc();
            // slicno kao kod th samo ovdje nam dalje pretvorbi nego se direktno vrijednost salje dronu
            //ocitano npr 0.53*100 - kalibracija =50 * multi koji je 3 znaci 150 + low us 1350 je 1500 i zbog obrnutih joysticka ovja put 3000-1500=1500
            //ali da je broj npr 1250, 3000-1250= 1750 (recimo za pitch unaprijed dron treba vise od 1500 ali mi kad taj joystick pomaknemo naprijed dobijemo manje od 1500 pa zato 3000-X nam daje suprotnu vrijednost
            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);
            double getme ();
            
            //jos uvijek nez za sta sam to koristio
        //svakim prolazom kroz petlju vracam rcom na 0 jer je to default i tek kad stisnes on postane nes drugo, flag za gps i slično
            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);
                pc.printf("%0.2f\n",vbat);
                pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
            }
            //testiranje na pcu
            //printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1,y1,x2,y2,sw1.read(),sw2.read());
            //printf("%hd,%hd,%hd,%hd\n",sw1.read(),sw2.read(),sw3.read(),tog1.read());
            
            vbat=(abat.read()*3.3f)/bat_divider;
            //ispis na komp
            pc.printf("%0.2f\n",vbat);
            wait_ms(10);//
        
        }
        
        else{
            
            hc.printf("MSG,%hd,%hd,%hd,%hd,%hd,%d,%d,%d\n",1000,1500,1500,1500,2000,0,0,0);
            wait_ms(100);
        
        }
    }
}

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);
    //pc.printf("%hd,%hd,%hd,%hd,%hd,%hd\n",x1_cor,y1_cor,x2_cor,y2_cor,x3_cor,y3_cor);
}

void th_calc(){
    
    if(abs(TH)<20){
        TH_def+=0;
    }
    else{
        //ako je veca povecaj th def za 10, ako je manja od 0 smanji th def za 10
        if(TH>0){
            TH_def+=10;
        }
        else{
            TH_def-=10;
        }
    }
    //sprjecavamo da ode izvan granica
    if(TH_def<1000)TH_def=1000;
    if(TH_def>2000)TH_def=2000;
    // u slucaju da osoba stisne gumb na joysticku lijevo th se spusta na 1000 th zaustavlaju se motori
    if(JSW2.read()==1)TH_def=1000;
}

//Spojio na standardne pinove 14,15  s desne strane ploce mislimd a su to pb9 i pb8
//onda ko i kog magnetometras idu otpornici sa sda i scl na 3.3v
//VCC 5 ili 3.3 probaj s 3.3 ak
//GND gnd
//SCL scl
//sda sda
//eda i ecl na nista
//AD0 na GND
//i ova ostala 3 na nista, INT,NCS,FSYNC


float sum = 0;
uint32_t sumCount = 0;

MPU9250 mpu9250;

Timer t;

//Serial pc(PA_2, PA_3); // tx, rx

volatile bool newData = false;

InterruptIn isrPin(D12);   //k64 D12  dragon PD_0

void mpuisr()
{
    newData=true;
}

void ziroskop()
{
    pc.baud(9600);

    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C

    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);

    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
    pc.printf("I AM 0x%x\n\r", whoami);
    pc.printf("I SHOULD BE 0x71\n\r");

    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) {
        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++;


        // Pass gyro rate as rad/s
        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
        // Serial print and/or display at 0.5 s rate independent of data rates
        delt_t = t.read_ms() - count;
        if (delt_t > 500) { // update LCD once per half-second independent of read rate
            pc.printf("readycnt %d us %d\n",readycnt,us);
            readycnt=0;
            pc.printf("ax = %f", 1000*ax);
            pc.printf(" ay = %f", 1000*ay);
            pc.printf(" az = %f  mg\n\r", 1000*az);

            pc.printf("gx = %f", gx);
            pc.printf(" gy = %f", gy);
            pc.printf(" gz = %f  deg/s\n\r", gz);

            pc.printf("gx = %f", mx);
            pc.printf(" gy = %f", my);
            pc.printf(" gz = %f  mG\n\r", mz);

            tempCount = mpu9250.readTempData();  // Read the adc values
            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
            pc.printf("temperature = %f  C\n\r", temperature);

            pc.printf("q0 = %f\n\r", q[0]);
            pc.printf("q1 = %f\n\r", q[1]);
            pc.printf("q2 = %f\n\r", q[2]);
            pc.printf("q3 = %f\n\r", q[3]);
            
            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;

            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
           
            count = t.read_ms();
            sum = 0;
            sumCount = 0;
        
        }
    }

}