TMS320F28379D: Position Manager EnDat2.2 gets incorrect position for Heidehain encoder

Hi，

It's my first time to use TMS320F28379D for absolute encoder position deccoding,  the Error1 reports 1 in my test and the posyion value is not correct. The code is transformed from the development kits under C:\ti\controlSUITE\development_kits\TMDSIDDK_v2.0\IDDK_PM_Servo_F2837x_v2_00_00_00 and the main codes are listed below: the endat.c, the initilization file in the main function and the postion reading function in the interrupt. During our test, the position can be only got correctly under the EnDat CLK1MHz with the CRC checking code annotated, and there are large jump points under high-voltage DC bus of the servo motor driver is loaded. Unfortunately, the connecting line for the Heidahein encoder was broken unintentionaly, after we repair it, we cannot get correct position data, and the endat22Data is pasted as below. Theoretically, the position_hi should be zero and the position_lo is also not correct. We have checked the CLK and Data signal, they seem correct. And we have also excluded the fault of the encoder using the servo drive from Kollomogen. The hardware circuit is transferred from the TIDM1008.

This is the endat.c file.

//----------------------------------------------------------------------------------
//  FILE:           endat.c
//
//  Description:    Contains all the initialization, data declarations and setup
//                  for EnDat encoder interface. This file serves are a template for
//                  using PM_endat22 Library to interface and incorporates all the encoder
//                  and library specific initializations and other important aspects of usage.
//
//  Version:        1.0
//
//  Target:         TMS320F28377D,
//
//----------------------------------------------------------------------------------
//  Copyright Texas Instruments � 2004-2015
//----------------------------------------------------------------------------------
//  Revision History:
//----------------------------------------------------------------------------------
//  Date      | Description / Status
//----------------------------------------------------------------------------------
// 4 Nov 2015  - Example project for PM EnDAT Library Usage
//----------------------------------------------------------------------------------

#include "F28x_Project.h"     // Device Headerfile and Examples Include File
#include "endat.h"

uint16_t endat22CRCtable[SIZEOF_ENDAT_CRCTABLE];    // Declare CRC table for EnDat CRC calculations

ENDAT_DATA_STRUCT endat22Data;      //PM EnDat22 data structure

uint16_t crc5_result;   //variable for calculated crc result checking
uint16_t retval1;       //used for function return val storage and checks


// Function to initialize EnDat operation
void EnDat_Init(void) {

    EALLOW;
//Enable clocks to PWM1/2/3/4
    CpuSysRegs.PCLKCR2.bit.EPWM1 = 1;
    CpuSysRegs.PCLKCR2.bit.EPWM2 = 1;
    CpuSysRegs.PCLKCR2.bit.EPWM3 = 1;
    CpuSysRegs.PCLKCR2.bit.EPWM4 = 1;
    EDIS;

//Configure EPWM4 to drive default values on GPIO6 and GPIO7
    EPWM4_Config();

// Generate table for EnDat Polynomial defied as POLY1
    PM_endat22_generateCRCTable(NBITS_POLY1, POLY1, endat22CRCtable);
//GPIO configuration for ENDat operation
    Endat_setup_GPIO();
//XBAR configuration for ENDat operation
    Endat_config_XBAR();

    endat22Data.spi = &SpibRegs;
    PM_endat22_setupPeriph();

    EALLOW;
// Interrupts that are used in this example are re-mapped to
// ISR functions found within this file.
    PieVectTable.SPIB_RX_INT = &spiRxFifoIsr;
    PieCtrlRegs.PIECTRL.bit.ENPIE = 1;   // Enable the PIE block
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;     // Enable PIE Group 6, INT 9
    IER = 0x20;                            // Enable CPU INT6
    EINT;
    EDIS;

    // Power up EnDat 5v supply through GPIO32
//    GpioDataRegs.GPBDAT.bit.GPIO32 = 1;
//    DELAY_US(10000L);   //Delay 10us
    //EncCLK high for 100ms
    EALLOW;
    GpioCtrlRegs.GPADIR.bit.GPIO6 = 1;  //set as GPIO output
    GpioDataRegs.GPASET.bit.GPIO6 = 1; //set GPIO output to be high once GPIO6 is turned to be a GPIO
    GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 0;
    DELAY_US(100000L);
    //EncCLK low for ~200ns (>125ns)
    GpioDataRegs.GPADAT.bit.GPIO6 = 0;
    //EncCLK high for 425ms (>381ms)
    GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 1;
    DELAY_US(425000L);

    //Ensure that EncData is now low
    if (GpioDataRegs.GPBDAT.bit.GPIO63 == 1) {
          ESTOP0;
    }
    DELAY_US(1000L);    //Delay 1s

    PM_endat22_setFreq(ENDAT_INIT_FREQ_DIVIDER);

    DELAY_US(1000L);

//Encoder Receive Reset - Command ERR
    retval1 = PM_endat22_setupCommand (ENCODER_RECEIVE_RESET, 0xAA, 0x2222, 0); //data1=any; data2=any
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_RECEIVE_RESET, 0);
    DELAY_US(1000000L);     //Delay 1s

    retval1 = PM_endat22_setupCommand (ENCODER_RECEIVE_RESET, 0xAA, 0x2222, 0); //data1=any; data2=any
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_RECEIVE_RESET, 0);

    DELAY_US(2000L);

