ATY_LIB/MT6816_ATY.c

/**
* @file MT6816_ATY.c
*
* @param Project DEVICE_DRIVER_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2026 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief functions of MT6816 magnetic encoder for C platform
*
* @note SPI_POLARITY_HIGH + SPI_PHASE_2EDGE = CPOL=1, CPHA=1 = SPI Mode 3
*
* @version
*       - 1_01_251222 > ATY
*           -# Preliminary version, first Release
********************************************************************************
*/

#ifndef __MT6816_ATY_C
#define __MT6816_ATY_C

#include "MT6816_ATY.h"
#define MT6816_ATY_TAG "\r\n[MT6816_ATY] "

/******************************* For user *************************************/

/******************************************************************************/

uint8_t wrData[3] = {0};

/**
 * @brief   Parity check
 *
 * @param   dev Device structure pointer
 * @param   data Data
 * @return  uint8_t Execution result
 */
uint8_t MT6816_ParityCheck(struct MT6816_ATY_Dev* dev, uint16_t data){
    uint8_t parityCheck = 0;
    if(data == 0) data = dev->angleValue;
    dev->error &= ~(uint8_t)MT6816_PARITY_CHECK_MASK;

    for(int i = 0; i < 15; i++){
        data >>= 1;
        if(data & 0x01){
            parityCheck ^= 1;
        }
    }
    // Check if parity bit matches (parity bit is bit 7 of angle_lsb)
    if((dev->angleValue & 0x01) != parityCheck){
        dev->error |= MT6816_PARITY_CHECK_MASK;
        printf_ATY_D("%sParity check failed", MT6816_ATY_TAG);
    }
    return dev->error;
}

/**
 * @brief   Error check
 *
 * @param   dev Device structure pointer
 * @param   data Data
 * @return  uint8_t Execution result
 */
uint8_t MT6816_ErrorCheck(struct MT6816_ATY_Dev* dev, uint8_t* data){
    dev->error = 0;
    MT6816_ParityCheck(dev, (data[0] << 8) | data[1]);
    if(data[1] & MT6816_NO_MAG_WARNING_MASK){
        dev->error |= MT6816_NO_MAG_WARNING_MASK;
        printf_ATY_D("%sNo magnetic field warning", MT6816_ATY_TAG);
    }
    if(data[2] & MT6816_OVER_SPEED_MASK){
        dev->error |= MT6816_OVER_SPEED_MASK;
        printf_ATY_D("%sOver speed warning", MT6816_ATY_TAG);
    }
    return dev->error;
}

/**
 * @brief   Read angle value from MT6816 for only angle value
 *
 * @param   dev Device structure pointer
 * @return  uint8_t Execution result
 */
uint8_t MT6816_ReadAngleBase(struct MT6816_ATY_Dev* dev){
    __ATY_LOCK(dev);

    // Reading MSB (ANGLE_MSB register)
    wrData[0] = MT6816_REG_ANGLE_13_6 | 0x80;

    dev->nssSet(__ATY_HL_L);
    dev->spiProcess(wrData, 1, __ATY_RW_W);
    dev->spiProcess(wrData, 1, __ATY_RW_R);
    dev->nssSet(__ATY_HL_H);
    dev->angleValue = ((uint16_t)wrData[0] << 8);

    // Reading LSB (ANGLE_LSB register)
    wrData[0] = MT6816_REG_ANGLE_5_0 | 0x80;

    dev->nssSet(__ATY_HL_L);
    dev->spiProcess(wrData, 1, __ATY_RW_W);
    dev->spiProcess(wrData, 1, __ATY_RW_R);
    dev->nssSet(__ATY_HL_H);
    dev->angleValue |= (wrData[0]);

    MT6816_ParityCheck(dev, 0);

    // dev->angleValue >>= 2;
    MT6816_CalculateAngleReal(dev, 0);

    __ATY_UNLOCK(dev);
    return 0;
}


/**
 * @brief   Read angle value from MT6816 for all 3 regs
 *
 * @param   dev Device structure pointer
 * @return  uint8_t Execution result
 */
uint8_t MT6816_ReadAngleFull(struct MT6816_ATY_Dev* dev){
    __ATY_LOCK(dev);

    wrData[0] = MT6816_REG_ANGLE_13_6 | 0x80;

    dev->nssSet(__ATY_HL_L);
    dev->spiProcess(wrData, 1, __ATY_RW_W);
    dev->spiProcess(wrData, 3, __ATY_RW_R);
    dev->nssSet(__ATY_HL_H);

    // Combine MSB and LSB to form 14-bit angle value
    dev->angleValue = ((uint16_t)wrData[0] << 8 | wrData[1]);

    MT6816_ErrorCheck(dev, wrData);

    // dev->angleValue >>= 2;
    MT6816_CalculateAngleReal(dev, 0);

    __ATY_UNLOCK(dev);
    return 0;
}

