LGfiltr | Mbed

Users » Kovalev_D » Code » LGfiltr
Dmitry Kovalev / Mbed 2 deprecated LGfiltr
Fork of LGstaandart by Dmitry Kovalev
Command.c

Committer:: igor_v
Date:: 2016-02-05
Revision:: 30:17c84ed091b3
Parent:: 25:1a6798ccd3cd
Child:: 38:716472a4492e
File content as of revision 30:17c84ed091b3:

#include "stdlib.h"
#include "LPC17xx.h"
#include "Command.h"
#include "el_lin.h"
#include "vibro.h"
#include "Global.h"
#include "uart_m.h"
#include "CntrlGLD.h"
#include "QEI.h"
#include "InputOutput.h"
#include "SPI.h"
unsigned int Rate_Flag;
unsigned int CountBuFFIn;
unsigned int N=0,CRC_N;
unsigned int Param1=0;
unsigned int Param2=0;
unsigned int Param3=0;
unsigned int Param4=0;

/*
void exec_CMD(void)       //r. === процедура окончательной дешифрации и исполнения команды
{
	unsigned int Comand_code;

	//rx_buf_copy = 1;                  //r. предустанов флага копирования приемного буфера

	Comand_code = (rcv_buf[2] & 0xFF) << 8;
	PC_Comand = Comand_code | (rcv_buf[3] & 0xFF);	//e. save it in the memory for echo-transmission //r. сохранить его в памяти для обратной передачи
}*/
void CMD_Maintenance(void)
{
    BuffTemp[0] = Gyro.SOC_Out;
    BuffTemp[1] =  Gyro .My_Addres;
    BuffTemp[2] =  Gyro .Firmware_Version;
    BuffTemp[3] = Gyro .GLD_Serial;
    BuffTemp[4]=0x00;
    BuffTemp[5]=0x00;
    Check(BuffTemp, 8);
    WriteConN (BuffTemp,8);
}

void CMD_Rate(void)
{

    unsigned int T_VibH   , T_VibL;				// Частота вибропривода.
    unsigned int Cnt_PlsL , Cnt_PlsH;			// Счетчик +.
    unsigned int Cnt_MnsL , Cnt_MnsH;			// Счетчик -.
    unsigned int Cnt_DifL , Cnt_DifH;			// Разность счетчиков.
    unsigned int F_rasL   , F_rasH;				// Частота расщепления.

    unsigned int HF_regL  , HF_regH;      // Выход регулятора ГВЧ.               НЕ СДЕЛАНО
    unsigned int HF_difL  , HF_difH;      // Сигнал ошибки регулячтора ГВЧ.      НЕ СДЕЛАНО

    unsigned int WP_regL  , WP_regH;

    unsigned int ADC1L, ADC2L, ADC3L, ADC4L, ADC5L;
    unsigned int ADC1H, ADC2H, ADC3H, ADC4H, ADC5H;


    LoopOn
//	Gyro .T_Vib=(unsigned int)((7680000*16/Gyro.Frq)*4096);
    Gyro.T_Vib=(unsigned int)((7680000*16/(Gyro.Frq>>12)));
    Gyro.Firmware_Version=0xff; /// промежуточная затычка

    T_VibH   = ( Gyro.T_Vib>>8  ) TakeByte;
    T_VibL   = ( Gyro.T_Vib     ) TakeByte;

    Cnt_PlsH = ( Gyro.Cnt_Pls>>8) TakeByte;
    Cnt_PlsL = ( Gyro.Cnt_Pls   ) TakeByte;

    Cnt_MnsH = ( Gyro.Cnt_Mns>>8) TakeByte;
    Cnt_MnsL = ( Gyro.Cnt_Mns   ) TakeByte;

    Cnt_DifH = ( Gyro.Cnt_Dif>>8) TakeByte;
    Cnt_DifL = ( Gyro.Cnt_Dif   ) TakeByte;

    F_rasH   = ( Gyro.F_ras>>8  ) TakeByte;
    F_rasL   = ( Gyro.F_ras     ) TakeByte;

    ADC1H    = ( Spi.ADC1>>8    ) TakeByte;
    ADC1L		 = ( Spi.ADC1       ) TakeByte;

    ADC2H    = ( Spi.ADC2>>8    ) TakeByte;
    ADC2L		 = ( Spi.ADC2       ) TakeByte;

    ADC3H    = ( Spi.ADC3>>8    ) TakeByte;
    ADC3L		 = ( Spi.ADC3       ) TakeByte;

    ADC4H    = ( Spi.ADC4>>8    ) TakeByte;
    ADC4L		 = ( Spi.ADC4       ) TakeByte;

    ADC5H    = ( Spi.ADC5>>8    ) TakeByte;
    ADC5L		 = ( Spi.ADC5       ) TakeByte;

    WP_regH  = ( Spi.DAC_B>>8   ) TakeByte;
    WP_regL  = ( Spi.DAC_B      ) TakeByte;

    HF_regH=0xff;
    HF_regL=0xff;

    HF_difH=0xff;
    HF_difL=0xff;


    BuffTemp[ 0] = Gyro.SOC_Out;
    BuffTemp[ 1] = Gyro.My_Addres;

    BuffTemp[ 2] = Cnt_PlsH;//старший байт счетчика +.
    BuffTemp[ 3] = Cnt_PlsL;//младший байт счетчика +.

    BuffTemp[ 4] = Cnt_MnsH;//старший байт счетчика -.
    BuffTemp[ 5] = Cnt_MnsL;//младший байт счетчика -.

    BuffTemp[ 6] = Cnt_DifH;//старший байт разности счетчиков
    BuffTemp[ 7] = Cnt_DifL;//младший байт разности счетчиков

    BuffTemp[ 8] = F_rasH;
    BuffTemp[ 9] = F_rasL;

    BuffTemp[10] = HF_regH;
    BuffTemp[11] = HF_regL;

    BuffTemp[12] = HF_difH;
    BuffTemp[13] = HF_difL;

    BuffTemp[14] = T_VibH ;//
    BuffTemp[15] = T_VibL; //?

    BuffTemp[16] = Gyro.Firmware_Version;//ХЗ
    BuffTemp[17] = Gyro.Firmware_Version;//ХЗ

    BuffTemp[18] = T_VibH>>1;
    BuffTemp[19] = T_VibL;

    BuffTemp[20] = Gyro.Firmware_Version;//ХЗ
    BuffTemp[21] = Gyro.Firmware_Version;//ХЗ

    BuffTemp[22] = WP_regH;              //СРП    Spi.DAC_B
    BuffTemp[23] = WP_regL;              //СРП

    BuffTemp[24] = Gyro.Firmware_Version;// ХЗ
    BuffTemp[25] = Gyro.Firmware_Version;// ХЗ

    BuffTemp[26] = ADC1H;
    BuffTemp[27] = ADC1L;

    BuffTemp[28] = ADC1H;
    BuffTemp[29] = ADC1L;

    BuffTemp[30] = ADC1H;
    BuffTemp[31] = ADC1L;

    BuffTemp[32] = ADC1H;
    BuffTemp[33] = ADC1L;

    BuffTemp[34] = ADC1H;
    BuffTemp[35] = ADC1L;

    BuffTemp[36] = 0x00;//ADC6
    BuffTemp[37] = 0x00;//ADC6

    BuffTemp[38] = Gyro.Firmware_Version;
    BuffTemp[39] = Gyro.Firmware_Version;
    BuffTemp[40] = Gyro.Firmware_Version;
    BuffTemp[41] = Gyro.Firmware_Version;


    Check(BuffTemp, 44);
    WriteConN (BuffTemp,44);
    LoopOff
}

