MotorStep42L/Core/Src/Backup/MT6816_ATY.c.bak

/**
* @file MT6816_ATY.c
*
* @param Project DEVICE_GENERAL_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_1EDGE = CPOL=1, CPHA=0 = SPI Mode 2
*
* @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[6] = {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   MT6816_ErrorCheck
 *
 * @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);
    wrData[3] = wrData[0];
    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);
    wrData[4] = wrData[0];
    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;
    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) angle += 16384;
    dev->angle = (float)(angle - dev->zeroPoint) * 360.0 / 16384.0;
    dev->angle = 360.0 - dev->angle;
    return dev->angle;
}

#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,
//     .error = 0,
//
//     .lock = __ATY_UNLOCKED
// };


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

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


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