File content as of revision 3:835df5760305:

/*------------------LIBS------------------*/
#include "mbed.h"
#include "bno055.h"
#include "CANlibrary.h"
#include "CANadressID.h"
#include "GPSM.h"
#include "SDcardM.h"
#include "SDBlockDevice.h"
#include <string>
/*---------------DATA TYPES---------------*/

CAN can(PA_11, PA_12,1000000);

//10DOF
BNO055 imu(PB_14, PB_13);
char calibration[22]= {0x03, 0x00, 0xF2, 0xFF, 0x08, 0x00, 0xB4,
                       0x00, 0xAB, 0xFF, 0xD9, 0xFE, 0xFF, 0xFF,
                       0xF5, 0xFF, 0x00, 0x00, 0xE8, 0x03, 0x95, 0x02
                      };
//XBEE
Serial pc(PA_0, PA_1, 115200);

//SD Card Object
SDBlockDevice sd(PA_7, PA_6, PA_5, PA_4, 100000);

uint8_t buff[512] = "";
bd_addr_t* blockAddr = (bd_addr_t*) buff;
//uint32_t* byteAddr = blockAddr+1;

int main()
{
    *blockAddr=512;
    //*byteAddr=14;
    pc.printf("INITIALIZING...\r\n");
//GPS INIT
    pc.printf("GPS...\r\n");
    GPSM::GPSM_Init(PC_10, PC_11);
    while(GPSM::_gps->readable()) GPSM::_gps->getc();
    GPSM::_gps->attach(callback(GPSM::rx_interrupt));
//10DOF INIT
    //imu.reset();
    //while(!imu.check());
    pc.printf("IMU...\r\n");
//10DOF SET MODE AND CALIBRATE
    imu.set_calibration_data(calibration);
    imu.setmode(OPERATION_MODE_NDOF);
    wait_ms(10);
    imu.write_calibration_data();

    pc.printf("CAN...\r\n");
    //can.frequency(1000000); //10hz???
    can_initRX();

    pc.printf("SD...\r\n");
    SD_init();
    //sd.init();
    //sd.program(buff, 0, 512);
    SD_log_string("PILETINA SE ZA PRSTE LEPI!");
    SD_log_float(3.14159);
    SD_log_int(73);
    SD_flush();
    sd.read(buff, 512, 512);
    pc.printf("%s", buff);
    

    pc.printf("STATUS NOMINAL: ALL FUCKED UP\r\n");
    //ticker.attach(&tick, 1);
    //ticked=false;
    /*----------------MAIN LOOP----------------*/
    while (true) {
        //while(!ticked);
        //ticked=false;
        wait_ms(1000);

//GET YPR
        imu.get_angles();
        pc.printf("Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);

//GET ACCEL
        imu.get_accel();
        pc.printf("ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);

        if(imu.getmode()!=OPERATION_MODE_NDOF) {
            pc.printf("IMU_FEJL\r\n");
            imu.setmode(OPERATION_MODE_NDOF);
            wait_ms(10);
            imu.write_calibration_data();
            pc.printf("REINITED\r\n");
        }

        if(GPSM::poll())
            pc.printf("LON: %f, LAT: %f\r\n", GPSM::get_dec_longitude(), GPSM::get_dec_latitude());


        if(check_flag(10)) {
            pc.printf("ASAD1: %d\n", get_data(10, 0));
        }
        if(check_flag(12)) {
            pc.printf("ASAD2: %d\n", get_data(12, 0));
        }
        pc.printf(" - - - - - - -  DTA  - - - - - - - \n");
        pc.printf("DTA1: RPM - %d TPS - %d WaterTemp - %d AirTemp - %d\n", get_data(DTA1_RPM), get_data(DTA1_TPS), get_data(DTA1_WaterTemp), get_data(DTA1_AirTemp));
        pc.printf("DTA2: MAP - %d Lambda - %d Speed - %d OilPres - %d\n", get_data(DTA2_MAP), get_data(DTA2_Lambda), get_data(DTA2_Speed), get_data(DTA2_OilPres));
        pc.printf("DTA3: OilTemp - %d Volts - %d\n", get_data(DTA3_OilTemp), get_data(DTA3_Volts));
        pc.printf("DTA4: Gear - %d\n", get_data(DTA4_Gear));
        pc.printf("DTA6: Crank - %d\n", get_data(DTA6_Crank));
        

    }
}