2025-09-28 09:17:22 +00:00
|
|
|
|
#include "spi.h"
|
|
|
|
|
|
#include "APPDEF.H"
|
|
|
|
|
|
#include "math.h"
|
2026-03-17 02:33:04 +00:00
|
|
|
|
#include "string.h"
|
|
|
|
|
|
#include "stdio.h"
|
2025-09-28 09:17:22 +00:00
|
|
|
|
// AD7190<39>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>
|
|
|
|
|
|
#define AD7190_REG_COMM 0x00 << 3
|
|
|
|
|
|
#define AD7190_REG_STAT 0x00 << 3
|
|
|
|
|
|
#define AD7190_REG_MODE 0x01 << 3
|
|
|
|
|
|
#define AD7190_REG_CONF 0x02 << 3
|
|
|
|
|
|
#define AD7190_REG_DATA 0x03 << 3
|
|
|
|
|
|
#define AD7190_REG_GPOC 0x05 << 3
|
|
|
|
|
|
|
|
|
|
|
|
// AD7190<39><30><EFBFBD><EFBFBD><EEB6A8>
|
|
|
|
|
|
#define AD7190_CMD_NOP 0xFF
|
|
|
|
|
|
#define AD7190_CMD_WR 0x00
|
|
|
|
|
|
#define AD7190_CMD_RD 0x40
|
|
|
|
|
|
|
|
|
|
|
|
// AD7190<39><30><EFBFBD>üĴ<C3BC><C4B4><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
|
2026-03-17 02:33:04 +00:00
|
|
|
|
#define AD7190_CONF_CHOP 0x800000
|
|
|
|
|
|
#define AD7190_CONF_REFSEL2 0x100000
|
|
|
|
|
|
// AD7190<39><30><EFBFBD>üĴ<C3BC><C4B4><EFBFBD>ͨ<EFBFBD><CDA8>ѡ<EFBFBD><D1A1>
|
|
|
|
|
|
#define AD7190_CONF_CH7 0x008000
|
|
|
|
|
|
#define AD7190_CONF_CH6 0x004000
|
|
|
|
|
|
#define AD7190_CONF_CH5 0x002000
|
|
|
|
|
|
#define AD7190_CONF_CH4 0x001000
|
|
|
|
|
|
#define AD7190_CONF_CH3 0x000800
|
|
|
|
|
|
#define AD7190_CONF_CH2 0x000400
|
|
|
|
|
|
#define AD7190_CONF_CH1 0x000200
|
|
|
|
|
|
#define AD7190_CONF_CH0 0x000100
|
|
|
|
|
|
// AD7190<39><30><EFBFBD>üĴ<C3BC><C4B4><EFBFBD>
|
|
|
|
|
|
#define AD7190_CONF_BURNOUT 0x000080
|
|
|
|
|
|
#define AD7190_CONF_REFDET 0x000040
|
|
|
|
|
|
#define AD7190_CONF_BUF 0x000010
|
|
|
|
|
|
#define AD7190_CONF_UB 0x000008
|
2025-09-28 09:17:22 +00:00
|
|
|
|
// AD7190<39><30><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>涨<EFBFBD><E6B6A8>
|
2026-03-17 02:33:04 +00:00
|
|
|
|
#define AD7190_CONF_GAIN_1 0x000000
|
|
|
|
|
|
#define AD7190_CONF_GAIN_8 0x000003
|
|
|
|
|
|
#define AD7190_CONF_GAIN_16 0x000004
|
|
|
|
|
|
#define AD7190_CONF_GAIN_32 0x000005
|
|
|
|
|
|
#define AD7190_CONF_GAIN_64 0x000006
|
|
|
|
|
|
#define AD7190_CONF_GAIN_128 0x000007
|
2025-09-28 09:17:22 +00:00
|
|
|
|
|
|
|
|
|
|
// AD7190<39><30><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD>
|
|
|
|
|
|
#define AD7190_CONF_MODE_CONT 0x000000
|
|
|
|
|
|
#define AD7190_CONF_MODE_ZEROSEL 0x800000
|
|
|
|
|
|
#define AD7190_CONF_MODE_FullSEL 0xA00000
|
2026-03-17 02:33:04 +00:00
|
|
|
|
#define AD7190_CONF_MODE_FS9_0 0x0003FF // 4.7Hz
|
|
|
|
|
|
#define AD7190_CONF_MODE_FS9_02 0x0001E0//10Hz
|
|
|
|
|
|
#define AD7190_CONF_MODE_FS9_03 0x000096//50Hz
|
|
|
|
|
|
|
|
|
|
|
|
#define AD7190_CONF_MODE_InCLK 0x080000// 4.92 MHz<48>ڲ<EFBFBD>ʱ<EFBFBD><CAB1> MCLK2<4B><32><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>̬
|
2025-09-28 09:17:22 +00:00
|
|
|
|
// AD7190 <20><><EFBFBD>ſ<EFBFBD><C5BF><EFBFBD>
|
|
|
|
|
|
#define AD7190_GPOCON_BDPSW 0x40
|
|
|
|
|
|
|
|
|
|
|
|
void AD7190_WriteRegister(uint8_t regAddr, uint32_t regValue)
|
|
|
|
|
|
{
|
|
|
|
|
|
SPI1_ReadWrite(AD7190_CMD_WR | regAddr);
|
|
|
|
|
|
SPI1_ReadWrite((regValue >> 16) & 0xFF);
|
|
|
|
|
|
SPI1_ReadWrite((regValue >> 8) & 0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(regValue & 0xFF);
|
|
|
|
|
|
}
|
|
|
|
|
|
uint32_t AD7190_ReadRegister(uint8_t regAddr)
|
|
|
|
|
|
{
|
|
|
|
|
|
uint32_t regValue = 0;
|
|
|
|
|
|
|
|
|
|
|
|
SPI1_ReadWrite(AD7190_CMD_RD | regAddr);
|
|
|
|
|
|
regValue = SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
regValue = (regValue << 8) + SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
regValue = (regValue << 8) + SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
|
|
|
|
|
|
return regValue;
|
|
|
|
|
|
}
|
|
|
|
|
|
void AD7190_WriteRegisterOnce(uint8_t regAddr, uint8_t regValue)
|
|
|
|
|
|
{
|
|
|
|
|
|
SPI1_ReadWrite(AD7190_CMD_WR | regAddr);
|
|
|
|
|
|
SPI1_ReadWrite(regValue);
|
|
|
|
|
|
}
|
|
|
|
|
|
uint8_t AD7190_ReadRegisterOnce(uint8_t regAddr)
|
|
|
|
|
|
{
|
|
|
|
|
|
uint32_t regValue = 0;
|
|
|
|
|
|
SPI1_ReadWrite(AD7190_CMD_RD | regAddr);
|
|
|
|
|
|
regValue = SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
return regValue;
|
|
|
|
|
|
}
|
|
|
|
|
|
bool AD7190_WaitForReady(uint32_t timeout)
|
|
|
|
|
|
{
|
|
|
|
|
|
uint32_t count = 0;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
while (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6) && (count < timeout))
|
|
|
|
|
|
{
|
|
|
|
|
|
delay_us(1);
|
|
|
|
|
|
count++;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (count >= timeout)
|
|
|
|
|
|
{
|
|
|
|
|
|
return false;
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
return true;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
//extern uint16_t Read_Internal_Temperature();
|
|
|
|
|
|
//// AD7190<39><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡADת<44><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
//float V25 = 1.43; // 25 <20><>ʱ<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ(mV)
|
|
|
|
|
|
// float Avg_Slope = 4.3; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD>¶<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>б<EFBFBD><D0B1>(mV/<2F><>C)
|
|
|
|
|
|
// // <20><>ȡ ADC <20>ο<EFBFBD><CEBF><EFBFBD>ѹֵ<D1B9><D6B5>һ<EFBFBD><D2BB>Ϊ 3.3V<EFBFBD><EFBFBD>
|
|
|
|
|
|
// float Vref = 3.3; // <20>ο<EFBFBD><CEBF><EFBFBD>ѹֵ(V)
|
|
|
|
|
|
//float TemperatureIn;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
uint8_t ErrCount;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint16_t DifErrCount;
|
|
|
|
|
|
uint32_t lastdata = 0;
|
|
|
|
|
|
uint32_t UseuLBuf[20];
|
|
|
|
|
|
uint32_t UseuLBuf2[20];
|
|
|
|
|
|
uint16_t BufIndex = 0;
|
|
|
|
|
|
// <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>ƽ<EFBFBD><C6BD>ֵ
|
|
|
|
|
|
float calculateAverage(uint32_t arr[], int size)
|
|
|
|
|
|
{
|
|
|
|
|
|
double sum = 0.0;
|
|
|
|
|
|
|
|
|
|
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD>
|
|
|
|
|
|
int i;
|
|
|
|
|
|
for (i = 0; i < size; i++)
|
|
|
|
|
|
{
|
|
|
|
|
|
sum += arr[i];
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ֵ
|
|
|
|
|
|
double average = sum / size;
|
|
|
|
|
|
|
|
|
|
|
|
return average;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void bubbleSort(uint32_t *arr, int n) {
|
|
|
|
|
|
int i, j, temp;
|
|
|
|
|
|
for (i = 0; i < n-1; i++) {
|
|
|
|
|
|
for (j = 0; j < n-i-1; j++) {
|
|
|
|
|
|
if (arr[j] > arr[j+1]) {
|
|
|
|
|
|
// <20><><EFBFBD><EFBFBD> arr[j] <20><> arr[j+1]
|
|
|
|
|
|
temp = arr[j];
|
|
|
|
|
|
arr[j] = arr[j+1];
|
|
|
|
|
|
arr[j+1] = temp;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
|
***********************************************************
|
|
|
|
|
|
* @brief printf<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϴ<EFBFBD>ӡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD>
|
|
|
|
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><EFBFBD>fputc<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD>ڣ<EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ض<EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
* @param
|
|
|
|
|
|
* @return
|
|
|
|
|
|
***********************************************************
|
|
|
|
|
|
*/
|
|
|
|
|
|
int fputc(int ch, FILE *f)
|
|
|
|
|
|
{
|
|
|
|
|
|
while (!(READ_BIT(USART1->SR, USART_SR_TXE))) {
|
|
|
|
|
|
delay_us(100);
|
|
|
|
|
|
}
|
|
|
|
|
|
USART1->DR = ch;
|
|
|
|
|
|
return ch;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float average;
|
|
|
|
|
|
float center;
|
|
|
|
|
|
int ErrcountAdd = 0;
|
|
|
|
|
|
int DifErrCountAdd = 0;
|
|
|
|
|
|
uint32_t AD7190_ReadData( void )
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
uint32_t data = 0;
|
|
|
|
|
|
|
|
|
|
|
|
if(AD7190_WaitForReady(50000))
|
|
|
|
|
|
{
|
|
|
|
|
|
data = AD7190_ReadRegister(AD7190_REG_DATA);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if(fabs((int)data - (int)lastdata) > 100000 )
|
|
|
|
|
|
{
|
|
|
|
|
|
DifErrCount ++;
|
|
|
|
|
|
DifErrCountAdd++;
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
DifErrCount = 0;
|
|
|
|
|
|
}
|
|
|
|
|
|
lastdata = data;
|
|
|
|
|
|
if((data > 8388608) || (data < 100 ))
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
ErrCount++;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
ErrcountAdd++;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
ErrCount = 0;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
ErrCount++;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
ErrcountAdd++;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if(ErrCount > 3 || DifErrCount > 20)
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint32_t modeReg;
|
|
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
SPI1_Configuration();
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
SPI1_ReadWrite(0xFF); // 40<34><30><EFBFBD><EFBFBD><EFBFBD>帴λADC
|
2025-09-28 09:17:22 +00:00
|
|
|
|
osDelay(50);
|
|
|
|
|
|
// <20><><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>ѡ<EFBFBD>ο<F1A1A2B2><CEBF><EFBFBD>ѹԴ<D1B9><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><CDB9><EFBFBD>ģʽ
|
|
|
|
|
|
// <09><><EFBFBD><EFBFBD>PSW
|
|
|
|
|
|
uint8_t GPOC = AD7190_ReadRegisterOnce(AD7190_REG_GPOC);
|
|
|
|
|
|
GPOC |= AD7190_GPOCON_BDPSW;
|
|
|
|
|
|
AD7190_WriteRegisterOnce(AD7190_REG_GPOC, GPOC);
|
|
|
|
|
|
osDelay(5);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint32_t config = AD7190_CONF_GAIN_128 | AD7190_CONF_UB | AD7190_CONF_BUF | AD7190_CONF_CH0;// 0x0000011F;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
AD7190_WriteRegister(AD7190_REG_CONF, config);
|
|
|
|
|
|
osDelay(5);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
// <09><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ģʽ
|
|
|
|
|
|
modeReg = AD7190_CONF_MODE_FS9_0 | AD7190_CONF_MODE_InCLK;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
AD7190_WriteRegister(AD7190_REG_MODE, modeReg);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
ErrCount = 0;
|
|
|
|
|
|
osDelay(500);
|
|
|
|
|
|
if(AD7190_WaitForReady(50000))
|
|
|
|
|
|
{
|
|
|
|
|
|
data = AD7190_ReadRegister(AD7190_REG_DATA);
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
}
|
|
|
|
|
|
return data;
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
// <20><><EFBFBD>忨<EFBFBD><E5BFA8><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
typedef struct
|
|
|
|
|
|
{
|
2026-03-17 02:33:04 +00:00
|
|
|
|
double x; // ״̬<D7B4><CCAC><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
double P; // ״̬Э<CCAC><D0AD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double Q; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double R; // <20>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
|
|
|
|
|
|
double K; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
} KalmanFilter;
|
|
|
|
|
|
KalmanFilter kf2;
|
|
|
|
|
|
|
|
|
|
|
|
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
|
|
|
|
|
|
void kalman_filter_init2(KalmanFilter *kf, float initial_x, float initial_P, float process_noise, float measurement_noise)
|
|
|
|
|
|
{
|
|
|
|
|
|
kf->x = initial_x;
|
|
|
|
|
|
kf->P = initial_P;
|
|
|
|
|
|
kf->Q = process_noise;
|
|
|
|
|
|
kf->R = measurement_noise;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// <20><><EFBFBD>¿<EFBFBD><C2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>״̬
|
|
|
|
|
|
void kalman_filter_update2(KalmanFilter *kf, float measurement)
|
|
|
|
|
|
{
|
|
|
|
|
|
// Ԥ<>ⲽ<EFBFBD><E2B2BD>
|
|
|
|
|
|
float x_pred = kf->x; // Ԥ<><D4A4><EFBFBD><EFBFBD>״ֵ̬
|
|
|
|
|
|
float P_pred = kf->P + kf->Q; // Ԥ<><D4A4><EFBFBD><EFBFBD>״̬Э<CCAC><D0AD><EFBFBD><EFBFBD>
|
|
|
|
|
|
|
|
|
|
|
|
// <20><><EFBFBD>²<EFBFBD><C2B2><EFBFBD>
|
|
|
|
|
|
kf->K = P_pred / (P_pred + kf->R); // <20><><EFBFBD>㿨<EFBFBD><E3BFA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
kf->x = x_pred + kf->K * (measurement - x_pred); // <20><><EFBFBD><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
kf->P = (1 - kf->K) * P_pred; // <20><><EFBFBD><EFBFBD>״̬Э<CCAC><D0AD><EFBFBD><EFBFBD>
|
|
|
|
|
|
}
|
|
|
|
|
|
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
uint8_t FollowCount = 0; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>
|
|
|
|
|
|
uint8_t TimeCount = 0; // <09><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
uint32_t FliterCount = 0; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD>
|
|
|
|
|
|
uint16_t SetZeroCount = 0; // ж<>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
|
|
|
|
|
|
int32_t ADvalue2 = 0; // ADC<44><43><EFBFBD><EFBFBD>ԭʼֵ
|
|
|
|
|
|
|
|
|
|
|
|
double ADvalue2filter = 0.0f; // ADC<44><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ֵ
|
|
|
|
|
|
double ADvalue2filterOld = 0; // <09>ϴ<EFBFBD>ADC<44><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ֵ
|
|
|
|
|
|
|
|
|
|
|
|
double WeightData2Temp = 0.0f; // <09><><EFBFBD><EFBFBD>ADvalue2filter<65><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
double WeightData2TempOld; // <09>ϴ<EFBFBD>ADvalue2filter<65><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
|
|
|
|
|
|
double WeightData2Finally = 0.0f; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
|
|
|
|
|
|
double UninstiallRefWeight= 0.0f; // ж<><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
|
|
|
|
|
|
double WeightZeroOld = 0; // <09>ϴ<EFBFBD><CFB4>ȶ<EFBFBD>״̬ʱ<CCAC><CAB1><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
struct uCalibrateWeight CalibrateWeight2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> б<><D0B1> <20><><EFBFBD><EFBFBD>
|
|
|
|
|
|
|
|
|
|
|
|
double WeightData2 = 0.0f; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|
|
|
|
|
bool weightChanging = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>־
|
|
|
|
|
|
bool weight11g = false; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>־
|
|
|
|
|
|
float ZeroWeightOld;
|
|
|
|
|
|
float ZeroWeightChange;
|
|
|
|
|
|
|
|
|
|
|
|
float LowFecAlpha = 0.95f; // <09><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB>ʱƫ<CAB1><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>پ<EFBFBD>ֵ
|
|
|
|
|
|
float WeightRefStart = 0;
|
|
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
uint8_t i;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
#define ChangeMax 10
|
|
|
|
|
|
#define ZeroTrackMax 0.050f
|
|
|
|
|
|
#define FollowMax 0.030f
|
|
|
|
|
|
#define FliterCountMax 200
|
|
|
|
|
|
#define FollowDlteaMax 0.001f
|
|
|
|
|
|
#define FollowAlpha 0.9999f // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>ϵ<EFBFBD><CFB5>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
void AD7190_Run(const void *p_arg)
|
|
|
|
|
|
{
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint8_t FilterQOld, SensorQOld, ZeroTrackOld, RuBianLiangOld, ruBianTimeOld;
|
|
|
|
|
|
int8_t RubianLiang, RubianTime;
|
|
|
|
|
|
float ZeroTrackTime, ZeroTrackRange;
|
|
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
osDelay(600);
|
|
|
|
|
|
ADvalue2 = AD7190_ReadData();
|
|
|
|
|
|
osDelay(200);
|
|
|
|
|
|
ADvalue2 = AD7190_ReadData();
|
|
|
|
|
|
ADvalue2filterOld = ADvalue2filter = ADvalue2;
|
|
|
|
|
|
kalman_filter_init2(&kf2, ADvalue2, 1.0, 0.001, 0.1);
|
|
|
|
|
|
WeightZeroOld = CalibrateWeight2.WeightZero;
|
|
|
|
|
|
for (;;)
|
|
|
|
|
|
{
|
|
|
|
|
|
if (FilterQOld != Set.FilterQ)
|
|
|
|
|
|
{
|
|
|
|
|
|
FilterQOld = Set.FilterQ;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
switch (Set.FilterQ) // <20>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ζ<EFBFBD> <20><><EFBFBD>ζ<EFBFBD>Խ<EFBFBD><D4BD> <20><>ӦԽ<D3A6><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾԽ<CABE>ȶ<EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
case 0:
|
|
|
|
|
|
kf2.Q = 0.1;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 1:
|
|
|
|
|
|
kf2.Q = 0.08;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 2:
|
|
|
|
|
|
kf2.Q = 0.06;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 3:
|
|
|
|
|
|
kf2.Q = 0.04;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 4:
|
|
|
|
|
|
kf2.Q = 0.02;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 5:
|
|
|
|
|
|
kf2.Q = 0.009;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 6:
|
|
|
|
|
|
kf2.Q = 0.007;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 7:
|
|
|
|
|
|
kf2.Q = 0.005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 8:
|
|
|
|
|
|
kf2.Q = 0.003;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 9:
|
|
|
|
|
|
kf2.Q = 0.001;
|
|
|
|
|
|
break;
|
|
|
|
|
|
default:
|
|
|
|
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
if (SensorQOld != Set.SensorQ)
|
|
|
|
|
|
{
|
|
|
|
|
|
SensorQOld = Set.SensorQ;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
switch (Set.SensorQ) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ζ<EFBFBD> <20><><EFBFBD>ζ<EFBFBD>Խ<EFBFBD><D4BD> <20><>ӦԽѸ<D4BD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>Խ<EFBFBD><D4BD> <20><>ʾԽ<CABE><D4BD><EFBFBD>ȶ<EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
case 0:
|
|
|
|
|
|
kf2.R = 0.5;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 1:
|
|
|
|
|
|
kf2.R = 0.3;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 2:
|
|
|
|
|
|
kf2.R = 0.1;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 3:
|
|
|
|
|
|
kf2.R = 0.08;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 4:
|
|
|
|
|
|
kf2.R = 0.06;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 5:
|
|
|
|
|
|
kf2.R = 0.04;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 6:
|
|
|
|
|
|
kf2.R = 0.02;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 7:
|
|
|
|
|
|
kf2.R = 0.009;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 8:
|
|
|
|
|
|
kf2.R = 0.005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 9:
|
|
|
|
|
|
kf2.R = 0.001;
|
|
|
|
|
|
break;
|
|
|
|
|
|
default:
|
|
|
|
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if (ZeroTrackOld != Set.ZeroTrack) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>Խ<EFBFBD><D4BD> ԭʼ<D4AD><CABC><EFBFBD>ݷ<EFBFBD><DDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>仯ʱ<E4BBAF><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2>ᷢ<EFBFBD><E1B7A2><EFBFBD>仯<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ9ʱ<39><CAB1><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>4<EFBFBD><34><EFBFBD>ڱ仯С<E4BBAF><D0A1>2.5mg<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><EFBFBD>ᷢ<EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>cpu<EFBFBD>Ὣ<EFBFBD><EFBFBD>4<EFBFBD><EFBFBD><EFBFBD>ڱ仯<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD><EFBFBD>ӵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><EFBFBD>൱<EFBFBD>ڶ<EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˡ<EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
ZeroTrackOld = Set.ZeroTrack;
|
|
|
|
|
|
switch (ZeroTrackOld)
|
|
|
|
|
|
{
|
|
|
|
|
|
case 0:
|
2026-03-17 02:33:04 +00:00
|
|
|
|
ZeroTrackTime = 20; // <09><><EFBFBD><EFBFBD>20<32><30>
|
|
|
|
|
|
ZeroTrackRange = 0; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>0.000g
|
2025-09-28 09:17:22 +00:00
|
|
|
|
break;
|
|
|
|
|
|
case 1:
|
|
|
|
|
|
ZeroTrackTime = 15;
|
|
|
|
|
|
ZeroTrackRange = 0.0005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 2:
|
|
|
|
|
|
ZeroTrackTime = 10;
|
|
|
|
|
|
ZeroTrackRange = 0.0005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 3:
|
|
|
|
|
|
ZeroTrackTime = 7;
|
|
|
|
|
|
ZeroTrackRange = 0.0005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 4:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.0005;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 5:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.0008;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 6:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.001;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 7:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.0015;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 8:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.002;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 9:
|
|
|
|
|
|
ZeroTrackTime = 4;
|
|
|
|
|
|
ZeroTrackRange = 0.0025;
|
|
|
|
|
|
break;
|
|
|
|
|
|
default:
|
|
|
|
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if (ruBianTimeOld != Set.RuBianTime) // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> <20><><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD> <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Խ<EFBFBD>̣<EFBFBD><CCA3><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD> <20><>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
ruBianTimeOld = Set.RuBianTime;
|
|
|
|
|
|
switch (Set.RuBianTime)
|
|
|
|
|
|
{
|
|
|
|
|
|
case 0:
|
|
|
|
|
|
RubianTime = 100;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 1:
|
|
|
|
|
|
RubianTime = 60;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 2:
|
|
|
|
|
|
RubianTime = 30;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 3:
|
|
|
|
|
|
RubianTime = 20;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 4:
|
|
|
|
|
|
RubianTime = 15;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 5:
|
|
|
|
|
|
RubianTime = 10;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 6:
|
|
|
|
|
|
RubianTime = 8;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 7:
|
|
|
|
|
|
RubianTime = 6;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 8:
|
|
|
|
|
|
RubianTime = 4;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 9:
|
|
|
|
|
|
RubianTime = 2;
|
|
|
|
|
|
break;
|
|
|
|
|
|
default:
|
|
|
|
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if (RuBianLiangOld != Set.RuBianLiang) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>ɷ<EFBFBD><C9B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0ʱ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۲<EFBFBD>һ<EFBFBD><D2BB>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݱ仯 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD><EFBFBD>仯Ϊ<E4BBAF><CEAA><EFBFBD><EFBFBD> <20><>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>䡣<EFBFBD><E4A1A3>֮Ϊ<D6AE><CEAA><EFBFBD><EFBFBD><EFBFBD>䡣 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>𣨽<EFBFBD><F0A3A8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>鲻<EFBFBD><E9B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
RuBianLiangOld = Set.RuBianLiang;
|
|
|
|
|
|
switch (Set.RuBianLiang)
|
|
|
|
|
|
{
|
|
|
|
|
|
case 0:
|
|
|
|
|
|
RubianLiang = -5;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 1:
|
|
|
|
|
|
RubianLiang = -4;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 2:
|
|
|
|
|
|
RubianLiang = -3;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 3:
|
|
|
|
|
|
RubianLiang = -2;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 4:
|
|
|
|
|
|
RubianLiang = -1;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 5:
|
|
|
|
|
|
RubianLiang = 0;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 6:
|
|
|
|
|
|
RubianLiang = 1;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 7:
|
|
|
|
|
|
RubianLiang = 2;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 8:
|
|
|
|
|
|
RubianLiang = 3;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 9:
|
|
|
|
|
|
RubianLiang = 4;
|
|
|
|
|
|
break;
|
|
|
|
|
|
case 10:
|
|
|
|
|
|
RubianLiang = 5;
|
|
|
|
|
|
break;
|
|
|
|
|
|
default:
|
|
|
|
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2026-03-17 02:33:04 +00:00
|
|
|
|
osDelay(200);
|
|
|
|
|
|
ADvalue2 = AD7190_ReadData(); //4.7Hz 213ms
|
|
|
|
|
|
kalman_filter_update2(&kf2, ADvalue2); // <20><><EFBFBD>²<EFBFBD><C2B2><EFBFBD>
|
|
|
|
|
|
if(fabs(ADvalue2filter - kf2.x) < ChangeMax )
|
|
|
|
|
|
{
|
|
|
|
|
|
LowFecAlpha = 0.95; // <09><><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD><EFBFBD>ȶ<EFBFBD>״̬ δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>ֵ
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
LowFecAlpha = 0.5; // <09><><EFBFBD>ݴ<EFBFBD><DDB4>ڽϴ<CFB4>״̬ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>α<EFBFBD><CEB1><EFBFBD>ֵ
|
|
|
|
|
|
}
|
|
|
|
|
|
ADvalue2filter = kf2.x *(1-LowFecAlpha)+ADvalue2filter*LowFecAlpha;
|
|
|
|
|
|
// if(pageNum != 1)
|
|
|
|
|
|
// {
|
|
|
|
|
|
|
|
|
|
|
|
// if(WeightData2TempOld - WeightData2Temp > 0.500f) // ж<><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
// if(UninstiallRefWeight < 0.005f) // <09><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
// UninstiallRefWeight = WeightData2TempOld;
|
|
|
|
|
|
// WeightZeroOld = CalibrateWeight2.WeightZero; // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼֵ<CABC><D6B5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD>ȶ<EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ﵼ<EFBFBD><EFB5BC><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD>ȷ
|
|
|
|
|
|
// }
|
|
|
|
|
|
// }
|
|
|
|
|
|
// else if(WeightData2TempOld - WeightData2Temp < -0.500f)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
// UninstiallRefWeight = 0.0f;
|
|
|
|
|
|
// if( WeightZeroOld != CalibrateWeight2.WeightZero)
|
|
|
|
|
|
// {
|
|
|
|
|
|
// CalibrateWeight2.WeightZero = WeightZeroOld; // <20><><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD>ɣ<EFBFBD><C9A3>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD>ı<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD>ݲ<EFBFBD>ȷ
|
|
|
|
|
|
// }
|
|
|
|
|
|
// SetZeroCount = 0;
|
|
|
|
|
|
// }
|
|
|
|
|
|
// if(UninstiallRefWeight > 0.005f) // ж<><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
// if(SetZeroCount++ > 50 ) // <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
//
|
|
|
|
|
|
// UninstiallRefWeight = 0.0f;
|
|
|
|
|
|
// WeightZeroOld = CalibrateWeight2.WeightZero;
|
|
|
|
|
|
// SetZeroCount = 0;
|
|
|
|
|
|
// }
|
|
|
|
|
|
// if(fabs(WeightData2Temp) < 0.100f && fabs(WeightData2Temp) > 0.004f) // <09><><EFBFBD>Ϲ<EFBFBD><CFB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䷶Χ<E4B7B6><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
// {
|
|
|
|
|
|
// SetZeroCount = 0;
|
|
|
|
|
|
// CalibrateWeight2.WeightZero += WeightData2Temp / CalibrateWeight2.WeightSlope / 10;
|
|
|
|
|
|
// ZeroWeightChange = CalibrateWeight2.WeightSlope * (ADvalue2filter - CalibrateWeight2.WeightZero);
|
|
|
|
|
|
// }
|
|
|
|
|
|
// ZeroWeightOld = CalibrateWeight2.WeightSlope * (ADvalue2filter - WeightZeroOld);
|
|
|
|
|
|
// if( fabs(ZeroWeightOld) <= fabs(ZeroWeightChange) )
|
|
|
|
|
|
// {
|
|
|
|
|
|
// weight11g=0;
|
|
|
|
|
|
// CalibrateWeight2.WeightZero = WeightZeroOld;
|
|
|
|
|
|
// }
|
|
|
|
|
|
// else
|
|
|
|
|
|
// {
|
|
|
|
|
|
// weight11g=1;
|
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
|
|
// }
|
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
|
|
WeightData2TempOld = WeightData2Temp;
|
|
|
|
|
|
WeightData2Temp = CalibrateWeight2.WeightSlope * (ADvalue2filter - CalibrateWeight2.WeightZero);
|
|
|
|
|
|
|
|
|
|
|
|
// printf("%f,%f,%f,%.4f\n",Temperature,ADvalue2filter,WeightData2Temp,WeightData2Finally);
|
|
|
|
|
|
|
|
|
|
|
|
if( RubianLiang != 0 ) // <09><><EFBFBD><EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
if (TimeCount++ > 4 * RubianTime) // <20><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
TimeCount = 0;
|
|
|
|
|
|
if (CalibrateWeight2.WeightZero == WeightZeroOld)
|
|
|
|
|
|
{
|
|
|
|
|
|
CalibrateWeight2.WeightZero += RubianLiang;
|
|
|
|
|
|
}
|
|
|
|
|
|
WeightZeroOld = CalibrateWeight2.WeightZero;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
2026-03-17 02:33:04 +00:00
|
|
|
|
if (fabs(WeightData2TempOld - WeightData2Temp) < ZeroTrackRange && fabs(WeightData2Temp) < ZeroTrackMax && ( pageNum != 1)) // <09><><EFBFBD>ϸ<EFBFBD><CFB8>ٷ<EFBFBD>Χ <20><><EFBFBD><EFBFBD> <20><>ʾֵΪ<D6B5><CEAA><EFBFBD><EFBFBD><EFBFBD>൱<EFBFBD><E0B5B1>ë<EFBFBD><C3AB>Ϊ0ʱ <20><><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD>У<EFBFBD><D0A3><EFBFBD>ˮ<EFBFBD>ֲⶨ<D6B2>Ǵ<EFBFBD><C7B4><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>ȶ<EFBFBD>״̬ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
if (FollowCount++ > 4 * ZeroTrackTime) // <20><><EFBFBD>㶯̬<E3B6AF><CCAC><EFBFBD><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
FollowCount = 0;
|
|
|
|
|
|
TimeCount = 0;
|
|
|
|
|
|
CalibrateWeight2.WeightZero += ADvalue2filter - ADvalue2filterOld;
|
2026-03-17 02:33:04 +00:00
|
|
|
|
WeightZeroOld = CalibrateWeight2.WeightZero;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
ADvalue2filterOld = ADvalue2filter;
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
FollowCount = 0;
|
|
|
|
|
|
ADvalue2filterOld = ADvalue2filter;
|
|
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
if ( fabs(WeightData2Temp) > ZeroTrackMax && fabs(WeightData2TempOld - WeightData2Temp) < FollowDlteaMax && fabs(WeightData2Temp - WeightRefStart) < FollowMax && ( pageNum != 1)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҵ<EFBFBD><D2B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٷ<EFBFBD>Χδ<CEA7><CEB4><EFBFBD>Ƚ<EFBFBD><C8BD>г<EFBFBD><D0B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
if(FliterCount < FliterCountMax)
|
|
|
|
|
|
FliterCount++;
|
|
|
|
|
|
else
|
|
|
|
|
|
FliterCount = FliterCountMax;
|
|
|
|
|
|
WeightData2Finally = WeightData2Finally + (WeightData2Temp - WeightData2Finally) * ( 1.00000f - FollowAlpha * FliterCount / FliterCountMax);
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
WeightData2Finally = WeightRefStart = WeightData2Temp;
|
|
|
|
|
|
FliterCount = 0;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (fabs(WeightData2 - WeightData2Finally) > 0.006f) // <20>仯<EFBFBD><E4BBAF><EFBFBD><EFBFBD>50mg/s <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD>־
|
2025-09-28 09:17:22 +00:00
|
|
|
|
{
|
|
|
|
|
|
weightChanging = 1;
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
weightChanging = 0;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
|
|
|
|
|
if (fabs(WeightData2 - WeightData2Finally) > 0.0002f) // <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|
|
|
|
|
{
|
|
|
|
|
|
WeightData2 = (int32_t)(WeightData2Finally * 10000) / 10000.0f;
|
|
|
|
|
|
}
|
|
|
|
|
|
else if(fabs(WeightData2Finally) < 0.001f)
|
|
|
|
|
|
{
|
|
|
|
|
|
WeightData2 = (int32_t)(WeightData2Finally * 10000) / 10000.0f;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
}
|
2026-03-17 02:33:04 +00:00
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
osThreadDef(AD7190_Run, osPriorityNormal, 1, 0);
|
|
|
|
|
|
// AD7190<39><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>AD7190
|
|
|
|
|
|
void AD7190_Init()
|
|
|
|
|
|
{
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint32_t modeReg;
|
|
|
|
|
|
|
2025-09-28 09:17:22 +00:00
|
|
|
|
SPI1_Configuration();
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
|
|
|
|
|
SPI1_ReadWrite(0xFF);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
SPI1_ReadWrite(0xFF); // 40<34><30><EFBFBD><EFBFBD><EFBFBD>帴λADC
|
2025-09-28 09:17:22 +00:00
|
|
|
|
osDelay(50);
|
|
|
|
|
|
// <20><><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>ѡ<EFBFBD>ο<F1A1A2B2><CEBF><EFBFBD>ѹԴ<D1B9><D4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><CDB9><EFBFBD>ģʽ
|
|
|
|
|
|
// <09><><EFBFBD><EFBFBD>PSW
|
|
|
|
|
|
uint8_t GPOC = AD7190_ReadRegisterOnce(AD7190_REG_GPOC);
|
|
|
|
|
|
GPOC |= AD7190_GPOCON_BDPSW;
|
|
|
|
|
|
AD7190_WriteRegisterOnce(AD7190_REG_GPOC, GPOC);
|
|
|
|
|
|
osDelay(5);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
uint32_t config = AD7190_CONF_GAIN_128 | AD7190_CONF_UB | AD7190_CONF_BUF | AD7190_CONF_CH0;// 0x0000011F;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
AD7190_WriteRegister(AD7190_REG_CONF, config);
|
|
|
|
|
|
osDelay(5);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
modeReg = AD7190_CONF_MODE_FS9_0 ;// <20>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ִ<><D6B4><EFBFBD>ϵ<EFBFBD>У
|
2025-09-28 09:17:22 +00:00
|
|
|
|
AD7190_WriteRegister(AD7190_REG_MODE, modeReg);
|
|
|
|
|
|
osDelay(5);
|
2026-03-17 02:33:04 +00:00
|
|
|
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ģʽ
|
|
|
|
|
|
modeReg = AD7190_CONF_MODE_FS9_0 | AD7190_CONF_MODE_InCLK;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
AD7190_WriteRegister(AD7190_REG_MODE, modeReg);
|
|
|
|
|
|
|
2026-03-17 02:33:04 +00:00
|
|
|
|
CalibrateWeight2.WeightSlope = 0.0001530345f;
|
|
|
|
|
|
// CalibrateWeight2.WeightZero = 745430;
|
2025-09-28 09:17:22 +00:00
|
|
|
|
|
|
|
|
|
|
osThreadCreate(osThread(AD7190_Run), NULL);
|
|
|
|
|
|
}
|