/*
 * UserCan.c
 *
 *  Created on: 2023年1月16日
 *      Author: senco
 */
#include "var.h"    // DSP2803x Examples Include File
#include "UserCan.h"      // DSP2803x Headerfile Include File
//#include "DSP2803x_Device.h"     // DSP28 Headerfile Include File
#include "DSP2803x_Examples.h"   // DSP28 Examples Include File
#define Reverse 0
#define Normal 1
// Global variable for this example
#define TX_PERIOD 20 //ms

//绘制曲线
#pragma DATA_SECTION(CanCurveData,"MemoryFile");

int16 CanCurveData[0x400];
int16 CanCurveFlag = 0;
int16 CanDebugFlag = 1;


struct CANMBOX_USER_REGS CanMboxUserRegsT;      //用户CAN数据发送结构体
struct CANMBOX_USER_REGS CanMboxUserRegsT2;      //用户CAN数据发送结构体
struct CANMBOX_USER_REGS CanMboxUserRegsR;      //用户CAN接收数据结构体1
struct CANMBOX_USER_REGS CanMboxUserRegsR2;     //用户CAN接收数据结构体2
struct CANMBOX_USER_REGS CanMboxUserRegsR3;     //用户CAN接收数据结构体2

#ifdef MOTORLEFT
#define ID_MOTOR_TIMERBACK 0x014E6909
#define ID_MOTOR_PARBACK 0X014E6709
#else
#define ID_MOTOR_TIMERBACK 0x014E6A08
#define ID_MOTOR_PARBACK 0X014E6808
#endif
#define ID_MOTOR1_COMMAND 0x014E540A
#define ID_MOTOR2_COMMAND 0x014E560A
#define ID_MOTOR3_COMMAND 0x014E5A0A

#define ID_FAULT_CLEAR 0x014E510A
#define ID_MOTOR1_SET 0x014E5C0A


