Stefan Sofijanić / Mbed 2 deprecated IntegrationCAN_15feb

15.02.2019. revision

Dependencies:   mbed LCD_DISCO_F469NIa SD_DISCO_F469NI BSP_DISCO_F469NIa EEPROM_DISCO_F469NI

CAN_library.cpp@6:15433396d657, 2019-02-15 (annotated)

Committer:
Stefan_Sofijanic
Date:
Fri Feb 15 21:01:16 2019 +0000
Revision:
6:15433396d657
Parent:
5:4b63e6845a8e 
15.02.2019. revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
formulas 0:38c3afd99aeb 1 #include "CAN_library.h"
formulas 0:38c3afd99aeb 2
formulas 0:38c3afd99aeb 3 extern CAN can;
formulas 0:38c3afd99aeb 4 extern Serial pc;
formulas 2:f54f01527ebb 5 extern SD_DISCO_F469NI sd;
formulas 2:f54f01527ebb 6 //extern EEPROM_DISCO_F469NI eep;
formulas 2:f54f01527ebb 7 extern Timer t_int,t_store;
formulas 2:f54f01527ebb 8 extern CANMessage msgDTA1, msgDTA2, msgDTA3, msgDTA4, msgDTA5, msgDTA6, msgLVDTFront, msgLVDTRear, msgBrakes, msgDistance;
formulas 0:38c3afd99aeb 9 extern uint16_t rx_flag;
formulas 2:f54f01527ebb 10 extern uint8_t distance_flag;
formulas 0:38c3afd99aeb 11 extern uint8_t lvdtref;
formulas 0:38c3afd99aeb 12
formulas 4:0e8413277df8 13 extern uint16_t Rpm0, Speed0, Gear0, Water_Temp0, Oil_Temp0, TPS0, Brakes0, MAP0, Air_Temp0, Lambda0, Volts0;
formulas 4:0e8413277df8 14 extern uint16_t Rpm, Speed, Gear, Water_Temp, Oil_Temp, TPS, Brakes, MAP, Air_Temp, Lambda, Volts;
formulas 0:38c3afd99aeb 15 extern int FL_LVDT0, FR_LVDT0, RL_LVDT0, RR_LVDT0, FL_LVDT, FR_LVDT, RL_LVDT, RR_LVDT;
formulas 0:38c3afd99aeb 16 extern int FL_LVDT_Ref,FR_LVDT_Ref,RL_LVDT_Ref,RR_LVDT_Ref;
formulas 4:0e8413277df8 17 int LVDT_Max=1024;
formulas 4:0e8413277df8 18 //extern float Meter_counter,Meter_counter0;
nemanja1994 3:a06e417a5a2c 19 extern char cmd[3];
nemanja1994 3:a06e417a5a2c 20 extern int addr;
nemanja1994 3:a06e417a5a2c 21 extern I2C i2c;
nemanja1994 3:a06e417a5a2c 22
nemanja1994 3:a06e417a5a2c 23 extern void showLedRpm(int rpm);
nemanja1994 3:a06e417a5a2c 24
formulas 0:38c3afd99aeb 25
formulas 0:38c3afd99aeb 26 void CANMsgReceive(){ // CAN RX Interrupt Function
formulas 0:38c3afd99aeb 27 CANMessage tmpMsg;
formulas 0:38c3afd99aeb 28 if (can.read(tmpMsg)) { //Detect message
formulas 0:38c3afd99aeb 29 //pc.printf("%x\n",tmpMsg.id);
formulas 0:38c3afd99aeb 30 switch(tmpMsg.id){ //Find which DTA message is received
nemanja1994 3:a06e417a5a2c 31 case(0x2000):
formulas 0:38c3afd99aeb 32 rx_flag|=(1<<0);
formulas 0:38c3afd99aeb 33 msgDTA1=tmpMsg;
nemanja1994 3:a06e417a5a2c 34 //pc.printf("%x\n", tmpMsg.id);
formulas 0:38c3afd99aeb 35 break;
nemanja1994 3:a06e417a5a2c 36 case(0x2001):
formulas 0:38c3afd99aeb 37 rx_flag|=(1<<1);
formulas 0:38c3afd99aeb 38 msgDTA2=tmpMsg;
formulas 0:38c3afd99aeb 39 //pc.printf("rx_flag=%x\n",rx_flag);
formulas 0:38c3afd99aeb 40 break;
nemanja1994 3:a06e417a5a2c 41 case(0x2002):
formulas 0:38c3afd99aeb 42 rx_flag|=(1<<2);
formulas 0:38c3afd99aeb 43 msgDTA3=tmpMsg;
formulas 0:38c3afd99aeb 44 break;
nemanja1994 3:a06e417a5a2c 45 case(0x2003):
formulas 0:38c3afd99aeb 46 rx_flag|=(1<<3);
formulas 0:38c3afd99aeb 47 msgDTA4=tmpMsg;
formulas 0:38c3afd99aeb 48 break;
nemanja1994 3:a06e417a5a2c 49 case(0x2004):
formulas 0:38c3afd99aeb 50 rx_flag|=(1<<4);
formulas 0:38c3afd99aeb 51 msgDTA5=tmpMsg;
formulas 0:38c3afd99aeb 52 break;
nemanja1994 3:a06e417a5a2c 53 case(0x2005):
formulas 0:38c3afd99aeb 54 rx_flag|=(1<<5);
formulas 0:38c3afd99aeb 55 msgDTA6=tmpMsg;
formulas 0:38c3afd99aeb 56 break;
nemanja1994 3:a06e417a5a2c 57 case(0x1006):
formulas 0:38c3afd99aeb 58 rx_flag|=(1<<6);
formulas 0:38c3afd99aeb 59 msgLVDTFront=tmpMsg;
formulas 0:38c3afd99aeb 60 break;
nemanja1994 3:a06e417a5a2c 61 case(0x1007):
formulas 0:38c3afd99aeb 62 rx_flag|=(1<<7);
formulas 0:38c3afd99aeb 63 msgLVDTRear=tmpMsg;
formulas 0:38c3afd99aeb 64 break;
nemanja1994 3:a06e417a5a2c 65 case(0x1008):
formulas 0:38c3afd99aeb 66 rx_flag|=(1<<8);
formulas 0:38c3afd99aeb 67 msgBrakes=tmpMsg;
formulas 0:38c3afd99aeb 68 break;
nemanja1994 3:a06e417a5a2c 69 case(0x1009):
formulas 2:f54f01527ebb 70 rx_flag|=(1<<9);
formulas 2:f54f01527ebb 71 msgDistance=tmpMsg;
formulas 0:38c3afd99aeb 72 };
formulas 0:38c3afd99aeb 73 //pc.printf("rx_flag=%d\n",rx_flag);
formulas 0:38c3afd99aeb 74 };
formulas 0:38c3afd99aeb 75 };
formulas 0:38c3afd99aeb 76
formulas 2:f54f01527ebb 77 void UpdateInfo(){ // Update info for received values
formulas 2:f54f01527ebb 78 int noID=10;
formulas 0:38c3afd99aeb 79 uint16_t *newData;
formulas 0:38c3afd99aeb 80 for (int i=0;i<noID;i++){ //For loop goes trough Message IDs
formulas 0:38c3afd99aeb 81 uint16_t flagBit;
formulas 0:38c3afd99aeb 82 flagBit = rx_flag & (1<<i); //Set flag bit for adequate message. If there is a message waiting to be stored, flagBit will be different than 0.
formulas 0:38c3afd99aeb 83 if (flagBit) { // =/=0, there is a message, =0, there is no message
formulas 0:38c3afd99aeb 84 switch(i){
formulas 0:38c3afd99aeb 85 case(0):
formulas 0:38c3afd99aeb 86 newData=AllocData(msgDTA1); //Allocate message to a receiver
formulas 0:38c3afd99aeb 87 UpdateInfoDTA1(newData); //Update variables
formulas 0:38c3afd99aeb 88 rx_flag = rx_flag ^ (1<<0); //Set flag of adequate bit to 0
formulas 0:38c3afd99aeb 89 break;
formulas 0:38c3afd99aeb 90 case(1):
formulas 0:38c3afd99aeb 91 newData=AllocData(msgDTA2);
formulas 0:38c3afd99aeb 92 UpdateInfoDTA2(newData);
formulas 0:38c3afd99aeb 93 rx_flag = rx_flag ^ (1<<1);
formulas 0:38c3afd99aeb 94 break;
formulas 0:38c3afd99aeb 95 case(2):
formulas 0:38c3afd99aeb 96 newData=AllocData(msgDTA3);
formulas 0:38c3afd99aeb 97 UpdateInfoDTA3(newData);
formulas 0:38c3afd99aeb 98 rx_flag = rx_flag ^ (1<<2);
formulas 0:38c3afd99aeb 99 break;
formulas 0:38c3afd99aeb 100 case(3):
formulas 0:38c3afd99aeb 101 newData=AllocData(msgDTA4);
formulas 0:38c3afd99aeb 102 UpdateInfoDTA4(newData);
formulas 0:38c3afd99aeb 103 rx_flag = rx_flag ^ (1<<3);
formulas 0:38c3afd99aeb 104 break;
formulas 0:38c3afd99aeb 105 case(4):
formulas 0:38c3afd99aeb 106 newData=AllocData(msgDTA5);
formulas 0:38c3afd99aeb 107 UpdateInfoDTA5(newData);
formulas 0:38c3afd99aeb 108 rx_flag = rx_flag ^ (1<<4);
formulas 0:38c3afd99aeb 109 break;
formulas 0:38c3afd99aeb 110 case(5):
formulas 0:38c3afd99aeb 111 newData=AllocData(msgDTA6);
formulas 0:38c3afd99aeb 112 UpdateInfoDTA6(newData);
formulas 0:38c3afd99aeb 113 rx_flag = rx_flag ^ (1<<5);
formulas 0:38c3afd99aeb 114 break;
formulas 0:38c3afd99aeb 115 case(6):
formulas 0:38c3afd99aeb 116 newData=AllocData(msgLVDTFront);
formulas 0:38c3afd99aeb 117 UpdateInfoLVDTFront(newData);
formulas 0:38c3afd99aeb 118 rx_flag = rx_flag ^ (1<<6);
formulas 0:38c3afd99aeb 119 break;
formulas 0:38c3afd99aeb 120 case(7):
formulas 0:38c3afd99aeb 121 newData=AllocData(msgLVDTRear);
formulas 0:38c3afd99aeb 122 UpdateInfoLVDTRear(newData);
formulas 0:38c3afd99aeb 123 rx_flag = rx_flag ^ (1<<7);
formulas 0:38c3afd99aeb 124 break;
formulas 0:38c3afd99aeb 125 case(8):
formulas 0:38c3afd99aeb 126 newData=AllocData(msgBrakes);
formulas 0:38c3afd99aeb 127 UpdateInfoBrakes(newData);
formulas 0:38c3afd99aeb 128 rx_flag = rx_flag ^ (1<<8);
formulas 0:38c3afd99aeb 129 break;
formulas 2:f54f01527ebb 130 case(9):
formulas 2:f54f01527ebb 131 newData=AllocData(msgDistance);
formulas 4:0e8413277df8 132 //if(distance_flag==0){
formulas 4:0e8413277df8 133 // InitDistanceMeter(newData);
formulas 4:0e8413277df8 134 // distance_flag=1;
formulas 4:0e8413277df8 135 //};
formulas 2:f54f01527ebb 136 rx_flag = rx_flag ^ (1<<9);
formulas 0:38c3afd99aeb 137 // for each new id add new case statement
formulas 0:38c3afd99aeb 138 };
formulas 0:38c3afd99aeb 139 };
formulas 0:38c3afd99aeb 140 };
formulas 4:0e8413277df8 141 //UpdateMeterCounter();
formulas 0:38c3afd99aeb 142 };
formulas 0:38c3afd99aeb 143
formulas 0:38c3afd99aeb 144 uint16_t * AllocData(CANMessage msg){ //Unpack CAN message
formulas 0:38c3afd99aeb 145 uint16_t newData[4];
formulas 0:38c3afd99aeb 146 newData[0]=(msg.data[1]<<8) | msg.data[0];
formulas 0:38c3afd99aeb 147 newData[1]=(msg.data[3]<<8) | msg.data[2];
formulas 0:38c3afd99aeb 148 newData[2]=(msg.data[5]<<8) | msg.data[4];
formulas 0:38c3afd99aeb 149 newData[3]=(msg.data[7]<<8) | msg.data[6];
formulas 0:38c3afd99aeb 150 return newData;
formulas 0:38c3afd99aeb 151 };
formulas 0:38c3afd99aeb 152
formulas 4:0e8413277df8 153 /*void InitDistanceMeter(uint16_t newData[4]){
formulas 2:f54f01527ebb 154 uint32_t DistanceBuffer;
formulas 2:f54f01527ebb 155 DistanceBuffer=(uint32_t)((newData[1]<<16) | (newData[0]));
formulas 2:f54f01527ebb 156 Meter_counter=float(DistanceBuffer)*10;
formulas 4:0e8413277df8 157 //pc.printf("Init success, start km=%d,%f\n",DistanceBuffer,Meter_counter);
formulas 2:f54f01527ebb 158 t_int.start();
formulas 2:f54f01527ebb 159 t_store.start();
formulas 2:f54f01527ebb 160 };*/
formulas 2:f54f01527ebb 161
formulas 4:0e8413277df8 162
formulas 0:38c3afd99aeb 163 void UpdateInfoDTA1(uint16_t newData[4]){ //Update values for DTA1 message
formulas 5:4b63e6845a8e 164 if (newData[0]>=0 && newData[0]<=13000){
formulas 4:0e8413277df8 165 Rpm0=Rpm;
formulas 4:0e8413277df8 166 Rpm=newData[0];
formulas 4:0e8413277df8 167 showLedRpm(Rpm);
formulas 4:0e8413277df8 168 };
formulas 4:0e8413277df8 169 if (newData[1]>0 && newData[1]<100){
formulas 4:0e8413277df8 170 TPS0=TPS;
formulas 4:0e8413277df8 171 TPS=newData[1];
formulas 4:0e8413277df8 172 };
formulas 4:0e8413277df8 173 if (newData[2]>0 && newData[2]<1000){
formulas 4:0e8413277df8 174 Water_Temp0=Water_Temp;
formulas 4:0e8413277df8 175 Water_Temp=newData[2];
formulas 4:0e8413277df8 176 };
formulas 4:0e8413277df8 177 if (newData[3]>0 && newData[3]<1000){
formulas 4:0e8413277df8 178 Air_Temp0=Air_Temp;
formulas 4:0e8413277df8 179 Air_Temp=newData[3];
formulas 4:0e8413277df8 180 };
formulas 0:38c3afd99aeb 181 };
formulas 0:38c3afd99aeb 182
formulas 0:38c3afd99aeb 183 void UpdateInfoDTA2(uint16_t newData[4]){ //Update values for DTA2 message
formulas 4:0e8413277df8 184 if (newData[0]>0 && newData[0]<200){
formulas 4:0e8413277df8 185 MAP0=MAP;
formulas 4:0e8413277df8 186 MAP=newData[0];
formulas 4:0e8413277df8 187 };
formulas 4:0e8413277df8 188 if (newData[1]>0 && newData[2]<10000){
formulas 4:0e8413277df8 189 Lambda0=Lambda;
formulas 4:0e8413277df8 190 Lambda=newData[1];
formulas 4:0e8413277df8 191 };
formulas 5:4b63e6845a8e 192 if (newData[2]>=0 && newData[2]<1500){
formulas 5:4b63e6845a8e 193 if (abs(Speed-Speed0)>10 ){
formulas 5:4b63e6845a8e 194 Speed0=Speed;
formulas 5:4b63e6845a8e 195 };
formulas 4:0e8413277df8 196 Speed=newData[2];
formulas 4:0e8413277df8 197 };
formulas 0:38c3afd99aeb 198 };
formulas 0:38c3afd99aeb 199
formulas 0:38c3afd99aeb 200 void UpdateInfoDTA3(uint16_t newData[4]){ //Update values for DTA3 message
formulas 4:0e8413277df8 201 if (newData[1]>0 && newData[1]<200){
formulas 4:0e8413277df8 202 Oil_Temp0=Oil_Temp;
formulas 4:0e8413277df8 203 Oil_Temp=newData[1];
formulas 4:0e8413277df8 204 };
formulas 4:0e8413277df8 205 if (newData[2]>0 && newData[2]<200){
formulas 4:0e8413277df8 206 Volts0=Volts;
formulas 4:0e8413277df8 207 Volts=newData[2];
formulas 4:0e8413277df8 208 };
formulas 0:38c3afd99aeb 209 };
formulas 0:38c3afd99aeb 210
formulas 0:38c3afd99aeb 211 void UpdateInfoDTA4(uint16_t newData[4]){ //Update values for DTA4 message
formulas 5:4b63e6845a8e 212 if (newData[0]>=0 && newData[0]<=6){
formulas 4:0e8413277df8 213 Gear0=Gear;
formulas 4:0e8413277df8 214 Gear=newData[0];
formulas 4:0e8413277df8 215 };
formulas 0:38c3afd99aeb 216 };
formulas 0:38c3afd99aeb 217
formulas 0:38c3afd99aeb 218 void UpdateInfoDTA5(uint16_t newData[4]){ //Update values for DTA5 message
formulas 0:38c3afd99aeb 219 };
formulas 0:38c3afd99aeb 220
formulas 0:38c3afd99aeb 221 void UpdateInfoDTA6(uint16_t newData[4]){ //Update values for DTA6 message
formulas 0:38c3afd99aeb 222 };
formulas 0:38c3afd99aeb 223
formulas 4:0e8413277df8 224 /*void UpdateMeterCounter(){
formulas 2:f54f01527ebb 225 float ti=t_int.read();
formulas 2:f54f01527ebb 226 if(ti>0.1){
formulas 2:f54f01527ebb 227 Meter_counter0=Meter_counter;
formulas 2:f54f01527ebb 228 Meter_counter=Meter_counter+(float)(Speed*0.0277778); // 0.1*1000/3600
formulas 2:f54f01527ebb 229 t_int.reset();
formulas 2:f54f01527ebb 230 };
formulas 2:f54f01527ebb 231 if(t_store>2){
formulas 2:f54f01527ebb 232 char DistanceBuffer8[8];
formulas 2:f54f01527ebb 233 uint32_t DistanceBuffer32=uint32_t(Meter_counter);
formulas 2:f54f01527ebb 234 DistanceBuffer32=uint32_t(Meter_counter);
formulas 2:f54f01527ebb 235 DistanceBuffer8[0]=uint8_t(DistanceBuffer32>>24);
formulas 2:f54f01527ebb 236 DistanceBuffer8[1]=uint8_t((DistanceBuffer32 & (uint32_t)0x00FF0000)>>16);
formulas 2:f54f01527ebb 237 DistanceBuffer8[2]=uint8_t((DistanceBuffer32 & (uint32_t)0x0000FF00)>>8);
formulas 2:f54f01527ebb 238 DistanceBuffer8[3]=uint8_t((DistanceBuffer32 & (uint32_t)0x000000FF));
formulas 2:f54f01527ebb 239 CANMessage msg;
formulas 2:f54f01527ebb 240 if(can.write(CANMessage((uint16_t)DISTANCE,DistanceBuffer8, 8))) {
formulas 2:f54f01527ebb 241 pc.printf("%d\n",DistanceBuffer32);
formulas 2:f54f01527ebb 242 };
formulas 2:f54f01527ebb 243 t_store.reset();
formulas 2:f54f01527ebb 244
formulas 4:0e8413277df8 245 uint32_t distance_buffer;
formulas 4:0e8413277df8 246 distance_buffer=uint32_t(Meter_counter) ;
formulas 2:f54f01527ebb 247 pc.printf("distance=%d\n",distance_buffer);
formulas 2:f54f01527ebb 248 distance_buffer=0xFF000000;
formulas 2:f54f01527ebb 249 if(sd.WriteBlocks((uint32_t*)distance_buffer,DISTANCE_MEMORY_ADDR,1,SD_DATATIMEOUT)){
formulas 2:f54f01527ebb 250 pc.printf("Storage success\n");
formulas 2:f54f01527ebb 251 }else{
formulas 2:f54f01527ebb 252 pc.printf("Storage failed\n");
formulas 4:0e8413277df8 253 };
formulas 2:f54f01527ebb 254
formulas 4:0e8413277df8 255 uint32_t DistanceBuffer32;
formulas 2:f54f01527ebb 256 uint8_t DistanceBuffer8[4];
formulas 2:f54f01527ebb 257 DistanceBuffer32=uint32_t(Meter_counter);
formulas 2:f54f01527ebb 258 DistanceBuffer8[0]=DistanceBuffer32>>24;
formulas 2:f54f01527ebb 259 DistanceBuffer8[1]=(DistanceBuffer32 & (uint32_t)0x00FF0000)>>16;
formulas 2:f54f01527ebb 260 DistanceBuffer8[2]=(DistanceBuffer32 & (uint32_t)0x0000FF00)>>8;
formulas 2:f54f01527ebb 261 DistanceBuffer8[3]=(DistanceBuffer32 & (uint32_t)0x000000FF);
formulas 2:f54f01527ebb 262 if(eep.WriteBuffer(DistanceBuffer8,0,4)){
formulas 2:f54f01527ebb 263 pc.printf("Write success,Distance=%d\n",DistanceBuffer32);
formulas 4:0e8413277df8 264 };
formulas 2:f54f01527ebb 265 };
formulas 2:f54f01527ebb 266 };
formulas 2:f54f01527ebb 267
formulas 2:f54f01527ebb 268 void send(uint16_t ID, char dta[8]) {
formulas 2:f54f01527ebb 269 if(can.write(CANMessage((uint16_t)ID,dta, 8))) {
formulas 2:f54f01527ebb 270 printf("%x\n",ID);
formulas 2:f54f01527ebb 271 printf("Message sent: info1=%x%x, info2=%x%x, info3=%x%x, info4=%x%x\n", dta[1],dta[0],dta[3],dta[2],dta[5],dta[4],dta[7],dta[6]);
formulas 2:f54f01527ebb 272 }else{
formulas 2:f54f01527ebb 273 printf("Shit\n");
formulas 2:f54f01527ebb 274 }
formulas 4:0e8413277df8 275 };*/
formulas 2:f54f01527ebb 276
formulas 0:38c3afd99aeb 277 void UpdateInfoLVDTFront(uint16_t newData[4]){ //Update values for LVDTFront message
formulas 4:0e8413277df8 278 if (newData[0]>0 && newData[0]<=1024){
formulas 4:0e8413277df8 279 FL_LVDT0=FL_LVDT; //LVDTs are firstly written their voltage value, then overwritten by calculated pertentage value.
formulas 4:0e8413277df8 280 FL_LVDT=newData[0];
formulas 4:0e8413277df8 281 if(!(lvdtref & (1<<3))){ //Check if its the first time LVDT value is received. If yes than store it as Referrent value.
formulas 4:0e8413277df8 282 if (FL_LVDT>FL_LVDT_Ref){
formulas 4:0e8413277df8 283 FL_LVDT=(FL_LVDT-FL_LVDT_Ref)*100/(LVDT_Max-FL_LVDT_Ref);
formulas 4:0e8413277df8 284 }else{
formulas 4:0e8413277df8 285 FL_LVDT=(FL_LVDT-FL_LVDT_Ref)*100/FL_LVDT_Ref;
formulas 4:0e8413277df8 286 };
formulas 4:0e8413277df8 287 }else{
formulas 4:0e8413277df8 288 FL_LVDT_Ref=FL_LVDT;
formulas 4:0e8413277df8 289 FL_LVDT=0;
formulas 4:0e8413277df8 290 lvdtref=lvdtref^(1<<3);
formulas 0:38c3afd99aeb 291 };
formulas 0:38c3afd99aeb 292 };
formulas 4:0e8413277df8 293
formulas 4:0e8413277df8 294 if (newData[1]>0 && newData[1]<=1024){
formulas 4:0e8413277df8 295 FR_LVDT0=FR_LVDT;
formulas 4:0e8413277df8 296 FR_LVDT=newData[1];
formulas 4:0e8413277df8 297 if(!(lvdtref & (1<<2))){
formulas 4:0e8413277df8 298 if (FR_LVDT>FR_LVDT_Ref){
formulas 0:38c3afd99aeb 299 FR_LVDT=(FR_LVDT-FR_LVDT_Ref)*100/(LVDT_Max-FR_LVDT_Ref);
formulas 4:0e8413277df8 300 }else{
formulas 4:0e8413277df8 301 FR_LVDT=(FR_LVDT-FR_LVDT_Ref)*100/FR_LVDT_Ref;
formulas 4:0e8413277df8 302 };
formulas 4:0e8413277df8 303 }else{
formulas 4:0e8413277df8 304 FR_LVDT_Ref=FR_LVDT;
formulas 4:0e8413277df8 305 FR_LVDT=0;
formulas 4:0e8413277df8 306 lvdtref=lvdtref^(1<<2);
formulas 0:38c3afd99aeb 307 };
formulas 0:38c3afd99aeb 308 };
formulas 0:38c3afd99aeb 309 };
formulas 0:38c3afd99aeb 310
formulas 0:38c3afd99aeb 311 void UpdateInfoLVDTRear(uint16_t newData[4]){ //Update values for LVDTRear message
formulas 4:0e8413277df8 312 if (newData[0]>0 && newData[0]<=1024){
formulas 4:0e8413277df8 313 RL_LVDT0=RL_LVDT;
formulas 4:0e8413277df8 314 RL_LVDT=newData[0];
formulas 4:0e8413277df8 315 if(!(lvdtref & (1<<1))){
formulas 4:0e8413277df8 316 if (RL_LVDT>RL_LVDT_Ref){
formulas 4:0e8413277df8 317 RL_LVDT=(RL_LVDT-RL_LVDT_Ref)*100/(LVDT_Max-RL_LVDT_Ref);
formulas 4:0e8413277df8 318 }else{
formulas 4:0e8413277df8 319 RL_LVDT=(RL_LVDT-RL_LVDT_Ref)*100/RL_LVDT_Ref;
formulas 4:0e8413277df8 320 };
formulas 4:0e8413277df8 321 }else{
formulas 4:0e8413277df8 322 RL_LVDT_Ref=RL_LVDT;
formulas 4:0e8413277df8 323 RL_LVDT=0;
formulas 4:0e8413277df8 324 lvdtref=lvdtref^(1<<1);
formulas 0:38c3afd99aeb 325 };
formulas 0:38c3afd99aeb 326 };
formulas 4:0e8413277df8 327
formulas 4:0e8413277df8 328 if (newData[1]>0 && newData[1]<=1024){
formulas 4:0e8413277df8 329 RR_LVDT0=RR_LVDT;
formulas 4:0e8413277df8 330 RR_LVDT=newData[1];
formulas 4:0e8413277df8 331 if(!(lvdtref & (1<<0))){
formulas 4:0e8413277df8 332 if (RR_LVDT>RR_LVDT_Ref){
formulas 4:0e8413277df8 333 RR_LVDT=(RR_LVDT-RR_LVDT_Ref)*100/(LVDT_Max-RR_LVDT_Ref);
formulas 4:0e8413277df8 334 }else{
formulas 4:0e8413277df8 335 RR_LVDT=(RR_LVDT-RR_LVDT_Ref)*100/RR_LVDT_Ref;
formulas 4:0e8413277df8 336 };
formulas 4:0e8413277df8 337 }else{
formulas 4:0e8413277df8 338 RR_LVDT_Ref=RR_LVDT;
formulas 4:0e8413277df8 339 RR_LVDT=0;
formulas 4:0e8413277df8 340 lvdtref=lvdtref^(1<<0);
formulas 0:38c3afd99aeb 341 };
formulas 0:38c3afd99aeb 342 };
formulas 0:38c3afd99aeb 343 };
formulas 0:38c3afd99aeb 344
formulas 0:38c3afd99aeb 345 void UpdateInfoBrakes(uint16_t newData[4]){ //Update values for Brakes message
formulas 4:0e8413277df8 346 //if (newData[2]==0xFF || newData[2]==0x00){
formulas 4:0e8413277df8 347 Brakes0=Brakes;
formulas 4:0e8413277df8 348 Brakes=newData[2];
formulas 4:0e8413277df8 349 //pc.printf("%x\n",Brakes);
formulas 4:0e8413277df8 350 // };
formulas 0:38c3afd99aeb 351 };