//#include "LPC177x_8x.h" // Device header
#include "includes.h"
#define VALUE_VIBRATION 5
typedef struct
{
float k; // 需要存储
float zero_trace; // 零点追踪
float Loading_coef; // 加载值
float zero_coef; // 零点值 需要存储
float NetWeight_coef; // 去皮标定值
uint16_t wen_flag; // 大于10表示稳定
float weight; // 重量g
float weight_Last; // 上次重量g
uint32_t weight_show; // 显示重量
uint32_t weight_show_Last; // 显示重量
uint32_t ADC_VALUE_Last; // 上一次AD差分值
uint32_t ADC_VALUE; // 本次AD差分值
uint8_t Symbol; // 符号 1为负,0为正
uint32_t magnification; // 放大倍数
uint8_t coff; // 一级滤波强度
uint8_t Blance_Abnormal; // 传感器异常标志
uint8_t Valve_change; // 电磁阀状态改变
} ADS1232;
extern ADS1232 ADS_coef;
/* ADS1232 配置增益
#define GainX1 GAIN0_0;GAIN1_0;
#define GainX2 GAIN0_1;GAIN1_0;
#define GainX64 GAIN0_0;GAIN1_1; 目前使用
#define GainX128 GAIN0_1;GAIN1_1;
*/
uint16_t trace_s;
uint8_t FirstUp;
uint32_t liu_wen;
uint8_t liu_count;
uint8_t big_count;
ADS1232 ADS_coef;
extern struct COM com4;
extern struct BALANCEDATA BaData;
INT8U Amountadding[10];//质量变化数组//ly20241104
#define WINDOW_SIZE_MAX 20
#define WINDOW_LEISURE_SIZE 12
#define WINDOW_FILL 4
#define WINDOW_CARDINAL 6
#define ADCa_t 1
#define FIXED_VALUE (214748364.8 / 2 / 5)
#define SAMPLE_RATE_HZ 80 // ADC采样率
#define FILTER_CUTOFF_HZ 15 // 滤波器截止频率
#define M_PI 3.14159265358979323846f // 单精度浮点版本
// 平滑窗口结构体定义
typedef struct
{
uint32_t buffer[WINDOW_SIZE_MAX]; // 窗口数据存储
uint8_t index; // 当前写入位置
uint32_t sum; // 窗口数据总和
uint32_t mean; // 窗口数据取极值平均值
uint32_t window_number; // 窗口个数
uint8_t WINDOW_SIZE; // 采集个数
uint8_t EXCLUDE_NUM; // 去除最大/最小值数量
} SlidingFilter;
SlidingFilter SF;
SlidingFilter SF_W;
// IIR滤波器结构体
typedef struct
{
float alpha;
float prev_output;
} IIR_Filter;
IIR_Filter weight_filter; // IIR滤波器实例
IIR_Filter fill_filter;
// 观测维度定义为1(仅AD测量值)
#define OBS_DIM 1
// 状态维度:假设我们估计的是重量值及其变化率(速度)
#define STATE_DIM 2
#define WINDOW 10
// 自适应参数结构体
typedef struct
{
double lambda; // 遗忘因子(0.95~0.99)
double rho; // 衰减因子(0.95~0.99)
double Q_scale; // Q调整基准值
double R_scale; // R调整基准值
double innov_mean; // 新息均值
double innov_var; // 新息方差
double innov_window[WINDOW]; // 新息窗口
int innov_index; // 窗口索引
// 保存基准噪声值
double R_base; // 初始观测噪声基准值
double Q_pos_base; // 初始位置过程噪声基准值
double Q_vel_base; // 初始速度过程噪声基准值
} AdaptParams;
// 强跟踪滤波器结构体
typedef struct
{
double x[STATE_DIM]; // 状态向量 [位置, 速度]
double P[STATE_DIM][STATE_DIM]; // 状态协方差矩阵
double Q[STATE_DIM][STATE_DIM]; // 过程噪声协方差
double R; // 观测噪声协方差
double F[STATE_DIM][STATE_DIM]; // 状态转移矩阵
double H[OBS_DIM][STATE_DIM]; // 观测矩阵
AdaptParams adapt; // 自适应参数
int steady_state; // 稳态标志 (0:动态 1:稳态)
uint32_t steady_count; // 稳态计数器
} ASTKF;
ASTKF filter;
uint32_t uabs_diff(uint32_t value1, uint32_t value2)
{
return (value1 >= value2) ? (value1 - value2) : (value2 - value1);
}
/**
*一阶滤波算法
*@param new_value 新采样值
*@param old_value 上次滤波结果
*@param cof(一级滤波系数cof / 256)
*@return 本次滤波结果
*/
uint32_t first_filter(uint32_t new_value, uint32_t old_value, uint8_t cof)
{
unsigned long temp;
if (new_value < old_value)
{
temp = old_value - new_value;
temp = temp * cof / 256;
temp = old_value - temp;
}
else if (new_value > old_value)
{
temp = new_value - old_value;
temp = temp * cof / 256;
temp = old_value + temp;
}
else
{
temp = old_value;
}
return temp;
}
// 高级滑动滤波器
int compare_floats(const void *a, const void *b)
{
return *(uint32_t *)a > *(uint32_t *)b ? 1 : -1;
}
float advanced_sliding_filter(SlidingFilter *SF_t)
{
uint32_t sorted[SF_t->WINDOW_SIZE];
// 排序去极值
memset(sorted, 0, sizeof(sorted));
memcpy(sorted, SF_t->buffer, sizeof(sorted));
qsort(sorted, SF_t->WINDOW_SIZE, sizeof(uint32_t), compare_floats);
SF_t->sum = 0;
for (int i = SF_t->EXCLUDE_NUM; i < SF_t->WINDOW_SIZE - SF_t->EXCLUDE_NUM; i++)
{
SF_t->sum += sorted[i];
}
SF_t->mean = (SF_t->sum / (SF_t->WINDOW_SIZE - 2 * SF_t->EXCLUDE_NUM));
//SF_t.mean = division_value(SF_t.mean);
return SF_t->mean;
}
// IIR滤波器实现
void IIR_Filter_Init(IIR_Filter *filter, float alpha)
{
filter->alpha = alpha;
filter->prev_output = 0.0f;
}
float IIR_Filter_Update(IIR_Filter *filter, float input)
{
// 根据重量变化率动态调整截止频率
if (0 > (ADS_coef.weight * ADS_coef.weight_Last))
{
float delta_weight = fabsf(ADS_coef.weight - ADS_coef.weight_Last);
float dynamic_cutoff = FILTER_CUTOFF_HZ * (1.0 - fminf(0.7, delta_weight / 100.0));
filter->alpha = 1.0f - expf(-2 * M_PI * dynamic_cutoff / SAMPLE_RATE_HZ);
}
filter->prev_output = filter->alpha * input + (1.0f - filter->alpha) * filter->prev_output;
return filter->prev_output;
}
// 初始化滤波器
void astkf_init(ASTKF *filter, double init_pos, double init_vel, double q_pos, double q_vel, double r)
{
int i, j, k;
memset(filter, 0, sizeof(ASTKF));
// 初始状态
filter->x[0] = init_pos;
filter->x[1] = init_vel;
// 状态协方差初始化
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
filter->P[i][j] = 0.0;
}
}
filter->P[0][0] = 0.1;
filter->P[1][1] = 0.01;
// 过程噪声协方差 Q
filter->Q[0][0] = q_pos;
filter->Q[1][1] = q_vel;
// 观测噪声协方差 R
filter->R = r;
// 状态转移矩阵 F(假设匀速模型)
filter->F[0][0] = 1.0;
filter->F[0][1] = 0.05; // dt = 0.05
filter->F[1][0] = 0.0;
filter->F[1][1] = 1.0;
// 观测矩阵 H
filter->H[0][0] = 1.0;
filter->H[0][1] = 0.0;
// 新增自适应参数初始化
filter->adapt.lambda = 0.97;
filter->adapt.rho = 0.98;
filter->adapt.Q_scale = 1.0;
filter->adapt.R_scale = 1.0;
filter->adapt.innov_mean = 0.0;
filter->adapt.innov_var = 1.0;
filter->adapt.innov_index = 0;
for (int i = 0; i < WINDOW; i++)
{
filter->adapt.innov_window[i] = 0.0;
}
filter->adapt.R_base = r;
filter->adapt.Q_pos_base = q_pos;
filter->adapt.Q_vel_base = q_vel;
// 设置当前使用的噪声值(初始等于基准值)
filter->R = r;
filter->Q[0][0] = q_pos;
filter->Q[1][1] = q_vel;
filter->steady_state = 1; // 初始设为稳态
filter->steady_count = 0; // 稳态计数器清零
}
// 预测步骤
void astkf_predict(ASTKF *filter)
{
int i, j, k;
// 临时变量
double temp[STATE_DIM] = {0};
double FP[STATE_DIM][STATE_DIM] = {{0}};
double P_new[STATE_DIM][STATE_DIM] = {{0}};
// 状态预测: x = F * x
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
temp[i] += filter->F[i][j] * filter->x[j];
}
}
// 协方差预测: FP = F * P
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
for (k = 0; k < STATE_DIM; k++)
{
FP[i][j] += filter->F[i][k] * filter->P[k][j];
}
}
}
// P_new = FP * F^T + Q
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
for (k = 0; k < STATE_DIM; k++)
{
P_new[i][j] += FP[i][k] * filter->F[j][k];
}
P_new[i][j] += filter->Q[i][j]; // 添加过程噪声
}
}
// 更新状态和协方差
for (i = 0; i < STATE_DIM; i++)
{
filter->x[i] = temp[i];
for (j = 0; j < STATE_DIM; j++)
{
filter->P[i][j] = P_new[i][j];
}
}
}
// 更新新息统计信息
void update_innovation_statistics(ASTKF *filter, double innovation)
{
int i;
// 更新滑动窗口
filter->adapt.innov_window[filter->adapt.innov_index] = innovation;
filter->adapt.innov_index = (filter->adapt.innov_index + 1) % WINDOW;
// 计算窗口内的新息均值和方差
double sum = 0.0;
double sq_sum = 0.0;
int actual_count = 0;
for (i = 0; i < WINDOW; i++)
{
if (filter->adapt.innov_window[i] != 0.0) // 跳过未初始化的值
{
sum += filter->adapt.innov_window[i];
sq_sum += filter->adapt.innov_window[i] * filter->adapt.innov_window[i];
actual_count++;
}
}
if (actual_count > 0)
{
// 使用指数加权平均更新统计量
double mean = sum / actual_count;
double var = (sq_sum / actual_count) - (mean * mean);
// 应用遗忘因子
filter->adapt.innov_mean = filter->adapt.lambda * filter->adapt.innov_mean +
(1 - filter->adapt.lambda) * mean;
filter->adapt.innov_var = filter->adapt.lambda * filter->adapt.innov_var +
(1 - filter->adapt.lambda) * var;
}
}
// 更新步骤(带自适应强跟踪机制)
void astkf_update(ASTKF *filter, double z)
{
int i, j;
// 计算新息(观测残差)
double innovation = z - (filter->H[0][0] * filter->x[0] +
filter->H[0][1] * filter->x[1]);
// 更新新息统计信息
update_innovation_statistics(filter, innovation);
double innov_std = sqrt(filter->adapt.innov_var + 1e-6);
// ===== 优化后的稳态检测机制 =====
double noise_floor = 15.0; // 根据实际噪声范围设置
double steady_threshold = fmax(noise_floor, innov_std * 0.8);
// 稳态检测
if(fabs(innovation) < steady_threshold) {
filter->steady_count++;
if(filter->steady_count > 8) { // 降低连续稳态计数要求
filter->steady_state = 1;
}
} else {
filter->steady_count = 0;
filter->steady_state = 0;
}
// ===== 优化强跟踪阈值 =====
double beta = 1.0;
const double dynamic_thresh = 2.5; // 动态阈值提高到2.5σ
const double steady_thresh = 4.0; // 稳态阈值提高到4.0σ
double threshold = filter->steady_state ? steady_thresh : dynamic_thresh;
// 仅在超过阈值时启用强跟踪
if(fabs(innovation) > threshold * innov_std) {
beta = fmax(0.3, innov_std * innov_std / (innovation * innovation));
}
// ===== 自适应噪声基底管理 =====
static double base_r = 50.0; // 初始基准噪声
// 自动更新噪声基线(只在稳态时更新)
if(filter->steady_state && filter->steady_count > 20) {
base_r = 0.95 * base_r + 0.05 * filter->adapt.innov_var;
}
// 设置观测噪声R不低于基准值
filter->R = fmax(base_r, filter->adapt.R_base);
// ===== 自适应调整Q和R =====
// 1. 仅在动态状态下调整Q
if(!filter->steady_state) {
double innovation_ratio = fabs(innovation) / (innov_std + 1e-6);
// 新息异常增大时:线性增加Q缩放因子
if (innovation_ratio > threshold) {
filter->adapt.Q_scale += 0.2 * (innovation_ratio - threshold);
filter->adapt.Q_scale = fmin(filter->adapt.Q_scale, 3.0);
} else {
// 缓慢恢复Q缩放因子(衰减机制)
filter->adapt.Q_scale *= filter->adapt.rho;
filter->adapt.Q_scale = fmax(filter->adapt.Q_scale, 1.0);
}
// 应用Q调整
filter->Q[0][0] = filter->adapt.Q_scale * filter->adapt.Q_pos_base;
filter->Q[1][1] = filter->adapt.Q_scale * filter->adapt.Q_vel_base;
}
// 计算新息协方差 S = H * P * H^T + R
double S = 0.0;
for (i = 0; i < STATE_DIM; i++)
{
double temp = 0.0;
for (j = 0; j < STATE_DIM; j++)
{
temp += filter->H[0][j] * filter->P[j][i];
}
S += temp * filter->H[0][i];
}
S += filter->R;
/*
// // ===== 强跟踪因子 =====
// double beta = 1.0;
// const double INNOV_THRESH = 2.0; // 新息异常阈值
// // 根据新息大小调整增益
// double innov_size = fabs(innovation);
// double innov_std = sqrt(filter->adapt.innov_var + 1e-6); // 避免除以0
// if (innov_size > INNOV_THRESH * innov_std)
// {
// beta = innov_std * innov_std / (innovation * innovation);
// }
// // ===== 自适应调整Q和R =====
// // 1. 根据新息方差调整过程噪声Q
// double innovation_ratio = innov_size / (innov_std + 1e-6);
// if (innovation_ratio > INNOV_THRESH)
// {
// // 新息异常增大时:线性增加Q缩放因子
// filter->adapt.Q_scale += 0.5 * (innovation_ratio - INNOV_THRESH);
// filter->adapt.Q_scale = fmin(filter->adapt.Q_scale, 5.0); // 上限5倍
// }
// else
// {
// // 缓慢恢复Q缩放因子(衰减机制)
// filter->adapt.Q_scale *= filter->adapt.rho;
// filter->adapt.Q_scale = fmax(filter->adapt.Q_scale, 1.0);
// }
// // 应用Q调整
// filter->Q[0][0] = filter->adapt.Q_scale * filter->adapt.Q_pos_base;
// filter->Q[1][1] = filter->adapt.Q_scale * filter->adapt.Q_vel_base;
// // 2. 根据新息序列调整观测噪声R
// if (filter->adapt.innov_var > S)
// {
// // 实际方差 > 理论方差时增大R
// double r_factor = 1.0 + 0.2 * (filter->adapt.innov_var / S - 1.0);
// filter->R = fmin(r_factor * filter->adapt.R_base, 5.0 * filter->adapt.R_base);
// }
// else
// {
// // 恢复基准R值
// filter->R = filter->adapt.R_base;
// }*/
// ===== 卡尔曼滤波更新 =====
// 计算卡尔曼增益 K = P * H^T / S
double K[STATE_DIM] = {0};
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
K[i] += filter->P[i][j] * filter->H[0][j];
}
K[i] /= S;
}
// 更新状态 x = x + beta * K * innovation
for ( i = 0; i < STATE_DIM; i++)
{
filter->x[i] += beta * K[i] * innovation;
}
// 更新协方差 P = (I - beta * K * H) * P
double KH[STATE_DIM][STATE_DIM] = {{0}};
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
KH[i][j] = K[i] * filter->H[0][j];
}
}
double I_minus_betaKH[STATE_DIM][STATE_DIM] = {{0}};
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
I_minus_betaKH[i][j] = (i == j ? 1.0 : 0.0) - beta * KH[i][j];
}
}
double P_new[STATE_DIM][STATE_DIM] = {{0}};
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
for (int k = 0; k < STATE_DIM; k++)
{
P_new[i][j] += I_minus_betaKH[i][k] * filter->P[k][j];
}
}
}
// 保存更新后的协方差
for (i = 0; i < STATE_DIM; i++)
{
for (j = 0; j < STATE_DIM; j++)
{
filter->P[i][j] = P_new[i][j];
}
}
}
uint32_t division_value(uint32_t AD)
{
uint32_t i = 0;
i = AD / 25;
return i * 25;
}
uint32_t division_value50(uint32_t AD)
{
uint32_t i = 0;
i = AD / 50;
return i * 50;
}
unsigned long i_us;
unsigned long AD_dat = 0x00;
/*******************************************************************************
函数:Read_Ads1232()
功能: 从ADS1232芯片中读取24位数据值(3字节)
参数: Calibration
返回: AD_dat
*******************************************************************************/
unsigned long read_Ads1232(uint8_t Calibration) // 读取数值,并校准
{
unsigned char x = 0;
AD_dat = 0x00;
ADS_SCLK_L; // SCLK_L
while (ADS_DOUT_Read)
{
i_us++;
delay_us(100);
if (i_us > 0x1ffff)
return 0;
} // 等待数据线拉低,表示可以读取数据
for (x = 0; x < 24; x++) // 连续读取24位数据
{
AD_dat <<= 1; // 左移 1位
ADS_SCLK_H; // SCLK_H
delay_us(10);
ADS_SCLK_L; // SCLK_Ladvanced
if (ADS_DOUT_Read) // 获取数据口线的值
{
AD_dat |= 0x00000001;
}
else
{
AD_dat &= 0xfffffffe;
}
delay_us(10);
}
if (Calibration) // 增加一个脉冲,芯片校准
{
ADS_SCLK_H; // SCLK_H
delay_us(10);
ADS_SCLK_L; // SCLK_L
}
return division_value50(AD_dat & 0x00ffffff); // 防止数据溢出
// return (AD_dat&0x00ffffff);//防止数据溢出
}
// 将AD值转换为差分电压并放大ADS_coef.precision倍
uint32_t ADtoDiffVolt(uint32_t AD_Data)
{
return division_value((AD_Data / FIXED_VALUE) * ADS_coef.magnification);
}
// 将差分电压与零点之差计算重量单位g
float DiffVoltTOg(uint32_t DiffVolt)
{
float NetZero = (ADS_coef.zero_coef + ADS_coef.NetWeight_coef);
uint32_t AD_Diff = (DiffVolt >= NetZero) ? (DiffVolt - NetZero) : (NetZero - DiffVolt);
if (DiffVolt >= NetZero)
{
ADS_coef.Symbol = 0x00; //正值
BaData.TotalSymbol = 0x00;
BaData.NetSymbol = 0x00;
}
else
{
ADS_coef.Symbol = 0x01; //负值
BaData.TotalSymbol = 0x01;
BaData.NetSymbol = 0x01;
}
return ADS_coef.k * AD_Diff;
}
// 返回值为差分电压放大值
uint32_t Read_Ads1232(uint8_t Calibration)
{
static uint8_t i = 0;
static uint8_t index_last = 0;
static uint8_t window_size;
uint32_t temp;
uint32_t ADTemp;
if (Filling.State == FUELING) //加注中
{
window_size = WINDOW_FILL;
SF.EXCLUDE_NUM = 0;
}
else
{
window_size = WINDOW_CARDINAL;
SF.EXCLUDE_NUM = 1;
}
// 更新数据缓冲区
temp = read_Ads1232(Calibration);
temp = IIR_Filter_Update(&fill_filter, (float)temp);
if (Filling.State != IDLE && SF.buffer[index_last] != 0 && uabs_diff(temp, SF.buffer[index_last]) > ((SF.buffer[index_last] - ADS_coef.zero_coef) / 3))//本次与上一次之差大于上一次与零点之差的1/3
{
temp = SF.buffer[index_last];
}
if (i == 0) // 第一个先存起来,根据情况赋值
{}
else
{
SF.buffer[SF.index] = first_filter(temp, SF.buffer[index_last], ADS_coef.coff);
}
if ((SF.buffer[SF.index] >= 0x5FFFFF) || !(temp | 0X00)) // 改为传感器异常??
{
ADS_PDWN_L; // PDWN_L
delay_us(20);
ADS_PDWN_H; // PDWN_H
delay_ms(50); // 去电复位
SF.buffer[SF.index] = read_Ads1232(Calibration);
if (SF.buffer[SF.index] >= 0x5FFFFF || SF.buffer[SF.index] <= 0X100) // 传感器异常
{
ADS_coef.Blance_Abnormal = 1;
SetBit((uint8 *)&FillAlarm.STS, 6, 0); // 6-称传感器异常
}
}
else
{
ADS_coef.Blance_Abnormal = 0;
}
if (i == 0)
{
if (uabs_diff(ADtoDiffVolt(temp), ADS_coef.ADC_VALUE_Last) > (0x200)) // 差分值变化大
{
SF.WINDOW_SIZE = window_size;
SF.index = 0;
SF.buffer[SF.index] = temp;
}
else
{
if (WINDOW_FILL == SF.WINDOW_SIZE || 0x00 == SF.buffer[0]) // 从加注中模式切换过来,和第一个
{
SF.WINDOW_SIZE = window_size;
SF.index = 0;
SF.buffer[SF.index] = temp;
}
else
{
if (SF.WINDOW_SIZE == window_size)
{
SF.index = window_size;
SF.buffer[window_size] = temp;
SF.WINDOW_SIZE = window_size * 2;
}
else
{
SF.buffer[SF.index] = temp;
SF.EXCLUDE_NUM = 2;
}
}
}
}
index_last = SF.index;
SF.index = (SF.index + 1) % SF.WINDOW_SIZE;
if (i >= (window_size - 1) && ADS_coef.k > 0.0f && ADS_coef.k < 1.0f)
ADS1232_Processing(Calibration);
i = (i + 1) % window_size;
if (ADS_coef.k < 0.0f || ADS_coef.k > 1.0f)
SetBit((uint8 *)&FillAlarm.STS, 4, 0); // 6-电子秤标定异常
// 预测与更新
astkf_predict(&filter);
astkf_update(&filter, (double)ADtoDiffVolt(SF.buffer[index_last]));
if (filter.x[0] != 0)
return filter.x[0];
else
return SF.buffer[index_last];
}
// 零点标记
uint8_t zero_calibration()
{
LPC_GPIOINT->IO2IntEnF &= (~ADS_DOUT);//禁止ADS_DOUT中断
uint8_t i = 0;
FillFlag.ZeroBiaoding = 4; // 标记中
ADS_coef.zero_coef = 0x00;
for (i = 0; i < 90; i++)
{
ADS_coef.ADC_VALUE_Last = Read_Ads1232(ADCa_t);
if (ADS_coef.zero_coef == 0x00)
{
ADS_coef.zero_coef = ADS_coef.ADC_VALUE_Last;
}
else
{
ADS_coef.zero_coef = (ADS_coef.zero_coef + ADS_coef.ADC_VALUE_Last) / 2;
}
}
ADS_coef.zero_trace = ADS_coef.zero_coef;
if (1 != ADS_coef.Blance_Abnormal)
{
ADS_coef.NetWeight_coef = 0;
//FillPara.ADS1232_zero = ADS_coef.zero_coef;
FillFlag.ZeroBiaoding = 2; // 标定成功
Write_ADS1232(); // 保存
LPC_GPIOINT->IO2IntEnF |= ADS_DOUT;//启用ADS_DOUT中断
return 2;
}
else
{
LPC_GPIOINT->IO2IntEnF |= ADS_DOUT;//启用ADS_DOUT中断
return 3;
}
}
// 加载标记
uint8_t Loading_calibration(uint32_t loading_value)
{
uint8_t i = 0;
LPC_GPIOINT->IO2IntEnF &= (~ADS_DOUT);//禁止ADS_DOUT中断
for (i = 0; i < 90; i++)
{
ADS_coef.ADC_VALUE_Last = Read_Ads1232(ADCa_t);
if (ADS_coef.Loading_coef == 0x00)
{
ADS_coef.Loading_coef = ADS_coef.ADC_VALUE_Last;
}
else
{
ADS_coef.Loading_coef = (ADS_coef.Loading_coef + ADS_coef.ADC_VALUE_Last) / 2;
}
}
// 计算k值
ADS_coef.k = ((float)(loading_value * 10) / (ADS_coef.Loading_coef - ADS_coef.zero_coef));
if (ADS_coef.k > 0.0f && ADS_coef.k < 1.0f && 1 != ADS_coef.Blance_Abnormal)
{
ClrBit((uint8*)&FillAlarm.STS, 4);
//FillPara.ADS1232_k = (ADS_coef.k * 1000000000);
Write_ADS1232(); // 保存
LPC_GPIOINT->IO2IntEnF |= ADS_DOUT;//启用ADS_DOUT中断
return 2; // 标定成功
}
else
{
SetBit((uint8 *)&FillAlarm.STS, 4, 0); //电子秤标定异常
LPC_GPIOINT->IO2IntEnF |= ADS_DOUT;//启用ADS_DOUT中断
return 3; // 标定失败
}
}
//去皮标定
uint8_t NetWeight_calibration()
{
uint8_t i = 0;
float NetTemp;
for (i = 0; i < 90; i++)
{
ADS_coef.ADC_VALUE_Last = Read_Ads1232(ADCa_t);
if (NetTemp == 0x00)
{
NetTemp = ADS_coef.ADC_VALUE_Last;
}
else
{
NetTemp = (NetTemp + ADS_coef.ADC_VALUE_Last) / 2;
}
}
ADS_coef.NetWeight_coef = NetTemp - ADS_coef.zero_coef;
}
void FillingStream()
{
char Temp, i;
INT32U temp1;
INT8U TotalOffset;
if ((Filling.State == FUELING) && (BaData.TotalSymbol == 0)) //ly20240410 zxl20241210
{
//计算加注速率
if (FillFlag.ReadBalanceTimes < 50)
{
FillFlag.ReadBalanceTimes++;
}
FillFlag.Readflag = 0;
Temp = uabs_diff(Filling.TotalWeight, Filling.OldTotalWeight); //ly20241218
if ((Filling.PotType == FORTY_EIGHTKG) || (Filling.PotType == FIVTYKG)) //ly20250107
{
TotalOffset = 80;
}
else
{
TotalOffset = 50;
}
if (Temp > 0)
{
if ((Temp < TotalOffset) || (FillFlag.Calculateflag == 0) || (FillFlag.ReadBalanceTimes < 30)) //ly20241221
{
Amountadding[FillFlag.Index] = Temp;
FillFlag.OldAddTotal = Temp;
Filling.OldTotalWeight = Filling.TotalWeight;
Filling.TotalAbnormalTimes = 0;
}
else
{
Filling.TotalAbnormalTimes++;//ly20241221
if (Filling.TotalAbnormalTimes < 4)
{
Filling.TotalWeight = Filling.OldTotalWeight; //ly20241220
Amountadding[FillFlag.Index] = FillFlag.OldAddTotal;
Filling.OldTotalWeight += FillFlag.OldAddTotal;
}
else
{
Filling.State = SUSPEND; //保存数据
DCF_OUT_CTRL(DCF_V1, OFF); //关阀
DCF_OUT_CTRL(DCF_V2, OFF); //关阀
ADS_coef.Valve_change = VALUE_VIBRATION;
WriteWork_status(SUSPEND);
LogDispose("Goto Suspend4 \n", strlen("Goto Suspend4 \n"));
show.dislist = 59;
Filling.Abnormalstopflag = 1; //ly20240920
Filling.TotalAbnormalTimes = 0;
}
}
}
else
{
Filling.OldTotalWeight = Filling.TotalWeight;
Amountadding[FillFlag.Index] = 0x00;
}
FillFlag.Index++;
if (FillFlag.Index >= 10)
{
FillFlag.Calculateflag = 1;
FillFlag.Index = 0;
}
if (FillFlag.Calculateflag == 1) //ly20241104
{
temp1 = 0;
for (i = 0; i < 10; i++)
{
temp1 += Amountadding[i];
}
Filling.FillingSpeed = temp1 * 5; //计算流速平均值标识Ng/600ms流速
if (Filling.FillingSpeed > Filling.FillingMaxSpeed)
{
Filling.FillingMaxSpeed = Filling.FillingSpeed;
}
if (Filling.FillingSpeed > 350)
{
Filling.FillingSpeed = Filling.FillingoldSpeed;
}
else
{
Filling.FillingoldSpeed = Filling.FillingSpeed;
}
}
if (Filling.State == FUELING)
{
FuelingOperation();
}
}
}
uint32_t WeightShow_10(float weight)
{
uint32_t show_value = weight / 10;
uint8_t units_digit = (uint32_t)weight % 10;
if (0 == show_value && units_digit < 8)
{
ADS_coef.weight_show = show_value * 10;
}
else
{
if (units_digit >= 5)
{
ADS_coef.weight_show = (show_value + 1) * 10;
}
else
{
ADS_coef.weight_show = show_value * 10;
}
}
Filling.TotalWeight = ADS_coef.weight_show / 10;
BaData.TotalWeight = ADS_coef.weight_show / 10;
BaData.LastData = BaData.TotalWeight;
Filling.NetWeight = ADS_coef.weight_show / 10;
BaData.NetWeight = ADS_coef.weight_show / 10;
FillingStream();
BaData.DataUpdateflag = 1;
}
uint32_t WeightShow_20(float weight)
{
uint32_t show_value = weight / 20;
uint8_t units_digit = (uint32_t)weight % 20;
if (weight > 15 && ADS_coef.wen_flag >= 30 && (fabsf(weight - ADS_coef.weight_show) < 15))
{}
else
{
if (0 == show_value && units_digit < 15)
{
ADS_coef.weight_show = show_value * 20;
}
else
{
if (units_digit >= 10)
{
ADS_coef.weight_show = (show_value + 1) * 20;
}
else
{
ADS_coef.weight_show = show_value * 20;
}
}
}
Filling.TotalWeight = ADS_coef.weight_show / 10;
BaData.TotalWeight = ADS_coef.weight_show / 10;
BaData.LastData = BaData.TotalWeight;
Filling.NetWeight = ADS_coef.weight_show / 10;
BaData.NetWeight = ADS_coef.weight_show / 10;
FillingStream();
BaData.DataUpdateflag = 1;
}
// 初始化
void ADS1232_init()
{
uint8_t i = 0;
uint32 int32Temp;
// 初始化
FirstUp = 1;
ADS_coef.magnification = 10000000;
ADS_coef.coff = 180;
ADS_coef.NetWeight_coef = 0;
ADS_coef.Valve_change = 0;
ADS_PDWN_L; // PDWN_L
delay_us(20);
ADS_PDWN_H; // PDWN_H
delay_us(40); // 去电复位
// 读取几次,待数据稳定后再读取输出
for (i = 0; i < 12; i++)
{
ADS_coef.ADC_VALUE_Last = Read_Ads1232(ADCa_t);
delay_us(20);
}
//读取铁电保存值
ReadContinuumeMBRS64A((uint8 *)(&RealTimeData.ADS1232[0]) - & (RealTimeData.Head), (void *)&RealTimeData.ADS1232[0], 12, FillFlag.FmDeviceType);
int32Temp = ComputeCRC32((uint32_t *)(&RealTimeData.ADS1232[0]), 2);
if (int32Temp != RealTimeData.ADS1232[2])
{
SetBit((uint8 *)&FillAlarm.STS, 4, 0); //电子秤标定异常
}
else
{
ADS_coef.zero_coef = RealTimeData.ADS1232[0];
ADS_coef.k = ((float)RealTimeData.ADS1232[1] / 1000000000.0);
if (ADS_coef.k > 0.0f && ADS_coef.k < 1.0f)
{
ClrBit((uint8*)&FillAlarm.STS, 4);
}
else
{
SetBit((uint8 *)&FillAlarm.STS, 4, 0); //电子秤标定异常
}
}
// 滤波器初始化
float alpha = 1.0f - expf(-2 * M_PI * FILTER_CUTOFF_HZ / SAMPLE_RATE_HZ);
IIR_Filter_Init(&weight_filter, alpha);
IIR_Filter_Init(&fill_filter, alpha);
// 初始化滤波器:初始位置=4000,初始速度=0
// Q:过程噪声,R:观测噪声
astkf_init(&filter, 80000.0, 0, 15000, 2000000, ADS_coef.ADC_VALUE_Last);
//zero_calibration(); // 零点标记
//Loading_calibration(20000); // 假设加载标定值为20000g
LPC_GPIOINT->IO2IntEnF |= ADS_DOUT;//启用ADS_DOUT中断
}
uint8_t SFW_A;
float LLw;
float Lw_w;
float sum = 0, sq_sum = 0, r = 0;
uint8_t kfi;
void ADS1232_Processing(uint8_t Calibration)
{
uint32_t ad;
uint32_t IIRTemp;
float weight_abs = 0.0f;
if (1 == ADS_coef.Blance_Abnormal)
SetBit((uint8 *)&FillAlarm.STS, 6, 0); // 6-称传感器异常
else
ClrBit((uint8*)&FillAlarm.STS, 6);
advanced_sliding_filter(&SF);
SF.window_number++;
if (SF.window_number > 0xffff && (Filling.State == IDLE)) // 加上没有在加注状态判断
{
SF.window_number = 0;
ADS_PDWN_L; // PDWN_L
delay_us(40);
ADS_PDWN_H; // PDWN_H 去电复位
delay_us(40);
read_Ads1232(Calibration);
delay_us(10);
read_Ads1232(Calibration);
}
ADS_coef.ADC_VALUE = ADtoDiffVolt(SF.mean); // 读取差分电压值
ad = ADS_coef.ADC_VALUE;
IIRTemp = IIR_Filter_Update(&weight_filter, (float)ADS_coef.ADC_VALUE);
if (uabs_diff(IIRTemp, ADS_coef.ADC_VALUE) < 200)
ADS_coef.ADC_VALUE = IIRTemp;
// 预测与更新
if (ADS_coef.Valve_change > 0) //阀门有改变AD值不可信
{
ADS_coef.Valve_change--;
ADS_coef.ADC_VALUE = filter.x[0] + (fabsf(ADS_coef.weight_Last - LLw)/4);
}
else
{
//卡尔曼滤波
astkf_predict(&filter);
astkf_update(&filter, ADS_coef.ADC_VALUE);
ADS_coef.ADC_VALUE = filter.x[0];
}
ADS_coef.weight = DiffVoltTOg(ADS_coef.ADC_VALUE);
Lw_w = fabsf(ADS_coef.weight_Last - ADS_coef.weight);
// if (Lw_w < 10 && Filling.State == IDLE)
// ADS_coef.weight = (ADS_coef.weight + ADS_coef.weight_Last) / 2;
// //计算流动稳定值(预测)
// if (Lw_w < (Filling.FillingSpeed + 100))
// {
// if (liu_wen == 0)
// liu_wen = ADS_coef.weight;
// else
// liu_wen = (liu_wen + ADS_coef.weight + (Filling.FillingSpeed / 6)) / 2;
// if (uabs_diff(liu_wen, ADS_coef.weight) > 200)
// {
// if (liu_count > 3) //修正
// {
// liu_wen = ADS_coef.weight;
// }
// liu_count++;
// }
// else
// liu_count = 0;
// }
// if (Filling.State != IDLE)
// {
// //防止突变 连续3次突变
// if (Lw_w > ADS_coef.weight_Last / 20 && ADS_coef.weight > (liu_wen + Filling.FillingSpeed))
// {
// if (big_count < 5)
// {
// ADS_coef.weight = ADS_coef.weight_Last;
// big_count++;
// }
// }
// else
// big_count = 0;
// }
//稳定性加强
if (Filling.State == FUELING && Filling.FillingSpeed > 0) //上次重量差小于上上次重量差三倍
{
SF_W.WINDOW_SIZE = ((SFW_A >= 3) ? (SFW_A = 9) : SFW_A++);
SF_W.EXCLUDE_NUM = 1;
SF_W.buffer[SF_W.index] = ADS_coef.weight;
if (SFW_A > 3)
{
//SF_W.buffer[SF_W.index] += (Filling.FillingSpeed/10*3);
advanced_sliding_filter(&SF_W);
if (fabsf(SF_W.mean - ADS_coef.weight) < 25)
ADS_coef.weight = SF_W.mean;
else
{
SFW_A = 0;
SF_W.index = 0;
}
}
SF_W.index = (SF_W.index + 1) % 3;
}
else if (Lw_w < 60)
{
SF_W.WINDOW_SIZE = ((SFW_A >= 14) ? (SFW_A = 14) : SFW_A++);
if (Filling.State == SAVEDATA || Filling.State == SUSPEND)
SF_W.EXCLUDE_NUM = 4;
else
SF_W.EXCLUDE_NUM = 2;
SF_W.buffer[SF_W.index] = ADS_coef.weight;
if (SFW_A > 9)
{
advanced_sliding_filter(&SF_W);
ADS_coef.weight = SF_W.mean;
}
SF_W.index = (SF_W.index + 1) % 14;
}
else
{
SFW_A = 0;
SF_W.index = 0;
}
// 判稳
if (fabsf(ADS_coef.weight_Last - ADS_coef.weight) < 5) //g
{
ADS_coef.wen_flag++;
if (20 < ADS_coef.wen_flag) // 连续20次于上一次重量相差在5g以内
{
BaData.DataStable = 1;
FillFlag.DataState = 1; // 判断为数据稳定
if (1 == FirstUp && ADS_coef.weight != 0 && ADS_coef.weight < 200 && ADS_coef.zero_coef > 0)
{
FirstUp = 0;
ADS_coef.NetWeight_coef = ADS_coef.ADC_VALUE - ADS_coef.zero_coef;
}
}
if (ADS_coef.wen_flag > 0x2fff) // 保持稳定状态
ADS_coef.wen_flag = 0x2f;
if (ADS_coef.wen_flag > 0x40 && ADS_coef.weight < 200) // 零点跟踪
{
trace_s++;
ADS_coef.zero_trace += ADS_coef.ADC_VALUE;
ADS_coef.zero_trace = ADS_coef.zero_trace / 2;
if (trace_s > 0x70)
{
ADS_coef.NetWeight_coef = ADS_coef.zero_trace - ADS_coef.zero_coef;
trace_s = 0;
}
}
else
{
}
ADS_coef.wen_flag = 0;
BaData.DataStable = 0;
FillFlag.DataState = 0; // 判断为数据不稳
}
WeightShow_10(ADS_coef.weight);
sprintf((char *)com4.TX_Buffer, "show:%d weight:%f AD:%d ->%d Lw_LLw:%f speed:%d i_us:%ld\n",
ADS_coef.weight_show, ADS_coef.weight, ad, ADS_coef.ADC_VALUE, fabsf(ADS_coef.weight_Last - LLw),
(Filling.FillingSpeed), i_us);
com4.TX_tailPtr = strlen(com4.TX_Buffer);
STARTtx4();
i_us = 0;
LLw = ADS_coef.weight_Last;
ADS_coef.weight_Last = ADS_coef.weight;
ADS_coef.ADC_VALUE_Last = ADS_coef.ADC_VALUE;
ADS_coef.ADC_VALUE = 0;
}
这是一个电子秤项目发现在充装过程中有时卡尔曼滤波预测下一个的值相差很大,有时预测下一个的值相差很小,观测的都是在加注中,所以预测都是想上预测的,这样很不稳定统一,如何解决
最新发布