![](/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-05-25
- Revision:
- 7:83d56dc36f01
- Child:
- 8:03e50b5d9d10
File content as of revision 7:83d56dc36f01:
/*------------------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); //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 msgGPS; // GPS position data CANMessage msgGForce; // Accel data CANMessage msgGyro; // Gyro data 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. /*-----------tRF Send Telemetry-----------*/ void tRF_sendCAN(CANMessage SendMe) { tRF.printf("%x%s%d%d%d", SendMe.id, SendMe.data, SendMe.len, SendMe.format,SendMe.type); } /*------------------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/WriteMeBaby.txt","a"); if (fp == NULL) { error("Could not open file for read\n"); errorCode(FATAL); } //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) { //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 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 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 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(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); } }