Uint32 motor_command[]={ID_MOTOR1_COMMAND,ID_MOTOR2_COMMAND,ID_MOTOR3_COMMAND};
void CanVar(void){

    CanMboxUserRegsR.MboxRxFlg = 0;
    CanMboxUserRegsR.MboxNum = 0;
    CanMboxUserRegsR.MboxDLC = 0;
    CanMboxUserRegsR.MboxID = 0;
    CanMboxUserRegsR.MboxWord1 = 0;
    CanMboxUserRegsR.MboxWord2 = 0;
    CanMboxUserRegsR.MboxWord3 = 0;
    CanMboxUserRegsR.MboxWord4 = 0;

    CanMboxUserRegsR2.MboxRxFlg = 0;
    CanMboxUserRegsR2.MboxNum = 0;
    CanMboxUserRegsR2.MboxDLC = 0;
    CanMboxUserRegsR2.MboxID = 0;
    CanMboxUserRegsR2.MboxWord1 = 0;
    CanMboxUserRegsR2.MboxWord2 = 0;
    CanMboxUserRegsR2.MboxWord3 = 0;
    CanMboxUserRegsR2.MboxWord4 = 0;

    CanMboxUserRegsR3.MboxRxFlg = 0;
    CanMboxUserRegsR3.MboxNum = 0;
    CanMboxUserRegsR3.MboxDLC = 0;
    CanMboxUserRegsR3.MboxID = 0;
    CanMboxUserRegsR3.MboxWord1 = 0;
    CanMboxUserRegsR3.MboxWord2 = 0;
    CanMboxUserRegsR3.MboxWord3 = 0;
    CanMboxUserRegsR3.MboxWord4 = 0;

    CanMboxUserRegsT.MboxRxFlg = 0;
    CanMboxUserRegsT.MboxNum = 0;
    CanMboxUserRegsT.MboxDLC = 0;
    CanMboxUserRegsT.MboxID = 0;
    CanMboxUserRegsT.MboxWord1 = 0;
    CanMboxUserRegsT.MboxWord2 = 0;
    CanMboxUserRegsT.MboxWord3 = 0;
    CanMboxUserRegsT.MboxWord4 = 0;


}
void InitCan(void){
    struct ECAN_REGS ECanaShadow;
    InitECanaGpio();//引脚初始化
    //设置波特率
    InitECana(); // Initialize eCAN-A module

    // Mailboxes can be written to 16-bits or 32-bits at a time
        // Write to the MSGID field of TRANSMIT mailboxes MBOX0 - 15
        ECanaMboxes.MBOX0.MSGID.all = ID_MOTOR_TIMERBACK|EXTFRAME;//定时上传
        ECanaMboxes.MBOX1.MSGID.all = ID_MOTOR_TIMERBACK|EXTFRAME;//定时上传
        ECanaMboxes.MBOX2.MSGID.all = ID_MOTOR_TIMERBACK|EXTFRAME;//定时上传
        ECanaMboxes.MBOX3.MSGID.all = ID_MOTOR_PARBACK|EXTFRAME;//飞控参数返回
        ECanaMboxes.MBOX4.MSGID.all = ID_MOTOR_PARBACK|EXTFRAME;//飞控参数返回
        ECanaMboxes.MBOX5.MSGID.all = 0x9555AAA5;//调试参数返回1 给定速度 目标速度 反馈速度 正反转
        ECanaMboxes.MBOX6.MSGID.all = 0x9555AAA6;//调试参数返回2 速度PID输出  运行占空比
        ECanaMboxes.MBOX7.MSGID.all = 0x9555AAA7;//调试参数返回3
        ECanaMboxes.MBOX8.MSGID.all = 0x9555AAA8;//调试参数返回4
        ECanaMboxes.MBOX9.MSGID.all = 0x9555AAA9;//调试参数返回5
        ECanaMboxes.MBOX10.MSGID.all = 0x9555AAAA;
        ECanaMboxes.MBOX11.MSGID.all = 0x9555AAAB;
        ECanaMboxes.MBOX12.MSGID.all = 0x9555AAAC;
        ECanaMboxes.MBOX13.MSGID.all = 0x9555AAAD;
        ECanaMboxes.MBOX14.MSGID.all = 0x9555AAAE;
        ECanaMboxes.MBOX15.MSGID.all = 0x9555AAAF;

        // Write to the MSGID field of RECEIVE mailboxes MBOX16 - 31
        ECanaMboxes.MBOX16.MSGID.all = 0xC0000000;//014E540A esc1 esc2
        ECanaMboxes.MBOX17.MSGID.all = 0x814E540A;
        ECanaMboxes.MBOX18.MSGID.all = 0x814E560A;
        ECanaMboxes.MBOX19.MSGID.all = 0x814E5A0A;
        ECanaMboxes.MBOX20.MSGID.all = 0x814E5C0A;
        ECanaMboxes.MBOX21.MSGID.all = 0x814E510A;//故障消除
        ECanaMboxes.MBOX22.MSGID.all = 0xC0000000;
        ECanaMboxes.MBOX23.MSGID.all = 0xC0000000;
        ECanaMboxes.MBOX24.MSGID.all = 0x9555AAA8;
        ECanaMboxes.MBOX25.MSGID.all = 0x9555AAA9;
        ECanaMboxes.MBOX26.MSGID.all = 0x9555AAAA;
        ECanaMboxes.MBOX27.MSGID.all = 0x9555AAAB;
        ECanaMboxes.MBOX28.MSGID.all = 0x9555AAAC;
        ECanaMboxes.MBOX29.MSGID.all = 0x9555AAAD;
        ECanaMboxes.MBOX30.MSGID.all = 0x9555AAAE;
        ECanaMboxes.MBOX31.MSGID.all = 0x9555AAAF;

        // Configure Mailboxes 0-15 as Tx, 16-31 as Rx
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        ECanaRegs.CANMD.all = 0xFFFF0000;// 0 transmit or 1 receive

        // Enable all Mailboxes
        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
       /* ECanaLAMRegs.LAM20.all = 0x9FFFFFFF;//20号邮箱不在乎ID号
        // ECanaLAMRegs.LAM21.all = 0x9FFFFFFF;//21号邮箱不在乎ID号，当没有设置优先级的时候高位邮箱优先接收
        //ECanaRegs.CANME.all = 0x000FC3FF;//16-19号接收邮箱必须完全等于ID才能接收
         ECanaRegs.CANME.all = 0x001FC3FF;//由于设置20号邮箱不在乎ID，接收信息优先进入20号邮箱*/
        ECanaLAMRegs.LAM16.all = 0x9FFFFFFF;//16号邮箱不在乎ID号 不在乎ID的放在低位
        //20 19-16 15-11 11-8 8-4 3-0
        //11  1111   1100 0011 1111 1111
        ECanaRegs.CANME.all = 0x003FC3FF;//1 Mailbox enable bits 0-4 飞控协议 5-9用于发送调试上位机代码  14-15 用于发送绘图数据
        //17-20 用于飞控格式接收  16 用于调试上位机数据接收
       //ECanaRegs.CANME.all = 0x0001C7E0;//屏蔽飞控相关传输


        // Specify that 8 bits will be sent/received
        ECanaMboxes.MBOX0.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX1.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX2.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX3.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX4.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX5.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX6.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX7.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX8.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX9.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX10.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX11.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX12.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX13.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX14.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX15.MSGCTRL.bit.DLC = 8;
        ECanaMboxes.MBOX16.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX17.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX18.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX19.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX20.MSGCTRL.all = 0x00000008;
        ECanaMboxes.MBOX21.MSGCTRL.all = 0x00000008;

        // Write to the mailbox RAM field of MBOX0 - 15
        //  Message Data Registers MBOX0.MDL低字节和MBOX0.MDH高字节

        // Since this write is to the entire register (instead of a bit
        // field) a shadow register is not required.
        EALLOW;
        ECanaRegs.CANMIM.all = 0xFFFFFFFF;//中断使能

        // Configure the eCAN for self test mode
        // Enable the enhanced features of the eCAN.
        EALLOW;
        ECanaShadow.CANMC.all = ECanaRegs.CANMC.all;
        ECanaShadow.CANMC.bit.STM = 0;    // Configure CAN for self-test mode //1回环模式
        ECanaRegs.CANMC.all = ECanaShadow.CANMC.all;



        EDIS;

        CanVar();//数据初始化

}

