/******************************************************************************
文 件 名;UDS_ISO14229_Services.c
功能描述;ISO 14229 规范规定的诊断服务服务器端服务实现函数库文件
作    者;张暄
版    本;V1.0
日    期;2016.11.1
******************************************************************************/
//#include "Include.h"
//#include "Const.h"


#include <hidef.h>           /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include "mc9s12xhy256.h"
#include "stdint.h"
#include <string.h> 
#include "comdef.h"
#include "Unit_Conv.h"


//#include "AutoGUI.h"
#include "main.h" 

/* Driver */
#include "CRG.h"
#include "ADC.h"
#include "GPIO.h"
#include "MSCAN0.h"
#include "PWM.h"
#include "SCI0_UART.h"

#include "Sound_List.h"

/* System */ 
#include "PowerManagement.h"
#include "API.h" 
#include "Line_In.h"
#include "Key.h"
#include "CRG.h"

/* Protocol */
#include "CAN_Communication_Matrix.h"
#include "Analog_Signal_Conversion.h"
#include "Communication_Over_CAN.h"
#include "Simulated_IIC_Master.h"
#include "Simulated_SPI_Master.h"
#include "YGV642_SPI.h"

/* Device */
#include "EEPROM_24Cxx.h"
#include "EEPROM_24Cxx_Remap.h"
#include "ISD2360.h"
#include "LED.h"
#include "GUI.h"
#include "YGV642.h"

/* Module */
#include "Flash_synchronizer.h"

#include "Non_volatile_Memorys.h"
#include "Sound_Player.h"
#include "System_Status_Monitor.h"
#include "TFT_LCD.h"

/* Implementation */
#include "Popup_List.h"
#include "IO_Manager.h"
#include "Popup_Scheduler.h"
#include "Sound_Scheduler.h"
#include "Warning_Processor.h"
#include "Data_Processor.h"
#include "IPC.h"

/* Application */

#include "Telltales.h"
//#include "Back_Light.h"
#include "Drive_Info.h"
#include "Gauges.h"
#include "Menus.h"
#include "Popups.h"

#include "Startup_Animation.h"

#include "FuelConfig.h" 
//#include "FuelConfig1.h" 
 
 
/*Diagnostic*/
#include "Diag_ID_Def.h"
#include "DoCAN_ISO15765_Config.h"
#include "DoCAN_ISO15765.h"
#include "UDS_ISO14229_Services.h"
#include "Diag_ID_Def.h"
#include "UDS_ISO14229_Server.h"
#include "UDS_ISO14229_Server_Config.h"

#include "API_BD8379.h"
//#define TESTDIAG


NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U007388;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U300316;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U300317;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U014087;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U010087;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U012987;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U012887;
NVMDTC_SnapshotUnion             NVMDTC_Snapshot_U012887;

const uint32_t MASK0_Safe= 0x8EACBD9F;
const uint32_t MASK1_Safe= 0x1CBE5D3A;
const uint32_t MASK2_Safe= 0x2CBDE543 ;
DIGKeyValue      DigKeyValue; 

//extern void UDS_Service_Response ( uint8_t si, uint8_t RspType, uint16_t A_TA_type, uint16_t A_Length, uint8_t *A_Data );

static uint8_t Send_Command[] =
{
    0x1C, 0x01, 0x06, 0x80, 0x1F, 0x01, 0x06, 0x80, 0xFB, 0x3D
};

typedef struct
{
    uint8_t code[15];             /* Structure required to copy code to ram memory */

} FnCmdInRamStruct;

typedef void ( * near pFnCmdInRam ) ( void );


#pragma CODE_SEG __NEAR_SEG NON_BANKED

static void FnCmdInRam_ ( void )
{
    FSTAT = 0x80U;                       /* Clear flag command buffer empty */

    while ( FSTAT_CCIF == 0U ) {}        /* Wait to command complete */

    return;
}

#pragma MESSAGE DISABLE C1805          /* Disable warning C1805 "Non-standard conversion used" */
/*lint -save  -e740 -e931 Disable MISRA rule (1.2) checking. */
static void CallFnCmdInRam()
{
    FnCmdInRamStruct FnCmdInRam = * ( FnCmdInRamStruct * ) ( FnCmdInRam_ ); /* Create a copy of Wait in RAM routine on stack */
    ( ( pFnCmdInRam ) &FnCmdInRam ) ();  /* Call code in RAM */
    return;
}

#pragma CODE_SEG DEFAULT

//extern NVMConfigF110Union    NVMConfigF110;
//extern NVMConfigF101Union    NVMConfigF101;
//extern NVMUserSettingsUnion  NVMUserSettings;
INT8U EraserFlag;
INT8U EraseMemoryFF[128];
INT8U  APPFLAG;
DiagSendDataRes PosRes;
DiagSendDataNeg NegRes;
uint8_t S3_ServerEN;        // 扩展会话5s定时器使能开关
uint16_t S3_ServerCnt;       //扩展会话定时器计数器
uint8_t S3_Server_refresh;  //刷新扩展会话S3时间标志
uint16_t S19DataCntOfReceive; //S19数据的接收总长度
SRecord_t srcd;
//uint16_t CheckSum;
uint8_t BlockSCT;   //BlockSequenceCounter   (1,2,...255,0)
uint8_t REC_DLlen;  //已经接收到的数据个数(多少字节)
uint8_t  DTCControlType;   //DTC记录控制开关  0
uint8_t BlocKCnt;           //S19 传输的block数目
uint8_t RequestTransfer;   //请求下载的标志
//uint32_t SectorAddress;     //扇区地址
uint32_t EraseAddress;      //擦除地址
uint8_t ControlType;       // 通信控制的类型
uint8_t CommunicationType; // 通信控制的类型
uint8_t Wait10s;           //27服务尝试失败需要等待10秒
uint8_t wait10cnt = 0;
uint8_t Wait10sFlag;        //等待10秒标志
uint8_t DiagLockFlag;       //27服务解锁标志
//uint8_t AttemptCnt;         //27服务尝试次数
//uint8_t RequestSeedCnt;      //请求种子的次数
uint8_t SeedkeyValidFlag;    //置1 表示秘钥生效
uint32_t ValidSeedKey1;       //表示有效的秘钥
//uint32_t ValidSeedKey4;       //表示有效的秘钥
uint32_t ValidSeedKey2;       //表示有效的秘钥
uint16_t SeedHigh;
uint16_t SeedLow;
uint8_t Services27_01_Requested;
uint8_t noDataLenth;
uint8_t Time10sLock;
uint16_t EngineSpeed;       //发动机转速
uint8_t MotorPosition;      //电机位置
uint8_t DisplayColor;       //需要显示的颜色
uint8_t ControlMotor;       //需要控制的是哪个电机

uint16_t ESpeedVal;         //转速
uint16_t VspeedVal;         //车速
uint8_t TempSeg;            //水温

uint8_t BuzzerStatus;       //蜂鸣器状态

uint8_t SessionType = 1;    //会话类型
uint8_t IO_Group_1;
uint8_t IO_Group_2;
uint8_t IO_Group_3;
uint8_t IO_Group_4;
uint8_t IO_MASK_1;
uint8_t IO_MASK_2;
uint8_t IO_MASK_3;
uint8_t IO_MASK_4;
uint8_t  Error;
uint32_t DFLASHeep_adr;
INT16U MYeturnCode;
uint8_t seed[4];//用于27服务存放种子可以为局部变量
uint32_t RANDOM;//用于随机数计算  必须为全局变量
const uint8_t XorArray[4] = {0x31, 0x23, 0x56, 0x71};
uint8_t UDS_ISO14229_Transfer[250];


uint8_t  ContrlorResumeofSOC;     //soc控制
uint8_t  SocSeg;                  //Soc
uint8_t  ContrlorResumeofEspeed;  //转速控制
uint16_t ESpeedValNUM;            //转速
uint8_t  ContrlorResumeofVspeed;  //车速控制
uint16_t VspeedValNUM;            //车速
//----hyq--20190827 定义变量
uint8_t  ContrlorResumeofGAS1  ;  //气压1

uint8_t  ContrlorResumeofLamp;    //所有报警灯测试
uint8_t LampWarningStatus;        //灯光状态


uint8_t  ContrlorResumeofBuzzer;  //蜂鸣器控制
uint8_t  ContrlorResumeofGauge;   //所有表头控制
uint8_t  ContrlorResumeofLCD;     //LCD控制
uint8_t  ContrlorResumeofIO;      //IO控制
uint8_t  ContrlorResumeofIOLast;  //IO控制

uint8_t  ContrlorResumeofTEMP;    //水温表控制
uint8_t  CAN_NORMAL_TT;
uint8_t  CAN_NORMAL_2SFF;
uint8_t  CAN_DTC_OFF;
uint8_t   ACC_OK_FF;

uint8_t  CANNetTxOFF = 0;
uint8_t  CANNetRxOFF = 0;
uint8_t  CoCanTxOff = 0;
DiagReceiveData diag;
Diag27Data  diag27;
DiagReceiveData1 diag1;
DiagReceiveData2 config;
uint8_t UPDOWNFLAG;
uint8_t DTCstatusCHGFF;
uint8_t CJINF[18];		//存储厂家信息(18位ASSIC字符串)(保存在EEPROM但不备份)
uint8_t DTC_STATUS[cnDTCslen];
RecDTC_WaitBusoff_STRUCT	RecDTC_Wait_NoBusoff[cnDTCslen];
INT8U HIGH_VOLTAGE;
INT8U LOW_VOLTAGE;
INT8U BUS_OFF_FLAG;
INT8U VoltageACKFF;
//DTC定时器
INT16U VoltageAbnormalTimer = 0;
INT16U BusoffTimer = 0;
INT16U DTCU010087Timer = 0;
INT16U DTCU010187Timer = 0;
INT16U DTCU012987Timer = 0;
INT16U DTCU012887Timer = 0;
INT16U DTCU014087Timer = 0;

extern INT8U  GUIIconSeatbeltRStatus;
extern INT8U  GUIIconDRLGStatus;
extern INT8U  GUIIconSVSYStatus;
extern INT8U  GUIIconHiTempRStatus;
extern INT8U  GUIIconLowFuelYStatus;
extern INT8U  GUIIconTirePressureYStatus;
extern INT8U  GUIIconEPBYStatus ;
extern INT8U  GUIIconCruiseGStatus  ;
extern INT8U  GUIIconEBDYStatus  ;
extern INT8U  GUIIconEPSYStatus  ;
extern INT8U  GUIIconCruiseStateGStatus  ;
extern INT8U  GUIIconTCUYStatus  ;
extern INT8U  GUIIconLowBatteryRStatus ;
extern INT8U  GUIIconESCOffYStatus;
extern INT8U  GUIIconESCYStatus;
extern INT8U  GUIIconHDCYStatus;
extern INT8U  GUIIconHDCGStatus;
extern INT8U  GUIIconAVHGStatus;
extern INT8U  GUIIconAVHRStatus;
extern INT8U  GUIIconDRLGStatus;
extern INT8U  GUIIconABSYStatus;
extern INT8U  GUIIconBrakeRStatus;
/*****************************************************************
FlashDriver.c
********************************************************************/
uint32_t ChangeLocalAddressToGlobalAddress ( uint32_t address )
{
#if 0

    if ( ( 0xF08000 <= address ) && ( address <= 0xF0BFFF ) )
    {
        address = address - 0xF08000 + 0x7C0000;
        return address;
    }

    if ( ( 0xF18000 <= address ) && ( address <= 0xF1BFFF ) )
    {
        address = address - 0xF18000 + 0x7C4000;
        return address;
    }

    if ( ( 0xF28000 <= address ) && ( address <= 0xF2BFFF ) )
    {
        address = address - 0xF28000 + 0x7C8000;
        return address;
    }

    if ( ( 0xF38000 <= address ) && ( address <= 0xF3BFFF ) )
    {
        address = address - 0xF38000 + 0x7CC000;
        return address;
    }

    if ( ( 0xF48000 <= address ) && ( address <= 0xF4BFFF ) )
    {
        address = address - 0xF48000 + 0x7D0000;
        return address;
    }

    if ( ( 0xF58000 <= address ) && ( address <= 0xF5BFFF ) )
    {
        address = address - 0xF58000 + 0x7D4000;
        return address;
    }

    if ( ( 0xF68000 <= address ) && ( address <= 0xF6BFFF ) )
    {
        address = address - 0xF68000 + 0x7D8000;
        return address;
    }

    if ( ( 0xF78000 <= address ) && ( address <= 0xF7BFFF ) )
    {
        address = address - 0xF78000 + 0x7DC000;
        return address;
    }

    if ( ( 0xF88000 <= address ) && ( address <= 0xF8BFFF ) )
    {
        address = address - 0xF88000 + 0x7E0000;
        return address;
    }

    if ( ( 0xF98000 <= address ) && ( address <= 0xF9BFFF ) )
    {
        address = address - 0xF98000 + 0x7E4000;
        return address;
    }

    if ( ( 0xFA8000 <= address ) && ( address <= 0xFABFFF ) )
    {
        address = address - 0xFA8000 + 0x7E8000;
        return address;
    }

    if ( ( 0xFB8000 <= address ) && ( address <= 0xFBBFFF ) )
    {
        address = address - 0xFB8000 + 0x7EC000;
        return address;
    }

    if ( ( 0xFC8000 <= address ) && ( address <= 0xFCBFFF ) )
    {
        address = address - 0xFC8000 + 0x7F0000;
        return address;
    }

    if ( ( ( address >= 0x4000 ) && ( address <= 0x7FFF ) ) || ( ( address >= 0xC000 ) && ( address <= 0xFFFF ) ) )
    {
        address = address | 0x7F0000; //7F E7FF
        return address;                                  //FFF8
    }

    if ( ( 0xFE8000 <= address ) && ( address <= 0xFEBFFF ) )
    {
        address = address - 0xFE8000 + 0x7F8000;
        return address;
    }
#endif
    return 0;

}
/******************************************************************************
Get Current Sector
******************************************************************************/
/*uint8_t GET_Global_sector_PRG ( uint32_t address )
{
#if 0
    uint8_t m8;
    //PAGE_F8       0xF88000 TO 0xF89FFF; //8K     7E0000 - 7E1FFF  (0--7)
    //PAGE_F9       0xF98000 TO 0xF9BFFF; //16K    7E4000 - 7E7FFF  (16--31)
    //PAGE_FA       0xFA8000 TO 0xFABFFF; //16K    7E8000 - 7EBFFF  (32--47)
    //PAGE_FB       0xFB8000 TO 0xFBBFFF; //16K    7EC000 - 7EFFFF  (48--63)
    //PAGE_FC       0xFC8000 TO 0xFCBFFF; //16K    7F0000 - 7F3FFF  (64--79)
    //PAGE_FD       0xFD8000 TO 0xFDBFFF; //16K    7F4000 - 7F7FFF  (80--95)    equivalent to ROM_4000
    //PAGE_FE       0xFE8000 TO 0xFEBFFF; //16K    7F8000 - 7FBFFF  (96--111)
    //PAGE_FF       0xFF8000 TO 0xFFBFFF; //15K    7FC000 - 7FFBFF  (112--126)  equivalent to ROM_C000
    m8 = ( uint8_t ) ( ( address - 0x7C0000 ) >> 10 );
    return m8;
#endif
}
*/

