File content as of revision 12:3c500a06c5f3:

/*------------------LIBS------------------*/
#include "mbed.h"
#include "BNO055.h"
#include "GPS.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "sample_hardware.hpp"
#include "CAN_library.h"
#include <string>
/*---------------DATA TYPES---------------*/
//GPS
GPS gps(PC_6, PC_7);
int NMEA_TYPE = 0;
//10DOF
BNO055 imu(PB_9, PB_8);
//DEBUG SERIAL
Serial pc(USBTX, USBRX, 57600);
//SD Card Object
SDBlockDevice sd(PB_5, PB_4, PB_3, PA_4);
//CAN
CAN can(PD_0, PD_1, 1000000);
//tRF
Serial tRF(PC_12, PD_2, 19200);

//CANMsg
CANMessage msgDTA1;         // RPM, TPS %, Water temp C, Air temp C
CANMessage msgDTA2;         // MAP Kpa, Lambda x1000, KPH x10, Oil P Kpa
CANMessage msgDTA3;         // Fuel P Kpa, Oil temp C, Volts x10, Fuel Con. L/Hr x10
CANMessage msgDTA4;         // Gear, Advance Deg x10, Injection ms x100, Fuel Con L/100km x10
CANMessage msgDTA5;         // Ana1 mV, Ana2 mV, Ana3 mV, Cam Advance x10
CANMessage msgDTA6;         // Cam Targ x10, Cam PWM x10, Crank Errors, Cam Errors
CANMessage msgLVDTFront;    // Left, Right, Steering Wheel 
CANMessage msgLVDTRear;     // Left, Right
CANMessage msgBrakes;       // Brake system preassure, Braking On/Off
CANMessage msgKm;           // KM
CANMessage msgGPS;          // GPS position data
CANMessage msgGForce;       // Accel data
CANMessage msgGyro;         // Gyro data
CANMessage msgTest;
char* temp_string;          // Temp data
char* msgGyro1;          // Temp data
char* msgGForce1;          // Temp data
char* msgGps1;
int CANlen = 0;
//Some stuff for CAN
// Variables received from DTA, LVDTs and brakes
uint16_t Steer0, Rpm0=0,Speed0=0,Gear0=0,Water_Temp0=0,Oil_Temp0=0,TPS0=0,Brakes0=0,Oil_P0=0,MAP0=0,Air_Temp0=0,Lambda0=0,Volts0=0,Crank0=0,BF_Oil_P0,BR_Oil_P0;
uint16_t Steer, Rpm=0,Speed=0,Gear=0,Water_Temp=0,Oil_Temp=0,TPS=0,Brakes=0,Oil_P=0,MAP=0,Air_Temp=0,Lambda=0,Volts=0,Crank=0,BF_Oil_P,BR_Oil_P;
int FL_LVDT0=0,FR_LVDT0=0,RL_LVDT0=0,RR_LVDT0=0,FL_LVDT=0,FR_LVDT=0,RL_LVDT=0,RR_LVDT=0;
//Referrent LVDT values. First received value is referrent.
int FL_LVDT_Ref,FR_LVDT_Ref,RL_LVDT_Ref,RR_LVDT_Ref;
uint16_t rx_flag=0x0000;        // Receive specific CAN data message
uint8_t lvdtref=0x0F;          // Flag if refferent LVDT value is received (first received LVDT value, 1=no, 0=yes). From highest to lowest bit: LL,LR,RL,RR.
float kilometraza;
/*-----------tRF Send Telemetry-----------*/
void tRF_sendCAN(CANMessage SendMe) {
    tRF.printf("S%x%02x%02x%02x%02x%02x%02x%02x%02x\n", SendMe.id, SendMe.data[1],SendMe.data[0],SendMe.data[3],SendMe.data[2],SendMe.data[5],SendMe.data[4],SendMe.data[7],SendMe.data[6]);
}


