Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

Command.c@122:fbacb932a30b, 2016-04-07 (annotated)

Committer:
Kovalev_D
Date:
Thu Apr 07 16:45:25 2016 +0000
Revision:
122:fbacb932a30b
Parent:
121:bbae560cdd43
Child:
124:9ae09249f842
???? 115  .  ???? ?????? 921600 ????? (????????? ??? ?????????)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 0:8ad47e2b6f00 1 #include "stdlib.h"
igor_v 0:8ad47e2b6f00 2 #include "LPC17xx.h"
igor_v 0:8ad47e2b6f00 3 #include "Global.h"
Kovalev_D 106:250ddd8629c6 4 int tempNH,tempNL;
igor_v 0:8ad47e2b6f00 5 unsigned int Rate_Flag;
igor_v 0:8ad47e2b6f00 6 unsigned int CountBuFFIn;
igor_v 21:bc8c1cec3da6 7 unsigned int N=0,CRC_N;
igor_v 21:bc8c1cec3da6 8 unsigned int Param1=0;
igor_v 21:bc8c1cec3da6 9 unsigned int Param2=0;
igor_v 21:bc8c1cec3da6 10 unsigned int Param3=0;
igor_v 21:bc8c1cec3da6 11 unsigned int Param4=0;
Kovalev_D 102:4270092be987 12 unsigned int Consol=123;
Kovalev_D 121:bbae560cdd43 13 unsigned int TempParam=1;
igor_v 0:8ad47e2b6f00 14 /*
igor_v 21:bc8c1cec3da6 15 void exec_CMD(void) //r. === процедура окончательной дешифрации и исполнения команды
igor_v 0:8ad47e2b6f00 16 {
igor_v 0:8ad47e2b6f00 17 unsigned int Comand_code;
igor_v 21:bc8c1cec3da6 18
igor_v 21:bc8c1cec3da6 19 //rx_buf_copy = 1; //r. предустанов флага копирования приемного буфера
igor_v 0:8ad47e2b6f00 20
igor_v 0:8ad47e2b6f00 21 Comand_code = (rcv_buf[2] & 0xFF) << 8;
igor_v 21:bc8c1cec3da6 22 PC_Comand = Comand_code | (rcv_buf[3] & 0xFF); //e. save it in the memory for echo-transmission //r. сохранить его в памяти для обратной передачи
igor_v 0:8ad47e2b6f00 23 }*/
igor_v 0:8ad47e2b6f00 24 void CMD_Maintenance(void)
igor_v 0:8ad47e2b6f00 25 {
Kovalev_D 122:fbacb932a30b 26
Kovalev_D 122:fbacb932a30b 27 BuffTemp[0] = Gyro.SOC_Out;
igor_v 30:17c84ed091b3 28 BuffTemp[1] = Gyro .My_Addres;
igor_v 30:17c84ed091b3 29 BuffTemp[2] = Gyro .Firmware_Version;
igor_v 30:17c84ed091b3 30 BuffTemp[3] = Gyro .GLD_Serial;
igor_v 21:bc8c1cec3da6 31 BuffTemp[4]=0x00;
igor_v 21:bc8c1cec3da6 32 BuffTemp[5]=0x00;
igor_v 21:bc8c1cec3da6 33 Check(BuffTemp, 8);
igor_v 21:bc8c1cec3da6 34 WriteConN (BuffTemp,8);
Kovalev_D 121:bbae560cdd43 35
igor_v 0:8ad47e2b6f00 36 }
Kovalev_D 122:fbacb932a30b 37 void CMD_Rate3(void)
Kovalev_D 122:fbacb932a30b 38 {
Kovalev_D 122:fbacb932a30b 39 unsigned int Temp;
Kovalev_D 122:fbacb932a30b 40 BuffTemp[ 0] = Gyro.SOC_Out;
Kovalev_D 122:fbacb932a30b 41 BuffTemp[ 1] = Gyro.My_Addres;
igor_v 0:8ad47e2b6f00 42
Kovalev_D 122:fbacb932a30b 43 Temp=0x00;
Kovalev_D 122:fbacb932a30b 44 BuffTemp[ 2] =(Temp >> 8) & 0xff;//
Kovalev_D 122:fbacb932a30b 45 BuffTemp[ 3] =(Temp >> 0) & 0xff;//
Kovalev_D 122:fbacb932a30b 46 Temp=0x00;
Kovalev_D 122:fbacb932a30b 47 BuffTemp[ 4] =(Temp >> 8) & 0xff;//
Kovalev_D 122:fbacb932a30b 48 BuffTemp[ 5] =(Temp >> 0) & 0xff;//
Kovalev_D 122:fbacb932a30b 49
Kovalev_D 122:fbacb932a30b 50 Check(BuffTemp, 44);
Kovalev_D 122:fbacb932a30b 51 WriteConN (BuffTemp,44);
Kovalev_D 122:fbacb932a30b 52 }
Kovalev_D 121:bbae560cdd43 53 void CMD_Rate2(void)
Kovalev_D 121:bbae560cdd43 54 {
Kovalev_D 122:fbacb932a30b 55 Rate2VibFlag=0;
Kovalev_D 121:bbae560cdd43 56 BuffTemp[ 0] = Gyro.SOC_Out;
Kovalev_D 121:bbae560cdd43 57 BuffTemp[ 1] = Gyro.My_Addres;
Kovalev_D 121:bbae560cdd43 58
Kovalev_D 121:bbae560cdd43 59
Kovalev_D 121:bbae560cdd43 60 BuffTemp[ 2] =(Gyro.CaunPlus >> 8) & 0xff;//старший байт счетчика +.
Kovalev_D 121:bbae560cdd43 61 BuffTemp[ 3] =(Gyro.CaunPlus >> 0) & 0xff;//младший байт счетчика +.
Kovalev_D 121:bbae560cdd43 62
Kovalev_D 121:bbae560cdd43 63 BuffTemp[ 4] =(Gyro.CaunMin >> 8) & 0xff;//старший байт счетчика -.
Kovalev_D 121:bbae560cdd43 64 BuffTemp[ 5] =(Gyro.CaunMin >> 0) & 0xff;//младший байт счетчика -.
Kovalev_D 121:bbae560cdd43 65
Kovalev_D 122:fbacb932a30b 66
Kovalev_D 122:fbacb932a30b 67
Kovalev_D 122:fbacb932a30b 68
Kovalev_D 122:fbacb932a30b 69 Check(BuffTemp, 8);
Kovalev_D 122:fbacb932a30b 70 WriteConN (BuffTemp,8);
Kovalev_D 121:bbae560cdd43 71 }
igor_v 0:8ad47e2b6f00 72 void CMD_Rate(void)
igor_v 21:bc8c1cec3da6 73 {
Kovalev_D 107:4d178bcc9d8a 74
Kovalev_D 115:e5a230e5af52 75
Kovalev_D 108:030cdde08314 76 unsigned int Temp;
Kovalev_D 107:4d178bcc9d8a 77
igor_v 30:17c84ed091b3 78 Gyro.Firmware_Version=0xff; /// промежуточная затычка
Kovalev_D 122:fbacb932a30b 79
igor_v 0:8ad47e2b6f00 80
igor_v 30:17c84ed091b3 81 BuffTemp[ 0] = Gyro.SOC_Out;
igor_v 30:17c84ed091b3 82 BuffTemp[ 1] = Gyro.My_Addres;
igor_v 21:bc8c1cec3da6 83
Kovalev_D 108:030cdde08314 84
Kovalev_D 112:4a96133a1311 85 BuffTemp[ 2] =(Gyro.CaunPlus >> 8) & 0xff;//старший байт счетчика +.
Kovalev_D 112:4a96133a1311 86 BuffTemp[ 3] =(Gyro.CaunPlus >> 0) & 0xff;//младший байт счетчика +.
Kovalev_D 108:030cdde08314 87
Kovalev_D 112:4a96133a1311 88 BuffTemp[ 4] =(Gyro.CaunMin >> 8) & 0xff;//старший байт счетчика -.
Kovalev_D 112:4a96133a1311 89 BuffTemp[ 5] =(Gyro.CaunMin >> 0) & 0xff;//младший байт счетчика -.
Kovalev_D 108:030cdde08314 90
Kovalev_D 113:8be429494918 91
Kovalev_D 108:030cdde08314 92 Temp = Gyro.CuruAngle;
Kovalev_D 108:030cdde08314 93 Gyro.CuruAngle = 0;
Kovalev_D 113:8be429494918 94
Kovalev_D 108:030cdde08314 95 BuffTemp[ 6] =(Temp >> 8) & 0xff;//старший байт разности счетчиков
Kovalev_D 108:030cdde08314 96 BuffTemp[ 7] =(Temp >> 0) & 0xff;//младший байт разности счетчиков
Kovalev_D 108:030cdde08314 97
Kovalev_D 108:030cdde08314 98
Kovalev_D 122:fbacb932a30b 99 Temp = Gyro.F_ras;
Kovalev_D 122:fbacb932a30b 100 BuffTemp[ 8] = (Temp >> 8) & 0xff;//расщипление частота
Kovalev_D 122:fbacb932a30b 101 BuffTemp[ 9] = (Temp >> 0) & 0xff;//
igor_v 21:bc8c1cec3da6 102
Kovalev_D 108:030cdde08314 103 BuffTemp[10] = (Temp >> 8) & 0xff;//выход регулятора гвч;
Kovalev_D 122:fbacb932a30b 104 BuffTemp[11] = (Temp >> 0) & 0xff;
igor_v 21:bc8c1cec3da6 105
Kovalev_D 122:fbacb932a30b 106 Temp = (Gyro.AD_Slow >> 16)- 0x7fff;
Kovalev_D 108:030cdde08314 107 BuffTemp[12]=(Temp >> 8) & 0xff;////
Kovalev_D 108:030cdde08314 108 BuffTemp[13]=(Temp >> 0) & 0xff;////
igor_v 21:bc8c1cec3da6 109
Kovalev_D 108:030cdde08314 110
Kovalev_D 108:030cdde08314 111 Temp = (unsigned int)((7680000*16/(Gyro.Frq>>12)));
Kovalev_D 108:030cdde08314 112 BuffTemp[14] = (Temp >> 8) & 0xff;
Kovalev_D 108:030cdde08314 113 BuffTemp[15] = (Temp >> 0) & 0xff;
igor_v 21:bc8c1cec3da6 114
Kovalev_D 107:4d178bcc9d8a 115 BuffTemp[16] = 0xf;
Kovalev_D 106:250ddd8629c6 116 BuffTemp[17] = 0x02;
Kovalev_D 108:030cdde08314 117
igor_v 21:bc8c1cec3da6 118
Kovalev_D 108:030cdde08314 119 Temp = (unsigned int)(((7680000*16/200) *Gyro.AmpPer /(Gyro.Frq>>12)));
Kovalev_D 108:030cdde08314 120 BuffTemp[18] = (Temp >> 8) & 0xff;
Kovalev_D 108:030cdde08314 121 BuffTemp[19] = (Temp >> 0) & 0xff;
igor_v 21:bc8c1cec3da6 122
Kovalev_D 106:250ddd8629c6 123 BuffTemp[20] = 0x00;
Kovalev_D 106:250ddd8629c6 124 BuffTemp[21] = 0x00;
Kovalev_D 120:9f446f1495e8 125
Kovalev_D 119:4d7fcece9e8e 126
Kovalev_D 121:bbae560cdd43 127 // if (Spi.DAC_B < 0x7fff) Temp = Spi.DAC_B - 0x7fff; else Temp = 0x7fff - Spi.DAC_B ;
Kovalev_D 121:bbae560cdd43 128 Temp = Spi.DAC_B - 0x7fff;
Kovalev_D 108:030cdde08314 129 BuffTemp[22] = (Temp >> 8) & 0xff;
Kovalev_D 108:030cdde08314 130 BuffTemp[23] = (Temp >> 0) & 0xff;
igor_v 21:bc8c1cec3da6 131
Kovalev_D 121:bbae560cdd43 132
Kovalev_D 121:bbae560cdd43 133 Temp = Gyro.PLC_Delta >> 16;
Kovalev_D 121:bbae560cdd43 134 BuffTemp[24] = (Temp >> 8) & 0xff;
Kovalev_D 121:bbae560cdd43 135 BuffTemp[25] = (Temp >> 0) & 0xff;// ХЗ
Kovalev_D 113:8be429494918 136
Kovalev_D 121:bbae560cdd43 137 Temp = 100;
Kovalev_D 113:8be429494918 138 BuffTemp[26] = (Temp >> 8) & 0xff;
Kovalev_D 113:8be429494918 139 BuffTemp[27] = (Temp >> 0) & 0xff;
Kovalev_D 113:8be429494918 140
Kovalev_D 113:8be429494918 141 Temp = Gyro.AD_Slow >> 16;
Kovalev_D 113:8be429494918 142 BuffTemp[28] = (Temp >> 8) & 0xff;
Kovalev_D 113:8be429494918 143 BuffTemp[29] = (Temp >> 0) & 0xff;
Kovalev_D 113:8be429494918 144
Kovalev_D 113:8be429494918 145 Temp = Gyro.In1;
Kovalev_D 113:8be429494918 146 BuffTemp[30] = (Temp >> 8) & 0xff;//in1//2
Kovalev_D 113:8be429494918 147 BuffTemp[31] = (Temp >> 0) & 0xff;
Kovalev_D 113:8be429494918 148
Kovalev_D 113:8be429494918 149 Temp = Gyro.In2;
Kovalev_D 113:8be429494918 150 BuffTemp[32] = (Temp >> 8) & 0xff;
Kovalev_D 113:8be429494918 151 BuffTemp[33] = (Temp >> 0) & 0xff;//in2//3
Kovalev_D 113:8be429494918 152
Kovalev_D 113:8be429494918 153 Temp = Gyro.DeltaT;
Kovalev_D 113:8be429494918 154 BuffTemp[34] = (Temp >> 8) & 0xff;//дельта
Kovalev_D 113:8be429494918 155 BuffTemp[35] = (Temp >> 0) & 0xff;
Kovalev_D 115:e5a230e5af52 156
Kovalev_D 116:66f1f0ff2dab 157 Temp = Gyro.Termo;//0xa4=164
Kovalev_D 115:e5a230e5af52 158
Kovalev_D 113:8be429494918 159 BuffTemp[36] =(Temp >> 8) & 0xff;//температура
Kovalev_D 115:e5a230e5af52 160 BuffTemp[37] =(Temp >> 0) & 0xff;//ADC6 sesnsor 5
igor_v 21:bc8c1cec3da6 161
Kovalev_D 106:250ddd8629c6 162 BuffTemp[38] =0x00;
Kovalev_D 106:250ddd8629c6 163 BuffTemp[39] =0x00;
Kovalev_D 106:250ddd8629c6 164 BuffTemp[40] =0x00;
Kovalev_D 106:250ddd8629c6 165 BuffTemp[41] =0x00;
igor_v 21:bc8c1cec3da6 166
igor_v 21:bc8c1cec3da6 167
igor_v 21:bc8c1cec3da6 168 Check(BuffTemp, 44);
igor_v 21:bc8c1cec3da6 169 WriteConN (BuffTemp,44);
Kovalev_D 92:c892f0311aa7 170
igor_v 0:8ad47e2b6f00 171 }
igor_v 0:8ad47e2b6f00 172
igor_v 21:bc8c1cec3da6 173 void CMD_M_Control_D8()///установка\сброс регистров управления
igor_v 21:bc8c1cec3da6 174 {
Kovalev_D 104:ab1cb4ff56b2 175 unsigned int bit,NReg,Pa;
Kovalev_D 104:ab1cb4ff56b2 176 unsigned int SR,V,A,Bit_num;
Kovalev_D 104:ab1cb4ff56b2 177 SR=0;
Kovalev_D 104:ab1cb4ff56b2 178 V=0;
Kovalev_D 104:ab1cb4ff56b2 179 A=0;
Kovalev_D 104:ab1cb4ff56b2 180 Bit_num=0;
Kovalev_D 104:ab1cb4ff56b2 181
igor_v 30:17c84ed091b3 182 BuffTemp[0] = Gyro.SOC_Out; //DD
igor_v 30:17c84ed091b3 183 BuffTemp[1] = Gyro.My_Addres; //00
igor_v 30:17c84ed091b3 184 BuffTemp[2] = Gyro.CMD_In; //D8
Kovalev_D 104:ab1cb4ff56b2 185
Kovalev_D 104:ab1cb4ff56b2 186 /*
Kovalev_D 104:ab1cb4ff56b2 187
Kovalev_D 104:ab1cb4ff56b2 188 FrqON
Kovalev_D 104:ab1cb4ff56b2 189 PlcON
Kovalev_D 104:ab1cb4ff56b2 190 HFOOFF
Kovalev_D 104:ab1cb4ff56b2 191 HFOON
igor_v 21:bc8c1cec3da6 192
Kovalev_D 104:ab1cb4ff56b2 193 PlcOFF
Kovalev_D 104:ab1cb4ff56b2 194 PlcON
Kovalev_D 104:ab1cb4ff56b2 195 */
Kovalev_D 104:ab1cb4ff56b2 196 Pa = BuffTemp[3];
Kovalev_D 104:ab1cb4ff56b2 197
Kovalev_D 104:ab1cb4ff56b2 198 SR = Pa >> 0x7;
Kovalev_D 104:ab1cb4ff56b2 199
Kovalev_D 104:ab1cb4ff56b2 200 V = Pa >> 0x5;
Kovalev_D 104:ab1cb4ff56b2 201 V = V & 0x3;
Kovalev_D 104:ab1cb4ff56b2 202
Kovalev_D 104:ab1cb4ff56b2 203 A = Pa >> 0x4;
Kovalev_D 104:ab1cb4ff56b2 204 A = A & 0x1;
Kovalev_D 104:ab1cb4ff56b2 205
Kovalev_D 104:ab1cb4ff56b2 206 Bit_num = Pa & 0xf;
Kovalev_D 104:ab1cb4ff56b2 207
Kovalev_D 104:ab1cb4ff56b2 208 if(SR) {
Kovalev_D 104:ab1cb4ff56b2 209 switch (Bit_num){
Kovalev_D 105:bd01d8d20fb6 210
Kovalev_D 104:ab1cb4ff56b2 211 case 0x06:
Kovalev_D 104:ab1cb4ff56b2 212 FrqON
Kovalev_D 105:bd01d8d20fb6 213 break;
Kovalev_D 105:bd01d8d20fb6 214
Kovalev_D 104:ab1cb4ff56b2 215 case 0x03:
Kovalev_D 105:bd01d8d20fb6 216 HFOON
Kovalev_D 105:bd01d8d20fb6 217 break;
Kovalev_D 105:bd01d8d20fb6 218
Kovalev_D 105:bd01d8d20fb6 219 case 0x01:
Kovalev_D 105:bd01d8d20fb6 220 PlcON
Kovalev_D 105:bd01d8d20fb6 221 break;
Kovalev_D 104:ab1cb4ff56b2 222 }
Kovalev_D 104:ab1cb4ff56b2 223 }
Kovalev_D 104:ab1cb4ff56b2 224 else{
Kovalev_D 104:ab1cb4ff56b2 225 switch (Bit_num){
Kovalev_D 105:bd01d8d20fb6 226
Kovalev_D 105:bd01d8d20fb6 227 case 0x06:
Kovalev_D 104:ab1cb4ff56b2 228 FrqOff
Kovalev_D 105:bd01d8d20fb6 229 break;
Kovalev_D 105:bd01d8d20fb6 230
Kovalev_D 105:bd01d8d20fb6 231 case 0x03:
Kovalev_D 105:bd01d8d20fb6 232 HFOOFF
Kovalev_D 105:bd01d8d20fb6 233 break;
Kovalev_D 105:bd01d8d20fb6 234
Kovalev_D 105:bd01d8d20fb6 235 case 0x01:
Kovalev_D 105:bd01d8d20fb6 236 PlcOFF
Kovalev_D 105:bd01d8d20fb6 237 break;
Kovalev_D 104:ab1cb4ff56b2 238 }
Kovalev_D 104:ab1cb4ff56b2 239 }
igor_v 21:bc8c1cec3da6 240
igor_v 0:8ad47e2b6f00 241 }
igor_v 0:8ad47e2b6f00 242
igor_v 21:bc8c1cec3da6 243 void CMD_M_Control_D9()///чтение регистров управления
igor_v 21:bc8c1cec3da6 244 {
igor_v 21:bc8c1cec3da6 245 int bit,NReg;
igor_v 30:17c84ed091b3 246 BuffTemp[0] = Gyro.SOC_Out; //DD
igor_v 30:17c84ed091b3 247 BuffTemp[1] = Gyro.My_Addres; //00
igor_v 30:17c84ed091b3 248 BuffTemp[2] = Gyro.CMD_In; //D9
igor_v 21:bc8c1cec3da6 249 if ((Param1 & 0x10) == 0) {
igor_v 21:bc8c1cec3da6 250 BuffTemp[3]=0<<4;
igor_v 30:17c84ed091b3 251 BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
igor_v 30:17c84ed091b3 252 BuffTemp[5] = Gyro.RgConA & 0xff;
igor_v 21:bc8c1cec3da6 253 } else {
igor_v 21:bc8c1cec3da6 254 BuffTemp[3]=1<<4;
igor_v 30:17c84ed091b3 255 BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
igor_v 30:17c84ed091b3 256 BuffTemp[5] = Gyro.RgConB & 0xff;
igor_v 21:bc8c1cec3da6 257 }
igor_v 21:bc8c1cec3da6 258 Check(BuffTemp, CRC_N);
igor_v 21:bc8c1cec3da6 259 WriteConN (BuffTemp,CRC_N);
igor_v 21:bc8c1cec3da6 260 }
igor_v 21:bc8c1cec3da6 261 // (номер (код) ЦАПа 0...3, старший байт требуемого ЦАПа, младший байт треб ЦАПа)
igor_v 0:8ad47e2b6f00 262 void CMD_M_Stymul()
igor_v 0:8ad47e2b6f00 263 {
igor_v 21:bc8c1cec3da6 264 int temp;
Kovalev_D 102:4270092be987 265 int HFO;
Kovalev_D 102:4270092be987 266 temp=BuffTemp[3];
Kovalev_D 102:4270092be987 267 Consol = temp&0x3;
Kovalev_D 103:e96f08947def 268 HFO = temp&0x3;// // (номер (код) ЦАПа 0...3, HFO - 3dac, PLC - 0DAC
igor_v 21:bc8c1cec3da6 269 temp =((BuffTemp[4]<<8) | BuffTemp[5]);
igor_v 21:bc8c1cec3da6 270 // temp=0;
igor_v 21:bc8c1cec3da6 271 temp=temp&0xFFFF;
Kovalev_D 102:4270092be987 272
Kovalev_D 102:4270092be987 273 if(HFO)
Kovalev_D 102:4270092be987 274 {
Kovalev_D 102:4270092be987 275 Spi.DAC_A=(unsigned int)temp;
Kovalev_D 102:4270092be987 276 }
Kovalev_D 102:4270092be987 277 else{
igor_v 21:bc8c1cec3da6 278 DACF =(temp*K_DAC)+deltaDAC;
igor_v 21:bc8c1cec3da6 279 Spi.DAC_B =(unsigned int)(DACF) /*(unsigned int)(temp*K_DAC+deltaDAC)*/; // K_DAC);
igor_v 21:bc8c1cec3da6 280 }
igor_v 0:8ad47e2b6f00 281 }
Kovalev_D 103:e96f08947def 282 void Gph_W()
Kovalev_D 103:e96f08947def 283 {
Kovalev_D 108:030cdde08314 284 Out_G_photo(BuffTemp[4],BuffTemp[5]);
Kovalev_D 103:e96f08947def 285 }
igor_v 0:8ad47e2b6f00 286
Kovalev_D 98:95b8e79f13e1 287 void CMD_M_vib()
Kovalev_D 115:e5a230e5af52 288 {
Kovalev_D 102:4270092be987 289 unsigned int temp1,temp2;
Kovalev_D 102:4270092be987 290
Kovalev_D 102:4270092be987 291 temp1 =((BuffTemp[4]<<8) | BuffTemp[5]);
Kovalev_D 102:4270092be987 292 temp1=temp1&0xFFFF;
Kovalev_D 102:4270092be987 293
Kovalev_D 102:4270092be987 294 temp2 =((BuffTemp[6]<<8) | BuffTemp[7]);
Kovalev_D 102:4270092be987 295 temp2=temp2&0xFFFF;
Kovalev_D 102:4270092be987 296 Gyro.Frq = ((unsigned int) ((7680000*16/temp1)))*16*16*16;
Kovalev_D 102:4270092be987 297 Consol=Gyro.Frq ;
Kovalev_D 98:95b8e79f13e1 298 }
igor_v 0:8ad47e2b6f00 299 unsigned int Check(char *c, unsigned int Count)
igor_v 0:8ad47e2b6f00 300 {
igor_v 21:bc8c1cec3da6 301 int i=1;
igor_v 21:bc8c1cec3da6 302 unsigned int temp,CRC;
igor_v 21:bc8c1cec3da6 303
igor_v 21:bc8c1cec3da6 304
igor_v 21:bc8c1cec3da6 305 temp=1;
igor_v 21:bc8c1cec3da6 306 CRC=0;
igor_v 21:bc8c1cec3da6 307
igor_v 21:bc8c1cec3da6 308
igor_v 21:bc8c1cec3da6 309 for(; i<Count-2; i++) {
igor_v 21:bc8c1cec3da6 310 CRC+=c[i];
igor_v 21:bc8c1cec3da6 311 }
igor_v 0:8ad47e2b6f00 312
igor_v 21:bc8c1cec3da6 313 if(c[Count-2]!=((CRC>>8)&0xFF)) {
igor_v 21:bc8c1cec3da6 314 temp=0;
igor_v 30:17c84ed091b3 315 Gyro.RsErrLine = (Gyro.RsErrLine)&=0x2;
igor_v 21:bc8c1cec3da6 316 }
igor_v 0:8ad47e2b6f00 317
igor_v 21:bc8c1cec3da6 318 if(c[Count-1]!=((CRC>>0)&0xFF)) {
igor_v 21:bc8c1cec3da6 319 temp=0;
igor_v 30:17c84ed091b3 320 // Gyro.RsErrLine= (Gyro.RsErrLine)=0x2;
igor_v 21:bc8c1cec3da6 321 }
igor_v 21:bc8c1cec3da6 322
igor_v 21:bc8c1cec3da6 323 c[Count-2]=(CRC>>8)&0xFF;
igor_v 21:bc8c1cec3da6 324 c[Count-1]=(CRC>>0)&0xFF;
igor_v 21:bc8c1cec3da6 325
igor_v 21:bc8c1cec3da6 326
igor_v 21:bc8c1cec3da6 327
igor_v 21:bc8c1cec3da6 328 return temp;
igor_v 0:8ad47e2b6f00 329 }
igor_v 0:8ad47e2b6f00 330
igor_v 0:8ad47e2b6f00 331 int Getlengf(void)
igor_v 0:8ad47e2b6f00 332 {
Kovalev_D 121:bbae560cdd43 333 unsigned int lengf;
Kovalev_D 121:bbae560cdd43 334 lengf = 1;
igor_v 30:17c84ed091b3 335 switch(Gyro.CMD_In) {
igor_v 30:17c84ed091b3 336 case 0x99://Gyrotainance
Kovalev_D 92:c892f0311aa7 337
igor_v 21:bc8c1cec3da6 338 lengf=6;
igor_v 21:bc8c1cec3da6 339 CRC_N=8;
igor_v 21:bc8c1cec3da6 340 break;
igor_v 0:8ad47e2b6f00 341
igor_v 21:bc8c1cec3da6 342 case 0x0A: //m_stymul
igor_v 21:bc8c1cec3da6 343 lengf=8;
igor_v 21:bc8c1cec3da6 344 break;
igor_v 21:bc8c1cec3da6 345
igor_v 21:bc8c1cec3da6 346 case 0xA5://DeviceMode
igor_v 21:bc8c1cec3da6 347
igor_v 21:bc8c1cec3da6 348 lengf=6;
igor_v 21:bc8c1cec3da6 349 CRC_N=8;
igor_v 21:bc8c1cec3da6 350 break;
igor_v 21:bc8c1cec3da6 351
igor_v 21:bc8c1cec3da6 352 case 0xDD://m_rate
igor_v 21:bc8c1cec3da6 353 lengf=6;
igor_v 21:bc8c1cec3da6 354 CRC_N=44;
igor_v 21:bc8c1cec3da6 355 break;
igor_v 21:bc8c1cec3da6 356
igor_v 21:bc8c1cec3da6 357 case 0xB0://DeviceMode
igor_v 21:bc8c1cec3da6 358
igor_v 21:bc8c1cec3da6 359 lengf=6;
igor_v 21:bc8c1cec3da6 360 CRC_N=8;
igor_v 21:bc8c1cec3da6 361 break;
igor_v 21:bc8c1cec3da6 362
igor_v 21:bc8c1cec3da6 363 case 0xD8://m_control
igor_v 0:8ad47e2b6f00 364
igor_v 21:bc8c1cec3da6 365 lengf=6;
igor_v 21:bc8c1cec3da6 366 CRC_N=8;
igor_v 21:bc8c1cec3da6 367 break;
igor_v 21:bc8c1cec3da6 368
igor_v 21:bc8c1cec3da6 369 case 0xD9://m_control
igor_v 0:8ad47e2b6f00 370
igor_v 21:bc8c1cec3da6 371 lengf=6;
igor_v 21:bc8c1cec3da6 372 CRC_N=8;
igor_v 21:bc8c1cec3da6 373 break;
Kovalev_D 102:4270092be987 374
Kovalev_D 102:4270092be987 375 case 0xE4:
Kovalev_D 98:95b8e79f13e1 376 lengf=8;
Kovalev_D 98:95b8e79f13e1 377 break;
Kovalev_D 103:e96f08947def 378
Kovalev_D 103:e96f08947def 379
Kovalev_D 103:e96f08947def 380 case 0xE6:
Kovalev_D 103:e96f08947def 381 lengf=8;
Kovalev_D 103:e96f08947def 382 break;
Kovalev_D 103:e96f08947def 383
igor_v 21:bc8c1cec3da6 384 }
Kovalev_D 98:95b8e79f13e1 385
igor_v 21:bc8c1cec3da6 386 return lengf;
igor_v 0:8ad47e2b6f00 387 }
igor_v 0:8ad47e2b6f00 388
igor_v 0:8ad47e2b6f00 389 void Read_CMD(void)
igor_v 21:bc8c1cec3da6 390 {
Kovalev_D 92:c892f0311aa7 391
Kovalev_D 45:cbc955aecebe 392
Kovalev_D 45:cbc955aecebe 393 Gyro.RsErrLine = (Gyro.RsErrLine)& 0xffff;//для сброса ошибки
igor_v 21:bc8c1cec3da6 394
Kovalev_D 93:b3803774f110 395 CountBuFFIn=ReadChekCon1(BuffTemp); // чтение данных из консоли
igor_v 42:6fc307c4963e 396
Kovalev_D 92:c892f0311aa7 397 if(CountBuFFIn==1) { // если есть первый байт
igor_v 21:bc8c1cec3da6 398 if (BuffTemp[0] != SOC_In) {
Kovalev_D 93:b3803774f110 399 ReadCon1(BuffTemp);
igor_v 30:17c84ed091b3 400 Gyro.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
Kovalev_D 93:b3803774f110 401 BuffTemp[99]=Gyro.RsErrLine;
Kovalev_D 102:4270092be987 402
igor_v 21:bc8c1cec3da6 403 }
Kovalev_D 92:c892f0311aa7 404 } else if(CountBuFFIn==2) { //если второй ,fqn
igor_v 30:17c84ed091b3 405 if (BuffTemp[1] != Gyro.My_Addres) {
Kovalev_D 93:b3803774f110 406 ReadCon1(BuffTemp);
igor_v 30:17c84ed091b3 407 Gyro.RsErrLine += 0x1;//WriteCon("\n\r ...Error_ADDRES.... ");
igor_v 21:bc8c1cec3da6 408 }
igor_v 42:6fc307c4963e 409 } else if(CountBuFFIn==3) { // если ьоетий байт
igor_v 30:17c84ed091b3 410 Gyro.CMD_In=BuffTemp[2];
Kovalev_D 92:c892f0311aa7 411 N=Getlengf();
Kovalev_D 92:c892f0311aa7 412 }
Kovalev_D 92:c892f0311aa7 413 else if(CountBuFFIn==4 && (N==6))
Kovalev_D 92:c892f0311aa7 414 { // N - длина посылки аскглд(для записи параметров). определяется по коду команды в Getlengf();
igor_v 21:bc8c1cec3da6 415 Param1=BuffTemp[3];
Kovalev_D 92:c892f0311aa7 416 }
Kovalev_D 92:c892f0311aa7 417 else if((CountBuFFIn==5)&&(N==7))
Kovalev_D 92:c892f0311aa7 418 {
Kovalev_D 92:c892f0311aa7 419
igor_v 21:bc8c1cec3da6 420 Param1=BuffTemp[3];
igor_v 21:bc8c1cec3da6 421 Param2=BuffTemp[4];
igor_v 0:8ad47e2b6f00 422
Kovalev_D 92:c892f0311aa7 423 }
Kovalev_D 92:c892f0311aa7 424 else if((CountBuFFIn==6)&&(N==8))
Kovalev_D 92:c892f0311aa7 425 {
igor_v 21:bc8c1cec3da6 426 Param1=BuffTemp[3];
igor_v 21:bc8c1cec3da6 427 Param2=BuffTemp[4];
igor_v 21:bc8c1cec3da6 428 Param3=BuffTemp[5];
Kovalev_D 92:c892f0311aa7 429 }
Kovalev_D 102:4270092be987 430 else if((CountBuFFIn==8)&&(N==10))
Kovalev_D 102:4270092be987 431 {
Kovalev_D 102:4270092be987 432 Param1=BuffTemp[4];
Kovalev_D 102:4270092be987 433 Param2=BuffTemp[5];
Kovalev_D 102:4270092be987 434 Param3=BuffTemp[6];
Kovalev_D 102:4270092be987 435 Param4=BuffTemp[7];
Kovalev_D 102:4270092be987 436 }
Kovalev_D 98:95b8e79f13e1 437
Kovalev_D 92:c892f0311aa7 438 else if(CountBuFFIn > (N-1))
Kovalev_D 92:c892f0311aa7 439 {
Kovalev_D 93:b3803774f110 440 ReadCon1(BuffTemp);
igor_v 30:17c84ed091b3 441 switch(Gyro.CMD_In) {
Kovalev_D 122:fbacb932a30b 442 Rate_Flag=0;
Kovalev_D 92:c892f0311aa7 443 case 0x99:
Kovalev_D 122:fbacb932a30b 444 Rate_Flag=0;
igor_v 21:bc8c1cec3da6 445 CMD_Maintenance();
Kovalev_D 121:bbae560cdd43 446
igor_v 21:bc8c1cec3da6 447 break;
igor_v 21:bc8c1cec3da6 448 case 0xD8:
igor_v 21:bc8c1cec3da6 449 CMD_M_Control_D8();
igor_v 21:bc8c1cec3da6 450 case 0xD9:
igor_v 21:bc8c1cec3da6 451 CMD_M_Control_D9();
igor_v 21:bc8c1cec3da6 452 break;
igor_v 21:bc8c1cec3da6 453 case 0x0A:
Kovalev_D 45:cbc955aecebe 454 CMD_M_Stymul();
igor_v 21:bc8c1cec3da6 455 break;
igor_v 21:bc8c1cec3da6 456 case 0xDD:
Kovalev_D 122:fbacb932a30b 457 Rate_Flag=0;
Kovalev_D 122:fbacb932a30b 458 TempParam=(BuffTemp[3]) & 0x9f;
Kovalev_D 122:fbacb932a30b 459 if (TempParam == 0x01) { Rate_Flag=0; CMD_Rate(); WriteCon("Rate11");}
Kovalev_D 122:fbacb932a30b 460 else if (TempParam == 0x02) { Rate_Flag=0; CMD_Rate2(); WriteCon("Rate21");}
Kovalev_D 122:fbacb932a30b 461 else if (TempParam == 0x81) { Rate_Flag=1; WriteCon("Rate12");}
Kovalev_D 122:fbacb932a30b 462 else if (TempParam == 0x82) { Rate_Flag=2; Rate2VibFlag==0; WriteCon("Rate22");}
Kovalev_D 122:fbacb932a30b 463
Kovalev_D 122:fbacb932a30b 464 break;
Kovalev_D 102:4270092be987 465 case 0xE4:
Kovalev_D 102:4270092be987 466 CMD_M_vib();
Kovalev_D 102:4270092be987 467 break;
Kovalev_D 103:e96f08947def 468 case 0xE6:
Kovalev_D 103:e96f08947def 469 Gph_W();
Kovalev_D 103:e96f08947def 470 break;
Kovalev_D 108:030cdde08314 471
Kovalev_D 103:e96f08947def 472
igor_v 30:17c84ed091b3 473 Gyro.RsErrLine = 0;
Kovalev_D 103:e96f08947def 474
Kovalev_D 103:e96f08947def 475
Kovalev_D 102:4270092be987 476 }
Kovalev_D 121:bbae560cdd43 477
igor_v 21:bc8c1cec3da6 478 }
Kovalev_D 121:bbae560cdd43 479 // else if(CountBuFFIn > 40 )
Kovalev_D 121:bbae560cdd43 480 // {
Kovalev_D 121:bbae560cdd43 481 // ReadCon1(BuffTemp);
Kovalev_D 121:bbae560cdd43 482 // }
Kovalev_D 121:bbae560cdd43 483
igor_v 0:8ad47e2b6f00 484 }
igor_v 0:8ad47e2b6f00 485
igor_v 0:8ad47e2b6f00 486
igor_v 21:bc8c1cec3da6 487