|
|
@@ -6,21 +6,22 @@
|
|
|
#define TOF_PWFW_DIFF ((uint16_t)(0x1620u * 25 / 100))
|
|
|
#define TOF_PWFW_LOW ((uint16_t)(0x1620u * 40 / 100))
|
|
|
#define TOF_PWFW_HIGH ((uint16_t)(0x1620u * 50 / 100))
|
|
|
-#define TOF_OFST_LOW (9) //modefiy //modified by HBR 20260911 10->9
|
|
|
-#define TOF_OFST_HIGH (27) //modefiy //modified by HBR 20260911 26->27
|
|
|
+#define TOF_OFST_LOW (10)
|
|
|
+#define TOF_OFST_HIGH (26)
|
|
|
|
|
|
uint8_t TdcFwOfst = (TOF_OFST_LOW + TOF_OFST_HIGH) / 2;
|
|
|
|
|
|
uint32_t CntErrEvent = 0;
|
|
|
uint8_t TdcIntStep = 0xFFu;
|
|
|
|
|
|
+uint8_t TDC_SAMPLE_CNT = 0;//add by yuewei 261014 to count the tdc sample times
|
|
|
+uint8_t TDC_SAMPLE_TIMES_PER_SECOND = 1;//add by yuewei 261014 to define tdc sample times per 1 second
|
|
|
+
|
|
|
float SumToF;
|
|
|
float LastSumToF;
|
|
|
float UToFAvg;
|
|
|
float DToFAvg;
|
|
|
float DiffToF;
|
|
|
-float DiffToF_LAST; // add by yuewei 20260325 用于记录上次飞行时间,便于低通平滑滤波
|
|
|
-float APSR_LAST; // add by yuewei 20260325 用于记录上次APSR值,便于防止二次修正来回跳变
|
|
|
float DiffToFAvg;
|
|
|
float DiffToFAvg2;
|
|
|
float DiffToFAvg128;
|
|
|
@@ -82,50 +83,33 @@ void set_tdc_int_step(uint8_t int_step)
|
|
|
}
|
|
|
|
|
|
// 可调滤波参数
|
|
|
-#define BUF_SIZE 3 // 减小窗口,避免整段温漂进入缓存
|
|
|
-#define LPF_ALPHA 0.22f // 大幅降低,强化平滑,压制缓慢漂移
|
|
|
-
|
|
|
-// 卡尔曼参数:大幅降低Q、提高R,不信任微小缓慢波动
|
|
|
-#define KALMAN_Q 0.002f
|
|
|
-#define KALMAN_R 0.025f
|
|
|
-#define KALMAN_INIT_P 0.1f
|
|
|
-
|
|
|
+#define BUF_SIZE 5 // 窗口大小
|
|
|
+#define LPF_ALPHA 0.20f // 一阶低通系数
|
|
|
#define TOF_MAX_CHANNELS 1
|
|
|
-
|
|
|
typedef struct
|
|
|
{
|
|
|
float filter_buf[BUF_SIZE];
|
|
|
uint8_t buf_index;
|
|
|
uint8_t buffer_ready;
|
|
|
-
|
|
|
- float x_est;
|
|
|
- float p_est;
|
|
|
- const float Q;
|
|
|
- const float R;
|
|
|
-
|
|
|
float lpf_out;
|
|
|
uint8_t lpf_ready;
|
|
|
-
|
|
|
float last_filtered;
|
|
|
} TOF_Channel_t;
|
|
|
-
|
|
|
static TOF_Channel_t tof_channels[TOF_MAX_CHANNELS] = {
|
|
|
{
|
|
|
- .Q = KALMAN_Q,
|
|
|
- .R = KALMAN_R,
|
|
|
.buf_index = 0,
|
|
|
.buffer_ready = 0,
|
|
|
.lpf_ready = 0,
|
|
|
}
|
|
|
};
|
|
|
-
|
|
|
extern uint8_t G_STATIC_OT_SETTING_FLAG;
|
|
|
-
|
|
|
static float filter_single_channel(float raw)
|
|
|
{
|
|
|
TOF_Channel_t *f = &tof_channels[0];
|
|
|
float temp_buf[BUF_SIZE];
|
|
|
uint8_t i, j;
|
|
|
+ float sum_mid = 0.0f;
|
|
|
+ uint8_t mid_cnt;
|
|
|
|
|
|
f->filter_buf[f->buf_index++] = raw;
|
|
|
if (f->buf_index >= BUF_SIZE)
|
|
|
@@ -136,8 +120,6 @@ static float filter_single_channel(float raw)
|
|
|
|
|
|
if (!f->buffer_ready)
|
|
|
{
|
|
|
- f->x_est = raw;
|
|
|
- f->p_est = KALMAN_INIT_P;
|
|
|
f->lpf_out = raw;
|
|
|
f->lpf_ready = 1;
|
|
|
f->last_filtered = raw;
|
|
|
@@ -145,6 +127,7 @@ static float filter_single_channel(float raw)
|
|
|
}
|
|
|
|
|
|
memcpy(temp_buf, f->filter_buf, sizeof(temp_buf));
|
|
|
+ // 冒泡排序
|
|
|
for (i = 0; i < BUF_SIZE - 1; i++)
|
|
|
{
|
|
|
for (j = 0; j < BUF_SIZE - i - 1; j++)
|
|
|
@@ -157,29 +140,24 @@ static float filter_single_channel(float raw)
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
- float median_val = temp_buf[BUF_SIZE / 2];
|
|
|
-
|
|
|
- // 卡尔曼更新
|
|
|
- float x_pred = f->x_est;
|
|
|
- float p_pred = f->p_est + f->Q;
|
|
|
- float k_gain = p_pred / (p_pred + f->R);
|
|
|
- float x_cur = x_pred + k_gain * (median_val - x_pred);
|
|
|
- float p_cur = (1.0f - k_gain) * p_pred;
|
|
|
|
|
|
- f->x_est = x_cur;
|
|
|
- f->p_est = p_cur;
|
|
|
+ // 取中间2个值求平均(BUF_SIZE=5:索引1,2,3,取中间3点平均;BUF_SIZE偶数取中间2点)
|
|
|
+ mid_cnt = (BUF_SIZE % 2 == 1) ? 3 : 2;
|
|
|
+ uint8_t start_idx = (BUF_SIZE - mid_cnt) / 2;
|
|
|
+ for(i = 0; i < mid_cnt; i++)
|
|
|
+ {
|
|
|
+ sum_mid += temp_buf[start_idx + i];
|
|
|
+ }
|
|
|
+ float mid_avg = sum_mid / mid_cnt;
|
|
|
|
|
|
// 一阶低通
|
|
|
if (!f->lpf_ready)
|
|
|
{
|
|
|
- f->lpf_out = x_cur;
|
|
|
+ f->lpf_out = mid_avg;
|
|
|
f->lpf_ready = 1;
|
|
|
}
|
|
|
- f->lpf_out = LPF_ALPHA * x_cur + (1.0f - LPF_ALPHA) * f->lpf_out;
|
|
|
-
|
|
|
- // 仅保存上一帧输出,不再计算方差
|
|
|
+ f->lpf_out = LPF_ALPHA * mid_avg + (1.0f - LPF_ALPHA) * f->lpf_out;
|
|
|
f->last_filtered = f->lpf_out;
|
|
|
-
|
|
|
return f->lpf_out;
|
|
|
}
|
|
|
|
|
|
@@ -190,8 +168,6 @@ void tof_filter_reset(void)
|
|
|
memset(f->filter_buf, 0, sizeof(f->filter_buf));
|
|
|
f->buf_index = 0;
|
|
|
f->buffer_ready = 0;
|
|
|
- f->x_est = 0.0f;
|
|
|
- f->p_est = KALMAN_INIT_P;
|
|
|
f->lpf_out = 0.0f;
|
|
|
f->lpf_ready = 0;
|
|
|
f->last_filtered = 0.0f;
|
|
|
@@ -215,6 +191,7 @@ float tof_filter_input_poll(uint8_t ch, float raw)
|
|
|
return filter_single_channel(raw);
|
|
|
}
|
|
|
|
|
|
+
|
|
|
void DiffToF2Flow(float dtof)
|
|
|
{
|
|
|
float TmpFloat;
|
|
|
@@ -238,184 +215,137 @@ void read_tof_and_convert_to_flow(void)
|
|
|
float TmpFloat;
|
|
|
uint32_t Tu32;
|
|
|
uint32_t ch = get_mux_ch();
|
|
|
-// LOG("a\n");
|
|
|
-// LOG("%x\n",TdcStatus);
|
|
|
- // LOG("ch-%d-1-U:%.6f-D:%.6f\n\n", ch, TdcScale*TdcUToF[0],TdcScale*TdcDToF[0]);
|
|
|
- // if(Tu32 == TDC_CFG_FIRE_UP)
|
|
|
- // {
|
|
|
- // LOG("ch-%d-1-U, %.6f\n\n", ch, TdcScale*TdcUToF[0]);
|
|
|
- // }
|
|
|
- // else
|
|
|
- // {
|
|
|
- // LOG("ch-%d-1-D, %.6f\n\n", ch, TdcScale*TdcDToF[0]);
|
|
|
- // }
|
|
|
- // 清反向计量标识
|
|
|
- pTdcItf->bw_flow_flag = 0;
|
|
|
- // 空管
|
|
|
- if (tof_flag & TOF_FLAG_BLANK_PIPE)
|
|
|
- {
|
|
|
- // 设置对应德通道为空管
|
|
|
- pTdcItf->set_blank(ch);
|
|
|
- // DiffTof from normal channel
|
|
|
- // 正常的通道进行计量
|
|
|
- if (pTdcItf->ch_bit_map ^ TdcItf.blank_flag)
|
|
|
- {
|
|
|
- DiffToF2Flow(DiffToF);
|
|
|
- SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, CumFlow);
|
|
|
- }
|
|
|
- if(alarm_status(ERR_BLANK_PIPE))
|
|
|
- {
|
|
|
- tdc_turn_on_off_high_clk(0);
|
|
|
- }
|
|
|
- return;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // 指定通道清除空管
|
|
|
- pTdcItf->clr_blank(ch);
|
|
|
- }
|
|
|
|
|
|
- // 计算脉冲宽度
|
|
|
- Tmp16 = TdcUPwFw - TdcDPwFw;
|
|
|
- // 判断脉冲宽度有效性
|
|
|
- if (Tmp16 < (0 - TOF_PWFW_DIFF) || Tmp16 > TOF_PWFW_DIFF)
|
|
|
+ // 清反向计量标识
|
|
|
+ pTdcItf->bw_flow_flag = 0;
|
|
|
+ // 空管
|
|
|
+ if (tof_flag & TOF_FLAG_BLANK_PIPE)
|
|
|
+ {
|
|
|
+ // 设置对应德通道为空管
|
|
|
+ pTdcItf->set_blank(ch);
|
|
|
+ // DiffTof from normal channel
|
|
|
+ // 正常的通道进行计量
|
|
|
+ if (pTdcItf->ch_bit_map ^ TdcItf.blank_flag)
|
|
|
{
|
|
|
- // 无效脉冲宽度
|
|
|
- tof_flag |= TOF_FLAG_UNPAIR_WAVE;
|
|
|
- CntErrEvent++;
|
|
|
+ DiffToF2Flow(DiffToF);
|
|
|
+ SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, CumFlow);
|
|
|
}
|
|
|
- else
|
|
|
+ if(alarm_status(ERR_BLANK_PIPE))
|
|
|
{
|
|
|
- // 有效脉冲宽度
|
|
|
- tof_flag &= ~TOF_FLAG_UNPAIR_WAVE;
|
|
|
- // 记录UP和DOWM平均飞行时间
|
|
|
- UToFAvg = TdcScale * TdcUToF[0];
|
|
|
- DToFAvg = TdcScale * TdcDToF[0];
|
|
|
+ tdc_turn_on_off_high_clk(0);
|
|
|
}
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // 指定通道清除空管
|
|
|
+ pTdcItf->clr_blank(ch);
|
|
|
+ }
|
|
|
|
|
|
- // 判断offset有效性
|
|
|
- if (TdcFwOfst < TOF_OFST_LOW || TdcFwOfst > TOF_OFST_HIGH)
|
|
|
- {
|
|
|
- // out of range
|
|
|
- TdcFwOfst = (TOF_OFST_LOW + TOF_OFST_HIGH) / 2; // default offset
|
|
|
- }
|
|
|
- else if (tof_flag & TOF_FLAG_UNPAIR_WAVE)
|
|
|
- {
|
|
|
- // first wave for up and down is different
|
|
|
- TdcFwOfst += (TdcFwOfst <= (TOF_OFST_LOW + 3)) ? 2 : -2;
|
|
|
- }
|
|
|
- else if (TdcUPwFw < TOF_PWFW_LOW && TdcDPwFw < TOF_PWFW_LOW && TdcFwOfst > TOF_OFST_LOW)
|
|
|
- {
|
|
|
- TdcFwOfst -= 1;
|
|
|
- }
|
|
|
- else if (TdcUPwFw > TOF_PWFW_HIGH && TdcDPwFw > TOF_PWFW_HIGH && TdcFwOfst < TOF_OFST_HIGH)
|
|
|
- {
|
|
|
- TdcFwOfst += 1;
|
|
|
- }
|
|
|
+ // 记录UP和DOWM平均飞行时间
|
|
|
+ UToFAvg = TdcScale * TdcUToF[0];
|
|
|
+ DToFAvg = TdcScale * TdcDToF[0];
|
|
|
|
|
|
- // 本次UP和DOWN飞行时间求和
|
|
|
- SumToF = UToFAvg + DToFAvg; // alomost fixed regardless of water speed
|
|
|
- // 本次飞行时间和与上次飞行时间和德差值在50内认为有效
|
|
|
- if (fabsf(SumToF - LastSumToF) < 50.0f)
|
|
|
- {
|
|
|
- // 计算飞行时间
|
|
|
- DiffToF = UToFAvg - DToFAvg - pTdcItf->path_skew[ch];
|
|
|
- // add by yuewei 20260325 低通平滑滤波
|
|
|
- // DiffToF = DiffToF * DIFF_TOFF_FILTER_ALPHA + (1 - DIFF_TOFF_FILTER_ALPHA) * DiffToF_LAST;
|
|
|
-
|
|
|
- // DiffToF = tof_filter(DiffToF);//20260429
|
|
|
- DiffToF = tof_filter_input_poll(ch, DiffToF);
|
|
|
-// LOG("ch%d, U=%.6f, D=%.6f\n", ch, UToFAvg, DToFAvg);
|
|
|
- // LOG("int_cnt=%d, step=%d, ch=%d, DiffToF=%.6f\n\n", int_cnt, get_tdc_int_step(), ch, DiffToF);
|
|
|
- }
|
|
|
- // 本次飞行时间和赋值给上次飞行时间和
|
|
|
- LastSumToF = SumToF;
|
|
|
+ // 本次UP和DOWN飞行时间求和
|
|
|
+ SumToF = UToFAvg + DToFAvg; // alomost fixed regardless of water speed
|
|
|
+ // 本次飞行时间和与上次飞行时间和德差值在50内认为有效
|
|
|
+ if (fabsf(SumToF - LastSumToF) < 50.0f)
|
|
|
+ {
|
|
|
+ // 计算飞行时间
|
|
|
+ DiffToF = UToFAvg - DToFAvg - pTdcItf->path_skew[ch];
|
|
|
+ //DiffToF = UToFAvg - DToFAvg;
|
|
|
|
|
|
- if (tof_state & (TOF_ST_METER | TOF_ST_CALIB))
|
|
|
+ DiffToF = tof_filter_input_poll(ch, DiffToF);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 本次飞行时间和赋值给上次飞行时间和
|
|
|
+ LastSumToF = SumToF;
|
|
|
+
|
|
|
+ if (tof_state & (TOF_ST_METER | TOF_ST_CALIB))
|
|
|
+ {
|
|
|
+ DiffToFAvg += (DiffToF - DiffToFAvg) * (1.0f / 20);
|
|
|
+ DiffToFAvg2 += (DiffToF - DiffToFAvg2) * (1.0f / 2);
|
|
|
+ DiffToFAvg128 += (DiffToF - DiffToFAvg128) * (1.0f / 128);
|
|
|
+
|
|
|
+ if (tof_flag & TOF_FLAG_VALID_FLOW)
|
|
|
{
|
|
|
- DiffToFAvg += (DiffToF - DiffToFAvg) * (1.0f / 6);
|
|
|
- DiffToFAvg2 += (DiffToF - DiffToFAvg2) * (1.0f / 2);
|
|
|
- DiffToFAvg128 += (DiffToF - DiffToFAvg128) * (1.0f / 128);
|
|
|
- if (tof_flag & TOF_FLAG_VALID_FLOW)
|
|
|
+ if (fabsf(DiffToFAvg) < 0.35f)
|
|
|
{
|
|
|
- if (fabsf(DiffToFAvg) < 0.35f)
|
|
|
+ tof_flag &= ~TOF_FLAG_VALID_FLOW;
|
|
|
+ if (tof_state & TOF_ST_CALIB && CalibTime > CALIB_TIME_LTH)
|
|
|
{
|
|
|
- tof_flag &= ~TOF_FLAG_VALID_FLOW;
|
|
|
- if (tof_state & TOF_ST_CALIB && CalibTime > CALIB_TIME_LTH)
|
|
|
- {
|
|
|
- tof_flag |= TOF_FLAG_CALIB_DONE;
|
|
|
- }
|
|
|
+ tof_flag |= TOF_FLAG_CALIB_DONE;
|
|
|
}
|
|
|
}
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // modified by yuewei 20260519,加入n值,始动流量
|
|
|
+ // if (fabsf(DiffToFAvg) > 0.45f || fabsf(DiffToFAvg2) > 1.0f)
|
|
|
+ if (fabsf(DiffToFAvg * 100.00f) > Paramx.n || fabsf(DiffToFAvg2) > 1.0f)
|
|
|
+ {
|
|
|
+ tof_flag |= TOF_FLAG_VALID_FLOW;
|
|
|
+ TDC_SAMPLE_TIMES_PER_SECOND = 6;//add by yuewei 261015,有水大于始动流量->流采集频次1秒6次
|
|
|
+ }
|
|
|
else
|
|
|
{
|
|
|
-
|
|
|
- // modified by yuewei 20260519,加入n值,始动流量
|
|
|
- // if (fabsf(DiffToFAvg) > 0.45f || fabsf(DiffToFAvg2) > 1.0f)
|
|
|
- if (fabsf(DiffToFAvg * 100.00f) > Paramx.n || fabsf(DiffToFAvg2) > 1.0f)
|
|
|
- {
|
|
|
- tof_flag |= TOF_FLAG_VALID_FLOW;
|
|
|
- if (tof_state & TOF_ST_CALIB)
|
|
|
- {
|
|
|
- tof_flag |= TOF_FLAG_CALIB_FLOW;
|
|
|
- CumFlow = 0;
|
|
|
- CalibTime = 0;
|
|
|
- CalibSpdLvl = 0;
|
|
|
- CalibTemp = 0;
|
|
|
- }
|
|
|
- }
|
|
|
+ tof_flag &= ~TOF_FLAG_VALID_FLOW;
|
|
|
+ TDC_SAMPLE_TIMES_PER_SECOND = 2;//add by yuewei 261015,有水小于始动流量->流采集频次1秒2次
|
|
|
}
|
|
|
+ }
|
|
|
|
|
|
- // 是有效的流量
|
|
|
- if (tof_flag & TOF_FLAG_VALID_FLOW)
|
|
|
- {
|
|
|
- TmpFloat = DiffToF * UsonicSpd * UsonicSpd * 0.00001f;
|
|
|
+ // 是有效的流量
|
|
|
+ if (tof_flag & TOF_FLAG_VALID_FLOW)
|
|
|
+ {
|
|
|
+ TmpFloat = DiffToF * UsonicSpd * UsonicSpd * 0.00001f;
|
|
|
|
|
|
- // 一次校准模式
|
|
|
- if (tof_state & TOF_ST_CALIB)
|
|
|
+ // 一次校准模式
|
|
|
+ if (tof_state & TOF_ST_CALIB)
|
|
|
+ {
|
|
|
+ CumFlow += TmpFloat * pTdcItf->pipe_coe;
|
|
|
+ pTdcItf->check_flow(CumFlow, TmpFloat, WaterTemp);
|
|
|
+ if (tof_flag & TOF_FLAG_CALIB_FLOW)
|
|
|
{
|
|
|
- CumFlow += TmpFloat * pTdcItf->pipe_coe;
|
|
|
- pTdcItf->check_flow(CumFlow, TmpFloat, WaterTemp);
|
|
|
- if (tof_flag & TOF_FLAG_CALIB_FLOW)
|
|
|
+ if (DiffToFAvg2 > 0.5f)
|
|
|
{
|
|
|
- if (DiffToFAvg2 > 0.5f)
|
|
|
- {
|
|
|
- CalibSpdLvl += TmpFloat;
|
|
|
- CalibTemp += WaterTemp;
|
|
|
- CalibTime++;
|
|
|
- }
|
|
|
- else
|
|
|
+ CalibSpdLvl += TmpFloat;
|
|
|
+ CalibTemp += WaterTemp;
|
|
|
+ CalibTime++;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ tof_flag &= ~TOF_FLAG_CALIB_FLOW;
|
|
|
+ if (CalibTime > CALIB_TIME_LTH)
|
|
|
{
|
|
|
- tof_flag &= ~TOF_FLAG_CALIB_FLOW;
|
|
|
- if (CalibTime > CALIB_TIME_LTH)
|
|
|
- {
|
|
|
- CalibSpdLvl /= CalibTime;
|
|
|
- CalibTemp /= CalibTime;
|
|
|
- tof_flag |= TOF_FLAG_CALIB_FALL;
|
|
|
- }
|
|
|
+ CalibSpdLvl /= CalibTime;
|
|
|
+ CalibTemp /= CalibTime;
|
|
|
+ tof_flag |= TOF_FLAG_CALIB_FALL;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
- // 正常计量模式
|
|
|
- DiffToF2Flow(DiffToF);
|
|
|
- // DiffToF2Flow(DiffToFAvg);
|
|
|
- }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- FlowVol = 0; // 始动流量切除时,瞬时流量清零yw2026-6-25
|
|
|
+ // 正常计量模式
|
|
|
+ //DiffToF2Flow(DiffToF);
|
|
|
+ //add by yuewei 261015,当采集次数大于等于TDC_SAMPLE_TIMES_PER_SECOND时,才进行流量计算(节约功耗)
|
|
|
+ if(TDC_SAMPLE_CNT >= TDC_SAMPLE_TIMES_PER_SECOND)
|
|
|
+ {
|
|
|
+ DiffToF2Flow(DiffToFAvg);
|
|
|
+ }
|
|
|
}
|
|
|
- // 串口打印,功能未开启
|
|
|
- // SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, DiffToFAvg);
|
|
|
- SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, DiffToF * UsonicSpd * UsonicSpd * 0.00001f, DToFAvg, DiffToFAvg);
|
|
|
}
|
|
|
+ else
|
|
|
+ {
|
|
|
+ FlowVol = 0; // 始动流量切除时,瞬时流量清零yw2026-6-25
|
|
|
+ }
|
|
|
+ // 串口打印,功能未开启
|
|
|
+ // SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, UToFAvg, DToFAvg, DiffToFAvg);
|
|
|
+ SERPLOT_PRINT_4FLOAT_1PREAMBLE(DiffToF, DiffToF * UsonicSpd * UsonicSpd * 0.00001f, DToFAvg, DiffToFAvg);
|
|
|
+ }
|
|
|
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-
|
|
|
void tof_isr_convert_tof_to_flow(void)
|
|
|
{
|
|
|
switch (TdcIntStep)
|
|
|
@@ -428,22 +358,30 @@ void tof_isr_convert_tof_to_flow(void)
|
|
|
|
|
|
case (0x01u):
|
|
|
tdc_read_cal(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
|
|
|
-
|
|
|
tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
|
|
|
TdcIntStep = 0x03u;
|
|
|
break;
|
|
|
|
|
|
case (0x02u):
|
|
|
tdc_read_temp(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
|
|
|
-
|
|
|
tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
|
|
|
TdcIntStep = 0x03u;
|
|
|
break;
|
|
|
|
|
|
case (0x03u):
|
|
|
+ TDC_SAMPLE_CNT++;
|
|
|
tdc_read_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS);
|
|
|
read_tof_and_convert_to_flow();
|
|
|
- TdcIntStep = 0xFFu;
|
|
|
+ if(TDC_SAMPLE_CNT >= TDC_SAMPLE_TIMES_PER_SECOND)
|
|
|
+ {
|
|
|
+ TdcIntStep = 0xFFu;
|
|
|
+ TDC_SAMPLE_CNT = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ tdc_start_tof(TDC_CFG_DO_CKENA | TDC_CFG_DO_CKDIS | TDC_CFG_DO_WREG);
|
|
|
+ TdcIntStep = 0x03u;
|
|
|
+ }
|
|
|
break;
|
|
|
|
|
|
case (0x80u):
|