Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

Command.c@107:4d178bcc9d8a, 2016-04-01 (annotated)

Committer:
Kovalev_D
Date:
Fri Apr 01 16:13:55 2016 +0000
Revision:
107:4d178bcc9d8a
Parent:
106:250ddd8629c6
Child:
108:030cdde08314
1

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