Motor_v2/Robot_V2/device/ADC.cpp

#include "adc.h"
#include "Filter.h"
#include "math.h"
#include <cstdio>
#include <cstring>
#include <stdint.h>

constexpr float ERROR_ADC_VAL = 255.0f;
constexpr float ADC_SCALE = 3.3f / 4096.0f;
constexpr float TEMP_FACTOR = 1000.0f / 4.3f;
constexpr float VCC_FACTOR = 11.0f;
// constexpr float CURRENT_OFFSET = 0 - 4096.0f / 2.0f;
constexpr float CURRENT_OFFSET = 0;
constexpr float CURRENT_FACTOR =
	1.0f / 50.0f * 100.0f; // 50倍放大，100倍电流采样电阻, 单位mA
constexpr float MAX_TEMP = 150.0f;
constexpr float MIN_TEMP = -40.0f;
constexpr float MAX_CURRENT = 65.0f;
constexpr float MIN_CURRENT = -0.1f;
constexpr float MAX_VCC = 65.0f;
constexpr float MIN_VCC = -0.1f;

u16 adc_value[6] = {0}; // 显式初始化数组元素为0

/**
 * @brief 采样类初始化
 */
void sampler::init() {
	for (int i = 0; i < FILTER_SIZE + 1; i++) {
		mcu_temp_samples[i] =
			(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;

		currU_samples[i] =
			(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
		currV_samples[i] =
			(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
		currW_samples[i] =
			(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;

		vbus_samples[i] = adc_value[5] * ADC_SCALE * VCC_FACTOR;

		float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
		drv_temp_samples[i] =
			1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
	}
}
/**
 * @brief 获取MCU温度
 * @return float
 * 温度值，单位为摄氏度，范围应在-40到145之间，如果读取失败则返回0.0f
 */
float sampler::get_mcu_temp() {
	tempora_sample = medium_filter(mcu_temp_samples, FILTER_SIZE + 1);
	// 检查温度读数是否在有效范围内
	if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
		return ERROR_ADC_VAL;
	}
	return tempora_sample;
}

/**
 * @brief 获取电机驱动温度
 * @return float 温度值，单位为摄氏度
 *
 * 通过热敏电阻分压电路计算温度值，使用Steinhart-Hart方程进行温度转换。
 * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
 */
float sampler::get_drv_temp() {
	tempora_sample = medium_filter(drv_temp_samples, FILTER_SIZE + 1);
	// 检查温度读数是否在有效范围内
	if (tempora_sample < MIN_TEMP || tempora_sample > MAX_TEMP) {
		return ERROR_ADC_VAL;
	}
	return tempora_sample;
}

/**
 * @brief 获取电流值
 * @param type 电流类型，'U'表示U相电流，'V'表示V相电流，'W'表示W相电流
 * @return float 电流值，单位为安培
 *
 * 通过ADC采样计算各相电流值
 */
float sampler::get_current(char type) {
	switch (type) {
	case 'U': {
		tempora_sample = medium_filter(currU_samples, FILTER_SIZE + 1);
		break;
	}
	case 'V': {
		tempora_sample = medium_filter(currV_samples, FILTER_SIZE + 1);
		break;
	}
	case 'W': {
		tempora_sample = medium_filter(currW_samples, FILTER_SIZE + 1);
		break;
	}
	default: {
		return ERROR_ADC_VAL;
	}
	}
	if (tempora_sample < MIN_CURRENT || tempora_sample > MAX_CURRENT) {
		return ERROR_ADC_VAL;
	}
	return tempora_sample;
}

/**
 * @brief 获取供电电压
 * @return float 电压值，单位为伏特
 *
 * 通过ADC采样计算供电电压值，使用迭代方式确保获取有效的电压读数。
 * 如果计算结果超出有效范围(-40到255伏特)，则重新采样计算。
 */
float sampler::get_vbus_voltage() {
	tempora_sample = medium_filter(vbus_samples, FILTER_SIZE + 1);
	// 检查电压读数是否在有效范围内
	if (vbus_samples[FILTER_SIZE / 2] > MAX_VCC ||
		vbus_samples[FILTER_SIZE / 2] < MIN_VCC) {
		return ERROR_ADC_VAL;
	}
	return tempora_sample;
}

/**
 * @brief 更新MCU温度
 *
 * 通过ADC采样计算MCU温度值，使用Steinhart-Hart方程进行温度转换。
 * 如果计算结果超出有效范围(-40到255摄氏度)，则重新采样计算。
 */
void sampler::update_mcu_temp() {
	memmove(mcu_temp_samples, mcu_temp_samples + 1,
			FILTER_SIZE * sizeof(float));
	mcu_temp_samples[FILTER_SIZE] =
		(1.43f - adc_value[0] * ADC_SCALE) * TEMP_FACTOR + 35.0f;
}

/**
 * @brief 更新MCU温度
 */
void sampler::update_drv_temp() {
	memmove(drv_temp_samples, drv_temp_samples + 1,
			FILTER_SIZE * sizeof(float));
	float R_now = (float)adc_value[1] / (4096 - (float)adc_value[1]) * 4700;
	drv_temp_samples[FILTER_SIZE] =
		1.0f / (0.00335f + 0.00025f * log(R_now / 10000.0f)) - 273.15f;
}

/**
 * @brief 更新相电流
 */
void sampler::update_current() {
	memmove(currU_samples, currU_samples + 1, FILTER_SIZE * sizeof(float));
	memmove(currV_samples, currV_samples + 1, FILTER_SIZE * sizeof(float));
	memmove(currW_samples, currW_samples + 1, FILTER_SIZE * sizeof(float));

	currU_samples[FILTER_SIZE] =
		(adc_value[2] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
	currV_samples[FILTER_SIZE] =
		(adc_value[3] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
	currW_samples[FILTER_SIZE] =
		(adc_value[4] * ADC_SCALE + CURRENT_OFFSET) * CURRENT_FACTOR;
}

/**
 * @brief 更新供电电压
 */
void sampler::update_vbus_voltage() {
	memmove(vbus_samples, vbus_samples + 1, FILTER_SIZE * sizeof(float));
	vbus_samples[FILTER_SIZE] = adc_value[5] * ADC_SCALE * VCC_FACTOR;
}