Motor_v2/Robot_V2/device/ang_sensor.cpp

/**
 * @file device/ang_sensor.cpp
 * @brief 底层设备驱动与外设封装源文件
 * @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
 */

#include "ang_sensor.h"
#include "Can.h"
#include "Filter.h"
#include "config.h"
#include "flash.h"
#include "gd32c10x_dma.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include <cstdio>
#include <cstring>
#include <stdint.h>

// ANG_SENS 调试宏定义
#if ANG_SENS_DEBUG
/** @brief ANG_SENS_DEBUG_PRINT 宏定义 */
#define ANG_SENS_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
/** @brief ANG_SENS_DEBUG_PRINT 宏定义 */
#define ANG_SENS_DEBUG_PRINT(fmt, ...) ((void)0)
#endif

/** @brief ANG_SPI_CS_ENABLE 宏定义 */
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
/** @brief ANG_SPI_CS_DISABLE 宏定义 */
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
/** @brief DRV_SPI_CS_ENABLE 宏定义 */
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
/** @brief DRV_SPI_CS_DISABLE 宏定义 */
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)

// 全局角度传感器单例：系统只有一个 KTH7823 角度传感器。
// 初始化顺序由 main::hardware_init() 调用 KTH7823.init() 保证。
angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
extern CAN_transmiter CAN0_transmiter; // CAN 初始化

/**
 * @brief Send or read half word through SPI by register polling
 *
 * @param[in] spi_periph SPI peripheral
 * @param[in] tx_data Data to transmit
 * @param[out] rx_data Pointer to store received data
 * @return 0 if success, -1 if error
 * @note 单半字轮询收发。DMA 版本（已移除）每次传输需要：重配 2 个 DMA
 *       通道(约 10 次寄存器写) + 2 次 FTF 中断 + 忙等完成标志，而 16bit
 *       @5MHz 帧本身只有 3.2μs——DMA 的准备与中断开销和传输时间同量级。
 *       轮询同样忙等帧时间，但砍掉了全部 DMA 开销与 40k 次/秒的中断。
 */
int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data,
							 uint16_t *rx_data) {
	u32 timeout = 1000;

	while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE) == RESET) {
		if (--timeout == 0U) {
			return -1;
		}
	}
	spi_i2s_data_transmit(spi_periph, tx_data);

	timeout = 1000;
	while (spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) == RESET) {
		if (--timeout == 0U) {
			return -1;
		}
	}
	*rx_data = spi_i2s_data_receive(spi_periph);

	// 等待移位完成再返回：调用方返回后立即拉高 CS，
	// 若最后一位仍在移位会截断帧
	while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS) != RESET) {
	}
	return 0;
}
/**
 * @brief 角度传感器初始化
 * @param 无
 */
void KTH7823_gpio_init(void) {
	rcu_periph_clock_enable(RCU_GPIOA);
	rcu_periph_clock_enable(RCU_GPIOB);
	rcu_periph_clock_enable(RCU_SPI0);
	rcu_periph_clock_enable(RCU_AF);
	/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
	gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);

	/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
	gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
	/* PA15 → NSS, 推挽输出 */
	gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
	/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
	gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ,
			  GPIO_PIN_3 | GPIO_PIN_5);
	/* PB4 → MISO, 浮空输入 */
	gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ,
			  GPIO_PIN_4 | GPIO_PIN_10 | GPIO_PIN_11);
}

/**
 * @brief SPI 初始化
 *
 * @details Configures SPI hardware and initializes necessary settings
 */
void KTH7823_spi_init(void) {
	spi_parameter_struct spi_init_str;
	spi_struct_para_init(&spi_init_str);

	/* SPI 参数：16bit, 主机，全双工，CPOL=1 CPHA=1, MSB first */
	spi_init_str.device_mode = SPI_MASTER;
	spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
	spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
	spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
	spi_init_str.nss = SPI_NSS_SOFT;
	spi_init_str.endian = SPI_ENDIAN_MSB;

	spi_init_str.prescale = 24; /* 5MHz @ 120MHz */
	spi_init(SPI0, &spi_init_str);
	spi_nss_output_enable(SPI0);
	spi_nssp_mode_enable(SPI0);
	spi_enable(SPI0);
}

/**
 * @ 角度传感器类 初始化
 * @return 0:初始化成功 1:初始化失败
 */
u8 angle_sensor::init(void) {
	KTH7823_gpio_init(); // GPIO 初始化
	KTH7823_spi_init();	 // SPI 初始化
	return 0;
}

/**
 * @ 角度传感器类 设置当前角度
 * @param angle 输出级角度值
 * @return 0 成功/1 失败
 */
u8 angle_sensor::angle_set(float angle) {
	m_cumulativeAngle = angle * REDUCTION_RATIO;
	m_firstUpdate = true;
	return 0;
}

/**
 * @brief 读取传感器数据
 * @return 传感器数据
 */
inline float angle_sensor::read_sensorAngle(void) {
	uint16_t raw_data;

	ANG_SPI_CS_ENABLE();
	u8 result = spi_rw_half_word_polling(SPI0, DEFAULT_COMMAND, &raw_data);
	ANG_SPI_CS_DISABLE();

	if (result == 0) {
		// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
		float angle_deg = -(float)raw_data * ANGLESENS_FACTOR + ANGLESENS_OFFSET;
		current_angle_deg = angle_deg;
		current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
		return angle_deg;
	}

	// 仅在调试模式下输出错误，避免发布版本中的 I/O 开销
	if (result != 0) {
		ANG_SENS_DEBUG_PRINT("Error reading angle from sensor.\n");
	}

	return (float)FAULT_RECEIVE;
}

/**
 * @brief 更新传感器角度（输出级）
 * @return 累积角度值
 */
float angle_sensor::update(void) {
	read_sensorAngle();

	if (m_firstUpdate) {
		m_lastAngle = current_angle_deg;
		m_firstUpdate = false;
		return m_cumulativeAngle;
	}

	// 计算角度变化
	float delta = current_angle_deg - m_lastAngle;

	// 处理角度绕回情况（0 度和 360 度之间的跳变）
	// 如果变化超过 300 度，说明发生了绕回
	if (delta > ANGLE_WRAP_THRESHOLD) {
		// 从接近 360 度绕回到接近 0 度
		delta -= ANGLE_WRAP_VALUE;
	} else if (delta < -ANGLE_WRAP_THRESHOLD) {
		// 从接近 0 度绕到接近 360 度
		delta += ANGLE_WRAP_VALUE;
	}

	m_cumulativeAngle += delta;
	m_lastAngle = current_angle_deg;
	return m_cumulativeAngle;
}

/**
 * @brief 获取累积角度值 (输出级)
 * @return 累积角度值
 */
float angle_sensor::getAngle(void) const {
	return m_cumulativeAngle / REDUCTION_RATIO;
}

/**
 * @brief 获取累积角度值 (原始级)
 * @return 累积角度值
 */
float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }

/**
 * @brief 获取角度值 (原始级)
 * @return 角度值 (原始级)
 */
float angle_sensor::getcurrent_sensorangle(void) const{
	return current_angle_deg;
}

/**
 * @brief 设置电角度零位校准角
 * @param[in] angle 校准角（度）
 * @return 0 成功
 */
u8 angle_sensor::set_calibration(float angle) {
	calibration_angle = normalizeAngle360(angle);
	return 0;
}