![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
7 jul
Dependencies: BSP_DISCO_F469NIa EEPROM_DISCO_F469NI LCD_DISCO_F469NIa SD_DISCO_F469NI mbed
Fork of IntegrationCAN_7jul by
CAN_library.cpp@4:0e8413277df8, 2018-07-07 (annotated)
- Committer:
- formulas
- Date:
- Sat Jul 07 14:42:17 2018 +0000
- Revision:
- 4:0e8413277df8
- Parent:
- 3:a06e417a5a2c
7 jul
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:0e8413277df8 | 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 | 4:0e8413277df8 | 192 | if (newData[2]>0 && newData[2]<1500){ |
formulas | 4:0e8413277df8 | 193 | Speed0=Speed; |
formulas | 4:0e8413277df8 | 194 | Speed=newData[2]; |
formulas | 4:0e8413277df8 | 195 | }; |
formulas | 0:38c3afd99aeb | 196 | }; |
formulas | 0:38c3afd99aeb | 197 | |
formulas | 0:38c3afd99aeb | 198 | void UpdateInfoDTA3(uint16_t newData[4]){ //Update values for DTA3 message |
formulas | 4:0e8413277df8 | 199 | if (newData[1]>0 && newData[1]<200){ |
formulas | 4:0e8413277df8 | 200 | Oil_Temp0=Oil_Temp; |
formulas | 4:0e8413277df8 | 201 | Oil_Temp=newData[1]; |
formulas | 4:0e8413277df8 | 202 | }; |
formulas | 4:0e8413277df8 | 203 | if (newData[2]>0 && newData[2]<200){ |
formulas | 4:0e8413277df8 | 204 | Volts0=Volts; |
formulas | 4:0e8413277df8 | 205 | Volts=newData[2]; |
formulas | 4:0e8413277df8 | 206 | }; |
formulas | 0:38c3afd99aeb | 207 | }; |
formulas | 0:38c3afd99aeb | 208 | |
formulas | 0:38c3afd99aeb | 209 | void UpdateInfoDTA4(uint16_t newData[4]){ //Update values for DTA4 message |
formulas | 4:0e8413277df8 | 210 | if (newData[0]>0 && newData[0]<=6){ |
formulas | 4:0e8413277df8 | 211 | Gear0=Gear; |
formulas | 4:0e8413277df8 | 212 | Gear=newData[0]; |
formulas | 4:0e8413277df8 | 213 | }; |
formulas | 0:38c3afd99aeb | 214 | }; |
formulas | 0:38c3afd99aeb | 215 | |
formulas | 0:38c3afd99aeb | 216 | void UpdateInfoDTA5(uint16_t newData[4]){ //Update values for DTA5 message |
formulas | 0:38c3afd99aeb | 217 | }; |
formulas | 0:38c3afd99aeb | 218 | |
formulas | 0:38c3afd99aeb | 219 | void UpdateInfoDTA6(uint16_t newData[4]){ //Update values for DTA6 message |
formulas | 0:38c3afd99aeb | 220 | }; |
formulas | 0:38c3afd99aeb | 221 | |
formulas | 4:0e8413277df8 | 222 | /*void UpdateMeterCounter(){ |
formulas | 2:f54f01527ebb | 223 | float ti=t_int.read(); |
formulas | 2:f54f01527ebb | 224 | if(ti>0.1){ |
formulas | 2:f54f01527ebb | 225 | Meter_counter0=Meter_counter; |
formulas | 2:f54f01527ebb | 226 | Meter_counter=Meter_counter+(float)(Speed*0.0277778); // 0.1*1000/3600 |
formulas | 2:f54f01527ebb | 227 | t_int.reset(); |
formulas | 2:f54f01527ebb | 228 | }; |
formulas | 2:f54f01527ebb | 229 | if(t_store>2){ |
formulas | 2:f54f01527ebb | 230 | char DistanceBuffer8[8]; |
formulas | 2:f54f01527ebb | 231 | uint32_t DistanceBuffer32=uint32_t(Meter_counter); |
formulas | 2:f54f01527ebb | 232 | DistanceBuffer32=uint32_t(Meter_counter); |
formulas | 2:f54f01527ebb | 233 | DistanceBuffer8[0]=uint8_t(DistanceBuffer32>>24); |
formulas | 2:f54f01527ebb | 234 | DistanceBuffer8[1]=uint8_t((DistanceBuffer32 & (uint32_t)0x00FF0000)>>16); |
formulas | 2:f54f01527ebb | 235 | DistanceBuffer8[2]=uint8_t((DistanceBuffer32 & (uint32_t)0x0000FF00)>>8); |
formulas | 2:f54f01527ebb | 236 | DistanceBuffer8[3]=uint8_t((DistanceBuffer32 & (uint32_t)0x000000FF)); |
formulas | 2:f54f01527ebb | 237 | CANMessage msg; |
formulas | 2:f54f01527ebb | 238 | if(can.write(CANMessage((uint16_t)DISTANCE,DistanceBuffer8, 8))) { |
formulas | 2:f54f01527ebb | 239 | pc.printf("%d\n",DistanceBuffer32); |
formulas | 2:f54f01527ebb | 240 | }; |
formulas | 2:f54f01527ebb | 241 | t_store.reset(); |
formulas | 2:f54f01527ebb | 242 | |
formulas | 4:0e8413277df8 | 243 | uint32_t distance_buffer; |
formulas | 4:0e8413277df8 | 244 | distance_buffer=uint32_t(Meter_counter) ; |
formulas | 2:f54f01527ebb | 245 | pc.printf("distance=%d\n",distance_buffer); |
formulas | 2:f54f01527ebb | 246 | distance_buffer=0xFF000000; |
formulas | 2:f54f01527ebb | 247 | if(sd.WriteBlocks((uint32_t*)distance_buffer,DISTANCE_MEMORY_ADDR,1,SD_DATATIMEOUT)){ |
formulas | 2:f54f01527ebb | 248 | pc.printf("Storage success\n"); |
formulas | 2:f54f01527ebb | 249 | }else{ |
formulas | 2:f54f01527ebb | 250 | pc.printf("Storage failed\n"); |
formulas | 4:0e8413277df8 | 251 | }; |
formulas | 2:f54f01527ebb | 252 | |
formulas | 4:0e8413277df8 | 253 | uint32_t DistanceBuffer32; |
formulas | 2:f54f01527ebb | 254 | uint8_t DistanceBuffer8[4]; |
formulas | 2:f54f01527ebb | 255 | DistanceBuffer32=uint32_t(Meter_counter); |
formulas | 2:f54f01527ebb | 256 | DistanceBuffer8[0]=DistanceBuffer32>>24; |
formulas | 2:f54f01527ebb | 257 | DistanceBuffer8[1]=(DistanceBuffer32 & (uint32_t)0x00FF0000)>>16; |
formulas | 2:f54f01527ebb | 258 | DistanceBuffer8[2]=(DistanceBuffer32 & (uint32_t)0x0000FF00)>>8; |
formulas | 2:f54f01527ebb | 259 | DistanceBuffer8[3]=(DistanceBuffer32 & (uint32_t)0x000000FF); |
formulas | 2:f54f01527ebb | 260 | if(eep.WriteBuffer(DistanceBuffer8,0,4)){ |
formulas | 2:f54f01527ebb | 261 | pc.printf("Write success,Distance=%d\n",DistanceBuffer32); |
formulas | 4:0e8413277df8 | 262 | }; |
formulas | 2:f54f01527ebb | 263 | }; |
formulas | 2:f54f01527ebb | 264 | }; |
formulas | 2:f54f01527ebb | 265 | |
formulas | 2:f54f01527ebb | 266 | void send(uint16_t ID, char dta[8]) { |
formulas | 2:f54f01527ebb | 267 | if(can.write(CANMessage((uint16_t)ID,dta, 8))) { |
formulas | 2:f54f01527ebb | 268 | printf("%x\n",ID); |
formulas | 2:f54f01527ebb | 269 | 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 | 270 | }else{ |
formulas | 2:f54f01527ebb | 271 | printf("Shit\n"); |
formulas | 2:f54f01527ebb | 272 | } |
formulas | 4:0e8413277df8 | 273 | };*/ |
formulas | 2:f54f01527ebb | 274 | |
formulas | 0:38c3afd99aeb | 275 | void UpdateInfoLVDTFront(uint16_t newData[4]){ //Update values for LVDTFront message |
formulas | 4:0e8413277df8 | 276 | if (newData[0]>0 && newData[0]<=1024){ |
formulas | 4:0e8413277df8 | 277 | FL_LVDT0=FL_LVDT; //LVDTs are firstly written their voltage value, then overwritten by calculated pertentage value. |
formulas | 4:0e8413277df8 | 278 | FL_LVDT=newData[0]; |
formulas | 4:0e8413277df8 | 279 | 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 | 280 | if (FL_LVDT>FL_LVDT_Ref){ |
formulas | 4:0e8413277df8 | 281 | FL_LVDT=(FL_LVDT-FL_LVDT_Ref)*100/(LVDT_Max-FL_LVDT_Ref); |
formulas | 4:0e8413277df8 | 282 | }else{ |
formulas | 4:0e8413277df8 | 283 | FL_LVDT=(FL_LVDT-FL_LVDT_Ref)*100/FL_LVDT_Ref; |
formulas | 4:0e8413277df8 | 284 | }; |
formulas | 4:0e8413277df8 | 285 | }else{ |
formulas | 4:0e8413277df8 | 286 | FL_LVDT_Ref=FL_LVDT; |
formulas | 4:0e8413277df8 | 287 | FL_LVDT=0; |
formulas | 4:0e8413277df8 | 288 | lvdtref=lvdtref^(1<<3); |
formulas | 0:38c3afd99aeb | 289 | }; |
formulas | 0:38c3afd99aeb | 290 | }; |
formulas | 4:0e8413277df8 | 291 | |
formulas | 4:0e8413277df8 | 292 | if (newData[1]>0 && newData[1]<=1024){ |
formulas | 4:0e8413277df8 | 293 | FR_LVDT0=FR_LVDT; |
formulas | 4:0e8413277df8 | 294 | FR_LVDT=newData[1]; |
formulas | 4:0e8413277df8 | 295 | if(!(lvdtref & (1<<2))){ |
formulas | 4:0e8413277df8 | 296 | if (FR_LVDT>FR_LVDT_Ref){ |
formulas | 0:38c3afd99aeb | 297 | FR_LVDT=(FR_LVDT-FR_LVDT_Ref)*100/(LVDT_Max-FR_LVDT_Ref); |
formulas | 4:0e8413277df8 | 298 | }else{ |
formulas | 4:0e8413277df8 | 299 | FR_LVDT=(FR_LVDT-FR_LVDT_Ref)*100/FR_LVDT_Ref; |
formulas | 4:0e8413277df8 | 300 | }; |
formulas | 4:0e8413277df8 | 301 | }else{ |
formulas | 4:0e8413277df8 | 302 | FR_LVDT_Ref=FR_LVDT; |
formulas | 4:0e8413277df8 | 303 | FR_LVDT=0; |
formulas | 4:0e8413277df8 | 304 | lvdtref=lvdtref^(1<<2); |
formulas | 0:38c3afd99aeb | 305 | }; |
formulas | 0:38c3afd99aeb | 306 | }; |
formulas | 0:38c3afd99aeb | 307 | }; |
formulas | 0:38c3afd99aeb | 308 | |
formulas | 0:38c3afd99aeb | 309 | void UpdateInfoLVDTRear(uint16_t newData[4]){ //Update values for LVDTRear message |
formulas | 4:0e8413277df8 | 310 | if (newData[0]>0 && newData[0]<=1024){ |
formulas | 4:0e8413277df8 | 311 | RL_LVDT0=RL_LVDT; |
formulas | 4:0e8413277df8 | 312 | RL_LVDT=newData[0]; |
formulas | 4:0e8413277df8 | 313 | if(!(lvdtref & (1<<1))){ |
formulas | 4:0e8413277df8 | 314 | if (RL_LVDT>RL_LVDT_Ref){ |
formulas | 4:0e8413277df8 | 315 | RL_LVDT=(RL_LVDT-RL_LVDT_Ref)*100/(LVDT_Max-RL_LVDT_Ref); |
formulas | 4:0e8413277df8 | 316 | }else{ |
formulas | 4:0e8413277df8 | 317 | RL_LVDT=(RL_LVDT-RL_LVDT_Ref)*100/RL_LVDT_Ref; |
formulas | 4:0e8413277df8 | 318 | }; |
formulas | 4:0e8413277df8 | 319 | }else{ |
formulas | 4:0e8413277df8 | 320 | RL_LVDT_Ref=RL_LVDT; |
formulas | 4:0e8413277df8 | 321 | RL_LVDT=0; |
formulas | 4:0e8413277df8 | 322 | lvdtref=lvdtref^(1<<1); |
formulas | 0:38c3afd99aeb | 323 | }; |
formulas | 0:38c3afd99aeb | 324 | }; |
formulas | 4:0e8413277df8 | 325 | |
formulas | 4:0e8413277df8 | 326 | if (newData[1]>0 && newData[1]<=1024){ |
formulas | 4:0e8413277df8 | 327 | RR_LVDT0=RR_LVDT; |
formulas | 4:0e8413277df8 | 328 | RR_LVDT=newData[1]; |
formulas | 4:0e8413277df8 | 329 | if(!(lvdtref & (1<<0))){ |
formulas | 4:0e8413277df8 | 330 | if (RR_LVDT>RR_LVDT_Ref){ |
formulas | 4:0e8413277df8 | 331 | RR_LVDT=(RR_LVDT-RR_LVDT_Ref)*100/(LVDT_Max-RR_LVDT_Ref); |
formulas | 4:0e8413277df8 | 332 | }else{ |
formulas | 4:0e8413277df8 | 333 | RR_LVDT=(RR_LVDT-RR_LVDT_Ref)*100/RR_LVDT_Ref; |
formulas | 4:0e8413277df8 | 334 | }; |
formulas | 4:0e8413277df8 | 335 | }else{ |
formulas | 4:0e8413277df8 | 336 | RR_LVDT_Ref=RR_LVDT; |
formulas | 4:0e8413277df8 | 337 | RR_LVDT=0; |
formulas | 4:0e8413277df8 | 338 | lvdtref=lvdtref^(1<<0); |
formulas | 0:38c3afd99aeb | 339 | }; |
formulas | 0:38c3afd99aeb | 340 | }; |
formulas | 0:38c3afd99aeb | 341 | }; |
formulas | 0:38c3afd99aeb | 342 | |
formulas | 0:38c3afd99aeb | 343 | void UpdateInfoBrakes(uint16_t newData[4]){ //Update values for Brakes message |
formulas | 4:0e8413277df8 | 344 | //if (newData[2]==0xFF || newData[2]==0x00){ |
formulas | 4:0e8413277df8 | 345 | Brakes0=Brakes; |
formulas | 4:0e8413277df8 | 346 | Brakes=newData[2]; |
formulas | 4:0e8413277df8 | 347 | //pc.printf("%x\n",Brakes); |
formulas | 4:0e8413277df8 | 348 | // }; |
formulas | 0:38c3afd99aeb | 349 | }; |