![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asd
Dependencies: BNO055 GPS IntegrationCAN SD_CARD_TWO mbed
Fork of BNO055 by
main.cpp
- Committer:
- Tafkal
- Date:
- 2018-06-17
- Revision:
- 8:03e50b5d9d10
- Parent:
- 7:83d56dc36f01
File content as of revision 8:03e50b5d9d10:
/*------------------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(SERIAL_TX, SERIAL_RX, 9600); //SD Card Object SDBlockDevice sd(PB_5, PB_4, PB_3, PA_4); //CAN CAN can(PD_0, PD_1, 500000); //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 int CANlen = 0; //Some stuff for CAN // Variables received from DTA, LVDTs and brakes uint16_t 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; uint16_t 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; 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%x\n", SendMe.id, SendMe.data); } /*------------------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/Test.txt","a"); //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) { //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"); UpdateInfo(); //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, "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",\ Rpm,Speed,Gear,Water_Temp,Oil_Temp,TPS,Brakes,Oil_P,MAP,Air_Temp,Lambda,Volts,Crank); fprintf(fp, "FL_LVDT0: %d, FR_LVDT0: %d, RL_LVDT0: %d, RR_LVDT0: %d, FL_LVDT: %d, FR_LVDT: %d, RL_LVDT: %d, RR_LVDT: %d\r\n",\ FL_LVDT0,FR_LVDT0,RL_LVDT0,RR_LVDT0,FL_LVDT,FR_LVDT,RL_LVDT,RR_LVDT); //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);/* 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);*/ } }