/**
 * @brief   Set zero point
 *
 * @param   dev Device structure pointer
 * @return  uint8_t Zero point value
 */
uint16_t MT6816_SetZeroPoint(struct MT6816_ATY_Dev* dev){
    MT6816_ReadAngleFull(dev);
    dev->zeroPoint = dev->angleValue >> 2;
    return dev->zeroPoint;
}


/**
 * @brief   Calculate angle in degrees from raw value
 *
 * @param   dev Device structure pointer
 * @param   angle Raw angle value, if 0, use internal angle value
 * @return  float Angle in degrees
 */
float MT6816_CalculateAngle(struct MT6816_ATY_Dev* dev, uint16_t angle){
    if(angle == 0) angle = dev->angleValue >> 2;
    dev->angle = (float)angle * 360.0 / 16384.0;
    if(dev->angleReverseDir == 1){ dev->angle = 360.0 - dev->angle; }
    return dev->angle;
}

/**
 * @brief   Calculate angle in degrees from raw value and remove zero diff
 *
 * @param   dev Device structure pointer
 * @param   angle Raw angle value, if 0, use internal angle value
 * @return  float Angle in degrees
 */
float MT6816_CalculateAngleReal(struct MT6816_ATY_Dev* dev, uint16_t angle){
    if(angle == 0) angle = dev->angleValue >> 2;

    if(angle == dev->zeroPoint) return 0.0;
    angle = 16384 + angle - dev->zeroPoint;
    if(angle > 16384) angle -= 16384;
    return MT6816_CalculateAngle(dev, angle);
}

/**
 * @brief   Update turn count
 *
 * @param   dev Device structure pointer
 * @return  uint8_t Execution result
 * @note    motor fast rpm = 1 / tCycle(sec) / 2 * 60(1min),
 *              be sure angle can read back every 180 degree.
 *          only use for increased angle value.
 */
uint32_t MT6816_TurnCount(struct MT6816_ATY_Dev* dev){
    // jump zero
    if(dev->angle - dev->angleLast < -180.0){
        dev->turnCount++;
    }
    dev->angleLast = dev->angle;
    return dev->turnCount;
}

uint32_t MT6816_TurnCountOffset(struct MT6816_ATY_Dev* dev){
    // jump zero
    if(dev->angle - dev->angleLast < -180.0){
        dev->angleTotal += dev->angle + 360.0 - dev->angleLast;
    }
    else{
        dev->angleTotal += dev->angle - dev->angleLast;
    }

    if(dev->angleTotal >= 720.0){
        dev->turnCount++;
        dev->angleTotal -= 360.0;
    }
    dev->angleLast = dev->angle;

    if(dev->start == 0){
        if(dev->angleTotal >= 360.0){
            dev->turnCount++;
            dev->angleTotal -= 360.0;
        }
    }
    return dev->turnCount;
}
void MT6816_TurnCountOffsetStart(struct  MT6816_ATY_Dev* dev, uint8_t dir){
    dev->start = 1;
    dev->turnCount = 0;
    dev->angleStart = dev->angle;
    dev->angleLast = dev->angle;
    dev->angleTotal = 0;
    /* call once TurnCountOffsetStart and TurnCountOffsetStop after change dir,
        like:
        if(MSSTC_Dev_1.direction != (uint8_t)RGF_MOTOR_DIRECTION){
            MSSTC_Dev_1.direction = (uint8_t)RGF_MOTOR_DIRECTION;
            MSSTC_Dev_1.dirSet(MSSTC_Dev_1.direction);
            MT6816_TurnCountOffsetStart(&MT6816_ATY_Dev_1, MSSTC_Dev_1.direction);
            MT6816_TurnCountOffsetStop(&MT6816_ATY_Dev_1);
        }
    */
    dev->angleReverseDir = dir ^ 0x01;

    MT6816_TurnCountOffset(dev);
}

void MT6816_TurnCountOffsetStop(struct  MT6816_ATY_Dev* dev){
    dev->start = 0;

    MT6816_TurnCountOffset(dev);
}

#endif /* __MT6816_ATY_C */