void ECanMboxSend(struct  CANMBOX_USER_REGS *pRegs)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    struct ECAN_REGS ECanaShadow;
    volatile struct MBOX *Mailbox;

    ECanaShadow.CANME.all = ECanaRegs.CANME.all;
    ECanaShadow.CANME.all &= ~((Uint32)1 << pRegs->MboxNum);//DISABLE
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;

    Mailbox = &ECanaMboxes.MBOX0 + pRegs->MboxNum;

    Mailbox->MSGID.all = pRegs->MboxID | 0x80000000;   //扩展帧
//  Mailbox->MSGID.all = pRegs->MboxID << 18;          //标准帧


    Mailbox->MDL.word.LOW_WORD = pRegs->MboxWord1;
    Mailbox->MDL.word.HI_WORD = pRegs->MboxWord2;
    Mailbox->MDH.word.LOW_WORD = pRegs->MboxWord3;
    Mailbox->MDH.word.HI_WORD = pRegs->MboxWord4;

    Mailbox->MSGCTRL.all = pRegs->MboxDLC;

    ECanaShadow.CANME.all = ECanaRegs.CANME.all;
    ECanaShadow.CANME.all |= (Uint32)1 << pRegs->MboxNum;//ENABLE
    ECanaRegs.CANME.all = ECanaShadow.CANME.all;

    ECanaRegs.CANTRS.all = (Uint32)1 << pRegs->MboxNum;
    ECanaRegs.CANTA.all = (Uint32)1 << pRegs->MboxNum;
}
// MBXnbr 16-31
struct CANMBOX_USER_REGS ECanMboxRead(int16 MBXnbr)
{//Mailboxes 0-15 as Tx, 16-31 as Rx
    struct CANMBOX_USER_REGS CanMboxUserRegs;
    volatile struct MBOX *Mailbox;
    Mailbox = &ECanaMboxes.MBOX0 + MBXnbr;


    if ((ECanaRegs.CANRMP.all & ((Uint32)1<<MBXnbr))!=0 ) //if (ECanaRegs.CANRMP.bit.RMP20 ==1 )
    {
        CanMboxUserRegs.MboxRxFlg = TRUE;
        CanMboxUserRegs.MboxNum = MBXnbr;
        CanMboxUserRegs.MboxID = Mailbox->MSGID.all & 0x1FFFFFFF; //扩展帧
        CanMboxUserRegs.MboxDLC = Mailbox->MSGCTRL.bit.DLC;
        CanMboxUserRegs.MboxWord1 = Mailbox->MDL.word.LOW_WORD;
        CanMboxUserRegs.MboxWord2 = Mailbox->MDL.word.HI_WORD;
        CanMboxUserRegs.MboxWord3 = Mailbox->MDH.word.LOW_WORD;
        CanMboxUserRegs.MboxWord4 = Mailbox->MDH.word.HI_WORD;

        ECanaRegs.CANRMP.all = ECanaRegs.CANRMP.all|((Uint32)1<<MBXnbr);//写1 clear
    }
    else
    {
        CanMboxUserRegs.MboxRxFlg = FALSE;
    }
    return CanMboxUserRegs;
}


