Stefan Vukcevic / Mbed 2 deprecated MAIN_UNIT_FSRA

asd

Dependencies:   BNO055 GPS IntegrationCAN SD_CARD_TWO mbed

Fork of BNO055 by Dave Turner

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 
LOL

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
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 }