aasda
Dependencies: BNO055 GPS IntegrationCANajnovije SD_CARD_TWO mbed
Fork of MAIN_UNIT_FSRA by
main.cpp@8:03e50b5d9d10, 2018-06-17 (annotated)
- Committer:
- Tafkal
- Date:
- Sun Jun 17 11:59:39 2018 +0000
- Revision:
- 8:03e50b5d9d10
- Parent:
- 7:83d56dc36f01
- Child:
- 9:1fbb0aae6a32
LOL
Who changed what in which revision?
User | Revision | Line number | New 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 |
Tafkal | 7:83d56dc36f01 | 17 | Serial pc(SERIAL_TX, SERIAL_RX, 9600); |
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 |
Tafkal | 8:03e50b5d9d10 | 21 | CAN can(PD_0, PD_1, 500000); |
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 |
Tafkal | 7:83d56dc36f01 | 41 | int CANlen = 0; |
Tafkal | 7:83d56dc36f01 | 42 | //Some stuff for CAN |
Tafkal | 7:83d56dc36f01 | 43 | // Variables received from DTA, LVDTs and brakes |
Tafkal | 7:83d56dc36f01 | 44 | 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; |
Tafkal | 7:83d56dc36f01 | 45 | 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; |
Tafkal | 7:83d56dc36f01 | 46 | 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 | 47 | //Referrent LVDT values. First received value is referrent. |
Tafkal | 7:83d56dc36f01 | 48 | int FL_LVDT_Ref,FR_LVDT_Ref,RL_LVDT_Ref,RR_LVDT_Ref; |
Tafkal | 7:83d56dc36f01 | 49 | uint16_t rx_flag=0x0000; // Receive specific CAN data message |
Tafkal | 7:83d56dc36f01 | 50 | 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 | 51 | float kilometraza; |
Tafkal | 7:83d56dc36f01 | 52 | /*-----------tRF Send Telemetry-----------*/ |
Tafkal | 7:83d56dc36f01 | 53 | void tRF_sendCAN(CANMessage SendMe) { |
Tafkal | 8:03e50b5d9d10 | 54 | tRF.printf("S%x%x\n", SendMe.id, SendMe.data); |
Tafkal | 7:83d56dc36f01 | 55 | } |
Tafkal | 7:83d56dc36f01 | 56 | |
Tafkal | 7:83d56dc36f01 | 57 | /*------------------MAIN------------------*/ |
Tafkal | 7:83d56dc36f01 | 58 | int main() { |
Tafkal | 7:83d56dc36f01 | 59 | // Attach interrupt function to CAN RX |
Tafkal | 7:83d56dc36f01 | 60 | can.attach(&CANMsgReceive,CAN::RxIrq); |
Tafkal | 7:83d56dc36f01 | 61 | |
Tafkal | 7:83d56dc36f01 | 62 | //10DOF INIT |
Tafkal | 7:83d56dc36f01 | 63 | imu.reset(); |
Tafkal | 8:03e50b5d9d10 | 64 | // pc.printf("RESET DONE!"); |
Tafkal | 7:83d56dc36f01 | 65 | while(!imu.check()){ |
Tafkal | 8:03e50b5d9d10 | 66 | //pc.printf("ejbg"); |
Tafkal | 7:83d56dc36f01 | 67 | } |
Tafkal | 8:03e50b5d9d10 | 68 | //pc.printf("CHECK DONE!"); |
Tafkal | 7:83d56dc36f01 | 69 | //10DOF SET MODE AND CALIBRATE |
Tafkal | 7:83d56dc36f01 | 70 | imu.setmode(OPERATION_MODE_IMUPLUS); |
Tafkal | 7:83d56dc36f01 | 71 | imu.get_calib(); |
Tafkal | 7:83d56dc36f01 | 72 | // Display sensor information |
Tafkal | 8:03e50b5d9d10 | 73 | /*pc.printf("BNO055 found\r\n\r\n"); |
Tafkal | 7:83d56dc36f01 | 74 | pc.printf("Chip ID: %0z\r\n",imu.ID.id); |
Tafkal | 7:83d56dc36f01 | 75 | pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel); |
Tafkal | 7:83d56dc36f01 | 76 | pc.printf("Gyroscope ID: %0z\r\n",imu.ID.gyro); |
Tafkal | 7:83d56dc36f01 | 77 | pc.printf("Magnetometer ID: %0z\r\n\r\n",imu.ID.mag); |
Tafkal | 7:83d56dc36f01 | 78 | pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]); |
Tafkal | 8:03e50b5d9d10 | 79 | pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);*/ |
Tafkal | 7:83d56dc36f01 | 80 | // Display chip serial number |
Tafkal | 8:03e50b5d9d10 | 81 | /*for (int i = 0; i<4; i++){ |
Tafkal | 7:83d56dc36f01 | 82 | 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 | 83 | }*/ |
Tafkal | 7:83d56dc36f01 | 84 | |
Tafkal | 8:03e50b5d9d10 | 85 | //pc.printf("\r\n"); |
Tafkal | 7:83d56dc36f01 | 86 | |
Tafkal | 7:83d56dc36f01 | 87 | //tRF INIT |
Tafkal | 7:83d56dc36f01 | 88 | tRF.printf("+++"); |
Tafkal | 7:83d56dc36f01 | 89 | tRF.printf("ATR\r"); |
Tafkal | 7:83d56dc36f01 | 90 | // Freq |
Tafkal | 7:83d56dc36f01 | 91 | tRF.printf("ATS206=12\r"); |
Tafkal | 7:83d56dc36f01 | 92 | // Channel |
Tafkal | 7:83d56dc36f01 | 93 | tRF.printf("ATS200=3\r"); |
Tafkal | 7:83d56dc36f01 | 94 | // Speed transmit uart |
Tafkal | 7:83d56dc36f01 | 95 | tRF.printf("ATS210=5\r"); |
Tafkal | 7:83d56dc36f01 | 96 | // Radio baud |
Tafkal | 7:83d56dc36f01 | 97 | tRF.printf("S201=4\r"); |
Tafkal | 7:83d56dc36f01 | 98 | // Go to send mode |
Tafkal | 7:83d56dc36f01 | 99 | tRF.printf("ATO\r"); |
Tafkal | 8:03e50b5d9d10 | 100 | // pc.printf("tRF INIT END\n"); |
Tafkal | 7:83d56dc36f01 | 101 | |
Tafkal | 7:83d56dc36f01 | 102 | //SD CARD |
Tafkal | 8:03e50b5d9d10 | 103 | //printf("Initialise\n"); |
Tafkal | 7:83d56dc36f01 | 104 | //FileSystemLike(*sd); |
Tafkal | 7:83d56dc36f01 | 105 | |
Tafkal | 7:83d56dc36f01 | 106 | // call the SDBlockDevice instance initialisation method. (not needed) |
Tafkal | 7:83d56dc36f01 | 107 | if ( sd.init() != 0) { |
Tafkal | 8:03e50b5d9d10 | 108 | //printf("Init failed \n"); |
Tafkal | 7:83d56dc36f01 | 109 | errorCode(FATAL); |
Tafkal | 7:83d56dc36f01 | 110 | } |
Tafkal | 7:83d56dc36f01 | 111 | |
Tafkal | 7:83d56dc36f01 | 112 | //Create a filing system for SD Card |
Tafkal | 7:83d56dc36f01 | 113 | FATFileSystem fs("sd", &sd); |
Tafkal | 7:83d56dc36f01 | 114 | |
Tafkal | 7:83d56dc36f01 | 115 | // Open to WRITE |
Tafkal | 8:03e50b5d9d10 | 116 | //printf("Write to a file\n"); |
Tafkal | 8:03e50b5d9d10 | 117 | FILE* fp = fopen("/sd/Test.txt","a"); |
Tafkal | 8:03e50b5d9d10 | 118 | //FILE* km = fopen("/sd/kilometraza.txt","r+"); |
Tafkal | 7:83d56dc36f01 | 119 | if (fp == NULL) { |
Tafkal | 7:83d56dc36f01 | 120 | error("Could not open file for read\n"); |
Tafkal | 7:83d56dc36f01 | 121 | errorCode(FATAL); |
Tafkal | 7:83d56dc36f01 | 122 | } |
Tafkal | 8:03e50b5d9d10 | 123 | /*if (km == NULL) { |
Tafkal | 8:03e50b5d9d10 | 124 | error("Could not open file for read\n"); |
Tafkal | 8:03e50b5d9d10 | 125 | errorCode(FATAL); |
Tafkal | 8:03e50b5d9d10 | 126 | } |
Tafkal | 8:03e50b5d9d10 | 127 | fscanf(km, "%f", &kilometraza);*/ |
Tafkal | 7:83d56dc36f01 | 128 | //Write something to SD |
Tafkal | 8:03e50b5d9d10 | 129 | //fputs("Drumska Strela!\r\n", fp); |
Tafkal | 7:83d56dc36f01 | 130 | /* |
Tafkal | 7:83d56dc36f01 | 131 | //Close File |
Tafkal | 7:83d56dc36f01 | 132 | fclose(fp); |
Tafkal | 7:83d56dc36f01 | 133 | |
Tafkal | 7:83d56dc36f01 | 134 | //Close down SD |
Tafkal | 7:83d56dc36f01 | 135 | sd.deinit(); |
Tafkal | 7:83d56dc36f01 | 136 | printf("All done...\n"); |
Tafkal | 7:83d56dc36f01 | 137 | errorCode(OK); |
Tafkal | 7:83d56dc36f01 | 138 | */ |
Tafkal | 7:83d56dc36f01 | 139 | /*----------------MAIN LOOP----------------*/ |
Tafkal | 7:83d56dc36f01 | 140 | while (true) { |
Tafkal | 8:03e50b5d9d10 | 141 | //printf("In while\n"); |
Tafkal | 7:83d56dc36f01 | 142 | //GET YPR |
Tafkal | 7:83d56dc36f01 | 143 | imu.get_angles(); |
Tafkal | 8:03e50b5d9d10 | 144 | //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 | 145 | 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 | 146 | //GET ACCEL |
Tafkal | 7:83d56dc36f01 | 147 | imu.get_accel(); |
Tafkal | 8:03e50b5d9d10 | 148 | //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 | 149 | 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 | 150 | //GET GPS DATA |
Tafkal | 8:03e50b5d9d10 | 151 | //printf("GPS\n"); |
Tafkal | 7:83d56dc36f01 | 152 | NMEA_TYPE = gps.sample(); |
Tafkal | 7:83d56dc36f01 | 153 | if (1) { |
Tafkal | 8:03e50b5d9d10 | 154 | //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 | 155 | 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 | 156 | } |
Tafkal | 7:83d56dc36f01 | 157 | //CAN TEST |
Tafkal | 8:03e50b5d9d10 | 158 | //printf("CANUpdate\n"); |
Tafkal | 7:83d56dc36f01 | 159 | UpdateInfo(); |
Tafkal | 7:83d56dc36f01 | 160 | //Send telemetry data |
Tafkal | 8:03e50b5d9d10 | 161 | //tRF.printf("POYY KEJTERI\n"); |
Tafkal | 7:83d56dc36f01 | 162 | //Store data |
Tafkal | 7:83d56dc36f01 | 163 | 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",\ |
Tafkal | 7:83d56dc36f01 | 164 | Rpm0,Speed0,Gear0,Water_Temp0,Oil_Temp0,TPS0,Brakes0,Oil_P0,MAP0,Air_Temp0,Lambda0,Volts0,Crank0); |
Tafkal | 7:83d56dc36f01 | 165 | 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",\ |
Tafkal | 7:83d56dc36f01 | 166 | Rpm,Speed,Gear,Water_Temp,Oil_Temp,TPS,Brakes,Oil_P,MAP,Air_Temp,Lambda,Volts,Crank); |
Tafkal | 7:83d56dc36f01 | 167 | 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",\ |
Tafkal | 7:83d56dc36f01 | 168 | FL_LVDT0,FR_LVDT0,RL_LVDT0,RR_LVDT0,FL_LVDT,FR_LVDT,RL_LVDT,RR_LVDT); |
Tafkal | 7:83d56dc36f01 | 169 | |
Tafkal | 7:83d56dc36f01 | 170 | //tRF SendAll |
Tafkal | 8:03e50b5d9d10 | 171 | //printf("sendCAN\n"); |
Tafkal | 7:83d56dc36f01 | 172 | tRF_sendCAN(msgDTA1); |
Tafkal | 7:83d56dc36f01 | 173 | tRF_sendCAN(msgDTA2); |
Tafkal | 7:83d56dc36f01 | 174 | tRF_sendCAN(msgDTA3); |
Tafkal | 7:83d56dc36f01 | 175 | tRF_sendCAN(msgDTA4); |
Tafkal | 7:83d56dc36f01 | 176 | tRF_sendCAN(msgDTA5); |
Tafkal | 7:83d56dc36f01 | 177 | tRF_sendCAN(msgDTA6); |
Tafkal | 7:83d56dc36f01 | 178 | tRF_sendCAN(msgLVDTFront); |
Tafkal | 7:83d56dc36f01 | 179 | tRF_sendCAN(msgLVDTRear); |
Tafkal | 8:03e50b5d9d10 | 180 | tRF_sendCAN(msgBrakes);/* |
Tafkal | 8:03e50b5d9d10 | 181 | while(1) { |
Tafkal | 8:03e50b5d9d10 | 182 | sprintf(temp_string, "401236789"); |
Tafkal | 8:03e50b5d9d10 | 183 | CANlen = strlen(temp_string); |
Tafkal | 8:03e50b5d9d10 | 184 | tRF_sendCAN(CANMessage(0x2001, temp_string, CANlen)); |
Tafkal | 8:03e50b5d9d10 | 185 | wait(1); |
Tafkal | 8:03e50b5d9d10 | 186 | tRF.printf("S20012D123678\n"); |
Tafkal | 8:03e50b5d9d10 | 187 | }*/ |
Tafkal | 8:03e50b5d9d10 | 188 | /*sprintf(temp_string, "%.5fN%.5fE", gps.get_nmea_longitude(), gps.get_dec_latitude()); |
Tafkal | 7:83d56dc36f01 | 189 | CANlen = strlen(temp_string); |
Tafkal | 7:83d56dc36f01 | 190 | msgGPS = CANMessage(0x0009, temp_string, CANlen); |
Tafkal | 7:83d56dc36f01 | 191 | tRF_sendCAN(msgGPS); |
Tafkal | 7:83d56dc36f01 | 192 | sprintf(temp_string, "%.5fY%.5fP%.5fR", imu.euler.yaw, imu.euler.pitch, imu.euler.roll); |
Tafkal | 7:83d56dc36f01 | 193 | CANlen = strlen(temp_string); |
Tafkal | 7:83d56dc36f01 | 194 | msgGForce= CANMessage(0x0010, temp_string, CANlen); |
Tafkal | 7:83d56dc36f01 | 195 | tRF_sendCAN(msgGForce); |
Tafkal | 7:83d56dc36f01 | 196 | sprintf(temp_string, "%.5fX%.5fY%.5fZ", imu.accel.x, imu.accel.y, imu.accel.z); |
Tafkal | 7:83d56dc36f01 | 197 | CANlen = strlen(temp_string); |
Tafkal | 8:03e50b5d9d10 | 198 | msgGyro= CANMessage(0x0011, temp_string, CANlen);*/ |
Tafkal | 7:83d56dc36f01 | 199 | |
Tafkal | 7:83d56dc36f01 | 200 | } |
Tafkal | 7:83d56dc36f01 | 201 | } |