void CanDebugTx(void)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    static int16 TxCnt = 0;
    static int16 TxPeriod = 0;
    static int16 step = 0;
#ifdef DEBUG_CAN
    static int16 step2 = 0;
#endif
    TxCnt++;

    if (TxCnt >= TxPeriod)
    {
        TxPeriod = TX_PERIOD*MAINLOOPRATE;// =TX_PERIOD/1000/(1/mainloop)  mainloop = 8k

       TxCnt = 0;
        step++;
#ifdef DEBUG_CAN
        step2++;
#endif

#ifdef PIXHAWK
        static Uint16 tail=0;
        if(step ==1){


           // //ID       Byte0           Byte1        Byte2     Byte3       Byte4   Byte5   Byte6   Tail
           // 电调ID    0X30|电调编号   母线电压L   母线电压H   当前速度L   当前速度H     0      0   见附表
            CanMboxUserRegsT.MboxID = ID_MOTOR_TIMERBACK;
            CanMboxUserRegsT.MboxNum = 0;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = ((int16)0x30|ESC_NUMBER )      + (((int16)DCbus_voltage & 0x00FF)<<8);
            CanMboxUserRegsT.MboxWord2 = (((int16)DCbus_voltage & 0xFF00)>>8) + (((int16)speed1.SpeedRpm& 0x00FF)<<8);
            CanMboxUserRegsT.MboxWord3 = ((int16)speed1.SpeedRpm & 0xFF00)>>8;
            tail = (tail++ >=31)? 0 : tail;
            CanMboxUserRegsT.MboxWord4 = ((int16)(0xC0+tail)&0x00FF)<<8;

            ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==2){
            //     Byte0         Byte1  Byte2  Byte3  Byte4   Byte5         Byte6   Tail
             //   0X50|电调编号   故障  故障    故障      故障  给定速度L   给定速度H   见附表
            CanMboxUserRegsT.MboxID = ID_MOTOR_TIMERBACK;
            CanMboxUserRegsT.MboxNum = 1;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = ((int16)0x50|ESC_NUMBER) + (((int16)FaultFlag.all & 0x00FF)<<8);
            CanMboxUserRegsT.MboxWord2 = (((int16)FaultFlag.all  & 0xFF00)>>8) +   0;
            CanMboxUserRegsT.MboxWord3 = (int16)0 + (((int16)(_IQtoIQ12(SpeedRef)*speed1.BaseRpm>>12 )& 0x00FF)<<8);
            tail = (tail++ >=31)? 0 : tail;
            CanMboxUserRegsT.MboxWord4 = (((int16)(_IQtoIQ12(SpeedRef)*speed1.BaseRpm>>12 ) & 0xFF00)>>8)+ (((int16)(0xC0 + tail)&0x00FF)<<8);

            ECanMboxSend(&CanMboxUserRegsT);
        }else if(step ==3){
            step = 0;
            // Byte0             Byte1       Byte2      Byte3       Byte4     Byte5     Byte6        Tail
            // 0XA0|电调编号   扭矩电流L   扭矩电流H   电调温度L   电调温度H   电机温度L   电机温度H   见附表

            CanMboxUserRegsT.MboxID = ID_MOTOR_TIMERBACK;
            CanMboxUserRegsT.MboxNum = 2;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = (((int16)0xA0|ESC_NUMBER))                   + (((int16)((DC_current_filter_avr*1000)>>12)&0x00FF)<<8);
            CanMboxUserRegsT.MboxWord2 = (((int16)((DC_current_filter_avr*1000)>>12)&0xFF00)>>8)   +  ((Tvot&0x00FF)<<8);
            CanMboxUserRegsT.MboxWord3 = (((int16)Tvot&0xFF00)>>8)    +  (((int16)Tmotor&0x00FF)<<8);//
            tail = (tail++ >=31)? 0 : tail;
            CanMboxUserRegsT.MboxWord4 = (((int16)Tmotor&0xFF00)>>8)  + (((int16)(0xC0+tail)&0x00FF)<<8);

            ECanMboxSend(&CanMboxUserRegsT);
        }
         /*   //参数设置---------------------------
            CanMboxUserRegsT.MboxID = 0x014E5D0B;
            CanMboxUserRegsT.MboxNum = 3;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = 0;
            CanMboxUserRegsT.MboxWord2 = 0;
            CanMboxUserRegsT.MboxWord3 = 0;
            tail = (tail++ >=31)? 0 : tail;
            CanMboxUserRegsT.MboxWord4 = 0                           + tail<<4;

            ECanMboxSend(&CanMboxUserRegsT);
            //参数设置---------------------------
            CanMboxUserRegsT.MboxID = 0x014E5D0B;
            CanMboxUserRegsT.MboxNum = 4;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = 0;
            CanMboxUserRegsT.MboxWord2 = 0;
            CanMboxUserRegsT.MboxWord3 = 0;
            tail = (tail++ >=31)? 0 : tail;
            CanMboxUserRegsT.MboxWord4 = 0                           + tail<<4;

            ECanMboxSend(&CanMboxUserRegsT);*/
#endif
#ifdef DEBUG_CAN
        if(step2 ==1){
      //给定速度 目标速度 反馈速度 正反转
        CanMboxUserRegsT.MboxID = 0x11111303;
        CanMboxUserRegsT.MboxNum = 5;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = _IQtoIQ15(rc1.TargetValue);//Q15主控给的速度
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_spd.Ref);//Q15 pid给定的速度
        CanMboxUserRegsT.MboxWord3 = _IQtoIQ15(pid1_spd.Fbk);//Q15 pid反馈速度
        CanMboxUserRegsT.MboxWord4 = _IQtoIQ12(pid1_spd.Ref)*speed1.BaseRpm>>12;//rpm PID给定速度

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step2 ==2){
        //速度PID输出  运行占空比 速度闭环 相位
        CanMboxUserRegsT.MboxID = 0x11111304;
        CanMboxUserRegsT.MboxNum = 6;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = speed1.SpeedRpm;//pid反馈速度 rpm
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_spd.Out);//PID输出
        CanMboxUserRegsT.MboxWord3 = pwm1.DutyFunc;//占空比
        CanMboxUserRegsT.MboxWord4 = startuptimer;//0-5

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step2 ==3){
        //是否触发过零 速度闭环标志 运行方向 0
        CanMboxUserRegsT.MboxID = 0x11111305;
        CanMboxUserRegsT.MboxNum = 7;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = pwmlimit;//过零点
        CanMboxUserRegsT.MboxWord2 = IsrTime;
        CanMboxUserRegsT.MboxWord3 = PwmSet;
        CanMboxUserRegsT.MboxWord4 =0;

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step2 ==4){
        //数据内容：UA UB UC IDC
        CanMboxUserRegsT.MboxID = 0x11111306;
        CanMboxUserRegsT.MboxNum = 8;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = hall1.HallGpioAccepted;//放大10倍
        CanMboxUserRegsT.MboxWord2 = test;
        CanMboxUserRegsT.MboxWord3 = test3;
        CanMboxUserRegsT.MboxWord4 = test5;

        ECanMboxSend(&CanMboxUserRegsT);

        }else if(step2 ==5){
//速度PI 电流PI

        CanMboxUserRegsT.MboxID = 0x11111307;
        CanMboxUserRegsT.MboxNum = 9;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = (int16)(pid1_spd.Kp);
        CanMboxUserRegsT.MboxWord2 = (int16)(pid1_spd.Ki);
        CanMboxUserRegsT.MboxWord3 = (int16)(pid1_idc.Kp);
        CanMboxUserRegsT.MboxWord4 = (int16)(pid1_idc.Ki);

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step2 ==6){
//电流给定 电流反馈 平均电流 滤波后的平均电流
        CanMboxUserRegsT.MboxID = 0x11111308;
        CanMboxUserRegsT.MboxNum = 10;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = _IQtoIQ15(pid1_idc.Ref);//电流给定
        CanMboxUserRegsT.MboxWord2 = _IQtoIQ15(pid1_idc.Fbk);//电流反馈pid1_idc.Fbk
        CanMboxUserRegsT.MboxWord3 = (DC_current_filter_avr*1000)>>12;
        CanMboxUserRegsT.MboxWord4 = (DC_current_filter_avr*1000)>>12;

        ECanMboxSend(&CanMboxUserRegsT);
        }else if(step2 ==7){
            step2 = 0;
        //
        CanMboxUserRegsT.MboxID = 0x11111309;
        CanMboxUserRegsT.MboxNum = 11;
        CanMboxUserRegsT.MboxDLC = 8;
        CanMboxUserRegsT.MboxWord1 = DCbus_voltage;//直流电压
        CanMboxUserRegsT.MboxWord2 = Tmotor;//电机温度
        CanMboxUserRegsT.MboxWord3 = Tvot;//电调温度
        CanMboxUserRegsT.MboxWord4 = FaultFlag.all;//故障

        ECanMboxSend(&CanMboxUserRegsT);
        }
