/**
* @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 -
*
* https://mengze.top/MZ-ATY_VCJS
* - CC 4.0 BY-NC-SA -
*
* https://creativecommons.org/licenses/by-nc-sa/4.0/
* - 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 *********************************/