/*
 * mss_CanYanyin.c
 *
 *  Created on: 2018年1月2日
 *    
 */

/*
 * Can_Trans.c
 *
 *  Created on: 2017年6月28日
 *    
 */

/**************************************************************************
 *************************** Include Files ********************************
 **************************************************************************/
/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <math.h>

/* BIOS/XDC Include Files. */
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/IHeap.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Memory.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Clock.h>
#include <ti/sysbios/heaps/HeapBuf.h>
#include <ti/sysbios/heaps/HeapMem.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/family/arm/v7a/Pmu.h>


/* Can Drivers Files: */
#include <ti/common/sys_common.h>
#include <ti/drivers/soc/soc.h>
#include <ti/drivers/canfd/canfd.h>
#include <ti/drivers/pinmux/pinmux.h>
#include <ti/drivers/esm/esm.h>
#include <ti/utils/testlogger/logger.h>
#include <ti/drivers/osal/HwiP.h>
#include "mss_Candriver.h"
#include "mss_qspiflash.h"
//#include "mss_alarm.h"

//**************************************************************************
// CAN 目標輸出
//**************************************************************************
void CANTargetOutput(TargetInfo* TargetImfomation,uint16_t DSPdoutlen)
{
    uint16_t i , j;
    uint32_t CAN_id1;
    CAN_id1 = TARGETOUTTXID1;                //500
    uint16_t indextempidinfo[32];
    uint16_t targetnumber = 0;

    memset(&indextempidinfo[0] , 0 , sizeof(uint16_t)*32);

    targetnumber = DSPdoutlen;
    for(i = 0 ; i < targetnumber ; i++) indextempidinfo[i] = TargetImfomation[i].Id;

    for(i = 0 ; i < 32 ; i++){

        if(TargetTrackingMcb[i].FullEnptyFlag != 0){

            TargetTrackingMcb[i].FullEnptyFlag = 0;

            for(j = 0 ; j < targetnumber; j++){

                if(TargetTrackingMcb[i].TargetId == TargetImfomation[j].Id){

                    TargetTrackingMcb[i].FullEnptyFlag = 1;
                    TargetTrackingMcb[i].TargetPtr = &TargetImfomation[j];
                    indextempidinfo[j] = 0;
                }
            }

            if(TargetTrackingMcb[i].FullEnptyFlag == 0) memset(&TargetTrackingMcb[i] , 0 , sizeof(TargetTrackingMcb_t));
        }
        else{

            TargetTrackingMcb[i].TargetPtr = (TargetInfo*)&enptytargetinfo[0];
        }
    }

    i = 0 ; j = 0;

    for(i = 0 ; i < targetnumber ; i++){

        if(indextempidinfo[i] != 0){

            j = 0;
            while(TargetTrackingMcb[j].FullEnptyFlag == 1){ j++;}

            TargetTrackingMcb[j].FullEnptyFlag = 1;
            TargetTrackingMcb[j].TargetId = TargetImfomation[i].Id;
            TargetTrackingMcb[j].TargetPtr = &TargetImfomation[i];
        }
    }

    for(i = 0; i < 32; i++)
    {
        memset(&ucTXMsgData[0] , 0 , sizeof(unsigned char)*8);

        //------------------------------------------------------------------------------------------------------------------------//ID1
        //SG_ MMR_TARGET_RANGE_RATE : 53|
 
[email protected]+ (0.02,-163.84) [-163.84|163.82] "m/s" Vector__XXX
        tempdata = TargetTrackingMcb[i].TargetPtr->Velocity*5;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[6] = (Htempdata & 0x3F);
        ucTXMsgData[7] = Ltempdata;

        //bit 54 for reserve

        //SG_ MMR_TARGET_PITCH_ANGLE : 47|
 
[email protected]+ (0.1,-25.6) [-25.6|25.5] "deg" Vector__XXX
        tempdata = 0;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[5] = (Htempdata & 0x01);
        ucTXMsgData[5] = ucTXMsgData[5] << 7;
        ucTXMsgData[5] = ucTXMsgData[5] | (Ltempdata >> 1);
        ucTXMsgData[6] = ucTXMsgData[6] | ((Ltempdata&0x01)<<7);

        //SG_ MMR_TARGET_RANGE : 28|
 
[email protected]+ (0.05,0) [0|300] "m" Vector__XXX
        tempdata = TargetTrackingMcb[i].TargetPtr->Distance*2;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[3] = (Htempdata & 0x1F);
        ucTXMsgData[4] = Ltempdata;

        //SG_ MMR_TARGET_ANGLE : 23|[email protected]+ (0.1,-102.4) [-102.4|102.3] "deg" Vector__XXX
        tempdata = TargetTrackingMcb[i].TargetPtr->Angle;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[2] = Htempdata & (0X07);
        ucTXMsgData[2] = ucTXMsgData[2] << 5;
        ucTXMsgData[2] = ucTXMsgData[2] | (Ltempdata >> 3);
        ucTXMsgData[3] = ucTXMsgData[3] | ((Ltempdata & (0X07)) << 5);

        //SG_ MMR_TARGET_RADARMODE : 15|[email protected]+ (1,0) [0|1] "" Vector__XXX
        ucTXMsgData[1] = ucTXMsgData[1] | SLMODEFLAG;

        //SG_ MMR_TARGET_CONFIDENCE : 14|[email protected]+ (1,0) [0|100] "%" Vector__XXX
        tempdata = 0;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[1] = ucTXMsgData[1] | (Ltempdata >> 1);

        //SG_ MMR_TARGET_FRAME_INDEX : 7|[email protected]+ (1,0) [0|0] "count" Vector__XXX
        tempdata = framecnt & 0x0F;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[0] = Ltempdata;
        ucTXMsgData[0] = ucTXMsgData[0] << 4;


        //SG_ MMR_TARGET_STATUS : 3|[email protected]+ (1,0) [0|15] "count" Vector__XXX
        tempdata = 0;
        Htempdata = (tempdata >> 8);
        Ltempdata = tempdata;
        ucTXMsgData[0] = ucTXMsgData[0] | (Ltempdata & 0x0F);

        retVal = CANFD_transmitData (txMsgObjHandle[i], CAN_id1+i , CANFD_MCANFrameType_CLASSIC, 8U, &ucTXMsgData[0], &errCode);
    }

    framecnt++;
}