#endif

    }
}
#ifdef DEBUG_CAN
void carveData1(){
    //当发送标志位为0时，采集并记录模拟数据，但不发送
     if (CanCurveFlag == 0)
     {
         DebugData1(0,test2);
         DebugData1(1,speed1.SpeedRpm);
         DebugData1(2,0);
         DebugData1(3,0);

     }

}

void CanCurve(void)
{// Configure Mailboxes 0-15 as Tx, 16-31 as Rx
    static int16 i = 0;
    static int16 DubugCnt = 0;
    static int16 DubugPer = 0;


    //当数据大于发送间隔时，发送绘图数据
    DubugCnt++;
    if (DubugCnt >= DubugPer)
    {
        //发送间隔数据
        DubugPer = TX_PERIOD*MAINLOOPRATE;
        DubugCnt = 0;
        //当发送标志位为1时，发送绘图波形数据
        if (CanCurveFlag == 1)
        {
            CanMboxUserRegsT.MboxID = 0x1111130E;
            CanMboxUserRegsT.MboxNum = 14;
            CanMboxUserRegsT.MboxDLC = 8;
            CanMboxUserRegsT.MboxWord1 = CanCurveData[i];
            CanMboxUserRegsT.MboxWord2 = CanCurveData[i+0x100];
            CanMboxUserRegsT.MboxWord3 = CanCurveData[i+0x200];
            CanMboxUserRegsT.MboxWord4 = CanCurveData[i+0x300];
            ECanMboxSend(&CanMboxUserRegsT);


            i++;
            if (i >= 0x100)
            {
                i = 0;
                CanCurveFlag = 0;
            }

        }
    }

}