//Select memory area to read out encoder manufacturer parameters.
//MRS Code A1 - Select Parameters of Encoder Manufacturer
//     do {
     retval1 = PM_endat22_setupCommand (SELECTION_OF_MEMORY_AREA, 0xA1, 0x5555, 0);  //data1=MRS; data2=any
     PM_endat22_startOperation();
     while (endat22Data.dataReady != 1) {}
     //CHECK CRC!
     retval1 = PM_endat22_receiveData(SELECTION_OF_MEMORY_AREA, 0);
     crc5_result =  PM_endat22_getCrcNorm(endat22Data.address, endat22Data.data, endat22CRCtable);
//    } while (!CheckCRC(crc5_result,endat22Data.data_crc));
     /*  if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
             ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

//Request Encoder to send the number of clock pulses needed to shift out Encoder Position Values
//Reading Address 0xD Returns no.of clock pulses needed to shift out position data
//     do {
     retval1 = PM_endat22_setupCommand (ENCODER_SEND_PARAMETER, 0xD, 0xAAAA, 0);    //data1=address; data2=any
     PM_endat22_startOperation();
     while (endat22Data.dataReady != 1) {}
     //CHECK CRC!
     retval1 = PM_endat22_receiveData(ENCODER_SEND_PARAMETER, 0);
     crc5_result =  PM_endat22_getCrcNorm(endat22Data.address, endat22Data.data, endat22CRCtable);
//     } while (!CheckCRC(crc5_result,endat22Data.data_crc));
     /*
     if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
             ESTOP0;
     }
*/
// No.of clocks for position data needs to be set before attempting any position data access
     endat22Data.position_clocks = endat22Data.data & 0xFF;

     DELAY_US(200L);
}

void Endat_setup_GPIO(void) {

    EALLOW;
    GpioCtrlRegs.GPAMUX1.bit.GPIO6 = 1; // Configure GPIO6 as EnDat Clk master
    GpioCtrlRegs.GPAMUX1.bit.GPIO7 = 1; // Configure GPIO7 as SPI Clk slave

    GpioCtrlRegs.GPBGMUX2.bit.GPIO63 = 3;
    GpioCtrlRegs.GPCGMUX1.bit.GPIO64 = 3;
    GpioCtrlRegs.GPCGMUX1.bit.GPIO65 = 3;
    GpioCtrlRegs.GPCGMUX1.bit.GPIO66 = 3;

    GpioCtrlRegs.GPBMUX2.bit.GPIO63 = 3; // Configure GPIO24 as SPISIMOB
    GpioCtrlRegs.GPCMUX1.bit.GPIO64 = 3; // Configure GPIO25 as SPISOMIB
    GpioCtrlRegs.GPCMUX1.bit.GPIO65 = 3; // Configure GPIO26 as SPICLKB
    GpioCtrlRegs.GPCMUX1.bit.GPIO66 = 3; // Configure GPIO27 as SPISTEB

    GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3; // Asynch input GPIO24 (SPISIMOB)
    GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3; // Asynch input GPIO25 (SPISOMIB)
    GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3; // Asynch input GPIO26 (SPICLKB)
    GpioCtrlRegs.GPCQSEL1.bit.GPIO66 = 3; // Asynch input GPIO27 (SPISTEB)

    GpioCtrlRegs.GPAMUX1.bit.GPIO9 = 3; // Configure GPIO34 as EnDat TxEN

//  GpioCtrlRegs.GPBDIR.bit.GPIO32 = 1; // Configure GPIO32 as EnDat Pwr Ctl

    EDIS;
}