void CMD_M_Control_D8()///установка\сброс регистров управления
{
    int bit,NReg;
    BuffTemp[0] = Gyro.SOC_Out;      //DD
    BuffTemp[1] = Gyro.My_Addres;    //00
    BuffTemp[2] = Gyro.CMD_In;       //D8
    //10?
    if((Param1&0x80)) {
        bit=1;
    } else {
        bit=0;
    }

    if ((Param1 & 0x10) == 0) {
        NReg=0;
        switch(Param1&0xf) { // Gyro.RgConA
            case 0x0:
                Gyro.RgConA|=bit;
                break;

            case 0x1:
                Gyro.RgConA|=bit<<0x1;
                break;

            case 0x2:
                Gyro.RgConA|=bit<<0x2;
                break;

            case 0x3:
                Gyro.RgConA|=bit<<0x3;
                break;

            case 0x4:
                Gyro.RgConA|=bit<<0x4;
                break;

            case 0x5:
                Gyro.RgConA|=bit<<0x5;
                break;

            case 0x6:
                Gyro.RgConA|=bit<<0x6;
                break;

            case 0x7:
                Gyro.RgConA|=bit<<0x7;
                break;
        }
        BuffTemp[3] = NReg<<4;
        BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
        BuffTemp[5] =  Gyro.RgConA     & 0xff;
    } else {
        NReg=1;            // Gyro.RgConB
        switch(Param1&0xf) { // Gyro.RgConB
            case 0x0:
                Gyro.RgConB|=bit;
                break;
            case 0x1:
                Gyro.RgConB|=bit<1;
                break;
            case 0x2:
                Gyro.RgConB|=bit<2;
                break;
        }
        BuffTemp[3] =  NReg<<4;
        BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
        BuffTemp[5] =  Gyro.RgConB      & 0xff;
    }

    Check(BuffTemp, CRC_N);
    WriteConN (BuffTemp,CRC_N);
}