#endif

/**********采集并存储绘图数据1**********/
void DebugData1(int16 num,int16 data)
{

    static int16 cnt[4] ={0,0,0,0};
    CanCurveData[cnt[num]+ 0x100*num] = data;

    cnt[num]++;
    if (cnt[num] >= 0x100)
    {
        cnt[num]=0;
        CanCurveFlag = 1;
    }
}



void CanReceive(void){
    struct CANMBOX_USER_REGS CanMboxTempRegs;
    //飞控上位机
    CanMboxTempRegs = ECanMboxRead(19);
    if (CanMboxTempRegs.MboxRxFlg == TRUE)
    {

        if (CanMboxTempRegs.MboxID == ID_MOTOR3_COMMAND )
        {
            CanMboxUserRegsR = CanMboxTempRegs;
        }
    }
    CanMboxTempRegs = ECanMboxRead(21);
    if (CanMboxTempRegs.MboxRxFlg == TRUE)
    {

        if (CanMboxTempRegs.MboxID == ID_FAULT_CLEAR )
        {
            CanMboxUserRegsR3 = CanMboxTempRegs;
        }
    }
#ifdef DEBUG_CAN
 //------------------------------调试上位机----------------------
     CanMboxTempRegs = ECanMboxRead(16);
     if (CanMboxTempRegs.MboxRxFlg == TRUE)
     {
          if (CanMboxTempRegs.MboxID == ID_DMC_DEBUG)
         {//握手 可以不握手
             if (CanMboxTempRegs.MboxWord1 == 0x5A5A && CanMboxTempRegs.MboxWord2 == 0x2013 &&
                 CanMboxTempRegs.MboxWord3 == 0x0218 && CanMboxTempRegs.MboxWord4 == 0xA5A5)
             {
                 CanDebugFlag = TRUE;
             }
             else
             {
                 CanDebugFlag = TRUE;//FALSE;
             }
         }

         else if (CanMboxTempRegs.MboxID == ID_DMC_SET || CanMboxTempRegs.MboxID == ID_DMC_CLEAR)
         {
             if (CanDebugFlag == TRUE)
             {
                 CanMboxUserRegsR2 = CanMboxTempRegs;//
             }
         }
         else if (CanMboxTempRegs.MboxID == ID_DMC_SET1 || CanMboxTempRegs.MboxID == ID_DMC_SET2\
                 ||CanMboxTempRegs.MboxID == ID_DMC_SET3 || CanMboxTempRegs.MboxID == ID_DMC_SET4|| CanMboxTempRegs.MboxID == ID_DMC_SET5)
         {
             if (CanDebugFlag == TRUE)
             {
                 CanMboxUserRegsR3 = CanMboxTempRegs;
             }
         }
     }
#endif

}