void Endat_config_XBAR(void)
{
    EALLOW;

//Choose SPISIMO GPIO as InputXbar Input1 - SPISIMO (B) on GPIO24 in this case
    InputXbarRegs.INPUT1SELECT = 63;    // GPTRIP XBAR TRIP1 -> GPIO24
    EDIS;
}

//Template for performing delay compensation
//This function should only be called while operating at low frequencies (~200KHz) only.
void EnDat_initDelayComp(void) {
    uint16_t delay1, delay2;

//Transmit Send position values command to the encoder
//Delay is internally measured during this command operation
//    do {
    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES, 0, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES, 0);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT21, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
//    } while (!CheckCRC(crc5_result,endat22Data.data_crc));
    /*  if (!CheckCRC(crc5_result,endat22Data.data_crc))
    {
            ESTOP0;
    }
*/
     DELAY_US(200L);    //Delay 200us

//Delay is internally measured during the above operation
//Read the measured delay and store in a variable - n * 2 * SYSCLKs
    delay1 = PM_endat22_getDelayCompVal();

//Redo the same procedure again
//    do {
    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES, 0, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES, 0);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT21, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
//    } while (!CheckCRC(crc5_result,endat22Data.data_crc));
    /*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
         ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

 //Delay is internally measured during the above operation
 //Read the measured delay and store in a variable - n * 2 * SYSCLKs
     delay2 = PM_endat22_getDelayCompVal();

//Average value of the measured delays has to be updated into endat22Data.delay_comp
// for delay compensation to take effect
    endat22Data.delay_comp = (delay1+delay2) >> 1;
}


void error(void) {
    asm("     ESTOP0");
    //Test failed!! Stop!
    for (;;);
}

interrupt void spiRxFifoIsr(void)
{
    uint16_t i;
    for (i=0;i<=endat22Data.fifo_level;i++){endat22Data.rdata[i]= endat22Data.spi->SPIRXBUF;}
    endat22Data.spi->SPIFFRX.bit.RXFFOVFCLR=1;  // Clear Overflow flag
    endat22Data.spi->SPIFFRX.bit.RXFFINTCLR=1;  // Clear Interrupt flag
    PieCtrlRegs.PIEACK.all|=0x20;       // Issue PIE ack
    endat22Data.dataReady=1;
}


void EPWM4_Config(void) {
// Set the PWMA and B high as default values of ENDAT clk.
    EALLOW;
    EPwm4Regs.TZCTL.bit.TZA = 1;
    EPwm4Regs.TZCTL.bit.TZB = 1;
    EPwm4Regs.TZFRC.bit.OST = 1;
    EDIS;
}

uint16_t CheckCRC (uint16_t expectcrc5, uint16_t receivecrc5) {
    if (expectcrc5 == receivecrc5) {
        return 1;
    }
    else {
        return 0;
    }
}



void endat22_setupAddlData(void)
{
// EnDat22 Command reading position values and performing selection of memory area
// 0xA1 MRS code for selection of encoder manufacturer parameters
// Additional data enabled at this time is 0

    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 0xA1, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 0);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT22, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
     ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

// EnDat22 Command reading position values and sending a parameter to encoder
// Send 0xD parameter address - will select no. of clock pulses to shift out position value
// Additional data enabled at this time is 0

    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES_AND_SEND_PARAMETER, 0xD, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES_AND_SEND_PARAMETER, 0);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT22, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
     ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

// Enable Additional data 1 - to read out no. of  clock pulses to shift out position value
// MRS code 0x45    - Acknowledge memory content LSB

    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 0x45, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 0);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT22, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
     ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

 // Enable Additional data 2 - to read out Operating status and error sources
 // MRS code 0x59   - operating status on additional data 2
 // Additional data enabled before this command is 1, after this command 2
    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 0x59, 0, 1);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES_AND_SELECTION_OF_THE_MEMORY_AREA, 1);

    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT22, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
     ESTOP0;
     }
*/
     DELAY_US(2000L);   //Delay 200us

}

