Diff: main.cpp

Revision:

7:83d56dc36f01

Child:

8:03e50b5d9d10

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 25 08:43:42 2018 +0000
@@ -0,0 +1,184 @@
+/*------------------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);
+       
+    }
+}
\ No newline at end of file