void CanMaster(void){
    CanReceive();
    CanDebugTx();

}
void UserCANprocess(void){

#ifdef PIXHAWK
    if (CanMboxUserRegsR.MboxID == ID_MOTOR3_COMMAND)
       {
#ifdef SPEEDCLOSED
        int16 speedrecieve = 0;
        if(ESC_NUMBER == 0x07){
            speedrecieve = ((CanMboxUserRegsR.MboxWord1 & 0xff00) >>8)  + ((CanMboxUserRegsR.MboxWord2 & 0x00ff)<<8);
            SpeedRef = _IQdiv(speedrecieve,speed1.BaseRpm);//速度给定
         }
         else if(ESC_NUMBER == 0x08){
             speedrecieve = ((CanMboxUserRegsR.MboxWord2 & 0xff00) >>8)  + ((CanMboxUserRegsR.MboxWord3 & 0x00ff)<<8);
            SpeedRef = _IQdiv(speedrecieve,speed1.BaseRpm);//速度给定
         }
#else
        if(ESC_NUMBER == 0x07){
            PwmSet = ((CanMboxUserRegsR.MboxWord1 & 0xff00) >>8)  + ((CanMboxUserRegsR.MboxWord2 & 0x00ff)<<8);
        }
        else if(ESC_NUMBER == 0x08){
            PwmSet = ((CanMboxUserRegsR.MboxWord2 & 0xff00) >>8)  + ((CanMboxUserRegsR.MboxWord3 & 0x00ff)<<8);
        }
#endif //SPEEDCLOSED
       }
        if(CanMboxUserRegsR3.MboxID == ID_FAULT_CLEAR){


            Fault_clear = 1;
            CanMboxUserRegsR3.MboxID = 0;
       }
#ifdef SPEEDCLOSED
        if(_IQabs(SpeedRef)>_IQ(0.1)){
            EnableFlag = 1;
        }else{
            if(_IQabs(speed1.SpeedRpm)<350){//现在烧写的程序还没有这一句
                EnableFlag = 0;
            }
        }
#else
        if(_IQabs(PwmSet)>500){
            EnableFlag = 1;
        }else{
            EnableFlag = 0;
        }
#endif //SPEEDCLOSED
#endif //PIXHAWK
#ifdef DEBUG_CAN
    if (CanMboxUserRegsR2.MboxID == ID_DMC_SET)
    {
        EnableFlag = 1;
        if (CanMboxUserRegsR3.MboxID == ID_DMC_SET1)
           {

               SpeedRef = _IQdiv(CanMboxUserRegsR3.MboxWord1,speed1.BaseRpm);//速度给定


           }
           else if (CanMboxUserRegsR3.MboxID == ID_DMC_SET2)
           {
              // CurrentSet = _IQ((float)CanMboxUserRegsR3.MboxWord1/1000);
               test = CanMboxUserRegsR3.MboxWord1;
           }
           else if (CanMboxUserRegsR3.MboxID == ID_DMC_SET3)
           {
           }

    }
    else if (CanMboxUserRegsR2.MboxID == ID_DMC_CLEAR)
    {
        SpeedRef = _IQabs(0.0);
        CurrentSet = _IQabs(0.0);
        EnableFlag = 0;
        if (CanMboxUserRegsR3.MboxID == ID_DMC_SET4)
       {
           pid1_spd.Kp = (int32)CanMboxUserRegsR3.MboxWord1;
           pid1_spd.Ki = (int32)(CanMboxUserRegsR3.MboxWord2);
           pid1_idc.Kp = (int32)(CanMboxUserRegsR3.MboxWord3);
           pid1_idc.Ki = (int32)(CanMboxUserRegsR3.MboxWord4);

       }

    }
    if (CanMboxUserRegsR3.MboxID == ID_DMC_SET5)
    {
       //
        Fault_clear = 1;
        CanMboxUserRegsR3.MboxID = 0;
    }
#endif

}