void endat22_readPositionWithAddlData(void)
{
// Read out position value with 2 additional data previously enabled
// Additional data enabled at this time is 2

    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES_WITH_ADDITIONAL_DATA, 0, 0, 2);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES_WITH_ADDITIONAL_DATA, 2);

// CRC check for Position Data
    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT22, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
     ESTOP0;
     }
*/
// CRC check for Additional Data 1
    crc5_result =  PM_endat22_getCrcNorm((endat22Data.additional_data1 >> 16), endat22Data.additional_data1, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.additional_data1_crc))
     {
     ESTOP0;
     }
*/
// CRC check for Additional Data 2
    crc5_result =  PM_endat22_getCrcNorm((endat22Data.additional_data2 >> 16), endat22Data.additional_data2, endat22CRCtable);
/*
    if (!CheckCRC(crc5_result,endat22Data.additional_data2_crc))
     {
     ESTOP0;
     }
*/
     DELAY_US(200L);    //Delay 200us

}

void endat21_readPosition(void)
{
// Read position value in EnDat21 mode - no additional data
    retval1 = PM_endat22_setupCommand (ENCODER_SEND_POSITION_VALUES, 0, 0, 0);
    PM_endat22_startOperation();
    while (endat22Data.dataReady != 1) {}
    retval1 = PM_endat22_receiveData(ENCODER_SEND_POSITION_VALUES, 0);

// CRC check for Position Data
//    while (!CheckCRC(crc5_result,endat22Data.data_crc)) {
    crc5_result =  PM_endat22_getCrcPos(endat22Data.position_clocks, ENDAT21, endat22Data.position_lo,
                    endat22Data.position_hi, endat22Data.error1, endat22Data.error2, endat22CRCtable);
//    }
/*
    if (!CheckCRC(crc5_result,endat22Data.data_crc))
     {
         ESTOP0;
     }
*/
}
//***************************************************************************
// End of file
// **************************************************************************

This is the initilization file in the main function.

// ****************************************************************************
// Parameter Initialization for EnDat2.2
// ****************************************************************************
    // Init EnDat22 parameters
    endat1.PolePairs    = POLES/2;

    //Initialization routine for endat operation - defined in endat.c
    //Configures the peripherals and enables clocks for required modules
    //Configures GPIO and XBar as needed for EnDat operation
    //Sets up the SPI peripheral in endat data structure and enables interrupt
    EnDat_Init();
    //Peforms cable propagation delay calculation.
    //This is required for long cable lengths and higher EnDat Clock frequencies
    //Function defined in endat.c

    EnDat_initDelayComp();

    //Switch to high frequency - 8.3MHz (=200/4*ENDAT_RUNTIME_FREQ_DIVIDER)
    PM_endat22_setFreq(ENDAT_RUNTIME_FREQ_DIVIDER);
    DELAY_US(800L);     //Delay 800us
    endat22Data.dataReady = 1;

This is the position reading file in the interrupt function.

.......
//Read position data in EnDat21 mode. Function defined in endat.c
    if(endat22Data.dataReady == 1)
    {
        endat21_readPosition();
    }

//    endat1.RawTheta = (1.0-((float)endat22Data.position_lo)/33554432.0); // Dividing by 2^25 : EnDat Encoder 25 bits
    endat1.RawTheta = endat22Data.position_lo;
    endat1.Position = (float)(endat1.RawTheta)*0.00001;
    // during alignment, assign the current shaft position as initial position
    endat1.InitTheta = 1391304;//对相位

    endat1.MechTheta   = endat1.RawTheta - endat1.InitTheta;      /* MechTheta in step counts */
    endat1.MechTheta = (endat1.MechTheta)%4800000;
    endat1.ElecTheta = (float)(endat1.MechTheta)/4800000;

    if(endat1.ElecTheta < 0)
        endat1.ElecTheta = endat1.ElecTheta + 1.0;
    else if (endat1.ElecTheta > 1.0)
        endat1.ElecTheta = endat1.ElecTheta - 1.0;

    // Compute the electrical angle in Q24
    endat1.ElecTheta  = _IQfrac(endat1.PolePairs * endat1.ElecTheta);

    // Position Sense Reversal
//    endat1.ElecTheta = 1.0 - endat1.ElecTheta;
//    endat1.MechTheta = 1.0 - endat1.MechTheta;