Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

Command.c@30:17c84ed091b3, 2016-02-05 (annotated)

Committer:
igor_v
Date:
Fri Feb 05 23:21:54 2016 +0000
Revision:
30:17c84ed091b3
Parent:
25:1a6798ccd3cd
Child:
38:716472a4492e
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 "Command.h"
igor_v 0:8ad47e2b6f00 4 #include "el_lin.h"
igor_v 0:8ad47e2b6f00 5 #include "vibro.h"
igor_v 0:8ad47e2b6f00 6 #include "Global.h"
igor_v 0:8ad47e2b6f00 7 #include "uart_m.h"
igor_v 0:8ad47e2b6f00 8 #include "CntrlGLD.h"
igor_v 0:8ad47e2b6f00 9 #include "QEI.h"
igor_v 0:8ad47e2b6f00 10 #include "InputOutput.h"
igor_v 0:8ad47e2b6f00 11 #include "SPI.h"
igor_v 0:8ad47e2b6f00 12 unsigned int Rate_Flag;
igor_v 0:8ad47e2b6f00 13 unsigned int CountBuFFIn;
igor_v 21:bc8c1cec3da6 14 unsigned int N=0,CRC_N;
igor_v 21:bc8c1cec3da6 15 unsigned int Param1=0;
igor_v 21:bc8c1cec3da6 16 unsigned int Param2=0;
igor_v 21:bc8c1cec3da6 17 unsigned int Param3=0;
igor_v 21:bc8c1cec3da6 18 unsigned int Param4=0;
igor_v 21:bc8c1cec3da6 19
igor_v 0:8ad47e2b6f00 20 /*
igor_v 21:bc8c1cec3da6 21 void exec_CMD(void) //r. === процедура окончательной дешифрации и исполнения команды
igor_v 0:8ad47e2b6f00 22 {
igor_v 0:8ad47e2b6f00 23 unsigned int Comand_code;
igor_v 21:bc8c1cec3da6 24
igor_v 21:bc8c1cec3da6 25 //rx_buf_copy = 1; //r. предустанов флага копирования приемного буфера
igor_v 0:8ad47e2b6f00 26
igor_v 0:8ad47e2b6f00 27 Comand_code = (rcv_buf[2] & 0xFF) << 8;
igor_v 21:bc8c1cec3da6 28 PC_Comand = Comand_code | (rcv_buf[3] & 0xFF); //e. save it in the memory for echo-transmission //r. сохранить его в памяти для обратной передачи
igor_v 0:8ad47e2b6f00 29 }*/
igor_v 0:8ad47e2b6f00 30 void CMD_Maintenance(void)
igor_v 0:8ad47e2b6f00 31 {
igor_v 30:17c84ed091b3 32 BuffTemp[0] = Gyro.SOC_Out;
igor_v 30:17c84ed091b3 33 BuffTemp[1] = Gyro .My_Addres;
igor_v 30:17c84ed091b3 34 BuffTemp[2] = Gyro .Firmware_Version;
igor_v 30:17c84ed091b3 35 BuffTemp[3] = Gyro .GLD_Serial;
igor_v 21:bc8c1cec3da6 36 BuffTemp[4]=0x00;
igor_v 21:bc8c1cec3da6 37 BuffTemp[5]=0x00;
igor_v 21:bc8c1cec3da6 38 Check(BuffTemp, 8);
igor_v 21:bc8c1cec3da6 39 WriteConN (BuffTemp,8);
igor_v 0:8ad47e2b6f00 40 }
igor_v 0:8ad47e2b6f00 41
igor_v 0:8ad47e2b6f00 42 void CMD_Rate(void)
igor_v 21:bc8c1cec3da6 43 {
igor_v 21:bc8c1cec3da6 44
igor_v 21:bc8c1cec3da6 45 unsigned int T_VibH , T_VibL; // Частота вибропривода.
igor_v 21:bc8c1cec3da6 46 unsigned int Cnt_PlsL , Cnt_PlsH; // Счетчик +.
igor_v 21:bc8c1cec3da6 47 unsigned int Cnt_MnsL , Cnt_MnsH; // Счетчик -.
igor_v 21:bc8c1cec3da6 48 unsigned int Cnt_DifL , Cnt_DifH; // Разность счетчиков.
igor_v 21:bc8c1cec3da6 49 unsigned int F_rasL , F_rasH; // Частота расщепления.
igor_v 21:bc8c1cec3da6 50
igor_v 21:bc8c1cec3da6 51 unsigned int HF_regL , HF_regH; // Выход регулятора ГВЧ. НЕ СДЕЛАНО
igor_v 21:bc8c1cec3da6 52 unsigned int HF_difL , HF_difH; // Сигнал ошибки регулячтора ГВЧ. НЕ СДЕЛАНО
igor_v 21:bc8c1cec3da6 53
igor_v 21:bc8c1cec3da6 54 unsigned int WP_regL , WP_regH;
igor_v 21:bc8c1cec3da6 55
igor_v 21:bc8c1cec3da6 56 unsigned int ADC1L, ADC2L, ADC3L, ADC4L, ADC5L;
igor_v 21:bc8c1cec3da6 57 unsigned int ADC1H, ADC2H, ADC3H, ADC4H, ADC5H;
igor_v 21:bc8c1cec3da6 58
igor_v 21:bc8c1cec3da6 59
igor_v 21:bc8c1cec3da6 60 LoopOn
igor_v 30:17c84ed091b3 61 // Gyro .T_Vib=(unsigned int)((7680000*16/Gyro.Frq)*4096);
igor_v 30:17c84ed091b3 62 Gyro.T_Vib=(unsigned int)((7680000*16/(Gyro.Frq>>12)));
igor_v 30:17c84ed091b3 63 Gyro.Firmware_Version=0xff; /// промежуточная затычка
igor_v 21:bc8c1cec3da6 64
igor_v 30:17c84ed091b3 65 T_VibH = ( Gyro.T_Vib>>8 ) TakeByte;
igor_v 30:17c84ed091b3 66 T_VibL = ( Gyro.T_Vib ) TakeByte;
igor_v 21:bc8c1cec3da6 67
igor_v 30:17c84ed091b3 68 Cnt_PlsH = ( Gyro.Cnt_Pls>>8) TakeByte;
igor_v 30:17c84ed091b3 69 Cnt_PlsL = ( Gyro.Cnt_Pls ) TakeByte;
igor_v 21:bc8c1cec3da6 70
igor_v 30:17c84ed091b3 71 Cnt_MnsH = ( Gyro.Cnt_Mns>>8) TakeByte;
igor_v 30:17c84ed091b3 72 Cnt_MnsL = ( Gyro.Cnt_Mns ) TakeByte;
igor_v 21:bc8c1cec3da6 73
igor_v 30:17c84ed091b3 74 Cnt_DifH = ( Gyro.Cnt_Dif>>8) TakeByte;
igor_v 30:17c84ed091b3 75 Cnt_DifL = ( Gyro.Cnt_Dif ) TakeByte;
igor_v 21:bc8c1cec3da6 76
igor_v 30:17c84ed091b3 77 F_rasH = ( Gyro.F_ras>>8 ) TakeByte;
igor_v 30:17c84ed091b3 78 F_rasL = ( Gyro.F_ras ) TakeByte;
igor_v 21:bc8c1cec3da6 79
igor_v 21:bc8c1cec3da6 80 ADC1H = ( Spi.ADC1>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 81 ADC1L = ( Spi.ADC1 ) TakeByte;
igor_v 0:8ad47e2b6f00 82
igor_v 21:bc8c1cec3da6 83 ADC2H = ( Spi.ADC2>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 84 ADC2L = ( Spi.ADC2 ) TakeByte;
igor_v 21:bc8c1cec3da6 85
igor_v 21:bc8c1cec3da6 86 ADC3H = ( Spi.ADC3>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 87 ADC3L = ( Spi.ADC3 ) TakeByte;
igor_v 21:bc8c1cec3da6 88
igor_v 21:bc8c1cec3da6 89 ADC4H = ( Spi.ADC4>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 90 ADC4L = ( Spi.ADC4 ) TakeByte;
igor_v 21:bc8c1cec3da6 91
igor_v 21:bc8c1cec3da6 92 ADC5H = ( Spi.ADC5>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 93 ADC5L = ( Spi.ADC5 ) TakeByte;
igor_v 21:bc8c1cec3da6 94
igor_v 21:bc8c1cec3da6 95 WP_regH = ( Spi.DAC_B>>8 ) TakeByte;
igor_v 21:bc8c1cec3da6 96 WP_regL = ( Spi.DAC_B ) TakeByte;
igor_v 21:bc8c1cec3da6 97
igor_v 21:bc8c1cec3da6 98 HF_regH=0xff;
igor_v 21:bc8c1cec3da6 99 HF_regL=0xff;
igor_v 21:bc8c1cec3da6 100
igor_v 21:bc8c1cec3da6 101 HF_difH=0xff;
igor_v 21:bc8c1cec3da6 102 HF_difL=0xff;
igor_v 21:bc8c1cec3da6 103
igor_v 0:8ad47e2b6f00 104
igor_v 30:17c84ed091b3 105 BuffTemp[ 0] = Gyro.SOC_Out;
igor_v 30:17c84ed091b3 106 BuffTemp[ 1] = Gyro.My_Addres;
igor_v 21:bc8c1cec3da6 107
igor_v 21:bc8c1cec3da6 108 BuffTemp[ 2] = Cnt_PlsH;//старший байт счетчика +.
igor_v 21:bc8c1cec3da6 109 BuffTemp[ 3] = Cnt_PlsL;//младший байт счетчика +.
igor_v 21:bc8c1cec3da6 110
igor_v 21:bc8c1cec3da6 111 BuffTemp[ 4] = Cnt_MnsH;//старший байт счетчика -.
igor_v 21:bc8c1cec3da6 112 BuffTemp[ 5] = Cnt_MnsL;//младший байт счетчика -.
igor_v 21:bc8c1cec3da6 113
igor_v 21:bc8c1cec3da6 114 BuffTemp[ 6] = Cnt_DifH;//старший байт разности счетчиков
igor_v 21:bc8c1cec3da6 115 BuffTemp[ 7] = Cnt_DifL;//младший байт разности счетчиков
igor_v 21:bc8c1cec3da6 116
igor_v 21:bc8c1cec3da6 117 BuffTemp[ 8] = F_rasH;
igor_v 21:bc8c1cec3da6 118 BuffTemp[ 9] = F_rasL;
igor_v 21:bc8c1cec3da6 119
igor_v 21:bc8c1cec3da6 120 BuffTemp[10] = HF_regH;
igor_v 21:bc8c1cec3da6 121 BuffTemp[11] = HF_regL;
igor_v 21:bc8c1cec3da6 122
igor_v 21:bc8c1cec3da6 123 BuffTemp[12] = HF_difH;
igor_v 21:bc8c1cec3da6 124 BuffTemp[13] = HF_difL;
igor_v 21:bc8c1cec3da6 125
igor_v 21:bc8c1cec3da6 126 BuffTemp[14] = T_VibH ;//
igor_v 21:bc8c1cec3da6 127 BuffTemp[15] = T_VibL; //?
igor_v 21:bc8c1cec3da6 128
igor_v 30:17c84ed091b3 129 BuffTemp[16] = Gyro.Firmware_Version;//ХЗ
igor_v 30:17c84ed091b3 130 BuffTemp[17] = Gyro.Firmware_Version;//ХЗ
igor_v 21:bc8c1cec3da6 131
igor_v 21:bc8c1cec3da6 132 BuffTemp[18] = T_VibH>>1;
igor_v 21:bc8c1cec3da6 133 BuffTemp[19] = T_VibL;
igor_v 21:bc8c1cec3da6 134
igor_v 30:17c84ed091b3 135 BuffTemp[20] = Gyro.Firmware_Version;//ХЗ
igor_v 30:17c84ed091b3 136 BuffTemp[21] = Gyro.Firmware_Version;//ХЗ
igor_v 21:bc8c1cec3da6 137
igor_v 21:bc8c1cec3da6 138 BuffTemp[22] = WP_regH; //СРП Spi.DAC_B
igor_v 21:bc8c1cec3da6 139 BuffTemp[23] = WP_regL; //СРП
igor_v 21:bc8c1cec3da6 140
igor_v 30:17c84ed091b3 141 BuffTemp[24] = Gyro.Firmware_Version;// ХЗ
igor_v 30:17c84ed091b3 142 BuffTemp[25] = Gyro.Firmware_Version;// ХЗ
igor_v 21:bc8c1cec3da6 143
igor_v 21:bc8c1cec3da6 144 BuffTemp[26] = ADC1H;
igor_v 21:bc8c1cec3da6 145 BuffTemp[27] = ADC1L;
igor_v 21:bc8c1cec3da6 146
igor_v 21:bc8c1cec3da6 147 BuffTemp[28] = ADC1H;
igor_v 21:bc8c1cec3da6 148 BuffTemp[29] = ADC1L;
igor_v 21:bc8c1cec3da6 149
igor_v 21:bc8c1cec3da6 150 BuffTemp[30] = ADC1H;
igor_v 21:bc8c1cec3da6 151 BuffTemp[31] = ADC1L;
igor_v 21:bc8c1cec3da6 152
igor_v 21:bc8c1cec3da6 153 BuffTemp[32] = ADC1H;
igor_v 21:bc8c1cec3da6 154 BuffTemp[33] = ADC1L;
igor_v 21:bc8c1cec3da6 155
igor_v 21:bc8c1cec3da6 156 BuffTemp[34] = ADC1H;
igor_v 21:bc8c1cec3da6 157 BuffTemp[35] = ADC1L;
igor_v 21:bc8c1cec3da6 158
igor_v 21:bc8c1cec3da6 159 BuffTemp[36] = 0x00;//ADC6
igor_v 21:bc8c1cec3da6 160 BuffTemp[37] = 0x00;//ADC6
igor_v 21:bc8c1cec3da6 161
igor_v 30:17c84ed091b3 162 BuffTemp[38] = Gyro.Firmware_Version;
igor_v 30:17c84ed091b3 163 BuffTemp[39] = Gyro.Firmware_Version;
igor_v 30:17c84ed091b3 164 BuffTemp[40] = Gyro.Firmware_Version;
igor_v 30:17c84ed091b3 165 BuffTemp[41] = Gyro.Firmware_Version;
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);
igor_v 21:bc8c1cec3da6 170 LoopOff
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 {
igor_v 21:bc8c1cec3da6 175 int bit,NReg;
igor_v 30:17c84ed091b3 176 BuffTemp[0] = Gyro.SOC_Out; //DD
igor_v 30:17c84ed091b3 177 BuffTemp[1] = Gyro.My_Addres; //00
igor_v 30:17c84ed091b3 178 BuffTemp[2] = Gyro.CMD_In; //D8
igor_v 21:bc8c1cec3da6 179 //10?
igor_v 21:bc8c1cec3da6 180 if((Param1&0x80)) {
igor_v 21:bc8c1cec3da6 181 bit=1;
igor_v 21:bc8c1cec3da6 182 } else {
igor_v 21:bc8c1cec3da6 183 bit=0;
igor_v 21:bc8c1cec3da6 184 }
igor_v 21:bc8c1cec3da6 185
igor_v 21:bc8c1cec3da6 186 if ((Param1 & 0x10) == 0) {
igor_v 21:bc8c1cec3da6 187 NReg=0;
igor_v 30:17c84ed091b3 188 switch(Param1&0xf) { // Gyro.RgConA
igor_v 21:bc8c1cec3da6 189 case 0x0:
igor_v 30:17c84ed091b3 190 Gyro.RgConA|=bit;
igor_v 21:bc8c1cec3da6 191 break;
igor_v 21:bc8c1cec3da6 192
igor_v 21:bc8c1cec3da6 193 case 0x1:
igor_v 30:17c84ed091b3 194 Gyro.RgConA|=bit<<0x1;
igor_v 21:bc8c1cec3da6 195 break;
igor_v 21:bc8c1cec3da6 196
igor_v 21:bc8c1cec3da6 197 case 0x2:
igor_v 30:17c84ed091b3 198 Gyro.RgConA|=bit<<0x2;
igor_v 21:bc8c1cec3da6 199 break;
igor_v 21:bc8c1cec3da6 200
igor_v 21:bc8c1cec3da6 201 case 0x3:
igor_v 30:17c84ed091b3 202 Gyro.RgConA|=bit<<0x3;
igor_v 21:bc8c1cec3da6 203 break;
igor_v 21:bc8c1cec3da6 204
igor_v 21:bc8c1cec3da6 205 case 0x4:
igor_v 30:17c84ed091b3 206 Gyro.RgConA|=bit<<0x4;
igor_v 21:bc8c1cec3da6 207 break;
igor_v 21:bc8c1cec3da6 208
igor_v 21:bc8c1cec3da6 209 case 0x5:
igor_v 30:17c84ed091b3 210 Gyro.RgConA|=bit<<0x5;
igor_v 21:bc8c1cec3da6 211 break;
igor_v 21:bc8c1cec3da6 212
igor_v 21:bc8c1cec3da6 213 case 0x6:
igor_v 30:17c84ed091b3 214 Gyro.RgConA|=bit<<0x6;
igor_v 21:bc8c1cec3da6 215 break;
igor_v 21:bc8c1cec3da6 216
igor_v 21:bc8c1cec3da6 217 case 0x7:
igor_v 30:17c84ed091b3 218 Gyro.RgConA|=bit<<0x7;
igor_v 21:bc8c1cec3da6 219 break;
igor_v 21:bc8c1cec3da6 220 }
igor_v 21:bc8c1cec3da6 221 BuffTemp[3] = NReg<<4;
igor_v 30:17c84ed091b3 222 BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
igor_v 30:17c84ed091b3 223 BuffTemp[5] = Gyro.RgConA & 0xff;
igor_v 21:bc8c1cec3da6 224 } else {
igor_v 30:17c84ed091b3 225 NReg=1; // Gyro.RgConB
igor_v 30:17c84ed091b3 226 switch(Param1&0xf) { // Gyro.RgConB
igor_v 21:bc8c1cec3da6 227 case 0x0:
igor_v 30:17c84ed091b3 228 Gyro.RgConB|=bit;
igor_v 21:bc8c1cec3da6 229 break;
igor_v 21:bc8c1cec3da6 230 case 0x1:
igor_v 30:17c84ed091b3 231 Gyro.RgConB|=bit<1;
igor_v 21:bc8c1cec3da6 232 break;
igor_v 21:bc8c1cec3da6 233 case 0x2:
igor_v 30:17c84ed091b3 234 Gyro.RgConB|=bit<2;
igor_v 21:bc8c1cec3da6 235 break;
igor_v 21:bc8c1cec3da6 236 }
igor_v 21:bc8c1cec3da6 237 BuffTemp[3] = NReg<<4;
igor_v 30:17c84ed091b3 238 BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
igor_v 30:17c84ed091b3 239 BuffTemp[5] = Gyro.RgConB & 0xff;
igor_v 21:bc8c1cec3da6 240 }
igor_v 21:bc8c1cec3da6 241
igor_v 21:bc8c1cec3da6 242 Check(BuffTemp, CRC_N);
igor_v 21:bc8c1cec3da6 243 WriteConN (BuffTemp,CRC_N);
igor_v 0:8ad47e2b6f00 244 }
igor_v 0:8ad47e2b6f00 245
igor_v 21:bc8c1cec3da6 246 void CMD_M_Control_D9()///чтение регистров управления
igor_v 21:bc8c1cec3da6 247 {
igor_v 21:bc8c1cec3da6 248 int bit,NReg;
igor_v 30:17c84ed091b3 249 BuffTemp[0] = Gyro.SOC_Out; //DD
igor_v 30:17c84ed091b3 250 BuffTemp[1] = Gyro.My_Addres; //00
igor_v 30:17c84ed091b3 251 BuffTemp[2] = Gyro.CMD_In; //D9
igor_v 21:bc8c1cec3da6 252 if ((Param1 & 0x10) == 0) {
igor_v 21:bc8c1cec3da6 253 BuffTemp[3]=0<<4;
igor_v 30:17c84ed091b3 254 BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
igor_v 30:17c84ed091b3 255 BuffTemp[5] = Gyro.RgConA & 0xff;
igor_v 21:bc8c1cec3da6 256 } else {
igor_v 21:bc8c1cec3da6 257 BuffTemp[3]=1<<4;
igor_v 30:17c84ed091b3 258 BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
igor_v 30:17c84ed091b3 259 BuffTemp[5] = Gyro.RgConB & 0xff;
igor_v 21:bc8c1cec3da6 260 }
igor_v 21:bc8c1cec3da6 261 Check(BuffTemp, CRC_N);
igor_v 21:bc8c1cec3da6 262 WriteConN (BuffTemp,CRC_N);
igor_v 21:bc8c1cec3da6 263 }
igor_v 21:bc8c1cec3da6 264 // (номер (код) ЦАПа 0...3, старший байт требуемого ЦАПа, младший байт треб ЦАПа)
igor_v 0:8ad47e2b6f00 265 void CMD_M_Stymul()
igor_v 0:8ad47e2b6f00 266 {
igor_v 21:bc8c1cec3da6 267 int temp;
igor_v 21:bc8c1cec3da6 268 temp =((BuffTemp[4]<<8) | BuffTemp[5]);
igor_v 21:bc8c1cec3da6 269 // temp=0;
igor_v 21:bc8c1cec3da6 270 temp=temp&0xFFFF;
igor_v 21:bc8c1cec3da6 271 DACF =(temp*K_DAC)+deltaDAC;
igor_v 21:bc8c1cec3da6 272 Spi.DAC_B =(unsigned int)(DACF) /*(unsigned int)(temp*K_DAC+deltaDAC)*/; // K_DAC);
igor_v 21:bc8c1cec3da6 273
igor_v 21:bc8c1cec3da6 274 //int temp;
igor_v 0:8ad47e2b6f00 275 // temp =((BuffTemp[4]<<8) | BuffTemp[5]);
igor_v 21:bc8c1cec3da6 276 //temp = 65535 - temp;
igor_v 21:bc8c1cec3da6 277 /*if(temp<1000)
igor_v 21:bc8c1cec3da6 278 {
igor_v 21:bc8c1cec3da6 279 temp=0;
igor_v 21:bc8c1cec3da6 280 }
igor_v 21:bc8c1cec3da6 281 else if(temp>60000)
igor_v 21:bc8c1cec3da6 282 {
igor_v 21:bc8c1cec3da6 283 temp=65535;
igor_v 21:bc8c1cec3da6 284 }*/
igor_v 21:bc8c1cec3da6 285 //Spi.DAC_B =(((BuffTemp[4]<<8) | BuffTemp[5])*K_DAC+deltaDAC);/*((BuffTemp[4]<<8) | BuffTemp[5])-0xFFFF*///; //((rcv_buf[4] << 8) | (int)rcv_buf[5]);K_DAC+deltaDAC
igor_v 21:bc8c1cec3da6 286 //BuffTemp[0] = temp; //DD
igor_v 21:bc8c1cec3da6 287 //WriteConN (BuffTemp,1);
igor_v 0:8ad47e2b6f00 288 }
igor_v 0:8ad47e2b6f00 289
igor_v 0:8ad47e2b6f00 290 unsigned int Check(char *c, unsigned int Count)
igor_v 0:8ad47e2b6f00 291 {
igor_v 21:bc8c1cec3da6 292 int i=1;
igor_v 21:bc8c1cec3da6 293 unsigned int temp,CRC;
igor_v 21:bc8c1cec3da6 294
igor_v 21:bc8c1cec3da6 295
igor_v 21:bc8c1cec3da6 296 temp=1;
igor_v 21:bc8c1cec3da6 297 CRC=0;
igor_v 21:bc8c1cec3da6 298
igor_v 21:bc8c1cec3da6 299
igor_v 21:bc8c1cec3da6 300 for(; i<Count-2; i++) {
igor_v 21:bc8c1cec3da6 301 CRC+=c[i];
igor_v 21:bc8c1cec3da6 302 }
igor_v 0:8ad47e2b6f00 303
igor_v 21:bc8c1cec3da6 304 if(c[Count-2]!=((CRC>>8)&0xFF)) {
igor_v 21:bc8c1cec3da6 305 temp=0;
igor_v 30:17c84ed091b3 306 Gyro.RsErrLine = (Gyro.RsErrLine)&=0x2;
igor_v 21:bc8c1cec3da6 307 }
igor_v 0:8ad47e2b6f00 308
igor_v 21:bc8c1cec3da6 309 if(c[Count-1]!=((CRC>>0)&0xFF)) {
igor_v 21:bc8c1cec3da6 310 temp=0;
igor_v 30:17c84ed091b3 311 // Gyro.RsErrLine= (Gyro.RsErrLine)=0x2;
igor_v 21:bc8c1cec3da6 312 }
igor_v 21:bc8c1cec3da6 313
igor_v 21:bc8c1cec3da6 314 c[Count-2]=(CRC>>8)&0xFF;
igor_v 21:bc8c1cec3da6 315 c[Count-1]=(CRC>>0)&0xFF;
igor_v 21:bc8c1cec3da6 316
igor_v 21:bc8c1cec3da6 317
igor_v 21:bc8c1cec3da6 318
igor_v 21:bc8c1cec3da6 319 return temp;
igor_v 0:8ad47e2b6f00 320 }
igor_v 0:8ad47e2b6f00 321
igor_v 0:8ad47e2b6f00 322 int Getlengf(void)
igor_v 0:8ad47e2b6f00 323 {
igor_v 21:bc8c1cec3da6 324 unsigned int lengf=0;
igor_v 30:17c84ed091b3 325 switch(Gyro.CMD_In) {
igor_v 30:17c84ed091b3 326 case 0x99://Gyrotainance
igor_v 21:bc8c1cec3da6 327 lengf=6;
igor_v 21:bc8c1cec3da6 328 CRC_N=8;
igor_v 21:bc8c1cec3da6 329 break;
igor_v 0:8ad47e2b6f00 330
igor_v 21:bc8c1cec3da6 331 case 0x0A: //m_stymul
igor_v 21:bc8c1cec3da6 332 lengf=8;
igor_v 21:bc8c1cec3da6 333 break;
igor_v 21:bc8c1cec3da6 334
igor_v 21:bc8c1cec3da6 335 case 0xA5://DeviceMode
igor_v 21:bc8c1cec3da6 336
igor_v 21:bc8c1cec3da6 337 lengf=6;
igor_v 21:bc8c1cec3da6 338 CRC_N=8;
igor_v 21:bc8c1cec3da6 339 break;
igor_v 21:bc8c1cec3da6 340
igor_v 21:bc8c1cec3da6 341 case 0xDD://m_rate
igor_v 0:8ad47e2b6f00 342
igor_v 21:bc8c1cec3da6 343 lengf=6;
igor_v 21:bc8c1cec3da6 344 CRC_N=44;
igor_v 21:bc8c1cec3da6 345 break;
igor_v 21:bc8c1cec3da6 346
igor_v 21:bc8c1cec3da6 347 case 0xB0://DeviceMode
igor_v 21:bc8c1cec3da6 348
igor_v 21:bc8c1cec3da6 349 lengf=6;
igor_v 21:bc8c1cec3da6 350 CRC_N=8;
igor_v 21:bc8c1cec3da6 351 break;
igor_v 21:bc8c1cec3da6 352
igor_v 21:bc8c1cec3da6 353 case 0xD8://m_control
igor_v 0:8ad47e2b6f00 354
igor_v 21:bc8c1cec3da6 355 lengf=6;
igor_v 21:bc8c1cec3da6 356 CRC_N=8;
igor_v 21:bc8c1cec3da6 357 break;
igor_v 21:bc8c1cec3da6 358
igor_v 21:bc8c1cec3da6 359 case 0xD9://m_control
igor_v 0:8ad47e2b6f00 360
igor_v 21:bc8c1cec3da6 361 lengf=6;
igor_v 21:bc8c1cec3da6 362 CRC_N=8;
igor_v 21:bc8c1cec3da6 363 break;
igor_v 21:bc8c1cec3da6 364 }
igor_v 21:bc8c1cec3da6 365 return lengf;
igor_v 0:8ad47e2b6f00 366 }
igor_v 0:8ad47e2b6f00 367
igor_v 0:8ad47e2b6f00 368 void Read_CMD(void)
igor_v 21:bc8c1cec3da6 369 {
igor_v 30:17c84ed091b3 370 Gyro.SOC_Out=0xDD;
igor_v 30:17c84ed091b3 371 Gyro.RsErrLine = (Gyro.RsErrLine)& 0xffff;
igor_v 21:bc8c1cec3da6 372
igor_v 21:bc8c1cec3da6 373
igor_v 21:bc8c1cec3da6 374 CountBuFFIn=ReadChekCon(BuffTemp);
igor_v 21:bc8c1cec3da6 375 if(CountBuFFIn==1) {
igor_v 21:bc8c1cec3da6 376 if (BuffTemp[0] != SOC_In) {
igor_v 21:bc8c1cec3da6 377 ReadCon(BuffTemp);
igor_v 30:17c84ed091b3 378 Gyro.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
igor_v 30:17c84ed091b3 379 BuffTemp[99]=Gyro.RsErrLine;
igor_v 21:bc8c1cec3da6 380 }
igor_v 21:bc8c1cec3da6 381 } else if(CountBuFFIn==2) {
igor_v 30:17c84ed091b3 382 if (BuffTemp[1] != Gyro.My_Addres) {
igor_v 21:bc8c1cec3da6 383 ReadCon(BuffTemp);
igor_v 30:17c84ed091b3 384 Gyro.RsErrLine += 0x1;//WriteCon("\n\r ...Error_ADDRES.... ");
igor_v 21:bc8c1cec3da6 385 }
igor_v 21:bc8c1cec3da6 386 } else if(CountBuFFIn==3) {
igor_v 30:17c84ed091b3 387 Gyro.CMD_In=BuffTemp[2];
igor_v 21:bc8c1cec3da6 388 N=Getlengf();
igor_v 21:bc8c1cec3da6 389 } else if(CountBuFFIn==4 && N==6) {
igor_v 21:bc8c1cec3da6 390 Param1=BuffTemp[3];
igor_v 21:bc8c1cec3da6 391 } else if((CountBuFFIn==5)&&(N==7)) {
igor_v 21:bc8c1cec3da6 392 //LoopOn
igor_v 21:bc8c1cec3da6 393 Param1=BuffTemp[3];
igor_v 21:bc8c1cec3da6 394 Param2=BuffTemp[4];
igor_v 0:8ad47e2b6f00 395
igor_v 21:bc8c1cec3da6 396 } else if((CountBuFFIn==6)&&(N==8)) {
igor_v 21:bc8c1cec3da6 397 Param1=BuffTemp[3];
igor_v 21:bc8c1cec3da6 398 Param2=BuffTemp[4];
igor_v 21:bc8c1cec3da6 399 Param3=BuffTemp[5];
igor_v 21:bc8c1cec3da6 400 } else if(CountBuFFIn > (N-1)) {
igor_v 21:bc8c1cec3da6 401 ReadCon(BuffTemp);
igor_v 21:bc8c1cec3da6 402 if(Check(BuffTemp, CountBuFFIn)) {
igor_v 30:17c84ed091b3 403 switch(Gyro.CMD_In) {
igor_v 21:bc8c1cec3da6 404 case CMD_MAINT:
igor_v 21:bc8c1cec3da6 405 CMD_Maintenance();
igor_v 21:bc8c1cec3da6 406 break;
igor_v 21:bc8c1cec3da6 407 case 0xD8:
igor_v 21:bc8c1cec3da6 408 CMD_M_Control_D8();
igor_v 21:bc8c1cec3da6 409 case 0xD9:
igor_v 21:bc8c1cec3da6 410 CMD_M_Control_D9();
igor_v 21:bc8c1cec3da6 411 break;
igor_v 21:bc8c1cec3da6 412 case 0x0A:
igor_v 21:bc8c1cec3da6 413 CMD_M_Stymul(Param1,Param2,Param3);
igor_v 21:bc8c1cec3da6 414 break;
igor_v 21:bc8c1cec3da6 415 case 0xDD:
igor_v 21:bc8c1cec3da6 416 Rate_Flag=1;
igor_v 21:bc8c1cec3da6 417 break;
igor_v 30:17c84ed091b3 418 Gyro.RsErrLine = 0;
igor_v 21:bc8c1cec3da6 419 }
igor_v 21:bc8c1cec3da6 420
igor_v 0:8ad47e2b6f00 421 }
igor_v 21:bc8c1cec3da6 422 }
igor_v 0:8ad47e2b6f00 423 }
igor_v 0:8ad47e2b6f00 424
igor_v 0:8ad47e2b6f00 425
igor_v 21:bc8c1cec3da6 426