/*------------------MAIN------------------*/
int main() {
// Attach interrupt function to CAN RX
    can.attach(&CANMsgReceive,CAN::RxIrq);
    
//10DOF INIT
    imu.reset();
   // pc.printf("RESET DONE!");
    while(!imu.check()){
        //pc.printf("ejbg");
            }
    //pc.printf("CHECK DONE!");
//10DOF SET MODE AND CALIBRATE
    imu.setmode(OPERATION_MODE_IMUPLUS);
    imu.get_calib();
// Display sensor information
    /*pc.printf("BNO055 found\r\n\r\n");
    pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
    pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
    pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
    pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
    pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);*/
// Display chip serial number
    /*for (int i = 0; i<4; i++){
        pc.printf("%0z.%0z.%0z.%0z\r\n",imu.ID.serial[i*4],imu.ID.serial[i*4+1],imu.ID.serial[i*4+2],imu.ID.serial[i*4+3]);
    }*/
    
    //pc.printf("\r\n");
    
//tRF INIT
  tRF.printf("+++");
  tRF.printf("ATR\r");
  // Freq
  tRF.printf("ATS206=12\r");
  // Channel
  tRF.printf("ATS200=3\r");
  // Speed transmit uart
  tRF.printf("ATS210=5\r");
  // Radio baud
  tRF.printf("S201=4\r");
  // Go to send mode
  tRF.printf("ATO\r");
 // pc.printf("tRF INIT END\n");
  
//SD CARD
    //printf("Initialise\n");
    //FileSystemLike(*sd);

// call the SDBlockDevice instance initialisation method. (not needed)
    if ( sd.init() != 0) {
        //printf("Init failed \n");
        errorCode(FATAL);
    }    
    
//Create a filing system for SD Card
    FATFileSystem fs("sd", &sd);
    
// Open to WRITE
    //printf("Write to a file\n");
    FILE* fp = fopen("/sd/FSRA18_LOG.txt","a");
    fprintf(fp, "\nNovi Podaci\n");
    //FILE* km = fopen("/sd/kilometraza.txt","r+");
    if (fp == NULL) {
        error("Could not open file for read\n");
        errorCode(FATAL);
    }   
    /*if (km == NULL) {
        error("Could not open file for read\n");
        errorCode(FATAL);
    }  
    fscanf(km, "%f", &kilometraza);*/
//Write something to SD
    //fputs("Drumska Strela!\r\n", fp);
/*
//Close File
    fclose(fp);
    
//Close down SD
    sd.deinit();
    printf("All done...\n");
    errorCode(OK);
*/
/*----------------MAIN LOOP----------------*/
    while (true) {
        UpdateInfo();

        //printf("In while\n");
//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);
        fprintf(fp, "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);
        fprintf(fp, "ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
//GET GPS DATA
        //printf("GPS\n");
        NMEA_TYPE = gps.sample();
        if (1) {
            //pc.printf("LON: %f, LAT: %f, SIV: %d, TYPE: %d\r\n",gps.get_nmea_longitude(), gps.get_dec_latitude(), gps.get_satelites(), NMEA_TYPE);
            fprintf(fp, "LON: %f, LAT: %f, SIV: %d, TYPE: %d\r\n",gps.get_nmea_longitude(), gps.get_dec_latitude(), gps.get_satelites(), NMEA_TYPE);
        }
//CAN TEST
        //printf("CANUpdate\n");
        //Send telemetry data
        //tRF.printf("POYY KEJTERI\n");
        //Store data
        //fprintf(fp, "Rpm0: %d, Speed0: %d, Gear0: %d, Water_Temp0: %d, Oil_Temp0: %d, TPS0: %d, Brakes0: %d, Oil_P0: %d, MAP0: %d, Air_Temp0: %d, Lambda0: %d, Volts0: %d, Crank0%d\r\n",\
        //Rpm0,Speed0,Gear0,Water_Temp0,Oil_Temp0,TPS0,Brakes0,Oil_P0,MAP0,Air_Temp0,Lambda0,Volts0,Crank0);
        fprintf(fp, "Rpm: %d, Speed: %d, Gear: %d, Water_Temp: %d, Oil_Temp: %d, TPS: %d, MAP: %d, Air_Temp: %d, Lambda: %d, Volts: %d, \r\n",\
        Rpm,Speed,Gear,Water_Temp,Oil_Temp,TPS,MAP,Air_Temp,Lambda,Volts);
        fprintf(fp, "Steer: %d, FL_LVDT: %d, FR_LVDT: %d, RL_LVDT: %d, RR_LVDT: %d, BF_Oil_P: %d, BR_Oil_P: %d Brakes= %d\r\n",\
        Steer, FL_LVDT,FR_LVDT,RL_LVDT,RR_LVDT,BF_Oil_P,BR_Oil_P,Brakes);
        
        
        //pc.printf("id=2001, MAP= %d, Lambda= %d, Speed= %d, Oil_P= %d",Lambda,Speed,Oil_P);

//tRF SendAll
        //printf("sendCAN\n");
        
     
        
      tRF_sendCAN(msgDTA1);
       tRF_sendCAN(msgDTA2);
       tRF_sendCAN(msgDTA3);
       tRF_sendCAN(msgDTA4);
       tRF_sendCAN(msgDTA5);
       tRF_sendCAN(msgDTA6);
       tRF_sendCAN(msgLVDTFront);
       tRF_sendCAN(msgLVDTRear);
       tRF_sendCAN(msgBrakes);
       
       
       
        sprintf(msgGyro1, "S1011%.2fY%.2fP%.2fR", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
       //pc.printf("%s\n",temp_string);
       tRF.printf("%s\n",msgGyro1);
       sprintf(msgGForce1, "S1010%.2fX%.2fY%.2fZ", imu.accel.x, imu.accel.y, imu.accel.z);
       tRF.printf("%s\n",msgGForce1);
       //sprintf(msgGps1, "S1012%.5fN%.5fE%dSIV", gps.get_nmea_longitude(), gps.get_dec_latitude(),gps.get_satelites());
        //tRF.printf("%s\n",msgGps1);
       /*while(1) {
       sprintf(temp_string, "401236789");
       CANlen = strlen(temp_string);
       tRF_sendCAN(CANMessage(0x2001, temp_string, CANlen));
       wait(1);
       tRF.printf("S20012D123678\n");
       }*/
       /*sprintf(temp_string, "%.5fN%.5fE", gps.get_nmea_longitude(), gps.get_dec_latitude());
       CANlen = strlen(temp_string);
       msgGPS = CANMessage(0x0009, temp_string, CANlen);
       tRF_sendCAN(msgGPS);
       sprintf(temp_string, "%.5fY%.5fP%.5fR", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
       CANlen = strlen(temp_string);
       msgGForce= CANMessage(0x0010, temp_string, CANlen);
       tRF_sendCAN(msgGForce);
       sprintf(temp_string, "%.5fX%.5fY%.5fZ", imu.accel.x, imu.accel.y, imu.accel.z);
       CANlen = strlen(temp_string);
       msgGyro= CANMessage(0x0011, temp_string, CANlen);*/
       
    }
}