void CMD_M_Control_D9()///чтение регистров управления
{
    int bit,NReg;
    BuffTemp[0] = Gyro.SOC_Out;      //DD
    BuffTemp[1] = Gyro.My_Addres;    //00
    BuffTemp[2] = Gyro.CMD_In;       //D9
    if ((Param1 & 0x10) == 0) {
        BuffTemp[3]=0<<4;
        BuffTemp[4] = (Gyro.RgConA>>8 )& 0xff;
        BuffTemp[5] =  Gyro.RgConA     & 0xff;
    } else {
        BuffTemp[3]=1<<4;
        BuffTemp[4] = (Gyro.RgConB>>8 ) & 0xff;
        BuffTemp[5] =  Gyro.RgConB      & 0xff;
    }
    Check(BuffTemp, CRC_N);
    WriteConN (BuffTemp,CRC_N);
}
// (номер (код) ЦАПа 0...3, старший байт требуемого ЦАПа, младший байт треб ЦАПа)
void CMD_M_Stymul()
{
    int temp;
    temp =((BuffTemp[4]<<8) | BuffTemp[5]);
    // temp=0;
    temp=temp&0xFFFF;
    DACF =(temp*K_DAC)+deltaDAC;
    Spi.DAC_B =(unsigned int)(DACF) /*(unsigned int)(temp*K_DAC+deltaDAC)*/; // K_DAC);

    //int temp;
//	temp =((BuffTemp[4]<<8) | BuffTemp[5]);
    //temp = 65535 - temp;
    /*if(temp<1000)
    {
    	temp=0;
    }
    else if(temp>60000)
    {
    	temp=65535;
    }*/
    //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
    //BuffTemp[0] = temp;      //DD
    //WriteConN (BuffTemp,1);
}

unsigned int Check(char *c, unsigned int Count)
{
    int i=1;
    unsigned int temp,CRC;


    temp=1;
    CRC=0;


    for(; i<Count-2; i++) {
        CRC+=c[i];
    }

    if(c[Count-2]!=((CRC>>8)&0xFF)) {
        temp=0;
        Gyro.RsErrLine = (Gyro.RsErrLine)&=0x2;
    }

    if(c[Count-1]!=((CRC>>0)&0xFF)) {
        temp=0;
//		Gyro.RsErrLine= (Gyro.RsErrLine)=0x2;
    }

    c[Count-2]=(CRC>>8)&0xFF;
    c[Count-1]=(CRC>>0)&0xFF;



    return temp;
}

int Getlengf(void)
{
    unsigned int lengf=0;
    switch(Gyro.CMD_In) {
        case 0x99://Gyrotainance
            lengf=6;
            CRC_N=8;
            break;

        case 0x0A: //m_stymul
            lengf=8;
            break;

        case 0xA5://DeviceMode

            lengf=6;
            CRC_N=8;
            break;

        case 0xDD://m_rate

            lengf=6;
            CRC_N=44;
            break;

        case 0xB0://DeviceMode

            lengf=6;
            CRC_N=8;
            break;

        case 0xD8://m_control

            lengf=6;
            CRC_N=8;
            break;

        case 0xD9://m_control

            lengf=6;
            CRC_N=8;
            break;
    }
    return lengf;
}

void Read_CMD(void)
{
    Gyro.SOC_Out=0xDD;
    Gyro.RsErrLine = (Gyro.RsErrLine)& 0xffff;


    CountBuFFIn=ReadChekCon(BuffTemp);
    if(CountBuFFIn==1) {
        if (BuffTemp[0] != SOC_In) {
            ReadCon(BuffTemp);
            Gyro.RsErrLine += 0x100;//WriteCon("\n\r ...Error.... ");
            BuffTemp[99]=Gyro.RsErrLine;
        }
    } else if(CountBuFFIn==2) {
        if (BuffTemp[1] != Gyro.My_Addres) {
            ReadCon(BuffTemp);
            Gyro.RsErrLine += 0x1;//WriteCon("\n\r ...Error_ADDRES.... ");
        }
    } else if(CountBuFFIn==3) {
        Gyro.CMD_In=BuffTemp[2];
        N=Getlengf();
    } else if(CountBuFFIn==4 && N==6) {
        Param1=BuffTemp[3];
    } else if((CountBuFFIn==5)&&(N==7)) {
        //LoopOn
        Param1=BuffTemp[3];
        Param2=BuffTemp[4];

    } else if((CountBuFFIn==6)&&(N==8)) {
        Param1=BuffTemp[3];
        Param2=BuffTemp[4];
        Param3=BuffTemp[5];
    } else if(CountBuFFIn > (N-1)) {
        ReadCon(BuffTemp);
        if(Check(BuffTemp, CountBuFFIn)) {
            switch(Gyro.CMD_In) {
                case CMD_MAINT:
                    CMD_Maintenance();
                    break;
                case 0xD8:
                    CMD_M_Control_D8();
                case 0xD9:
                    CMD_M_Control_D9();
                    break;
                case 0x0A:
                    CMD_M_Stymul(Param1,Param2,Param3);
                    break;
                case 0xDD:
                    Rate_Flag=1;
                    break;
                    Gyro.RsErrLine = 0;
            }

        }
    }
}
Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	09 Jan 2018
Imports:	4
Forks:	0
Commits:	233
Dependents:	0
Dependencies:	1
Followers:	1
Important changes to repositories hosted on mbed.com

Command.c

File content as of revision 30:17c84ed091b3:

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

Command.c

File content as of revision 30:17c84ed091b3:

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
