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@2:f54f01527ebb, 2018-06-28 (annotated)

Committer:
formulas
Date:
Thu Jun 28 16:33:23 2018 +0000
Revision:
2:f54f01527ebb
Parent:
0:38c3afd99aeb
Child:
3:a06e417a5a2c
a

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