INT8U  D_P_FLASH_Erase_Sector ( INT8U mTYPE, INT32U  address )
{
    INT16U code;
    INT32U m32;

    if ( mTYPE == 0 )
    {
        code = 0x1200;
        m32 = 0xFFFFFFFE;
    }
    else
    {
        code = 0x0A00;
        //m32=0x00000007;
        m32 = 0xFFFFFFF8;
    }

    address = address & m32;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = code | ( ( address & 0x007F0000 ) >> 16 );   //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( INT16U ) address;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}
//==============================================================================
//DFLASH/PFLASH Program
//@param D_P_FlashSelect    1  D_Flash    0 P_Flash
//@param address            Global_Address
//@param ptr                Data  which will be write to the Global Address
//@param number_of_words    Number of words will be write to P_D_Flash
//==============================================================================
INT8U  D_P_FLASH_Program ( INT8U mTYPE, INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    INT8U i;
    INT16U code;
    INT32U m32;

    if ( mTYPE == 0 )
    {
        code = 0x1100;
        m32 = 0x00000001;
    }
    else
    {
        code = 0x0600;
        m32 = 0x00000007;
    }

    if ( ( address & m32 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = code | ( ( address & 0x007F0000 ) >> 16 ); //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( INT16U ) address;

    for ( i = 1; i <= number_of_words; i++ ) //fill appropriate number of words to FCCOB
    {
        FCCOBIX = i + 1;
        FCCOB = *ptr;
        ptr++;
    }

    cli();								//我加的
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;
    sei();								//我加的

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    return OK;
}
//==============================================================================
//DFLASH_Erase_Verify_Section
//
//入口:address 为全局地址, number_of_words 为要校验的字单元个数
//==============================================================================
INT8U DFLASH_Erase_Verify_Section ( INT32U address, INT16U number_of_words )
{
    if ( ( address & 0x00000007 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1000 | ( ( address & 0x00FF0000 ) >> 16 );
    FCCOBIX = 0x01;
    FCCOB = ( address & 0x0000FFFF );
    FCCOBIX = 0x02;
    FCCOB = number_of_words;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return NON_ERASED;
    else
        return ERASED;
}

//==============================================================================
//DFLASH/PFLASH_N_Program              2015-5-13
//@param address            Global_Address
//@param ptr                Data  which will be write to the Global Address
//@param number_of_words    Number of words will be write to P_D_Flash
//@param return             Result of Program P Flash
//==============================================================================
uint8_t  PP_FLASH_N_Program ( uint32_t  address, uint8_t *ptr, uint8_t number_of_bytes )
{
    INT8U mRUL;
    INT8U  i;
    INT8U  buff[8];

    if ( number_of_bytes == 0 )
        return  OK;

    for ( ;; )
    {
        __RESET_WATCHDOG();

        if ( number_of_bytes >= 8 )
        {
            mRUL = D_P_FLASH_Program ( 1, address, ( INT16U * ) ptr, 4 );

            if ( mRUL != OK )
                return mRUL;

            ptr = ptr + 8;
            address = address + 8;
            number_of_bytes = number_of_bytes - 8;
        }
        else
        {
            if ( number_of_bytes == 0 )
                return  OK;

            for ( i = 0; i < 8; i++ )
                buff[i] = 0xFF;

            for ( i = 0; i < number_of_bytes; i++ )
                buff[i] = ptr[i];

            mRUL = D_P_FLASH_Program ( 1, address, ( INT16U * ) buff, 4 );
            return mRUL;
        }
    }
}

uint32_t DFLASH_Read_Word ( INT32U  address )
{
    INT16U Data16;
    //获取逻辑地址
    uint32_t LogicAddress = address - DFLASH_FirstAddress;
    //DFLASH的PAGE寄存器设置
    EPAGE = (uint8_t)(LogicAddress / 0x400);
    //在PAGE中的相对地址
    LogicAddress %= 0x400;
    //将全局地址转化为逻辑地址
    Data16 = ( *(volatile unsigned short * )(0x800 + LogicAddress));
    return Data16;
}

INT8U  DFLASH_Program ( INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    unsigned int i;

    if ( ( number_of_words < 1 ) || ( number_of_words > 4 ) )
        return LENGTH_OUT_OF_RANGE;

    //check if address is aligned (global address [0] != 0)
    if ( ( address & 0x00000001 ) != 0 )
        return MISALIGNED_ADDRESS;

    //check if the word(s) is/are erased
    if ( ( DFLASH_Erase_Verify_Section ( address, number_of_words ) ) == NON_ERASED )
        return NON_ERASED;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1100 | ( ( address & 0x007F0000 ) >> 16 ); //0x00030000
    FCCOBIX = 0x01;
    FCCOB = ( address & 0x0000FFFF );

    for ( i = 1; i <= number_of_words; i++ ) //fill appropriate number of words to FCCOB
    {
        FCCOBIX = i + 1;
        FCCOB = *ptr;
        ptr++;
    }

    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}


INT8U  DFLASH_Erase_Sector ( INT32U  address )
{
    //size of sector is 256 BYTE
    //check if address is aligned (global address [0] != 0)
    if ( ( address & 0x00000001 ) != 0 )
        return MISALIGNED_ADDRESS;

    while ( FSTAT_CCIF == 0 ); //wait if command in progress

    FSTAT = 0x30;             //clear ACCERR and PVIOL
    FCCOBIX = 0x00;
    FCCOB = 0x1200 | ( ( address & 0x007F0000 ) >> 16 );   //0x00030000
    //FCCOB =0x1210;
    FCCOBIX = 0x01;
    FCCOB = ( unsigned int ) address;
    cli();
    CallFnCmdInRam();//asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)asm JSR Send_Command;   //equal to    FSTAT_CCIF=1;   while(FSTAT_CCIF==0)
    sei();

    if ( ( FSTAT & ( FSTAT_ACCERR_MASK | FSTAT_FPVIOL_MASK ) ) != 0 )
        return ACCESS_ERROR;

    if ( FSTAT_MGSTAT != 0 )
        return VERIFICATION_FAILED;

    return OK;
}

//==============================================================================
//DFLASH_N_Program
//
//入口:address 为全局地址
//
//     1<=number_of_words<=128
//==============================================================================
INT8U  DFLASH_N_Program ( INT32U  address, INT16U *ptr, INT8U number_of_words )
{
    INT8U mRUL = 0;

    for ( ;; )
    {
        //复位看门狗
        __RESET_WATCHDOG();

        if ( number_of_words >= 4 )
        {
            mRUL = DFLASH_Program ( address, ptr, 4 );

            if ( mRUL != OK )
                return mRUL;

            ptr = ptr + 4;
            address = address + 8;
            number_of_words = number_of_words - 4;
        }
        else
        {
            if ( number_of_words == 0 )
                return  OK;

            mRUL = DFLASH_Program ( address, ptr, number_of_words );
            return mRUL;
        }
    }
}

//====================================================
//从FLASH中读N个字节数据到RAM   (mLEN必须为偶数)
//====================================================
void ReadNbyteFrDFalsh ( INT16U *mP, INT16U mLEN )
{
    INT8U i;
    INT16U k;
    __RESET_WATCHDOG();    //复位看门狗定时器
    k = mLEN / 2;

    for ( i = 0; i < k; i++ )
    {
        mP[i] = DFLASH_Read_Word ( DFLASHeep_adr );
        DFLASHeep_adr = DFLASHeep_adr + 2;
    }
}
//====================================================
//写N个字节数据到FLASH   (mLEN必须为偶数)
//====================================================
void WriteNbyteToDFalsh ( INT16U *mP, INT16U mLEN )
{
    __RESET_WATCHDOG();    //复位看门狗定时器
    MYeturnCode = DFLASH_N_Program ( DFLASHeep_adr, mP, mLEN / 2 );
    DFLASHeep_adr = DFLASHeep_adr + mLEN;
}

//读取改写APP部分程序标志位
void WRITE_BZ_inDFLAH2Sector ( INT16U bz )
{
    INT8U mRUL = 0;
    if ( DFLASH_Erase_Sector ( DFLASH_APPvAddress ) != OK )
        return;

    mRUL=DFLASH_Program ( DFLASH_APPvAddress, &bz, 1 );
}

//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_2E_DATA_FROM_FLASH ( void )
{
    //INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_DiagAddress ) != 0x7AA7 )
    {
        diag.attemptcnt = 0;
    }
    else
    {
        DFLASHeep_adr = DFLASH_DiagAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag, sizeof ( diag ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_2E_DATA_TO_FLASH ( void )
{
    INT16U m16;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_DiagAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_DiagAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &diag, sizeof ( diag ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_DiagAddress, &m16, 1 );
}

//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_BACKUP_DATA_FROM_FLASH ( void )
{
    INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_BackupAddress ) != 0x7AA7 )
    {
        for ( i = 1; i < 64; i++ )
            diag1.DFEC[i] = 0x00;

        diag1.DFEC[0] = 0x20;

        //-----------------------
        for ( i = 1; i < 64; i++ )
            diag1.DFED[i] = 0x00;

        diag1.DFED[0] = 0x20;

        //-----------------------
        for ( i = 1; i < 64; i++ )
            diag1.DFEE[i] = 0x00;

        diag1.DFEE[0] = 0x20;

        //-----------------
        for ( i = 1; i < 64; i++ )
            diag1.DFEF[i] = 0x00;

        diag1.DFEF[0] = 0x20;
        //-------------------
    }
    else
    {
        DFLASHeep_adr = DFLASH_BackupAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag1, sizeof ( diag1 ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_BACKUP_DATA_TO_FLASH ( void )
{
    INT16U m16;
    INT16U MYeturnCode;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_BackupAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_BackupAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &diag1, sizeof ( diag1 ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_BackupAddress, &m16, 1 );
}
//========================================================
//从FLASH中读诊断协议2E内容参数到RAM
//========================================================
void Read_CONFIG_DATA_FROM_FLASH ( void )
{
    //INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_USERAddress ) != 0x7AA7 )
    {
        config.F101[0] = 0xF8;
        config.F101[1] = 0xAC;
        config.F101[2] = 0x00;
        config.F101[3] = 0x00;
        config.F101[4] = 0x00;
        config.F101[5] = 0x00;
        config.F101[6] = 0x00;
        config.F101[7] = 0x00;
        //-----------------------
        config.F110[0] = 0x03;
        config.F110[1] = 0xA8;
        config.F110[2] = 0x00;
        config.F110[3] = 0x00;
        config.F110[4] = 0x58;
        config.F110[5] = 0x5C;
        config.F110[6] = 0x0F;
        config.F110[7] = 0x00;
    }
    else
    {
        DFLASHeep_adr = DFLASH_USERAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &config, sizeof ( config ) );
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_CONFIG_DATA_TO_FLASH ( void )
{
    INT16U m16;
    INT16U MYeturnCode;
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_USERAddress );							//擦除第3个扇区(256字节)

    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_USERAddress + 2;
    WriteNbyteToDFalsh ( ( INT16U * ) &config, sizeof ( config ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_USERAddress, &m16, 1 );
}


//========================================================
//从FLASH中读诊断协议27内容参数到RAM
//========================================================
void Read_27_DATA_FROM_FLASH ( void )
{
   // INT8U i;

    if ( DFLASH_Read_Word ( DFLASH_LockAddress ) != 0x7AA7 )
    {
        diag27.attemptcnt = 0;
        diag27.RequestSeedCnt = 0;
        diag27.attemptcnt11 = 0;
        diag27.RequestSeedCnt11 = 0;
        Write_27_DATA_TO_FLASH();
    }
    else
    {
        DFLASHeep_adr = DFLASH_LockAddress + 2;
        ReadNbyteFrDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
        
        if(diag27.RequestSeedCnt != 0 || diag27.attemptcnt != 0){
            diag27.RequestSeedCnt = 3;
            diag27.attemptcnt = 3;
            Wait10sFlag = 1;
            wait10cnt = 0;
        }
    }
}
//========================================================
//写 诊断协议2E内容 参数到FALSH
//========================================================
void Write_27_DATA_TO_FLASH ( void )
{
    INT16U m16;
    //擦除27服务flash存储扇区
    MYeturnCode = DFLASH_Erase_Sector ( DFLASH_LockAddress );							//擦除第3个扇区(256字节)
    //清除失败退出函数
    if ( MYeturnCode != OK )
        return;

    DFLASHeep_adr = DFLASH_LockAddress + 2;
    //将数据写入到falsh中
    WriteNbyteToDFalsh ( ( INT16U * ) &diag27, sizeof ( diag27 ) );
    m16 = 0x7AA7;
    MYeturnCode = DFLASH_N_Program ( DFLASH_LockAddress, &m16, 1 );
}
/******************************************************************************
The service access point of the diagnostics application layer provides a number
of services that all have the same general structure. For each service, three
service primitives are specified:

- a service indication primitive, used by the diagnostics application layer, to
  pass data to the server function of the ECU diagnostic application;

- a service response primitive, used by the server function in the ECU
  diagnostic application, to pass response data provided by the requested
  diagnostic service to the diagnostics application layer;

- a service response-confirmation primitive, used by the server function in the
  ECU diagnostic application, to indicate that the data passed in the service
  response primitive is successfully sent on the vehicle communication bus the
  ECU received the diagnostic request on;
******************************************************************************/

/******************************************************************************
每一个服务由三个函数实现;

 1. 函数名; <service_name>_Indication
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;应用层通过该函数向诊断服务实现函数请求服务并将相关数据传送给诊断服
            务实现函数
            The indication primitive is used by the application layer, to
            indicate an internal event which is significant to the ECU
            diagnostic application and pass data about the requested diagnostic
            service to the server function of the ECU diagnostic application.
    参  数;A_TA_type ;源地址类型,物理寻址或功能寻址
            A_Length  ;传输的数据的长度
            A_Data    ;传输的数据
    返回值;无

 2. 函数名; <service_name>_Response
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;该函数由诊断服务实现函数发起,用于向应用层发送诊断服务函数生成的反
            馈数据
            The response primitive is used by the server function in the ECU
            diagnostic application, to initiate the service and pass response
            data provided by the requested diagnostic service to the
            application layer.
    参  数;RspType   ;反馈类型  POSITIVE_RSP 正反馈
                                  NEGATIVE_RSP 负反馈
            A_TA_type ;目标地址类型,发送数据请使用 DIAG_ID_Tx
            A_Length  ;传输的数据的长度
            A_Data    ;传输的数据
    返回值;无

 3. 函数名; <service_name>_Rsp_Confirm
             <service_name> 统一命名为 UDS_Service_<SI>
    功  能;反馈确认函数由应用层发起,用于向诊断服务实现方告知前一次向应用层发送
            反馈数据的传输结果
            The response-confirm primitive is used by the application layer
            to indicate an internal event, which is significant to the server
            application, and pass communication results of an associated
            previous service response to the server function in the ECU
            application.
    参  数;A_TA_type ;目标地址类型
            A_Result  ;传输结果   A_OK  正确传输
                                   A_ERR 传输失败
    返回值;无
******************************************************************************/
void Randomcnt ( void )
{
    RANDOM++;

    if ( RANDOM >= 0xffff00f0 )
        RANDOM = 0;
}

/*static void AppExecute ( void )
{
    COPCTL = 0x01;        //enable watchdog
    ARMCOP = 0x00;
    //value written to ARMCOP register different from 0xAA or 0x55 will reset
    //the MCU immediately.
}
*/
void S3_ServerCNT ( void )
{
    EngineSpeed = DATA_ENGINE_SPEED_ACTUAL;

    if ( Wait10sFlag == 1 )
    {
        wait10cnt++;

        if ( wait10cnt >= 100 )
        {
            Wait10sFlag = 0;
            wait10cnt = 0;;
            if(diag27.RequestSeedCnt >= 2){
              diag27.RequestSeedCnt = 1;
            }
            if(diag27.attemptcnt >= 2){
              diag27.attemptcnt = 1;
            }
            Write_27_DATA_TO_FLASH();
        }
    }

    if ( S3_ServerEN == 1 )
    {
        if ( S3_Server_refresh == 1 )
        {
            S3_Server_refresh = 0;
            S3_ServerCnt = 0;
        }

        S3_ServerCnt++;

        if ( S3_ServerCnt > S3_SERVER ) //S3超时
        {
            S3_ServerCnt = 0;
            S3_ServerEN = 0;
            SessionType = DefaultSession;
            SeedkeyValidFlag = 0;
            DiagLockFlag = 0;

            ContrlorResumeofIO = 0;
            ContrlorResumeofSOC = 0;	 //soc控制
            SocSeg = 0;			//Soc
            ContrlorResumeofEspeed = 0;  //转速控制
            ESpeedValNUM = 0;		   //转速 		  
            ContrlorResumeofVspeed = 0;  //车速控制	  
            VspeedValNUM = 0;		   //车速 
            ContrlorResumeofLamp = 0;
           // ISD_Stop_Loop();

            IO_Group_1 = 0;
            IO_Group_2 = 0;
            IO_Group_3 = 0;
            IO_Group_4 = 0;
            IO_MASK_1 = 0;
            IO_MASK_2 = 0;
            IO_MASK_3 = 0;
            IO_MASK_4 = 0;
            Com_RxStart();
            Com_TxStart();
            CANNetRxOFF = 0;
			      CANNetTxOFF = 0;
            Services27_01_Requested = 0;
            CAN_DTC_OFF = 0;
            
        }
    }
}

/******************************************************************************
10# - DiagnosticSessionControl
******************************************************************************/
void UDS_Service_10_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t  Service10DiagDataLength;
    uint32_t  FunorPhy;
    FunorPhy = A_TA_type;
    UDS_ISO14229_Transfer[1] = 0;
    UDS_ISO14229_Transfer[2] = 50;
    UDS_ISO14229_Transfer[3] = 0x00;
    UDS_ISO14229_Transfer[4] = 200;
    NegRes.code  = 0;                                                     
    Service10DiagDataLength = A_Length;
    S3_ServerCnt = 0;

    //数据长度异常，返回13负反馈
    if ( Service10DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; 
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取数据
    for ( i = 0; i < Service10DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //判断子服务是否支持
    switch(UDS_ISO14229_Transfer[0]){
      case 0x81 :
      case 0x01 :
        if( DefaultSession !=SessionType )
        {
          S3_ServerEN      = 1;
          S3_ServerCnt     = 50;
          SeedkeyValidFlag = 0;
          DiagLockFlag     = NormalKeyLock;
          //切换到默认会话
          SessionType = DefaultSession;
        }
        //10 01需要正反馈 10 81不需要正反馈
        if ( UDS_ISO14229_Transfer[0] == 0x01 )
        {
          UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );;
        }
        break;
        
      case 0x82 :
      case 0x02 :

        //会话不支持，返回7E负反馈
        if( DefaultSession ==SessionType ){
          if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
          NegRes.code = serviceNotSupportedInActiveSession;
          UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          break;
        }
        
        if ( UDS_ISO14229_Transfer[0] == 0x02 ){
            NegRes.code = requestCorrectlyReceivedResponsePending;
            UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            for(i = 0; i < 500; ++i){
                nop();
            }
            WRITE_BZ_inDFLAH2Sector ( 0x3AA3 ); //进入编程模式
        }else {
            WRITE_BZ_inDFLAH2Sector ( 0x8AA8 ); //进入编程模式
        }
        COPCTL = 0x01;        //enable watchdog
        ARMCOP = 0x00;
        //相同会话不进行操作
        if( ProgrammingSession !=SessionType )
        {
          Com_TxStop();
  				Com_RxStop();
  				CANNetRxOFF = 1;
  				CANNetTxOFF = 1;
          S3_ServerEN      = 1;
          S3_ServerCnt     = 0;
          SeedkeyValidFlag = 0;
          DiagLockFlag     = NormalKeyLock;
          //切换到下载会话
          SessionType = ProgrammingSession;
        }
        //10 02需要正反馈 10 82不需要正反馈
        if ( UDS_ISO14229_Transfer[0] == 0x02 ) 
        {
            UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
        }
        break;  
        
      case 0x83 :
      case 0x03 :
        //会话不支持，返回7F负反馈
        if( ProgrammingSession ==SessionType ){
        if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
          //NegRes.code = serviceNotSupportedInActiveSession;
          //UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          break;
        }
        //相同会话不进行操作
        if( ExtendedDiagnosticSession !=SessionType )
        {
          //重置扩展会话的定时器
          S3_Server_refresh = 1;
          S3_ServerEN      = 1;
          S3_ServerCnt     = 0;
          SeedkeyValidFlag = 0;
        //  DiagLockFlag     = NormalKeyLock;
          //切换到扩展会话
          SessionType = ExtendedDiagnosticSession;
        }
        
        DiagLockFlag     = NormalKeyLock; //重新进入
        
        //10 03需要正反馈 10 83不需要正反馈
        if ( UDS_ISO14229_Transfer[0] == 0x03 ) 
        {      
            UDS_Service_Response ( 0x10, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
        }
        break;
        
      default  ://子功能不支持，返回12负反馈
        if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
        NegRes.code = subFunctionNotSupported;    //返回sub Function 不支持
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        break;
    }
}


/*void CalculateKey ( uint32_t Seed )    //根据seed计算得出key值 分别计算出Level1 和 Level4 的值
{
    uint8_t i;
    uint8_t xorArray[4]={0x74,0xF7,0x15,0x17     };
    uint8_t calData[4];
    uint8_t key[4];
    uint8_t seedArray[4];
    
    DigKeyValue.Value=Seed;
    
    memcpy(seedArray,DigKeyValue.Msg,4);

    for(i = 0; i < 4; ++i)
    {
      calData[i] = seedArray[i] ^ xorArray[i];
    }
    
    key[0] = (((calData[3]&0x0F)<<4)|(calData[3]&0xF0));
    key[1] = (((calData[1]&0x0F)<<4)|((calData[0]&0xF0)>>4));
    key[2] = ((calData[1]&0xF0)|((calData[2]&0xF0)>>4));
    key[3] = (((calData[0]&0x0f)<<4)|(calData[2]&0x0F));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey1 = DigKeyValue.Value;
    
    key[0] = (((calData[3]&0x07)<<5)|(calData[0]&0xF8)>>3);
    key[1] = (((calData[0]&0x07)<<5)|((calData[2]&0x1F)>>4));
    key[2] = ((calData[1]&0xF8)|((calData[3]&0xE0)>>5));
    key[3] = (((calData[2]&0xF8))|(calData[1]&0x07));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey2 = DigKeyValue.Value;
    
    key[0] = (((calData[2]&0x03)<<6)|(calData[3]&0xFC)>>2);
    key[1] = (((calData[3]&0x03)<<6)|((calData[0]&0x3F)));
    key[2] = ((calData[0]&0xFC)|((calData[1]&0xC0)>>6));
    key[3] = (((calData[1]&0xFC))|(calData[2]&0x03));
    
    memcpy(DigKeyValue.Msg, key,4); 
    
    ValidSeedKey4 = DigKeyValue.Value;
    
    SeedkeyValidFlag = 1;     
}  */

/* The array for SEED starts with [1], the array for KEY starts with [0] */
/* seed contains the SEED from the ECU */
/* length contains the number of bytes of seed */
/* key contains the KEY to send to ECU */
/* retLen contains the number of bytes to send to ECU as key */



//void CalculateKey(uint32_t Seed , DWORD length, uchar *key, DWORD *retLen)
void CalculateKey(uint32_t Seed,const uint32_t mask)
{
uint32_t i;
uint8_t key[4];
union
{
    uchar byte[4];
    uint32_t wort;
}seedlokal;
//const uint32_t mask = 0x1CBE5D3A;

//const uint32_t mask = 0x8EACBD9F;
if ((Seed>>16) == 0)
return;
else
{
    seedlokal.wort = Seed;
    for (i=0; i<35; i++)
    {
        if (seedlokal.wort & 0x80000000)
        {
            seedlokal.wort = seedlokal.wort << 1;
            seedlokal.wort = seedlokal.wort ^ mask;
        }
        else
        {
            seedlokal.wort = seedlokal.wort << 1;
        }
    }
    for (i=0; i<4; i++)
    {
       // key[3-i] = seedlokal.byte[i];
        key[i] = seedlokal.byte[i];         //20190726
    }
   (void) memcpy(DigKeyValue.Msg, key,4); 
   if( mask==MASK1_Safe)
    ValidSeedKey1 = DigKeyValue.Value;
   else if( mask==MASK2_Safe)
    ValidSeedKey2 = DigKeyValue.Value;
   // *retLen = length - 1;
}
SeedkeyValidFlag = 1;  
}  



//安全访问
void UDS_Service_27_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
   // uint16_t i;
   // uint32_t KeyReceive;
    uint8_t Service27DiagDataLength;
    //uint16_t SeedHigh;
    //uint16_t SeedLow;
    //uint8_t  Seed[4];
   // uint32_t Seedlong;
    uint16_t tempbuffer;
    uint32_t Diag_Id = 0;
    Diag_Id = A_TA_type;;
    //不支持功能寻址
    if ( Diag_Id == DIAG_ID_Rx_FUN ) 
        return;
    //供电电压异常，车速大于0，返回22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
  
    //不支持默认会话,返回7F负反馈
    if ( SessionType == DefaultSession ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
   
    S3_ServerCnt = 0;
    //获取数据长度
    Service27DiagDataLength = A_Length;
    //获取数据
    
    (void) memcpy(UDS_ISO14229_Transfer,A_Data,Service27DiagDataLength);
    
  
      //不支持除27 01和27 02以外的子服务，返回12负反馈
    if ( ( UDS_ISO14229_Transfer[0] != 0x01 ) && ( UDS_ISO14229_Transfer[0] != 0x02 ) && ( UDS_ISO14229_Transfer[0] != 0x05 ) && ( UDS_ISO14229_Transfer[0] != 0x06 ))
    {
        NegRes.code = subFunctionNotSupported;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
     //不支持除27 01和27 02以外的子服务，返回12负反馈
    if ( (( UDS_ISO14229_Transfer[0] == 0x01 ) || ( UDS_ISO14229_Transfer[0] == 0x02 )) && (SessionType != ExtendedDiagnosticSession))
    {
        NegRes.code = subfunctionNotSupportinActiveSession;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
    if (( ( UDS_ISO14229_Transfer[0] == 0x05 ) || ( UDS_ISO14229_Transfer[0] == 0x06 )) && (SessionType != ProgrammingSession))
    {
        NegRes.code = subfunctionNotSupportinActiveSession;
        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    //判断子服务类型
    switch ( UDS_ISO14229_Transfer[0] )
    {
        case 0x01:
            //数据长度异常，返回13负反馈
            if ( Service27DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength;
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }     
            //判断等待10s是否完成
            if(Wait10sFlag==1)
            { //超过最大尝试次数3次，或者连续4次发送请求种子而没有发送正确的秘钥 需要等待10秒 才可以进行一次尝试
                NegRes.code=requiredTimeDelayNotExpired;//延时10秒时间未达到
                UDS_Service_Response(0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
                return;
            }
            //请求标志位置1
            Services27_01_Requested = 1;
           
           //如果安全服务已解锁则不发送种子
            if (   DiagLockFlag == Level1UnLock|| DiagLockFlag == Level2UnLock  )
            {
                UDS_ISO14229_Transfer[0] = 0x01;
                UDS_ISO14229_Transfer[1] = 0;         //S1
                UDS_ISO14229_Transfer[2] = 0;         //S2
                UDS_ISO14229_Transfer[3] = 0;         //S3
                UDS_ISO14229_Transfer[4] = 0;         //S4
            }
            else
            {
              //请求次数+1
              ++diag27.RequestSeedCnt;
              //将27服务状态写入flash
              Write_27_DATA_TO_FLASH();
              //错误密钥达到最大次数，返回36负反馈
              if ( diag27.RequestSeedCnt > 2 ) //错误超过二次，需要等待10秒
              {
                  //将请求标志位清空
                  Services27_01_Requested = 0;
                  Wait10sFlag = 1;   //10秒计时器打开
                  wait10cnt = 0;     //10秒计时器重置
                  NegRes.code = exceedNumberOfAttempts; //最大次数
                  UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                  return;
              }  
              //如果当前种子无效，则生成一个种子
              if ( SeedkeyValidFlag == 0 ) 
              {
                  tempbuffer = ( uint16_t ) ( RANDOM );
                  SeedHigh = ( uint8_t ) ( tempbuffer >> 8 );
                  SeedLow = ( uint8_t ) ( tempbuffer & 0x00ff );
                  SeedkeyValidFlag = 1;
              }

              UDS_ISO14229_Transfer[0] = 0x01;
              UDS_ISO14229_Transfer[1] = 0x31 + ~SeedHigh;    //S1
              UDS_ISO14229_Transfer[2] = 0x23 + ~SeedLow;     //S2
              UDS_ISO14229_Transfer[3] = 0x56 + SeedHigh;     //S3
              UDS_ISO14229_Transfer[4] = 0x71 + SeedHigh;     //S4
            (void)   memcpy(DigKeyValue.Msg, UDS_ISO14229_Transfer+1,4);
              
              //根据种子计算密钥，为解锁做准备
              CalculateKey ( DigKeyValue.Value,MASK1_Safe );
            }
            UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            break;

        case 0x02:
            //数据长度不对，返回13负反馈
            if ( Service27DiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; 
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            //请求序列错误，返回24负反馈
            if ( Services27_01_Requested != 1 )
            {
                NegRes.code = requestSequenceError; //序列不对
                UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            
           (void)  memcpy(DigKeyValue.Msg, UDS_ISO14229_Transfer+1,4);

            //将请求标志位清空
            Services27_01_Requested = 0;

            if(DiagLockFlag == Level1UnLock){
               if(DigKeyValue.Value == 0x00000000){
                  //清空level 1尝试次数
                  diag27.attemptcnt = 0;
                  //清空level 1请求种子次数
                  diag27.RequestSeedCnt = 0;
                  UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
               }else{
                    //种子失效
                    SeedkeyValidFlag = 0;
                    //重新上锁
                    DiagLockFlag = 0;
                    //level 1尝试次数+1
                    diag27.attemptcnt = diag27.RequestSeedCnt;
                    //将27服务状态写入flash
                    Write_27_DATA_TO_FLASH();
                    //尝试次数超过设定值，开启10s定时器尝试次数减1，返回36负反馈
                    if ( diag27.attemptcnt >= 2 )
                    {
                        Wait10sFlag = 1;   //10秒计时器打开
                        wait10cnt = 0;
                        NegRes.code = exceedNumberOfAttempts; //最大次数
                        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }
                    //无效密钥，返回35负反馈
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
               }
           }else{
               //如果种子有效并且获取的密钥和计算的一致则解锁安全模式
               // if ( ( DigKeyValue.Value == ValidSeedKey1 ) && ( SeedkeyValidFlag == 1 ) )
                if ( ( DigKeyValue.Value == ValidSeedKey1 ) && ( SeedkeyValidFlag == 1 ))
                {
                    //安全服务解锁           0
                    DiagLockFlag = Level1UnLock;
                    //清空level 1尝试次数
                    diag27.attemptcnt = 0;
                    //清空level 1请求种子次数
                    diag27.RequestSeedCnt = 0;
                    //SeedkeyValidFlag = 1;
                    UDS_ISO14229_Transfer[0] = 0x02;
                    UDS_Service_Response ( 0x27, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
                    //将27服务状态写入flash
                    Write_27_DATA_TO_FLASH();                
                } 
                else
                {
                    #ifdef TESTDIAG
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                    #endif
                     
                    //种子失效
                    SeedkeyValidFlag = 0;
                    //level 1尝试次数+1
                    diag27.attemptcnt = diag27.RequestSeedCnt;
                    //将27服务状态写入flash
                    Write_27_DATA_TO_FLASH();
                    //尝试次数超过设定值，开启10s定时器尝试次数减1，返回36负反馈
                    if ( diag27.attemptcnt >= 2 )
                    {
                        Wait10sFlag = 1;   //10秒计时器打开
                        wait10cnt = 0;
                        NegRes.code = exceedNumberOfAttempts; //最大次数
                        UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                        return;
                    }
                    //无效密钥，返回35负反馈
                    NegRes.code = invalidKey; 
                    UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
            }
            break;  
        //子功能不支持,返回12负反馈
        default:
            NegRes.code = subFunctionNotSupported; 
            UDS_Service_Response ( 0x27, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}

extern uint8_t    BLPWMDutyCycle;
extern uint8_t GUIIconDRLGStatus;
/*************************************************************************************
诊断服务执行步骤;
  1、获取数据长度，判断数据长度是否有误
  2、获取目标地址类型，判断功能寻址和物理寻址
  3、根据长度获取数据。判断数据是否符合要求
  4、根据数据判断子服务是否符合要求
***************************************************************************************/


//DID读取服务
void UDS_Service_22_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
  uint16_t i;
  uint16_t DIDRequest;
  uint8_t  Service22DiagDataLength;
  uint32_t Diag_Id = 0;
 // uint16_t  m16;
 // uint16_t TempValue;
 // uint8_t  TempHi;
//  uint8_t  TempLo;
 // uint8_t  d[4];
 // uint32_t year;
  Diag_Id = A_TA_type;
  S3_ServerCnt = 0; 
  
    Service22DiagDataLength = A_Length;
    //数据长度异常，发挥13负反馈
    if ( Service22DiagDataLength != 2 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //供电电压异常，车速大于0，返回22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect; 
        UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    NegRes.code = 0;
    
    
    //获取数据
    for ( i = 0; i < Service22DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    DIDRequest = ( ( uint16_t ) UDS_ISO14229_Transfer[0] << 8 ) | ( ( uint16_t ) UDS_ISO14229_Transfer[1] );
    
    switch ( DIDRequest )
    {
      //软件刷写日期和设备等信息
      case 0xF184:
       //memcpy(UDS_ISO14229_Transfer+2,NVMConfigF184.Data,sizeof(NVMConfigF184.Data));
       memcpy(UDS_ISO14229_Transfer+2,diag.F184,sizeof(diag.F184));
       UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 9, UDS_ISO14229_Transfer );
      break;
      //ECU零件号
      case 0xF187:
          if(NVM_CONFIG_POWER == 1){//24V
              UDS_ISO14229_Transfer[2]  = '9';
              UDS_ISO14229_Transfer[3]  = '0';
              UDS_ISO14229_Transfer[4]  = '3';
              UDS_ISO14229_Transfer[5]  = '0';
              UDS_ISO14229_Transfer[6]  = '3';
              UDS_ISO14229_Transfer[7]  = '8';
              UDS_ISO14229_Transfer[8]  = '4';
              UDS_ISO14229_Transfer[9]  = '5';
              UDS_ISO14229_Transfer[10] = ' ';
              UDS_ISO14229_Transfer[11] = ' ';
              UDS_ISO14229_Transfer[12] = ' ';
              UDS_ISO14229_Transfer[13] = ' ';
          } else if(NVM_CONFIG_POWER == 0){//12V
              UDS_ISO14229_Transfer[2]  = '9';
              UDS_ISO14229_Transfer[3]  = '0';
              UDS_ISO14229_Transfer[4]  = '2';
              UDS_ISO14229_Transfer[5]  = '5';
              UDS_ISO14229_Transfer[6]  = '7';
              UDS_ISO14229_Transfer[7]  = '4';
              UDS_ISO14229_Transfer[8]  = '4';
              UDS_ISO14229_Transfer[9]  = '3';
              UDS_ISO14229_Transfer[10] = ' ';
              UDS_ISO14229_Transfer[11] = ' ';
              UDS_ISO14229_Transfer[12] = ' ';
              UDS_ISO14229_Transfer[13] = ' ';
          }
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 14, UDS_ISO14229_Transfer );
      break;
      //ECU供应商ID
      case 0xF18A:
          UDS_ISO14229_Transfer[2] = '2';
          UDS_ISO14229_Transfer[3] = '0';
          UDS_ISO14229_Transfer[4] = '2';
          UDS_ISO14229_Transfer[5] = '6';
          UDS_ISO14229_Transfer[6] = '5';
          UDS_ISO14229_Transfer[7] = '6';
          UDS_ISO14229_Transfer[8]  = ' ';
          UDS_ISO14229_Transfer[9]  = ' ';
          UDS_ISO14229_Transfer[10]  = ' ';
          UDS_ISO14229_Transfer[11]  = ' ';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );
      break;
      //车辆特征信息
     case 0xF1A8:
          memcpy(UDS_ISO14229_Transfer+2,NVMConfigF1A8.Data,sizeof(NVMConfigF1A8.Data));
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 22, UDS_ISO14229_Transfer );   
      break;
      //VIN码
      case 0xF190:
          memcpy(UDS_ISO14229_Transfer+2,NVMConfigF190.Data,sizeof(NVMConfigF190.Data));
          
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 19, UDS_ISO14229_Transfer );    
      break;   
      //供应商定义的ECU硬件版本编号
      case 0xF191:
          //memcpy(UDS_ISO14229_Transfer+2,diag.F191,sizeof(diag.F191));  //默认值空格
          UDS_ISO14229_Transfer[ 2] = 'H';
          UDS_ISO14229_Transfer[ 3] = 'W';
          UDS_ISO14229_Transfer[ 4] = ':';
          UDS_ISO14229_Transfer[ 5] = '1';
          UDS_ISO14229_Transfer[ 6] = '.';
          UDS_ISO14229_Transfer[ 7] = '0';
          UDS_ISO14229_Transfer[ 8] = '.';
          UDS_ISO14229_Transfer[ 9] = '4';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 10, UDS_ISO14229_Transfer );   
      break;
      //供应商定义的ECU软件版本编号
      case 0xF1A0:
         // memset(UDS_ISO14229_Transfer+2,0x20,16);  //默认值空格
          UDS_ISO14229_Transfer[ 2] = 'S';
          UDS_ISO14229_Transfer[ 3] = 'W';
          UDS_ISO14229_Transfer[ 4] = ':';
          UDS_ISO14229_Transfer[ 5] = 'a';
          UDS_ISO14229_Transfer[ 6] = '.';
          UDS_ISO14229_Transfer[ 7] = '0';
          UDS_ISO14229_Transfer[ 8] = '0';
          UDS_ISO14229_Transfer[ 9] = '.';
          UDS_ISO14229_Transfer[10] = '0';
          UDS_ISO14229_Transfer[11] = '8';
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
      //引导软件识别信息
      case 0xF183:
          memcpy(UDS_ISO14229_Transfer+2,diag.F183,sizeof(diag.F183));  //默认值空格
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
      //车型信息配置
     /* case 0xF101:
          memcpy(UDS_ISO14229_Transfer+2,NVMConfigF101.Data,sizeof(NVMConfigF101.Data));
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 6, UDS_ISO14229_Transfer );     
      break;
      */
      case 0x3300:
          memcpy(UDS_ISO14229_Transfer+2,NVMConfig3300.Data,sizeof(NVMConfig3300.Data));
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 18, UDS_ISO14229_Transfer );     
      break;
      //ECU序列号数据标识符
      case 0xF18C:
          UDS_ISO14229_Transfer[ 2] = 0;
          UDS_ISO14229_Transfer[ 3] = 0;
          UDS_ISO14229_Transfer[ 4] = 0;
          UDS_ISO14229_Transfer[ 5] = 0x20;
          UDS_ISO14229_Transfer[ 6] = 0x20;
          UDS_ISO14229_Transfer[ 7] = 0x20;
          UDS_ISO14229_Transfer[ 8] = 0x20;
          UDS_ISO14229_Transfer[ 9] = 19;
          UDS_ISO14229_Transfer[10] = 7;
          UDS_ISO14229_Transfer[11] = 28;
          UDS_Service_Response ( 0x22, POSITIVE_RSP, DIAG_ID_Tx, 12, UDS_ISO14229_Transfer );    
      break;
      default:
           if(Diag_Id == DIAG_ID_Rx_FUN )
            return ;
          NegRes.code = requestOutOfRange;             //返回31负反馈
          UDS_Service_Response ( 0x22, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
          break;
          
    } 
}





//通讯控制
void UDS_Service_28_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service28DiagDataLength;
    uint32_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
    //只支持扩展会话，返回7F负反馈
    if ( SessionType != ExtendedDiagnosticSession )
    {
        if(FunorPhy == DIAG_ID_Rx_FUN )
            return ;
        NegRes.code = serviceNotSupportedInActiveSession; 
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service28DiagDataLength = A_Length;
    //数据长度异常，返回13负反馈
    if ( Service28DiagDataLength != 2 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }                           
    //获取数据
    for ( i = 0; i < Service28DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    
    switch(UDS_ISO14229_Transfer[0])
	  {
	    
	  //使能收发
	  case 0x80:
	  case enableRxAndTx:
		 // CommunicationType=UDS_ISO14229_Transfer[0];
		 // ControlType=enableRxAndTx;
		 //发动机运行，返回22负反馈
  	/*	if ( EngineSpeed >= 400 )
      {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
      } */
		 //所有应用报文
		 if( 1 == UDS_ISO14229_Transfer[1])
		 	{
				Com_RxStart();
				Com_TxStart();
		 	}
		 	//所有网络管理报文
		 	else if (2 == UDS_ISO14229_Transfer[1])
		 	{
        CANNetRxOFF = 0;
        CANNetTxOFF = 0;
		 	}
		 	//所有应用报文和所有网络管理报文
		 	else if( 3 == UDS_ISO14229_Transfer[1] )
		 	{
				Com_RxStart();
				Com_TxStart();
		 		CANNetRxOFF = 0;
				CANNetTxOFF = 0;
		 	}
		 	//请求超出范围，返回31负反馈
		  else
		 	{
		 	//	if(FunorPhy == DIAG_ID_Rx_FUN )
      //    return ;
		 		NegRes.code = 	requestOutOfRange ;
				UDS_Service_Response(0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ));
				break;
		 	}
	 
		  if(bit_is_clear(UDS_ISO14229_Transfer[0],7))
		  UDS_Service_Response(0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
		break;
	
    //禁止收发       
	  case 0x83:
	  case disableRxAndTx:

		  //CommunicationType=UDS_ISO14229_Transfer[0];
		  //ControlType=disableRxAndEnableTx;
      //所有应用报文
			if( 1 == UDS_ISO14229_Transfer[1])
		 	{
				Com_TxStop();
				Com_RxStop();	
		 	}		 	
			//所有网络管理报文
		 	else if (2 == UDS_ISO14229_Transfer[1])
		 	{
        CANNetRxOFF = 1;
        CANNetTxOFF = 1;
		 	}
		 //所有应用报文和所有网络管理报文	
		 else if( 3 == UDS_ISO14229_Transfer[1] )
		 	{
				Com_TxStop();
				Com_RxStop();
				CANNetRxOFF = 1;
				CANNetTxOFF = 1;
		 	}
	 	 //请求超出范围，返回31负反馈
		 else
		 	{
  		 	
  		 //	if(FunorPhy == DIAG_ID_Rx_FUN )
       //   return ;
  		 	NegRes.code = 	requestOutOfRange ;
				UDS_Service_Response(0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ));
				break;
		 	}
      if(bit_is_clear(UDS_ISO14229_Transfer[0],7))
        UDS_Service_Response(0x28, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
      
      break;
	  //子功能不支持，返回12负反馈   
	  default:
  		if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
  		NegRes.code=subFunctionNotSupported;
  		UDS_Service_Response(0x28, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
		break;
	  }

}

//DTC记录控制
void UDS_Service_85_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service85DiagDataLength;
    uint32_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
   
    //只支持扩展会话，返回7F负反馈
    if ( SessionType == DefaultSession )
    { 
        if(FunorPhy == DIAG_ID_Rx_FUN )
          return ;
        NegRes.code = serviceNotSupportedInActiveSession;
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    } 
    //获取数据长度
    Service85DiagDataLength = A_Length;
    //数据长度异常，返回13负反馈
    if ( Service85DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength;
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
     //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    
    //获取数据
    for ( i = 0; i < Service85DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //子服务判断
    switch ( UDS_ISO14229_Transfer[0] )
    {   //01 01 开启DTC设置
        
        case 0x81:
        case DTCSettingOn:
            DTCControlType = DTCSettingOn;
            CAN_DTC_OFF = 0;

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x85, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );

            break;   
        //02 02 关闭DTC设置
        case 0x82:
        case DTCSettingOff:
            DTCControlType = DTCSettingOff;
            CAN_DTC_OFF = 1;

            if ( bit_is_clear ( UDS_ISO14229_Transfer[0], 7 ) )
                UDS_Service_Response ( 0x85, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
            
            break;

        default ://子功能不支持，返回12负反馈
             if(FunorPhy == DIAG_ID_Rx_FUN )
                return ;
            NegRes.code = subFunctionNotSupported;
            UDS_Service_Response ( 0x85, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}


//例程控制
void UDS_Service_31_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint8_t  Service31DiagDataLength;
   // uint8_t  EraserResult;
    uint16_t i;
    uint16_t RoutineID;
    uint32_t FunOrPhy;
    uint32_t checkSum;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;
    //只支持物理寻址
    if ( DIAG_ID_Rx_FUN == FunOrPhy )
        return;
    //供电电压异常，车速大于0，返回22负反馈
    //if ( EngineSpeed >= 400 )
     if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //只能编程会话？返回7F负反馈
    if ( SessionType != ProgrammingSession )
    {
        NegRes.code = serviceNotSupportedInActiveSession; 
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //安全访问被拒绝，返回33负反馈
    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取数据长度
    Service31DiagDataLength = A_Length;
    //数据长度异常，返回13负反馈
    if ( Service31DiagDataLength < 3 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取数据
    for ( i = 0; i < Service31DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    RoutineID = ( ( uint16_t ) UDS_ISO14229_Transfer[1] << 8 ) | ( uint16_t ) UDS_ISO14229_Transfer[2];

    switch ( RoutineID )
    {
        case MaintenanceReset:
            switch(UDS_ISO14229_Transfer[0]){
              case 0x01:
                wdt_reset();
                //NVM_USER_SETTINGS_SERVICE=5000;
                //NVM_Data_Update(NVM_USER_SETTINGS);
                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;
                UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                break;
              case 0x02:           
                break;
              case 0x03:
                break;
              default://请求超出范围，返回31负反馈  
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }
            break;

        case CheckMemory:
            switch(UDS_ISO14229_Transfer[0]){
              case 0x01:
                if(Service31DiagDataLength == 7){
                  checkSum = (((uint32_t)(UDS_ISO14229_Transfer[3])) << 24) | (((uint32_t)(UDS_ISO14229_Transfer[4])) << 16) | (((uint32_t)(UDS_ISO14229_Transfer[5])) << 8)| (((uint32_t)(UDS_ISO14229_Transfer[6])));
                  if(checkSum == 0xECF05989){
                    UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                  }else{
                    UDS_ISO14229_Transfer[3] = RoutineCompletedUnsuccessfully;
                    UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                  }
                }else{
                  NegRes.code = incorrectMessageLength;
                  UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                }
                break;
              case 0x02:           
                break;
              case 0x03:
                break;
              default://请求超出范围，返回31负反馈  
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }
            break;
            
        case CheckProgrammingPreconditions:
            switch(UDS_ISO14229_Transfer[0]){
              case 0x01:
                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;
                //执行ECU自检函数 返回值  0x04成功  0x05失败  待添加
                UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                break;
              case 0x02:           
                break;
              case 0x03:
                break;
              default://请求超出范围，返回31负反馈  
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }   
            break;
        
        case EraseMemory:
            switch(UDS_ISO14229_Transfer[0]){
              case 0x01:
                WRITE_BZ_inDFLAH2Sector ( 0x3AA3 ); //进入编程模式
                COPCTL = 0x01;        //enable watchdog
                ARMCOP = 0x00;
                break;
              case 0x02:           
                break;
              case 0x03:
                break;
              default://请求超出范围，返回31负反馈  
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            } 
            break;
        
        case CheckProgrammingDependencies:
            switch(UDS_ISO14229_Transfer[0]){
              case 0x01:
                UDS_ISO14229_Transfer[3] = RoutineCompletedSuccessfully;
                UDS_Service_Response ( 0x31, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                break;
              case 0x02:           
                break;
              case 0x03:
                break;
              default://请求超出范围，返回31负反馈  
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                break;
            }
            break;
        //子功能不支持，返回12负反馈           
        default:
            NegRes.code = subFunctionNotSupported; 
            UDS_Service_Response ( 0x31, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
}



//请求下载服务
void UDS_Service_34_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service34DiagDataLength;
   // uint32_t memoryAddress;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        return;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    Service34DiagDataLength = A_Length;

    if ( Service34DiagDataLength != 10 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
     //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    for ( i = 0; i < Service34DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( ( UDS_ISO14229_Transfer[0] != 0 ) || ( UDS_ISO14229_Transfer[1] != 0x44 ) )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x34, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    BlocKCnt =1;
    UDS_ISO14229_Transfer[0] = LengthFormatIdentifier;
    UDS_ISO14229_Transfer[1] = 0x00;
    UDS_ISO14229_Transfer[2] = 226;
    UDS_Service_Response ( 0x34, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
}

//数据下载服务
void UDS_Service_36_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t m8;
  //  uint8_t *p8;
    uint8_t Service36DiagDataLength;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;
    S3_ServerCnt = 0;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        return;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持  7F
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    if ( DiagLockFlag == BootKeyLock )
    {
        NegRes.code = securityAccessDenied; //安全模式未解锁   33
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    Service36DiagDataLength = A_Length;

    for ( i = 0; i < Service36DiagDataLength; i++ ) //取数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    //BlocKCnt++;

    if ( UDS_ISO14229_Transfer[0] != BlocKCnt )
    {
        NegRes.code = wrongBlockSequenceCounter; //请求序列错误    73
        UDS_Service_Response ( 0x36, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

   m8 =BlocKCnt;
   BlocKCnt++;
   
   UDS_ISO14229_Transfer[0]=m8;


    
    UDS_Service_Response ( 0x36, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
}

//请求下载服务
void UDS_Service_37_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
   // uint16_t i;
   // uint8_t Service37DiagDataLength;
    uint32_t FunOrPhy;
    //uint32_t SectorAddress;
    FunOrPhy = A_TA_type;

    if ( FunOrPhy == DIAG_ID_Rx_FUN )
        return;

    S3_ServerCnt = 0;

    if ( SessionType != ProgrammingSession ) //只支持编程会话模式
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持 7F
        UDS_Service_Response ( 0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
        //供电电压异常，车速大于0，返回会22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0)) 
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x10, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //if(RequestTransfer!=Download)
    //{
    //  NegRes.code=requestSequenceError;//会话模式不支持
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //Service37DiagDataLength=A_Length;
    //if(Service37DiagDataLength!=1)
    //{
    //  NegRes.code=incorrectMessageLength;//数据长度不对
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //for(i=0; i<Service37DiagDataLength; i++)  //长度不固定 要收数据
    //{
    //  UDS_ISO14229_Transfer[i]=*(A_Data+i);
    //}
    //if(UDS_ISO14229_Transfer[0]!=00)
    //{
    //  NegRes.code=subFunctionNotSupported;//数据长度不对
    //  UDS_Service_Response(0x37, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
    //  return;
    //}
    //UDS_ISO14229_Transfer[0]=0x00;
    UDS_Service_Response ( 0x37, POSITIVE_RSP, DIAG_ID_Tx, 0, UDS_ISO14229_Transfer );
}

void UDS_Service_3E_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service3EDiagDataLength;
    uint32_t  FunorPhy;
    FunorPhy = A_TA_type;
    Service3EDiagDataLength = A_Length;
    S3_ServerCnt = 0;
    //数据长度不对，返回13负反馈
    if ( Service3EDiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; 
        UDS_Service_Response ( 0x3E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取子服务信息
    for ( i = 0; i < Service3EDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //子服务不是00和80的，返回12负反馈不支持
    if ( ( UDS_ISO14229_Transfer[0] != 0 ) && ( UDS_ISO14229_Transfer[0] != 0x80 ) )
    //if ( UDS_ISO14229_Transfer[0] != 0 )
    {
        //吉利要求不响应
         if(FunorPhy == DIAG_ID_Rx_FUN )
            return ;
        NegRes.code = subFunctionNotSupported;
        UDS_Service_Response ( 0x3E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    S3_Server_refresh = 1;
    //3E 00有正反馈，3E 80没有正反馈
    if ( UDS_ISO14229_Transfer[0] == 0x00 )
    {
        UDS_Service_Response ( 0x3E, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
    }
    return;
}

void UDS_Service_11_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service11DiagDataLength;
    uint32_t  FunorPhy;
    FunorPhy = A_TA_type;
    S3_ServerCnt = 0;
    Service11DiagDataLength = A_Length;
    //供电电压异常，车速大于0，返回22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //不支持默认会话，返回7F负反馈
    /*
    if ( ( SessionType == DefaultSession ) ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    */
    //数据长度异常，返回13负反馈
    if ( Service11DiagDataLength != 1 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取子功能
    for ( i = 0; i < Service11DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //1.6版本不用支持11 02和11 82服务
   // if ( ( UDS_ISO14229_Transfer[0] == 0x01 ) || ( UDS_ISO14229_Transfer[0] == 0x03 ) || ( UDS_ISO14229_Transfer[0] == 0x81 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ))
     if(( UDS_ISO14229_Transfer[0] == 0x01 )|| ( UDS_ISO14229_Transfer[0] == 0x81 ))     //仅支持01子功能
    {
        //将会话类型重置
        SessionType = 1;
        if( UDS_ISO14229_Transfer[0] == 0x01 ){
          
        UDS_Service_Response ( 0x11, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer );
          i = 3000;
          while(i--){
           	  	nop();
          }
        }
          COPCTL = 0x01;        //enable watchdog
          ARMCOP = 0x00;  
        //if ( ( UDS_ISO14229_Transfer[0] == 0x81 ) || ( UDS_ISO14229_Transfer[0] == 0x83 ) ||( UDS_ISO14229_Transfer[0] == 0x82 ))
       // {
       //     return;
       // }
        
      //  return;
    }
    else//子功能不支持，返回12负反馈
    {
        if ( FunorPhy == DIAG_ID_Rx_FUN )
            return;
        NegRes.code = subFunctionNotSupported; //子功能不支持
        UDS_Service_Response ( 0x11, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}


//故障码存储列表

const uint32_t DTC_CODE_TABLE[] =
{
    U007388,   //BUS OFF    
    U300316,   //输入电压低 Internal local voltage < 16V  
    U300317,   //输入电压高 Internal local voltage > 32V   
    U014087,   //BCM通讯丢失 0x18FE4021或0x18FFDB21报文丢失，T=100ms 丢失5帧报文以上。  
    U010087,   //EMS通讯丢失 0x18FEE641报文丢失，T=1000ms，丢失5帧报文以上   
    U012987,   //EBS/ESC通讯丢失 0x18FFE033报文丢失，T=500ms，丢失5帧报文以上   
    U012887,   //EPB 节点丢失 0x18FDA5EC报文丢失，T=100ms，丢失5帧报文以上 
    U010187,   //TCU丢失通信  0x1801D003报文丢失，T=100ms，丢失5帧报文以上 
};

const NVMDTC_SnapshotUnion * DTC_CODE_add_TABLE[] = 
{    
    &NVMDTC_Snapshot_U007388,
    &NVMDTC_Snapshot_U300316,
    &NVMDTC_Snapshot_U300317,
    &NVMDTC_Snapshot_U014087,
    &NVMDTC_Snapshot_U010087,
    &NVMDTC_Snapshot_U012987,
    &NVMDTC_Snapshot_U012887,
};          


//=================================================
//根据序号输出3个字节的DTC码
//=================================================
void GJXHOUTDTCCODE ( INT8U xh )
{
    INT32U m32;
    INT8U *p;
    m32 = DTC_CODE_TABLE[xh];
    p = ( INT8U * ) &m32;
    CJINF[0] = p[1];		//H
    CJINF[1] = p[2];    //M
    CJINF[2] = p[3];    //L
}

//============================================
//每次开ACC后初始化DTC STATUS
//============================================
void INIT_DTC_STATUS_IGN_ON ( void )
{
    //  INT8U i;
      CAN_NORMAL_TT = 0;
      CAN_NORMAL_2SFF = 0;
      BUS_OFF_FLAG = 0;
      VoltageACKFF = 0;
      HIGH_VOLTAGE = 0;
      LOW_VOLTAGE = 0;
      VoltageAbnormalTimer = 0;


      S3_ServerCnt = 0;
      S3_ServerEN = 0;
      SessionType = DefaultSession;
      SeedkeyValidFlag = 0;
      DiagLockFlag = 0;


      ContrlorResumeofIO = 0;    //IO控制


      IO_Group_1 = 0;
      IO_Group_2 = 0;
      IO_Group_3 = 0;
      IO_Group_4 = 0;
      IO_MASK_1 = 0;
      IO_MASK_2 = 0;
      IO_MASK_3 = 0;
      IO_MASK_4 = 0;
      //Com_RxStart();
      Com_TxStart();
      Services27_01_Requested = 0;
      CAN_DTC_OFF = 0;
      CANNetRxOFF = 0;
      CANNetTxOFF = 0;

    //for ( i = 0; i < cnDTCslen; i++ )
    //    DTC_STATUS[i] = DTC_STATUS[i] | 0x50; //第4,6 BIT置1
}


//============================================
//	20ms 任务
//============================================
void Record_DTCStatus_Wait_No_Busoff_xxms ( void )
{
    INT8U i;
    INT8U m8;
    INT8U status;

    for ( i = 0; i < cnDTCslen; i++ )
    {
        if ( RecDTC_Wait_NoBusoff[i].Flag )
        {
            RecDTC_Wait_NoBusoff[i].Timer++;

            if ( RecDTC_Wait_NoBusoff[i].Timer >= 25 )		//	500ms
            {
                RecDTC_Wait_NoBusoff[i].Flag = 0;
                RecDTC_Wait_NoBusoff[i].Timer = 0;

                if ( RecDTC_Wait_NoBusoff[i].Status != RecDTC_Wait_NoBusoff[i].LastStatus )
                {
                    status = 0x28;		//	0x29 0x00
                }
                else
                {
                    status = RecDTC_Wait_NoBusoff[i].Status;
                }

                m8 = (( DTC_STATUS[i] & 0x28 ) | status) & 0x09;

                if ( bit_is_set ( m8, 3 ) && bit_is_clear ( DTC_STATUS[i], 3 ) )
                {
                    DTCstatusCHGFF = 1;
                }

                DTC_STATUS[i] = m8;
            }
        }
    }
}
//===========================================================
//修改DTC状态记录
//出口:DTCstatusCHGFF=1表示DTC记录有改变(需要存储到EEPROM中)
//===========================================================

void ChangeDTCs_ST_PRG ( INT8U xh, INT8U status )
{
    INT8U m8;
    
    if( (CAN_NORMAL_2SFF == 0) )     //CAN信号中断
    return;

    //	if( ( HIGH_VOLTAGE==1)||(LOW_VOLTAGE==1) ) //电压正常吗
    //		return;

    if((VoltageACKFF == 1) && (xh != cnU300316))      //电压低
        return;

    if((status == 0x09) && (xh !=  cnU007388))
    {
        DTC_STATUS[xh] = 0x09;
        RecDTC_Wait_NoBusoff[xh].Flag = 1;
        RecDTC_Wait_NoBusoff[xh].Status = 0x09;
        RecDTC_Wait_NoBusoff[xh].LastStatus = 0x09;
    }
    else
    {
        RecDTC_Wait_NoBusoff[xh].LastStatus = status;

        m8 = (DTC_STATUS[xh] & 0x08) | status;
        if( bit_is_set(m8, 3) && bit_is_clear(DTC_STATUS[xh], 3) )
        {
            DTCstatusCHGFF = 1;
        }
        DTC_STATUS[xh] = m8;
    }
    

    if ( ( VoltageACKFF == 1 ) && ( xh != cnU300316 ) )
        return;
   
 
}



//========================================
//如果DTC记录有改变则存入EEPROM中     100ms
//========================================

void SaveDTCsToEEPROM ( void )
{
    INT8U i;
   // INT8U d[cnDTCslen];

    //if( (MSY.CAN_NORMAL_2SFF==0)||(MSY.ACC_OK_FF==0) )
    //return;

    if ( DTCstatusCHGFF )			//DTC记录有改变?,YES
    {
        DTCstatusCHGFF = 0;

        for ( i = 0; i < cnDTCslen; i++ )
            NVMDTC.Data[i] = DTC_STATUS[i] & 0x08;

        NVM_Data_Update ( NVM_DTC_DATA );
        //Write_NbyteTo_EEPROM(DTC_STATUS,cnEEP_DTC_ADR,cnDTCslen);
        //		Write_NbyteTo_EEPROM(d,cnEEP_DTC_ADR,cnDTCslen);
    }
}



//=========================================
//+++++++++100MS+++++++++++
//
//CAN进入NORMAL状态2S计时
//=========================================
void CAN_NORMAL_TIME_PRG ( void )
{
    if(IO_DISPLAY_MODE == IO_DISP_MODE_IGN_ON )
    {
        CAN_NORMAL_TT++;

        if ( CAN_NORMAL_TT == 50 ) // 2S?,YES	//有ACC 6S
        {
            CAN_NORMAL_TT = 0;
       
            CAN_NORMAL_2SFF = 1;   // =1表示CAN进入NORMAL超过2S
        }
    }
}





void CanLoseDetected ( void )
{  
    if( CAN_DTC_OFF == 0 )
    {
        //电压判断
        if ( NVM_CONFIG_POWER==1 )
        {
            if ( ANALOG_KL15_VOLTAGE_VALID == 1 ) 
            {
                if( ( ANALOG_KL15_VOLTAGE < 16000 ) || ( ANALOG_KL15_VOLTAGE > 32000 ) )
                {
                    VoltageACKFF = 1;
                }
                else
                {
                    VoltageACKFF = 0;
                }
                if ( ( ANALOG_KL15_VOLTAGE < 18000 ) || ( ANALOG_KL15_VOLTAGE > 31000 ) )
                {
                    if ( VoltageAbnormalTimer <= 500 )
                    {
                        VoltageAbnormalTimer++;
                    }
                    else
                    {
                        if ( ANALOG_KL15_VOLTAGE > 32000 )
                        {
                            HIGH_VOLTAGE = 1;
                        }

                        if ( ANALOG_KL15_VOLTAGE < 18000 )
                        {
                            LOW_VOLTAGE = 1;
                        }
                    }
                }
                else
                {
                    if ( ( ANALOG_KL15_VOLTAGE > 18500 ) || ( ANALOG_KL15_VOLTAGE < 30500 ) )
                    {
                        if ( VoltageAbnormalTimer != 0 )
                        {
                            VoltageAbnormalTimer--;
                        }
                        else
                        {
                            if ( HIGH_VOLTAGE == 1 )
                            {
                                HIGH_VOLTAGE = 0;
                            }
                            if ( LOW_VOLTAGE == 1 )
                            {
                                LOW_VOLTAGE = 0;
                            } 
                        }
                    }
                }
            }
        }
        else
        {
            if ( ANALOG_KL15_VOLTAGE_VALID == 1 )
            {
                if( ( ANALOG_KL15_VOLTAGE < 7000 ) || ( ANALOG_KL15_VOLTAGE > 18000 ) )
                {
                    VoltageACKFF = 1;
                }
                else
                {
                    VoltageACKFF = 0;
                }
                if ( ( ANALOG_KL15_VOLTAGE < 9000 ) || ( ANALOG_KL15_VOLTAGE > 16000 ) )
                {
                    if ( VoltageAbnormalTimer <= 500 )
                    {
                        VoltageAbnormalTimer++;
                    }
                    else
                    {
                        if ( ANALOG_KL15_VOLTAGE > 16000 )
                        {
                            HIGH_VOLTAGE = 1;
                        }

                        if ( ANALOG_KL15_VOLTAGE < 9000 )
                        {
                            LOW_VOLTAGE = 1;
                        }
                    }
                }
                else
                {
                    if ( ( ANALOG_KL15_VOLTAGE > 9500 ) || ( ANALOG_KL15_VOLTAGE < 15500 ) )
                    {
                        if ( VoltageAbnormalTimer != 0 )
                        {
                            VoltageAbnormalTimer--;
                        }
                        else
                        {
                            if ( HIGH_VOLTAGE == 1 )
                            {
                                HIGH_VOLTAGE = 0;
                            }
                            if ( LOW_VOLTAGE == 1 )
                            {
                                LOW_VOLTAGE = 0;
                            } 
                        }
                    }
                }
            }
        }
        
        //高压
        if ( HIGH_VOLTAGE ) 
        {
            if ( ( IO_DISPLAY_MODE == IO_DISP_MODE_IGN_ON ) && ( CAN_NORMAL_2SFF ) )
                ChangeDTCs_ST_PRG ( cnU300317, 0x09 );
        }
        else
        {
            ChangeDTCs_ST_PRG ( cnU300317, 0x00 );
        }

        //低压
        if ( LOW_VOLTAGE )
        {
            if ( ( IO_DISPLAY_MODE == IO_DISP_MODE_IGN_ON ) && ( CAN_NORMAL_2SFF ) )
                ChangeDTCs_ST_PRG ( cnU300316, 0x09 );
        }
        else
        {
            ChangeDTCs_ST_PRG ( cnU300316, 0x00 );
        }

        //busoff
        if ( CAN0MISC_BOHOLD )
        {
            if( BusoffTimer == 100 )
            {
                if ( ( IO_DISPLAY_MODE == IO_DISP_MODE_IGN_ON ) && ( CAN_NORMAL_2SFF ) )
                {
                    ChangeDTCs_ST_PRG ( cnU007388, 0x09 );
                }
            }
            else
            {
                ++BusoffTimer;
            }
        }
        else
        {
            BusoffTimer = 0;
            ChangeDTCs_ST_PRG ( cnU007388, 0x00 );
        }
        
        //掉线
       if( ( HIGH_VOLTAGE == 0 ) && ( LOW_VOLTAGE == 0 ) && ( CAN0MISC_BOHOLD == 0 ) )
       {
           if(BCM_ExternalLight_STATUS&CAN_MSG_LOST)
           {
                if( DTCU014087Timer == 125 )
                {
                    ChangeDTCs_ST_PRG(cnU014087,0x09);
                }
                else
                {
                    ++DTCU014087Timer;
                }
           }
           else
           {
                DTCU014087Timer = 0;
                ChangeDTCs_ST_PRG(cnU014087,0x00);
           }

           if(EEPBS1_STATUS&CAN_MSG_LOST)
           {
                if( DTCU012887Timer == 250 )
                {
                    ChangeDTCs_ST_PRG(cnU012887,0x09);
                }
                else
                {
                    ++DTCU012887Timer;
                }
           }
           else
           {
                 DTCU012887Timer = 0;
                 ChangeDTCs_ST_PRG(cnU012887,0x00);
           }

           if(AEBS1_STATUS&CAN_MSG_LOST)
           {
                if( DTCU012987Timer == 125 )
                {
                    ChangeDTCs_ST_PRG(cnU012987,0x09);
                }
                else
                {
                    ++DTCU012987Timer;
                }
           }
           else
           {
                DTCU012987Timer = 0;
                ChangeDTCs_ST_PRG(cnU012987,0x00);
           }

           if(EEC1_STATUS&CAN_MSG_LOST)
           {
                if( DTCU010087Timer == 25 )
                {
                    ChangeDTCs_ST_PRG(cnU010087,0x09);
                }
                else
                {
                    ++DTCU010087Timer;
                }
           }
           else
           {
               DTCU010087Timer = 0;
               ChangeDTCs_ST_PRG(cnU010087,0x00);
           }
           
           if(NVM_CONFIG_TBOX == 1)
           {
               if(TCU_ICM_Display1_STATUS&CAN_MSG_LOST)
               {
                    if( DTCU010187Timer == 250 )
                    {
                        ChangeDTCs_ST_PRG(cnU010187,0x09);
                    }
                    else
                    {
                        ++DTCU010187Timer;
                    }
               }
               else
               {
                   DTCU010187Timer = 0;
                   ChangeDTCs_ST_PRG(cnU010187,0x00);
               }
           }
       }

      /*  // Vehicle Speed 实际车速（0x90）报文丢失
        if ( CANMsg090Status & CAN_MSG_LOST == 1 )
        {
            if ( ( SYS_OPR_STAT_IGN_ON ) && ( CAN_NORMAL_2SFF ) )
                ChangeDTCs_ST_PRG ( cnU1E0087, 0x29 );
        }
        else
        {
            ChangeDTCs_ST_PRG ( cnU1E0087, 0x00 );
        }
        */
    }
}


void UDS_Service_19_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint16_t DTCnum;
    uint16_t DTCcnt;
    uint16_t DTCcnt0A;
    uint8_t Service19DiagDataLength;
    uint32_t  FunorPhy;
  //  uint32_t DTCMaskRecord;
    uint8_t  DTCSnapshotRecordNumber;
 //   NVMDTC_SnapshotUnion m_SnapshotData;
    FunorPhy = A_TA_type;
    Service19DiagDataLength = A_Length;
    __RESET_WATCHDOG();
    S3_ServerCnt = 0;
    //不支持编程会话，返回7F负反馈
    if ( SessionType == ProgrammingSession )
    {
        NegRes.code = serviceNotSupportedInActiveSession;
        UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //长度异常，返回13负反馈
    if ( Service19DiagDataLength < 1 )
    {
       NegRes.code = incorrectMessageLength;
       UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
       return;
    }
    //获取数据
    for ( i = 0; i < Service19DiagDataLength; i++ )
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }
    //执行子服务
    switch ( UDS_ISO14229_Transfer[0] )
    {
        //通过状态掩码报告DTC的数
        
        case 0x01:
            //数据长度异常，返回13负反馈
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[1];
            
            DTCnum = 0;

            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DTC_STATUS[i]&UDS_ISO14229_Transfer[1] )
                    DTCnum++;
            }

            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_ISO14229_Transfer[2] = 0x00;
            UDS_ISO14229_Transfer[3] = ( INT8U ) ( DTCnum >> 8 );
            UDS_ISO14229_Transfer[4] = ( INT8U ) ( DTCnum & 0x00FF );
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
            break;                             
        //通过状态掩码报告DTC
        case 0x02:
            //数据长度异常，返回13负反馈
            if ( Service19DiagDataLength != 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[1];

            DTCcnt = 2;
            //返回DTC码（高位、中位、低位）和和DTC状态
            for ( i = 0; i < cnDTCslen; i++ )
            {
                if ( DTC_STATUS[i]&UDS_ISO14229_Transfer[1] )
                {
                    GJXHOUTDTCCODE ( i );	//根据序号输出3个字节的DTC码
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[0];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[1];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = CJINF[2];
                    DTCcnt++;
                    UDS_ISO14229_Transfer[DTCcnt] = DTC_STATUS[i];
                    DTCcnt++;
                }
            }
            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt, UDS_ISO14229_Transfer );
            break;
      
        //通过DTC报告Snapshot的记录
       /* case 0x04:
            //数据长度异常，返回13负反馈
            if ( Service19DiagDataLength != 5 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            DTCMaskRecord = ( ( uint32_t ) UDS_ISO14229_Transfer[1] << 16 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[2] << 8 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[3] );
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[4];
            
            if (( DTCSnapshotRecordNumber == 1 ) || ( DTCSnapshotRecordNumber == 0xff ) )
            {
            }
            else
            {
                NegRes.code = requestOutOfRange; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            
            for ( i = 0; i < cnDTCslen; i++)
            {
                if ( DTCMaskRecord == DTC_CODE_TABLE[i] )
                    goto 	label_temp;
            }
            
            NegRes.code = requestOutOfRange; //数据长度不对
            UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;   
        label_temp:    
            //已经找到了这请求的故障码了  开始回数
            //UDS_ISO14229_Transfer[0]=0x04;
            //UDS_ISO14229_Transfer[1]=0x04;
            //UDS_ISO14229_Transfer[2]=0x04;
            //UDS_ISO14229_Transfer[3]=0x04;
            m_SnapshotData = *DTC_CODE_add_TABLE[i];//别问我为啥  c环境试出来的
            UDS_ISO14229_Transfer[5] =	UDS_ISO14229_Transfer[4]; //当前的故障码状态
            UDS_ISO14229_Transfer[4] =	DTC_STATUS[i]; //当前的故障码状态
            UDS_ISO14229_Transfer[6] = 0x04;
            //ICUPowerVoltage           1 byte
            //ICUVspeed  								2 byte
            //OccurrenceCounter    			1 byte
            //TheOdometerOfLastMalfunction    3byte
            UDS_ISO14229_Transfer[7] = 0xDF;
            UDS_ISO14229_Transfer[8] = 0X00;
            UDS_ISO14229_Transfer[9] = m_SnapshotData.Sig.ECUPowerVoltage;
            UDS_ISO14229_Transfer[10] = 0xDF;
            UDS_ISO14229_Transfer[11] = 0X01;
            UDS_ISO14229_Transfer[12] = ( uint8_t ) ( m_SnapshotData.Sig.ICUVspeed >> 8 );
            UDS_ISO14229_Transfer[13] = ( uint8_t ) ( m_SnapshotData.Sig.ICUVspeed );
            UDS_ISO14229_Transfer[14] = 0xDF;
            UDS_ISO14229_Transfer[15] = 0X02;
            UDS_ISO14229_Transfer[16] = m_SnapshotData.Sig.OccurrenceCounter;;
            UDS_ISO14229_Transfer[17] = 0xDF;
            UDS_ISO14229_Transfer[18] = 0X04;
            UDS_ISO14229_Transfer[19] = ( uint8_t ) ( m_SnapshotData.Sig.TheOdometerOfLastMalfunction >> 16 );
            UDS_ISO14229_Transfer[20] = ( uint8_t ) ( m_SnapshotData.Sig.TheOdometerOfLastMalfunction >> 8 );
            UDS_ISO14229_Transfer[21] = ( uint8_t ) ( m_SnapshotData.Sig.TheOdometerOfLastMalfunction );
            
            if ( UDS_ISO14229_Transfer[0] == 0x04 )
                UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, 22 , UDS_ISO14229_Transfer );
            
            break;
        //报告支持的DTC */
        case 0x0A:  //Only available if more than 1 DTC exist
            //数据长度异常，返回13负反馈
            if ( Service19DiagDataLength != 1 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            /*
            DTCSnapshotRecordNumber = UDS_ISO14229_Transfer[4];
            if (( DTCSnapshotRecordNumber == 1 ) || ( DTCSnapshotRecordNumber == 0xff ) )
            {
            }
            else
            {
                NegRes.code = requestOutOfRange; //数据长度不对
                UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            */
            DTCcnt0A = 2;
            //返回DTC码（高位、中位、低位）和和DTC状态
            for ( i = 0; i < cnDTCslen; i++ )
            {
                __RESET_WATCHDOG();
                GJXHOUTDTCCODE ( i );	//根据序号输出3个字节的DTC码
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[0];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[1];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = CJINF[2];
                DTCcnt0A++;
                UDS_ISO14229_Transfer[DTCcnt0A] = DTC_STATUS[i];
                DTCcnt0A++;
            }
            UDS_ISO14229_Transfer[1] = 0x09;
            UDS_Service_Response ( 0x19, POSITIVE_RSP, DIAG_ID_Tx, DTCcnt0A, UDS_ISO14229_Transfer );
            break;
        //子功能不支持，返回12负反馈       
        default:
            if(FunorPhy == DIAG_ID_Rx_FUN )
            return ;
           
            NegRes.code = subFunctionNotSupported;
            UDS_Service_Response ( 0x19, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }

    /*
    UDS_ISO14229_Transfer[0]=0x00;
    S3_Server_refresh=1;
    UDS_Service_Response(0x19, POSITIVE_RSP, DIAG_ID_Tx, 1, UDS_ISO14229_Transfer);
    */
}
void UDS_Service_2E_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    //每个写入之后 要写入到EEPROM
    uint16_t i;
    uint16_t DID;
    uint8_t  Service2EDiagDataLength;
    uint32_t DiagIDNum = 0;
    Service2EDiagDataLength = A_Length;
    DiagIDNum = A_TA_type;
     S3_ServerCnt = 0;
    if ( DiagIDNum == DIAG_ID_Rx_FUN )
        return;

    if ( SessionType == DefaultSession ) //不支持默认会话
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持   7F
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

  
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))//供电电压异常，车速大于0
    {
        NegRes.code = conditionsNotCorrect; //22
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    for ( i = 0; i < Service2EDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    if ( Service2EDiagDataLength <= 2 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对  13
        UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }

    DID = ( ( ( uint16_t ) ( UDS_ISO14229_Transfer[0] << 8 ) ) | ( ( uint16_t ) ( UDS_ISO14229_Transfer[1] ) ) );



    switch ( DID ) //已经使用了2个数据长度，写入内容从[2]开始
    {  
       //case  0xF199:
       case  0xF184:
      
            if(DiagLockFlag!=Level2UnLock)
            {
              NegRes.code=securityAccessDenied;//未解锁  33
              UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
              return;
            }
          
            if ( Service2EDiagDataLength != 7 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对   13
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
               
            //memcpy(NVMConfigF184.Data,UDS_ISO14229_Transfer+2,7);
            //NVM_Data_Update ( NVM_CONFIG_F184 );
           (void)  memcpy(diag.F184,UDS_ISO14229_Transfer+2,sizeof(diag.F184));
            Write_2E_DATA_TO_FLASH();

            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );

        break;
      case  0xF1A8:
      
          if(DiagLockFlag!=Level1UnLock)
          {
            NegRes.code=securityAccessDenied;//未解锁   33
            UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
            return;
          }
            
          if ( Service2EDiagDataLength != 20 + 2 )
          {
              NegRes.code = incorrectMessageLength; //数据长度不对 13
              UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
          }
          
          //判断日期是否有效
          
          /*if(UDS_ISO14229_Transfer[2]!=0x20 || UDS_ISO14229_Transfer[3]<0x19 ||  UDS_ISO14229_Transfer[4]>12 ||  UDS_ISO14229_Transfer[5]>31) 
          {
             NegRes.code = requestOutOfRange; //数据不正确  0x31 数据超出范围
             
             UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
             return;
          }*/
          
          //2位为年的高位 0<n<=99，3位年的低位 0<=n<=99,4位月0<n<=12,5位日0<n<=31
           
                       
          if(( UDS_ISO14229_Transfer[2] <= 0x04 ) && (UDS_ISO14229_Transfer[3] <= 0xFF )&& \
          (UDS_ISO14229_Transfer[4] <= 0x0C ) && ( UDS_ISO14229_Transfer[5] <= 0x1F ) )
            {  
            (void)   memcpy(NVMConfigF1A8.Data,UDS_ISO14229_Transfer+2,20);

              NVM_Data_Update ( NVM_CONFIG_F1A8 );

              UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            }
            else
            {
               NegRes.code = requestOutOfRange; //数据不正确  0x31 数据超出范围
               
               UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
            } 
        break;

        case  0xF190:
                
            if(DiagLockFlag!=Level1UnLock)
            {
              NegRes.code=securityAccessDenied;//未解锁
              UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
              return;
            }

            if ( Service2EDiagDataLength != 17 + 2 )
            {
                NegRes.code = incorrectMessageLength; //数据长度不对
                UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            /*
            noDataLenth = 2;
            for(i = 2;i < Service2EDiagDataLength; ++i){
                if(UDS_ISO14229_Transfer[i] == 0){
                
                }
                else
                {
                    noDataLenth = i+1;
                }
            }
            if(noDataLenth != Service2EDiagDataLength){
                for(i = noDataLenth; i < Service2EDiagDataLength; ++i){
                    UDS_ISO14229_Transfer[i] = 0x20;
                }
            }
            */
            
           (void)  memcpy(NVMConfigF190.Data,UDS_ISO14229_Transfer+2,17);
            
            NVM_Data_Update ( NVM_CONFIG_F190 );

           /* NegRes.code = requestCorrectlyReceivedResponsePending; //pending
            UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            Write_2E_DATA_TO_FLASH(); */
            
            UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            break;

       case 0x3300:
       
          if(DiagLockFlag!=Level1UnLock)
          {
            NegRes.code=securityAccessDenied;//未解锁   33
            UDS_Service_Response(0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
            return;
          }
          
          if ( Service2EDiagDataLength != 16 + 2 )
          {
              NegRes.code = incorrectMessageLength; //数据长度不对   13
              UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
              return;
          }
          if(( UDS_ISO14229_Transfer[2] <= 0x01 ) && (UDS_ISO14229_Transfer[3] <= 0x04 )&&
          (UDS_ISO14229_Transfer[4] <= 0x02 ) && (UDS_ISO14229_Transfer[5] <= 0x02 ) &&
          (UDS_ISO14229_Transfer[6] <= 0x03 )&& (UDS_ISO14229_Transfer[7] <= 0x01 )&&
          (UDS_ISO14229_Transfer[8] <= 0x01 )&& (UDS_ISO14229_Transfer[9] <= 0x06 )&&
          (UDS_ISO14229_Transfer[10] <= 0x03 )&& ( UDS_ISO14229_Transfer[11] <= 0x06 )&&
          (UDS_ISO14229_Transfer[12] <= 0x01 )&& (UDS_ISO14229_Transfer[13] <= 0x03 )&&
          ( UDS_ISO14229_Transfer[14] <= 0xFF )&& (UDS_ISO14229_Transfer[15] <= 0xFF ))//20201208
            {
                   
            (void)   memcpy(NVMConfig3300.Data,UDS_ISO14229_Transfer+2,16);

              NVM_Data_Update ( NVM_CONFIG_3300 );
              // ST_GUI_BACKGROUND = TRUE;

              UDS_Service_Response ( 0x2E, POSITIVE_RSP, DIAG_ID_Tx, 2, UDS_ISO14229_Transfer );
            }
            else
            {
               NegRes.code = requestOutOfRange; //数据不正确  0x31 数据超出范围
               
               UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
               return;
            }


          break;     
      
         default:
            NegRes.code = requestOutOfRange; //31
            UDS_Service_Response ( 0x2E, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            return;
            break;
    }
}



void UDS_Service_2F_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{  
    uint16_t i;
    uint16_t Service2FDiagDataLength;
    uint16_t ControlDataIdentifier;
    uint32_t DiagIDNum;
   // uint32_t TempBuffer;
    DiagIDNum = A_TA_type;
     S3_ServerCnt = 0;
    NegRes.code = serviceNotSupported;    //功能不支持
    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );  
     
   #ifndef TESTDIAG
    //不支持功能寻址
   if ( DiagIDNum == DIAG_ID_Rx_FUN )
        return;
    //只支持扩展会话模式，返回7F负反馈
    if ( SessionType != ExtendedDiagnosticSession ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession;
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //安全模式未解锁，返回33负反馈
    if(DiagLockFlag!=Level1UnLock)
    {
     NegRes.code=securityAccessDenied;
     UDS_Service_Response(0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, (uint8_t*)(&NegRes.code));
     return;
    }
    Service2FDiagDataLength = A_Length; 
    //长度异常，返回13负反馈
    if ( Service2FDiagDataLength < 3 )
    {
        NegRes.code = incorrectMessageLength; 
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //供电电压异常，车速大于0，返回22负反馈
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }


   

    //获取数据
    for ( i = 0; i < Service2FDiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    ControlDataIdentifier = ( ( ( uint16_t ) UDS_ISO14229_Transfer[0] << 8 ) | ( ( uint16_t ) UDS_ISO14229_Transfer[1] ) );

    switch ( ControlDataIdentifier )
    {
        //蜂鸣器 
        case  0x62E1:
            //不再控制ECU
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
              //长度异常，返回13负反馈
              if ( Service2FDiagDataLength != 3 )
              {
                  NegRes.code = incorrectMessageLength; 
                  UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                  return;
              }
              ContrlorResumeofBuzzer = Resume;
              ContrlorResumeofIO = Resume;
              UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
              return;
            }
            //控制ECU
            else if ( UDS_ISO14229_Transfer[2] == 0x03 )
            {
                //长度异常，返回13负反馈
                if ( Service2FDiagDataLength != 4 )
                {
                    NegRes.code = incorrectMessageLength; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
                //蜂鸣器使用雷达音-----响
                if ( UDS_ISO14229_Transfer[3] == 0xFF )
                {
                    BuzzerStatus = 1; 
                    //ISD_Play_Vp_Loop ( 0x00, 0 );
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    return;
                }
                //蜂鸣器使用雷达音-----不响
                else if ( UDS_ISO14229_Transfer[3] == 0x00 )
                {
                    BuzzerStatus = 0; 
                    //ISD_Stop_Loop();
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    return;
                }
            }
            else //申请超出限制，返回31负反馈
            {
                NegRes.code = requestOutOfRange; 
                UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                return;
            }
            break;

        //指针测试
        case 0x62E2:    
            //不再控制ECU
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
              //长度异常，返回13负反馈
              if ( Service2FDiagDataLength != 3 )
              {
                  NegRes.code = incorrectMessageLength; 
                  UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                  return;
              }
              
              ContrlorResumeofVspeed = Resume;
              ContrlorResumeofIO = Resume;
              ContrlorResumeofEspeed = Resume;
              
              UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
              return;
            }
            //控制ECU 
            else if( UDS_ISO14229_Transfer[2] == 0x03 )
            {
              //长度异常，返回13负反馈
              if ( Service2FDiagDataLength != 5 )
              {
                  NegRes.code = incorrectMessageLength; 
                  UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                  return;
              }
              //车速表控制 
              if( UDS_ISO14229_Transfer[3] == 0x01 )
              {
                  ContrlorResumeofVspeed = Contrl;
    	          	ContrlorResumeofIO     = Contrl;
    	          	  
    	          	//零刻度
    	          	if( UDS_ISO14229_Transfer[4] == 0x00 ) 
    	          	{ 
    	              VspeedValNUM = 0;
    	              UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	            	return;
    	          	}
    	          	//半刻度
    	          	else if( UDS_ISO14229_Transfer[4] == 0x01 )
    	          	{
    	          	  VspeedValNUM = 700;
    	          	  UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	            	return;
    	          	} 
    	          	//满刻度
    	          	else if( UDS_ISO14229_Transfer[4] == 0x02 )
    	          	{
    	          	  VspeedValNUM = 1400;
    	          	  UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	            	return;
    	          	}
    	          	else//申请超出限制，返回31负反馈
    	          	{
    	          	  NegRes.code = requestOutOfRange; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
    	          	}
              } 
              //转速表控制
              else if(UDS_ISO14229_Transfer[3] == 0x02)
              {
                  ContrlorResumeofEspeed = Contrl;
    	          	ContrlorResumeofIO     = Contrl;
    	          	  
    	          	//零刻度
    	          	if( UDS_ISO14229_Transfer[4] == 0x00 ) 
    	          	{ 
    	              ESpeedValNUM = 0;
    	              UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	          	  return;
    	          	}
    	          	//半刻度
    	          	else if( UDS_ISO14229_Transfer[4] == 0x01 )
    	          	{
    	          	  ESpeedValNUM = 1250;
    	          	  UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	          	  return;
    	          	} 
    	          	//满刻度
    	          	else if( UDS_ISO14229_Transfer[4] == 0x02 )
    	          	{
    	          	  ESpeedValNUM = 2500;
    	          	  UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 5, UDS_ISO14229_Transfer );
    	          	  return;
    	          	}
    	          	else//申请超出限制，返回31负反馈
    	          	{
    	          	  NegRes.code = requestOutOfRange; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
    	          	}
              }
              else
              {
                   NegRes.code = requestOutOfRange; 
                   UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                   return;
              }
            }
            else
            {
                 NegRes.code = requestOutOfRange; 
                 UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                 return;
            }
            break;
        //报警灯测试
        case 0x62E4:
            //不再控制ECU
            if ( UDS_ISO14229_Transfer[2] == 0x00 )
            {
                //长度异常，返回13负反馈
                if ( Service2FDiagDataLength != 3 )
                {
                    NegRes.code = incorrectMessageLength; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
                ContrlorResumeofLamp = Resume;
                ContrlorResumeofIO = Resume;
                UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 3, UDS_ISO14229_Transfer );
                return;
            }
            //控制ECU
            else if ( UDS_ISO14229_Transfer[2] == 0x03 )
            {
                //长度异常，返回13负反馈
                if ( Service2FDiagDataLength != 4 )
                {
                    NegRes.code = incorrectMessageLength; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                }
                //关闭报警灯
                
                    ContrlorResumeofLamp = Contrl;
    	              ContrlorResumeofIO   = Contrl;
    	              
                if ( UDS_ISO14229_Transfer[3] == 0x00 )
                {
                    LampWarningStatus = Resume;
                    IO_Group_1 = 0x00;
                    IO_Group_2 = 0x00;
                    IO_Group_3 = 0x00;
                    IO_Group_4 = 0x00;
                    IO_MASK_1  = 0x00;
                    IO_MASK_2  = 0x00;
                    IO_MASK_3  = 0x00;
                    IO_MASK_4  = 0x00;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    return;
                }
                //开启报警灯
                else if( UDS_ISO14229_Transfer[3] == 0xFF )
                {
                    LampWarningStatus = Contrl;
                    IO_Group_1 = 0xFF;
                    IO_Group_2 = 0xFF;
                    IO_Group_3 = 0xFF;
                    IO_Group_4 = 0xFF;
                    IO_MASK_1  = 0xFF;
                    IO_MASK_2  = 0xFF;
                    IO_MASK_3  = 0xFF;
                    IO_MASK_4  = 0xFF;
                    UDS_Service_Response ( 0x2F, POSITIVE_RSP, DIAG_ID_Tx, 4, UDS_ISO14229_Transfer );
                    return;
                }
                else//申请超出限制，返回31负反馈
    	          {
    	          	  NegRes.code = requestOutOfRange; 
                    UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
                    return;
                } 
            }
            break;
        //DID不支持，返回12负反馈
        default:
            NegRes.code = subFunctionNotSupported;
            UDS_Service_Response ( 0x2F, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
            break;
    }
    #endif
}


void UDS_Service_14_Indication ( uint32_t A_TA_type, uint16_t A_Length, uint8_t *A_Data )
{
    uint16_t i;
    uint8_t Service14DiagDataLength;
    uint32_t ClearDTCGroup;
    Service14DiagDataLength = A_Length;
    S3_ServerCnt = 0;
    //供电电压异常，车速大于0，返回22负反馈          
    if (( HIGH_VOLTAGE )||( LOW_VOLTAGE )||(DATA_VEHICLE_SPEED_DISPLAYING>0))
    {
        NegRes.code = conditionsNotCorrect;
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //数据长度异常，返回13负反馈
    if ( Service14DiagDataLength != 3 )
    {
        NegRes.code = incorrectMessageLength; //数据长度不对
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //不支持编程会话，返回7E负反馈
    if ( SessionType == ProgrammingSession ) 
    {
        NegRes.code = serviceNotSupportedInActiveSession; //会话模式不支持
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
    //获取数据
    for ( i = 0; i < Service14DiagDataLength; i++ ) //长度不固定 要收数据
    {
        UDS_ISO14229_Transfer[i] = * ( A_Data + i );
    }

    ClearDTCGroup = ( ( ( uint32_t ) UDS_ISO14229_Transfer[0] << 16 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[1] << 8 ) | ( ( uint32_t ) UDS_ISO14229_Transfer[2] ) );
    //如果接收的是14 FF FF FF，清除诊断信息
    if ( ClearDTCGroup == 0xffffff )
    {
        //ClearAllDTC();
        VoltageAbnormalTimer = 0;
        BusoffTimer = 0;
        DTCU010087Timer = 0;
        DTCU010187Timer = 0;
        DTCU012987Timer = 0;
        DTCU012887Timer = 0;
        DTCU014087Timer = 0;
        for ( i = 0; i < cnDTCslen; i++ )
        {
            //DTC_STATUS[i] = ( DTC_STATUS[i] & 0x40 ) | 0x10;//修改DTC状态记录
            DTC_STATUS[i] = 0x00;
            RecDTC_Wait_NoBusoff[i].Flag = 0;
            RecDTC_Wait_NoBusoff[i].Timer = 0;
        }

        UDS_Service_Response ( 0x14, POSITIVE_RSP, DIAG_ID_Tx, 0, UDS_ISO14229_Transfer );
        DTCstatusCHGFF = 1;
        SaveDTCsToEEPROM();
    }
    else//如果不是接收的不是14 FF FF FF，返回31负反馈
    {
        NegRes.code = requestOutOfRange; //子功能不支持
        UDS_Service_Response ( 0x14, NEGATIVE_RSP, DIAG_ID_Tx, 1, ( uint8_t * ) ( &NegRes.code ) );
        return;
    }
}


void HandleOf2FServices ( void )      //lgh
{
    if ( ContrlorResumeofIO != Contrl )
    {
        if ( ContrlorResumeofIOLast == Contrl )
        {
              ContrlorResumeofIOLast = Resume;

              ContrlorResumeofIO = 0;
              ContrlorResumeofSOC = 0;     //soc控制
              SocSeg = 0;                  //Soc
              ContrlorResumeofEspeed = 0;  //转速控制
              ESpeedValNUM = 0;            //转速           
              ContrlorResumeofVspeed = 0;  //车速控制      
              VspeedValNUM = 0;            //车速 
              ContrlorResumeofLamp = 0;
              //ISD_Stop_Loop();
              IO_Group_1 = 0;
              IO_Group_2 = 0;
              IO_Group_3 = 0;
              IO_Group_4 = 0;
              
            return;
        }

        return;
    }

    ContrlorResumeofIOLast = ContrlorResumeofIO;
}



