Nemanja Neskovic / Mbed 2 deprecated MAIN_UNIT_FSRA_test

sdfd

Dependencies:   BNO055 GPS IntegrationCAN SD_CARD_TWO mbed

Fork of MAIN_UNIT_FSRA_test by Nenad Djalovic

main.cpp@12:3c500a06c5f3, 2018-06-24 (annotated)

Committer:
nemanja1994
Date:
Sun Jun 24 15:51:36 2018 +0000
Revision:
12:3c500a06c5f3
Parent:
10:2e912e37ee6d 
sa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tafkal 7:83d56dc36f01 1 /*------------------LIBS------------------*/
Tafkal 7:83d56dc36f01 2 #include "mbed.h"
Tafkal 7:83d56dc36f01 3 #include "BNO055.h"
Tafkal 7:83d56dc36f01 4 #include "GPS.h"
Tafkal 7:83d56dc36f01 5 #include "SDBlockDevice.h"
Tafkal 7:83d56dc36f01 6 #include "FATFileSystem.h"
Tafkal 7:83d56dc36f01 7 #include "sample_hardware.hpp"
Tafkal 7:83d56dc36f01 8 #include "CAN_library.h"
Tafkal 7:83d56dc36f01 9 #include <string>
Tafkal 7:83d56dc36f01 10 /*---------------DATA TYPES---------------*/
Tafkal 7:83d56dc36f01 11 //GPS
Tafkal 7:83d56dc36f01 12 GPS gps(PC_6, PC_7);
Tafkal 7:83d56dc36f01 13 int NMEA_TYPE = 0;
Tafkal 7:83d56dc36f01 14 //10DOF
Tafkal 7:83d56dc36f01 15 BNO055 imu(PB_9, PB_8);
Tafkal 7:83d56dc36f01 16 //DEBUG SERIAL
formulas 9:1fbb0aae6a32 17 Serial pc(USBTX, USBRX, 57600);
Tafkal 7:83d56dc36f01 18 //SD Card Object
Tafkal 7:83d56dc36f01 19 SDBlockDevice sd(PB_5, PB_4, PB_3, PA_4);
Tafkal 7:83d56dc36f01 20 //CAN
formulas 9:1fbb0aae6a32 21 CAN can(PD_0, PD_1, 1000000);
Tafkal 7:83d56dc36f01 22 //tRF
Tafkal 7:83d56dc36f01 23 Serial tRF(PC_12, PD_2, 19200);
Tafkal 7:83d56dc36f01 24
Tafkal 7:83d56dc36f01 25 //CANMsg
Tafkal 7:83d56dc36f01 26 CANMessage msgDTA1; // RPM, TPS %, Water temp C, Air temp C
Tafkal 7:83d56dc36f01 27 CANMessage msgDTA2; // MAP Kpa, Lambda x1000, KPH x10, Oil P Kpa
Tafkal 7:83d56dc36f01 28 CANMessage msgDTA3; // Fuel P Kpa, Oil temp C, Volts x10, Fuel Con. L/Hr x10
Tafkal 7:83d56dc36f01 29 CANMessage msgDTA4; // Gear, Advance Deg x10, Injection ms x100, Fuel Con L/100km x10
Tafkal 7:83d56dc36f01 30 CANMessage msgDTA5; // Ana1 mV, Ana2 mV, Ana3 mV, Cam Advance x10
Tafkal 7:83d56dc36f01 31 CANMessage msgDTA6; // Cam Targ x10, Cam PWM x10, Crank Errors, Cam Errors
Tafkal 7:83d56dc36f01 32 CANMessage msgLVDTFront; // Left, Right, Steering Wheel
Tafkal 7:83d56dc36f01 33 CANMessage msgLVDTRear; // Left, Right
Tafkal 7:83d56dc36f01 34 CANMessage msgBrakes; // Brake system preassure, Braking On/Off
Tafkal 8:03e50b5d9d10 35 CANMessage msgKm; // KM
Tafkal 7:83d56dc36f01 36 CANMessage msgGPS; // GPS position data
Tafkal 7:83d56dc36f01 37 CANMessage msgGForce; // Accel data
Tafkal 7:83d56dc36f01 38 CANMessage msgGyro; // Gyro data
Tafkal 8:03e50b5d9d10 39 CANMessage msgTest;
Tafkal 8:03e50b5d9d10 40 char* temp_string; // Temp data
formulas 10:2e912e37ee6d 41 char* msgGyro1; // Temp data
formulas 10:2e912e37ee6d 42 char* msgGForce1; // Temp data
nemanja1994 12:3c500a06c5f3 43 char* msgGps1;
Tafkal 7:83d56dc36f01 44 int CANlen = 0;
Tafkal 7:83d56dc36f01 45 //Some stuff for CAN
Tafkal 7:83d56dc36f01 46 // Variables received from DTA, LVDTs and brakes
nemanja1994 12:3c500a06c5f3 47 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;
nemanja1994 12:3c500a06c5f3 48 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;
Tafkal 7:83d56dc36f01 49 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;
Tafkal 7:83d56dc36f01 50 //Referrent LVDT values. First received value is referrent.
Tafkal 7:83d56dc36f01 51 int FL_LVDT_Ref,FR_LVDT_Ref,RL_LVDT_Ref,RR_LVDT_Ref;
Tafkal 7:83d56dc36f01 52 uint16_t rx_flag=0x0000; // Receive specific CAN data message
Tafkal 7:83d56dc36f01 53 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.
Tafkal 8:03e50b5d9d10 54 float kilometraza;
Tafkal 7:83d56dc36f01 55 /*-----------tRF Send Telemetry-----------*/
Tafkal 7:83d56dc36f01 56 void tRF_sendCAN(CANMessage SendMe) {
formulas 10:2e912e37ee6d 57 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]);
Tafkal 7:83d56dc36f01 58 }
Tafkal 7:83d56dc36f01 59
formulas 10:2e912e37ee6d 60
Tafkal 7:83d56dc36f01 61 /*------------------MAIN------------------*/
Tafkal 7:83d56dc36f01 62 int main() {
Tafkal 7:83d56dc36f01 63 // Attach interrupt function to CAN RX
Tafkal 7:83d56dc36f01 64 can.attach(&CANMsgReceive,CAN::RxIrq);
Tafkal 7:83d56dc36f01 65
Tafkal 7:83d56dc36f01 66 //10DOF INIT
Tafkal 7:83d56dc36f01 67 imu.reset();
Tafkal 8:03e50b5d9d10 68 // pc.printf("RESET DONE!");
Tafkal 7:83d56dc36f01 69 while(!imu.check()){
Tafkal 8:03e50b5d9d10 70 //pc.printf("ejbg");
Tafkal 7:83d56dc36f01 71 }
Tafkal 8:03e50b5d9d10 72 //pc.printf("CHECK DONE!");
Tafkal 7:83d56dc36f01 73 //10DOF SET MODE AND CALIBRATE
Tafkal 7:83d56dc36f01 74 imu.setmode(OPERATION_MODE_IMUPLUS);
Tafkal 7:83d56dc36f01 75 imu.get_calib();
Tafkal 7:83d56dc36f01 76 // Display sensor information
Tafkal 8:03e50b5d9d10 77 /*pc.printf("BNO055 found\r\n\r\n");
Tafkal 7:83d56dc36f01 78 pc.printf("Chip ID: %0z\r\n",imu.ID.id);
Tafkal 7:83d56dc36f01 79 pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
Tafkal 7:83d56dc36f01 80 pc.printf("Gyroscope ID: %0z\r\n",imu.ID.gyro);
Tafkal 7:83d56dc36f01 81 pc.printf("Magnetometer ID: %0z\r\n\r\n",imu.ID.mag);
Tafkal 7:83d56dc36f01 82 pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
Tafkal 8:03e50b5d9d10 83 pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);*/
Tafkal 7:83d56dc36f01 84 // Display chip serial number
Tafkal 8:03e50b5d9d10 85 /*for (int i = 0; i<4; i++){
Tafkal 7:83d56dc36f01 86 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]);
Tafkal 8:03e50b5d9d10 87 }*/
Tafkal 7:83d56dc36f01 88
Tafkal 8:03e50b5d9d10 89 //pc.printf("\r\n");
Tafkal 7:83d56dc36f01 90
Tafkal 7:83d56dc36f01 91 //tRF INIT
Tafkal 7:83d56dc36f01 92 tRF.printf("+++");
Tafkal 7:83d56dc36f01 93 tRF.printf("ATR\r");
Tafkal 7:83d56dc36f01 94 // Freq
Tafkal 7:83d56dc36f01 95 tRF.printf("ATS206=12\r");
Tafkal 7:83d56dc36f01 96 // Channel
Tafkal 7:83d56dc36f01 97 tRF.printf("ATS200=3\r");
Tafkal 7:83d56dc36f01 98 // Speed transmit uart
Tafkal 7:83d56dc36f01 99 tRF.printf("ATS210=5\r");
Tafkal 7:83d56dc36f01 100 // Radio baud
Tafkal 7:83d56dc36f01 101 tRF.printf("S201=4\r");
Tafkal 7:83d56dc36f01 102 // Go to send mode
Tafkal 7:83d56dc36f01 103 tRF.printf("ATO\r");
Tafkal 8:03e50b5d9d10 104 // pc.printf("tRF INIT END\n");
Tafkal 7:83d56dc36f01 105
Tafkal 7:83d56dc36f01 106 //SD CARD
Tafkal 8:03e50b5d9d10 107 //printf("Initialise\n");
Tafkal 7:83d56dc36f01 108 //FileSystemLike(*sd);
Tafkal 7:83d56dc36f01 109
Tafkal 7:83d56dc36f01 110 // call the SDBlockDevice instance initialisation method. (not needed)
Tafkal 7:83d56dc36f01 111 if ( sd.init() != 0) {
Tafkal 8:03e50b5d9d10 112 //printf("Init failed \n");
Tafkal 7:83d56dc36f01 113 errorCode(FATAL);
Tafkal 7:83d56dc36f01 114 }
Tafkal 7:83d56dc36f01 115
Tafkal 7:83d56dc36f01 116 //Create a filing system for SD Card
Tafkal 7:83d56dc36f01 117 FATFileSystem fs("sd", &sd);
Tafkal 7:83d56dc36f01 118
Tafkal 7:83d56dc36f01 119 // Open to WRITE
Tafkal 8:03e50b5d9d10 120 //printf("Write to a file\n");
nemanja1994 12:3c500a06c5f3 121 FILE* fp = fopen("/sd/FSRA18_LOG.txt","a");
nemanja1994 12:3c500a06c5f3 122 fprintf(fp, "\nNovi Podaci\n");
Tafkal 8:03e50b5d9d10 123 //FILE* km = fopen("/sd/kilometraza.txt","r+");
Tafkal 7:83d56dc36f01 124 if (fp == NULL) {
Tafkal 7:83d56dc36f01 125 error("Could not open file for read\n");
Tafkal 7:83d56dc36f01 126 errorCode(FATAL);
Tafkal 7:83d56dc36f01 127 }
Tafkal 8:03e50b5d9d10 128 /*if (km == NULL) {
Tafkal 8:03e50b5d9d10 129 error("Could not open file for read\n");
Tafkal 8:03e50b5d9d10 130 errorCode(FATAL);
Tafkal 8:03e50b5d9d10 131 }
Tafkal 8:03e50b5d9d10 132 fscanf(km, "%f", &kilometraza);*/
Tafkal 7:83d56dc36f01 133 //Write something to SD
Tafkal 8:03e50b5d9d10 134 //fputs("Drumska Strela!\r\n", fp);
Tafkal 7:83d56dc36f01 135 /*
Tafkal 7:83d56dc36f01 136 //Close File
Tafkal 7:83d56dc36f01 137 fclose(fp);
Tafkal 7:83d56dc36f01 138
Tafkal 7:83d56dc36f01 139 //Close down SD
Tafkal 7:83d56dc36f01 140 sd.deinit();
Tafkal 7:83d56dc36f01 141 printf("All done...\n");
Tafkal 7:83d56dc36f01 142 errorCode(OK);
Tafkal 7:83d56dc36f01 143 */
Tafkal 7:83d56dc36f01 144 /*----------------MAIN LOOP----------------*/
Tafkal 7:83d56dc36f01 145 while (true) {
formulas 9:1fbb0aae6a32 146 UpdateInfo();
formulas 9:1fbb0aae6a32 147
nemanja1994 12:3c500a06c5f3 148 //printf("In while\n");
Tafkal 7:83d56dc36f01 149 //GET YPR
Tafkal 7:83d56dc36f01 150 imu.get_angles();
nemanja1994 12:3c500a06c5f3 151 //pc.printf("Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
Tafkal 7:83d56dc36f01 152 fprintf(fp, "Yaw: %5.1f Pitch: %5.1f Roll: %5.1f\r\n", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
Tafkal 7:83d56dc36f01 153 //GET ACCEL
Tafkal 7:83d56dc36f01 154 imu.get_accel();
nemanja1994 12:3c500a06c5f3 155 // pc.printf("ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
Tafkal 7:83d56dc36f01 156 fprintf(fp, "ACX: %5.1f ACY: %5.1f ACZ: %5.1f\r\n", imu.accel.x, imu.accel.y, imu.accel.z);
Tafkal 7:83d56dc36f01 157 //GET GPS DATA
Tafkal 8:03e50b5d9d10 158 //printf("GPS\n");
Tafkal 7:83d56dc36f01 159 NMEA_TYPE = gps.sample();
Tafkal 7:83d56dc36f01 160 if (1) {
Tafkal 8:03e50b5d9d10 161 //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);
Tafkal 7:83d56dc36f01 162 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);
Tafkal 7:83d56dc36f01 163 }
Tafkal 7:83d56dc36f01 164 //CAN TEST
Tafkal 8:03e50b5d9d10 165 //printf("CANUpdate\n");
Tafkal 7:83d56dc36f01 166 //Send telemetry data
Tafkal 8:03e50b5d9d10 167 //tRF.printf("POYY KEJTERI\n");
Tafkal 7:83d56dc36f01 168 //Store data
formulas 10:2e912e37ee6d 169 //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",\
formulas 10:2e912e37ee6d 170 //Rpm0,Speed0,Gear0,Water_Temp0,Oil_Temp0,TPS0,Brakes0,Oil_P0,MAP0,Air_Temp0,Lambda0,Volts0,Crank0);
formulas 10:2e912e37ee6d 171 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",\
formulas 10:2e912e37ee6d 172 Rpm,Speed,Gear,Water_Temp,Oil_Temp,TPS,MAP,Air_Temp,Lambda,Volts);
nemanja1994 12:3c500a06c5f3 173 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",\
nemanja1994 12:3c500a06c5f3 174 Steer, FL_LVDT,FR_LVDT,RL_LVDT,RR_LVDT,BF_Oil_P,BR_Oil_P,Brakes);
Tafkal 7:83d56dc36f01 175
formulas 9:1fbb0aae6a32 176
formulas 9:1fbb0aae6a32 177 //pc.printf("id=2001, MAP= %d, Lambda= %d, Speed= %d, Oil_P= %d",Lambda,Speed,Oil_P);
formulas 9:1fbb0aae6a32 178
Tafkal 7:83d56dc36f01 179 //tRF SendAll
Tafkal 8:03e50b5d9d10 180 //printf("sendCAN\n");
formulas 10:2e912e37ee6d 181
formulas 10:2e912e37ee6d 182
formulas 10:2e912e37ee6d 183
formulas 10:2e912e37ee6d 184 tRF_sendCAN(msgDTA1);
Tafkal 7:83d56dc36f01 185 tRF_sendCAN(msgDTA2);
Tafkal 7:83d56dc36f01 186 tRF_sendCAN(msgDTA3);
Tafkal 7:83d56dc36f01 187 tRF_sendCAN(msgDTA4);
Tafkal 7:83d56dc36f01 188 tRF_sendCAN(msgDTA5);
Tafkal 7:83d56dc36f01 189 tRF_sendCAN(msgDTA6);
Tafkal 7:83d56dc36f01 190 tRF_sendCAN(msgLVDTFront);
Tafkal 7:83d56dc36f01 191 tRF_sendCAN(msgLVDTRear);
formulas 10:2e912e37ee6d 192 tRF_sendCAN(msgBrakes);
formulas 10:2e912e37ee6d 193
formulas 10:2e912e37ee6d 194
formulas 10:2e912e37ee6d 195
nemanja1994 12:3c500a06c5f3 196 sprintf(msgGyro1, "S1011%.2fY%.2fP%.2fR", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
formulas 10:2e912e37ee6d 197 //pc.printf("%s\n",temp_string);
formulas 10:2e912e37ee6d 198 tRF.printf("%s\n",msgGyro1);
nemanja1994 12:3c500a06c5f3 199 sprintf(msgGForce1, "S1010%.2fX%.2fY%.2fZ", imu.accel.x, imu.accel.y, imu.accel.z);
formulas 10:2e912e37ee6d 200 tRF.printf("%s\n",msgGForce1);
nemanja1994 12:3c500a06c5f3 201 //sprintf(msgGps1, "S1012%.5fN%.5fE%dSIV", gps.get_nmea_longitude(), gps.get_dec_latitude(),gps.get_satelites());
nemanja1994 12:3c500a06c5f3 202 //tRF.printf("%s\n",msgGps1);
formulas 10:2e912e37ee6d 203 /*while(1) {
Tafkal 8:03e50b5d9d10 204 sprintf(temp_string, "401236789");
Tafkal 8:03e50b5d9d10 205 CANlen = strlen(temp_string);
Tafkal 8:03e50b5d9d10 206 tRF_sendCAN(CANMessage(0x2001, temp_string, CANlen));
Tafkal 8:03e50b5d9d10 207 wait(1);
Tafkal 8:03e50b5d9d10 208 tRF.printf("S20012D123678\n");
Tafkal 8:03e50b5d9d10 209 }*/
Tafkal 8:03e50b5d9d10 210 /*sprintf(temp_string, "%.5fN%.5fE", gps.get_nmea_longitude(), gps.get_dec_latitude());
Tafkal 7:83d56dc36f01 211 CANlen = strlen(temp_string);
Tafkal 7:83d56dc36f01 212 msgGPS = CANMessage(0x0009, temp_string, CANlen);
Tafkal 7:83d56dc36f01 213 tRF_sendCAN(msgGPS);
Tafkal 7:83d56dc36f01 214 sprintf(temp_string, "%.5fY%.5fP%.5fR", imu.euler.yaw, imu.euler.pitch, imu.euler.roll);
Tafkal 7:83d56dc36f01 215 CANlen = strlen(temp_string);
Tafkal 7:83d56dc36f01 216 msgGForce= CANMessage(0x0010, temp_string, CANlen);
Tafkal 7:83d56dc36f01 217 tRF_sendCAN(msgGForce);
Tafkal 7:83d56dc36f01 218 sprintf(temp_string, "%.5fX%.5fY%.5fZ", imu.accel.x, imu.accel.y, imu.accel.z);
Tafkal 7:83d56dc36f01 219 CANlen = strlen(temp_string);
Tafkal 8:03e50b5d9d10 220 msgGyro= CANMessage(0x0011, temp_string, CANlen);*/
Tafkal 7:83d56dc36f01 221
Tafkal 7:83d56dc36f01 222 }
Tafkal 7:83d56dc36f01 223 }