/************************************ etc *************************************/
/* init
// MT6816 ---------------------------------------------------------------------
#include "MT6816_ATY.h"
void MT6816_1_NSS_SET(uint8_t level){
    if(level == __ATY_HL_L)
        HAL_GPIO_WritePin(SPI1_NSS_GPIO_Port, SPI1_NSS_Pin, GPIO_PIN_RESET);
    else if(level == __ATY_HL_H)
        HAL_GPIO_WritePin(SPI1_NSS_GPIO_Port, SPI1_NSS_Pin, GPIO_PIN_SET);
}
uint8_t MT6816_1_SPI(uint8_t* data_t, uint16_t len, uint8_t rw){
    if(rw == __ATY_RW_R){
        return HAL_SPI_Receive(&hspi1, (uint8_t*)data_t, len, 1000);
    }
    else{
        return HAL_SPI_Transmit(&hspi1, (uint8_t*)data_t, len, 1000);
    }
}
struct MT6816_ATY_Dev MT6816_ATY_Dev_1 = {
    .nssSet = MT6816_1_NSS_SET,
    .spiProcess = MT6816_1_SPI,

    .angleValue = 0,
    .angle = 0,
    .angleLast = 0,
    .angleStart = 0,
    .angleTotal = 0,
    .turnCount = 0,
    .start = 0,

    .error = 0,
    .zeroPoint = 0,
    .angleReverseDir = 1,

    .lock = __ATY_UNLOCKED
};
void MT6816_1_Init(void){
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);
    HAL_Delay(100);
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);

    RGF_MOTOR_ANGLE_SET_ZERO = 0;
    RGF_MOTOR_ANGLE_ZERO = MT6816_ATY_Dev_1.zeroPoint;
    RGF_MOTOR_ANGLE = MT6816_ATY_Dev_1.angle;
    RGF_MOTOR_ANGLE_ERR = MT6816_ATY_Dev_1.error;

    RGF_MOTOR_ANGLE_ANGLE_START = MT6816_ATY_Dev_1.angleStart;
    RGF_MOTOR_ANGLE_TURN_COUNT = MT6816_ATY_Dev_1.turnCount;
    RGF_MOTOR_ANGLE_ANGLE_TOTAL = MT6816_ATY_Dev_1.angleTotal;
    RGF_MOTOR_ANGLE_START = MT6816_ATY_Dev_1.start;
}
void MT6816_1_UpdateCycle(void){
    MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);
    MT6816_TurnCountOffset(&MT6816_ATY_Dev_1);
    if((uint8_t)RGF_MOTOR_ANGLE_SET_ZERO == 1){
        RGF_MOTOR_ANGLE_SET_ZERO = 0;
        MT6816_SetZeroPoint(&MT6816_ATY_Dev_1);
    }
    RGF_MOTOR_ANGLE_ZERO = MT6816_ATY_Dev_1.zeroPoint;
    RGF_MOTOR_ANGLE = MT6816_ATY_Dev_1.angle;
    RGF_MOTOR_ANGLE_ERR = MT6816_ATY_Dev_1.error;
    RGF_MOTOR_ANGLE_ANGLE_START = MT6816_ATY_Dev_1.angleStart;
    RGF_MOTOR_ANGLE_TURN_COUNT = MT6816_ATY_Dev_1.turnCount;
    RGF_MOTOR_ANGLE_ANGLE_TOTAL = MT6816_ATY_Dev_1.angleTotal;
    RGF_MOTOR_ANGLE_START = MT6816_ATY_Dev_1.start;
}
*/

/* use
MT6816_1_Init();

// read first value and abort
MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);
HAL_Delay(100);
MT6816_ReadAngleFull(&MT6816_ATY_Dev_1);

// if need to set zero point, read angle value first
MT6816_SetZeroPoint(&MT6816_ATY_Dev_1);

// calculate turn count for motor
MT6816_TurnCountOffset(&MT6816_ATY_Dev_1);

printf_ATY("\r\nAngle: 0x%04X, Degrees: %.2f",
    MT6816_ATY_Dev_1.angleValue,
    MT6816_ATY_Dev_1.angle);

void MSSTC_1_Cycle(void){
    ...
    MT6816_1_UpdateCycle();
    MSSTC_UpdateAngle(&MSSTC_Dev_1,
        MT6816_ATY_Dev_1.turnCount,
        MT6816_ATY_Dev_1.angleTotal + MT6816_ATY_Dev_1.angleStart);
    if((uint8_t)RGF_MOTOR_RUN == 1){
        MT6816_TurnCountOffsetStart(&MT6816_ATY_Dev_1, MSSTC_Dev_1.direction);
        MSSTC_Move(&MSSTC_Dev_1, RGF_MOTOR_STEP_COUNT, MT6816_ATY_Dev_1.angleStart);
    }
    else if((uint8_t)RGF_MOTOR_RUN == 2){
        MSSTC_Scram(&MSSTC_Dev_1);
    }
    RGF_MOTOR_RUN = 0;

    if(MSSTC_Dev_1.runState == MSSTC_STATE_STANDBY
        && MT6816_ATY_Dev_1.start == 1){
        MT6816_TurnCountOffsetStop(&MT6816_ATY_Dev_1);
    }
    ...
}
*/

/******************************************************************************/


/******************************** End Of File *********************************/