680 lines
20 KiB
C
680 lines
20 KiB
C
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include "freertos/semphr.h"
|
||
#include "esp_err.h"
|
||
#include "stdlib.h"
|
||
#include "config.h"
|
||
#include "utils.h"
|
||
#include "ads1220.h"
|
||
#include "fram.h"
|
||
#include "ModbusM.h"
|
||
#include "ModbusS.h"
|
||
|
||
#include "rotary_encoder.h"
|
||
|
||
#include "esp_log.h"
|
||
#include "esp_check.h"
|
||
#include "soc/rtc.h"
|
||
#include "driver/mcpwm.h"
|
||
#include "config.h"
|
||
#include "esp_gatts_api.h"
|
||
#include "bt_server.h"
|
||
|
||
#include "esp_system.h"
|
||
#include "driver/uart.h"
|
||
#include "string.h"
|
||
#include "driver/gpio.h"
|
||
|
||
#include <string.h>
|
||
#include <stdio.h>
|
||
#include "esp_timer.h"
|
||
|
||
static const char *TAG = "depth";
|
||
|
||
typedef struct
|
||
{
|
||
uint32_t capture_signal;
|
||
mcpwm_capture_signal_t sel_cap_signal;
|
||
} capture;
|
||
|
||
typedef struct capture_event
|
||
{
|
||
int ch;
|
||
uint32_t val;
|
||
} capture_event_t;
|
||
|
||
// #define DEPTH_PIN_CLK GPIO_NUM_36 // 捕获GPIO端口
|
||
// #define GPIO_PCNT_PIN_1 36 // Set GPIO 18 as phaseA/C1
|
||
// #define GPIO_PCNT_PIN_2 35 // Set GPIO 19 as phaseB/C2
|
||
// #define DEPTH_PIN_PULSE 36 //深度脉冲GPIO端口
|
||
// #define DEPTH_PIN_CTRL 35 //深度控制GPIO端口
|
||
|
||
#define FLOW_SYNC_PIN 7 // 捕获GPIO端口 SPI2_nIRQ
|
||
|
||
#define DEPTH_PIN_PULSE 39 // 深度脉冲GPIO端口 TIM15_CH1
|
||
#define DEPTH_PIN_CTRL 38 // 深度控制GPIO端口 TIM15_CH2
|
||
#define DEPTH_PIN_ENC_A 42 // 深度脉冲GPIO端口 TIM3_CH1
|
||
#define DEPTH_PIN_ENC_B 41 // 深度控制GPIO端口 TIM3_CH2
|
||
#define SEND_DATA_TEST 0
|
||
|
||
// #define DEPTH_PIN_ENC_A 36 // 深度脉冲GPIO端口
|
||
// #define DEPTH_PIN_ENC_B 35 // 深度控制GPIO端口
|
||
|
||
void add_recod_item(void);
|
||
|
||
extern int get_total_flow_by_time(int ch, uint32_t time);
|
||
extern void zero_totalflow(int ch);
|
||
|
||
extern void reset_depth(void);
|
||
// extern esp_err_t ec11_pcnt_clear(pcnt_unit_t unit);
|
||
|
||
void capture_depth_init();
|
||
extern flow_t *pflow;
|
||
extern uint16_t last_pile_id;
|
||
uint32_t enc1_update_time;
|
||
uint32_t _enc1_update_time;
|
||
int32_t enc1_value;
|
||
|
||
uint32_t enc2_update_time;
|
||
uint32_t _enc2_update_time;
|
||
int32_t enc2_value;
|
||
|
||
extern rotary_encoder_t *encoder;
|
||
|
||
typedef struct
|
||
{
|
||
int16_t max_depth;
|
||
uint16_t pile_id;
|
||
uint16_t count;
|
||
//uint16_t work_time;
|
||
struct _item
|
||
{
|
||
int16_t speed;
|
||
int16_t depth;
|
||
int16_t flow[2];
|
||
uint32_t total_flow[2];
|
||
int16_t tilt_x;
|
||
int16_t tilt_y;
|
||
uint16_t current1;
|
||
uint16_t current2;
|
||
} item[10];
|
||
} record_t;
|
||
|
||
//18字节
|
||
#pragma pack(1)
|
||
typedef struct {
|
||
uint8_t TAG;
|
||
int16_t speed;
|
||
int16_t depth;
|
||
int16_t tilt_x;
|
||
int16_t tilt_y;
|
||
uint16_t current1;
|
||
uint16_t current2;
|
||
}send_to_bt_t1;
|
||
|
||
//16字节
|
||
typedef struct{
|
||
uint8_t TAG;
|
||
int16_t flow[2];
|
||
uint32_t total_flow[2];
|
||
uint16_t time;
|
||
}send_to_bt_t2;
|
||
#pragma pack()
|
||
|
||
typedef struct
|
||
{
|
||
int16_t up_down; // 12
|
||
int16_t speed; // 13
|
||
int16_t depth; // 14
|
||
uint16_t sample_count; // 15
|
||
uint16_t depth_flow[2]; // 16~17
|
||
uint32_t last_total_flow[2]; //
|
||
int16_t depth_offset; // 22
|
||
uint16_t one_pile_work_time;//23 系统工作时间
|
||
// uint32_t update_time;
|
||
// int16_t tilt_x;
|
||
// int16_t tilt_y;
|
||
// uint16_t current1;
|
||
// uint16_t current2;
|
||
// uint16_t current3;
|
||
} depth_t;
|
||
|
||
|
||
record_t *record = (record_t *)&gWordVar[RECORD_REG_ADDR];
|
||
depth_t *depth_data = (depth_t *)&gWordVar[DEPTH_REG_ADDR];
|
||
move_t *pMoveCtx = (move_t *)&gWordVar[MOVE_REG_ADDR];
|
||
depth_config_t *depth_config = (depth_config_t *)&gWordVar[DEPTH_CONFIG_ADDR];
|
||
|
||
|
||
rotary_encoder_t *encoder = NULL; // 编码器测量深度参数
|
||
|
||
//static uint32_t cap_val_begin_of_depth = 0;
|
||
//static uint32_t cap_val_end_of_depth = 0;
|
||
//extern uint32_t rtc_clk_apb_freq;
|
||
/**
|
||
* @brief this is an ISR callback, we take action according to the captured edge
|
||
*/
|
||
static bool depth_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig, const cap_event_data_t *edata,
|
||
void *arg)
|
||
{
|
||
|
||
// 双边沿触发中断，只有一个边缘会更新值,所以只有在值发生变化是才更新时间
|
||
int value = encoder->get_counter_value(encoder);
|
||
if (value != enc1_value)
|
||
{
|
||
enc1_value = value;
|
||
enc1_update_time = edata->cap_value;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void capture_depth_init()
|
||
{
|
||
int pulse_pin;
|
||
if (depth_config->port)
|
||
{
|
||
pulse_pin = DEPTH_PIN_ENC_A;
|
||
}
|
||
else
|
||
{
|
||
pulse_pin = DEPTH_PIN_PULSE;
|
||
}
|
||
ESP_ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, pulse_pin));
|
||
// enable pull down CAP0, to reduce noise
|
||
ESP_ERROR_CHECK(gpio_pullup_en(pulse_pin));
|
||
// enable both edge capture on CAP0
|
||
mcpwm_capture_config_t conf = {
|
||
.cap_edge = MCPWM_NEG_EDGE,
|
||
.cap_prescale = 1,
|
||
.capture_cb = depth_isr_handler, // 绑定深度中断处理函数
|
||
.user_data = NULL};
|
||
if (depth_config->input_type > 1)
|
||
{
|
||
conf.cap_edge = MCPWM_BOTH_EDGE;
|
||
}
|
||
ESP_ERROR_CHECK(mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf));
|
||
ESP_LOGI(TAG, "capture_depth_init");
|
||
}
|
||
|
||
void pcnt_rotary_encoder_init(void)
|
||
{
|
||
// Rotary encoder underlying device is represented by a PCNT unit in this example
|
||
uint32_t pcnt_unit = 0;
|
||
int pulse_pin;
|
||
int ctrl_pin;
|
||
if (depth_config->port)
|
||
{
|
||
pulse_pin = DEPTH_PIN_ENC_A;
|
||
ctrl_pin = DEPTH_PIN_ENC_B;
|
||
}
|
||
else
|
||
{
|
||
pulse_pin = DEPTH_PIN_PULSE;
|
||
ctrl_pin = DEPTH_PIN_CTRL;
|
||
}
|
||
// Create rotary encoder instance
|
||
rotary_encoder_config_t config = ROTARY_ENCODER_DEFAULT_CONFIG((rotary_encoder_dev_t)pcnt_unit, pulse_pin, ctrl_pin);
|
||
config.flags = depth_config->input_type;
|
||
ESP_ERROR_CHECK(rotary_encoder_new_ec11(&config, &encoder));
|
||
// Filter out glitch (1us)
|
||
ESP_ERROR_CHECK(encoder->set_glitch_filter(encoder, 10));
|
||
// Start encoder
|
||
ESP_ERROR_CHECK(encoder->start(encoder));
|
||
}
|
||
|
||
void depth_init(void)
|
||
{
|
||
// if (depth_config->input_type == 1)
|
||
// { // 电压型 方向+脉冲信号 使用比较器通道,IO口中断方式采集
|
||
|
||
// // GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
// // /*Configure GPIO pin : PtPin */
|
||
// // GPIO_InitStruct.Pin = ROLLER_DIR_Pin;
|
||
// // GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||
// // GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||
// // HAL_GPIO_Init(ROLLER_GPIO_Port, &GPIO_InitStruct);
|
||
|
||
// // /*Configure GPIO pin : PtPin */
|
||
// // GPIO_InitStruct.Pin = ROLLER_CLK_Pin;
|
||
// // GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
|
||
// // GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||
// // HAL_GPIO_Init(ROLLER_GPIO_Port, &GPIO_InitStruct);
|
||
// }
|
||
// else if (depth_config->input_type == 2)
|
||
// { // 电压型 正交编码器 使用比较器通道,TIM1编码器通道采集
|
||
// MX_TIM1_Init();
|
||
// pcnt_rotary_encoder_init(); // 编码器初始化
|
||
// capture_depth_init();
|
||
// HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
|
||
// }
|
||
// else if (depth_config->input_type == 3)
|
||
// { // 开漏输出 正交编码器 使用光耦输入通道,TIM3编码器通道采集
|
||
// // MX_TIM3_Init();
|
||
// // HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
|
||
// }
|
||
depth_data->depth_offset = -depth_config->depth_offset;
|
||
depth_data->up_down = 1; // 默认工作
|
||
}
|
||
|
||
enum
|
||
{
|
||
REST = -1,
|
||
STOP = 0,
|
||
DOWN = 1,
|
||
UP = 2,
|
||
};
|
||
|
||
int last_enc_value = 0;
|
||
uint32_t _enc_update_time;
|
||
uint32_t enc_update_time;
|
||
int last_sample_depth = 0;
|
||
int last_flow[2] = {0};
|
||
int16_t prev_depth = 0;
|
||
uint32_t prev_update_time = 0;
|
||
int16_t target_sample_depth = 0;
|
||
|
||
// 正交编码器的深度 ： encoder->get_counter_value(encoder)
|
||
|
||
int abs_sub(uint32_t enc_update_time, uint32_t _enc_update_time)
|
||
{
|
||
if (enc_update_time >= _enc_update_time)
|
||
{
|
||
return enc_update_time - _enc_update_time;
|
||
}
|
||
else
|
||
{
|
||
return (0xffffffff - _enc_update_time + enc_update_time + 1);
|
||
}
|
||
}
|
||
|
||
|
||
send_to_bt_t1 send_to_bt1;
|
||
send_to_bt_t2 send_to_bt2;
|
||
extern bool is_connected;
|
||
extern uint16_t spp_handle_table[SPP_IDX_NB];
|
||
extern uint16_t spp_conn_id;
|
||
extern esp_gatt_if_t spp_gatts_if;
|
||
extern flow_t *pflow ;
|
||
/*把两个结构体中的数据通过蓝牙发送*/
|
||
void send_data_to_bt(void){
|
||
|
||
#if SEND_DATA_TEST
|
||
send_to_bt1.TAG = 1;
|
||
send_to_bt1.speed = 2;
|
||
send_to_bt1.depth = 3;
|
||
send_to_bt1.tilt_x = 4;
|
||
send_to_bt1.tilt_y = 5;
|
||
send_to_bt1.current1 = 6;
|
||
send_to_bt1.current2 = 7;
|
||
|
||
send_to_bt2.TAG = 2;
|
||
send_to_bt2.flow[0] = 3;
|
||
send_to_bt2.flow[1] = 4;
|
||
send_to_bt2.total_flow[0] = 5;
|
||
send_to_bt2.total_flow[1] = 6;
|
||
ESP_LOGI(TAG, "set text data that send to bt\n");
|
||
#else
|
||
ESP_LOGI(TAG, "use real time data that send to bt\n");
|
||
send_to_bt1.TAG = 1;
|
||
send_to_bt1.tilt_x = (short)gWordVar[TILT_SENSER_ADDR];
|
||
send_to_bt1.tilt_y = (short)gWordVar[TILT_SENSER_ADDR + 1];
|
||
send_to_bt1.speed = depth_data->speed;
|
||
send_to_bt1.depth = depth_data->depth;
|
||
if (depth_config->move_current_channel == 2){
|
||
send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR];
|
||
send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 1];
|
||
}
|
||
else if (depth_config->move_current_channel == 1){
|
||
send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR];
|
||
send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
|
||
}
|
||
else{
|
||
send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR + 1];
|
||
send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
|
||
}
|
||
send_to_bt2.TAG = 2;
|
||
send_to_bt2.flow[0] = pflow[0].flow;
|
||
send_to_bt2.flow[1] = pflow[1].flow;
|
||
send_to_bt2.total_flow[0] = pflow[0].total_flow;
|
||
send_to_bt2.total_flow[1] = pflow[1].total_flow;
|
||
|
||
/*计算开启后持续时间，并传给EC600U*/
|
||
uint16_t time = 0;
|
||
gWordVar[TIME] = time;
|
||
send_to_bt2.time = time;
|
||
#endif
|
||
ESP_LOGI(TAG, "flow 1 :%d\n",send_to_bt2.flow[0]);
|
||
ESP_LOGI(TAG, "flow 2 :%d\n",send_to_bt2.flow[1]);
|
||
ESP_LOGI(TAG, "total_flow 1 :%d\n",send_to_bt2.total_flow[0]);
|
||
ESP_LOGI(TAG, "total_flow 2 :%d\n",send_to_bt2.total_flow[1]);
|
||
if(is_connected == true){
|
||
ESP_LOGI(TAG, "is connected\n");
|
||
esp_err_t err = esp_ble_gatts_send_indicate(spp_gatts_if,
|
||
spp_conn_id,
|
||
spp_handle_table[4],
|
||
sizeof(send_to_bt1),
|
||
(uint8*)&send_to_bt1,
|
||
false);
|
||
if(err == ESP_OK) ESP_LOGI(TAG, "send data to bt 1 sucessfully\n");
|
||
else ESP_LOGI(TAG, "send data to bt 1 failed\n");
|
||
|
||
err = esp_ble_gatts_send_indicate(spp_gatts_if,
|
||
spp_conn_id,
|
||
spp_handle_table[4],
|
||
sizeof(send_to_bt2),
|
||
(uint8*)&send_to_bt2,
|
||
false);
|
||
if(err == ESP_OK) ESP_LOGI(TAG, "send data to bt 2 sucessfully\n");
|
||
else ESP_LOGI(TAG, "send data to bt 2 failed\n");
|
||
}
|
||
|
||
}
|
||
|
||
void depth_task(void *arg)
|
||
{
|
||
ESP_LOGI(TAG, "go to depth_task sucessfully\n");
|
||
// int enc_value;
|
||
int time_diff = 0;
|
||
int speed_timeout = 0;
|
||
int last_speed_enc_value = 0; // 上次速度计算的编码器值
|
||
int speed_enc_update_time = 0;
|
||
int speed_calc_count = 0;
|
||
int enc_value = 0;
|
||
//int count = 0;
|
||
int bt_time_count = 0;
|
||
depth_data->depth_offset = -depth_config->depth_offset;
|
||
depth_data->up_down = 1; // 默认工作
|
||
|
||
record->pile_id = ++last_pile_id;
|
||
//send_to_bt1.pile_id = ++last_pile_id;
|
||
|
||
gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;
|
||
|
||
/*获取当前时间*/
|
||
uint16_t last_work_time = 0;
|
||
uint16_t current_work_time = 0;
|
||
uint8_t is_work = 0;
|
||
|
||
while (1)
|
||
{
|
||
if (_enc1_update_time != enc1_update_time)
|
||
{
|
||
ESP_LOGI(TAG, "_enc1_update_time != enc1_update_time\n");
|
||
// int enc_update_time = enc1_update_time;
|
||
enc_update_time = enc1_update_time;
|
||
enc_value = enc1_value;
|
||
time_diff = abs_sub(enc_update_time, _enc1_update_time);
|
||
_enc1_update_time = enc_update_time;
|
||
}
|
||
|
||
if (time_diff != 0)
|
||
{
|
||
ESP_LOGI(TAG, "time_diff != 0\n");
|
||
int16_t depth = enc_value * depth_config->N / depth_config->M; // 1mm
|
||
depth_data->depth = depth - depth_data->depth_offset;
|
||
if (depth_data->depth > depth_config->max_depth)
|
||
{
|
||
ESP_LOGI(TAG, "depth_data->depth > depth_config->max_depth\n");
|
||
depth_data->depth_offset = depth - depth_config->max_depth;
|
||
depth_data->depth = depth_config->max_depth;
|
||
}
|
||
else if (depth_data->depth < depth_config->min_depth)
|
||
{
|
||
ESP_LOGI(TAG, "depth_data->depth < depth_config->min_depth\n");
|
||
depth_data->depth_offset = depth - depth_config->min_depth;
|
||
depth_data->depth = depth_config->min_depth;
|
||
}
|
||
if (depth_data->depth > record->max_depth)
|
||
{
|
||
ESP_LOGI(TAG, "depth_data->depth > record->max_depth\n");
|
||
record->max_depth = depth_data->depth;
|
||
//send_to_bt1.max_depth = depth_data->depth;
|
||
}
|
||
if (speed_calc_count++ > 50) // 500ms计算一次速度
|
||
{
|
||
ESP_LOGI(TAG, "speed_calc_count++ > 50\n");
|
||
speed_calc_count = 0;
|
||
int speed_time_diff = abs_sub(enc_update_time, speed_enc_update_time);
|
||
int time_diff_us = speed_time_diff / (APB_CLK_FREQ / 1000000);
|
||
if (time_diff_us > 0 && time_diff_us < 5000000)
|
||
{
|
||
ESP_LOGI(TAG, "time_diff_us:%d,dist_diff=%d", time_diff_us, enc_value - last_speed_enc_value);
|
||
// speed = dist_diff / speed_time_diff speed 单位mm/min dist_diff 单位mm speed_time_diff 单位us
|
||
depth_data->speed = (enc_value - last_speed_enc_value) * depth_config->N / depth_config->M * 600000ll / time_diff_us;
|
||
speed_timeout = 0;
|
||
speed_enc_update_time = enc_update_time;
|
||
last_speed_enc_value = enc_value;
|
||
}
|
||
else
|
||
{
|
||
if (++speed_timeout > 10)
|
||
{
|
||
depth_data->speed = 0;
|
||
speed_timeout = 0;
|
||
speed_enc_update_time = enc_update_time;
|
||
last_speed_enc_value = enc_value;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (depth_data->up_down > STOP)
|
||
{
|
||
is_work ++;
|
||
if(is_work == 1){
|
||
depth_data->one_pile_work_time = 0;
|
||
last_work_time = (uint16_t)(esp_timer_get_time()/1000000);
|
||
}
|
||
if(is_work > 1){
|
||
current_work_time = (uint16_t)(esp_timer_get_time()/1000000);
|
||
depth_data->one_pile_work_time = current_work_time - last_work_time;
|
||
if(depth_data->one_pile_work_time < 0){
|
||
depth_data->one_pile_work_time = 0;
|
||
}
|
||
last_work_time = current_work_time;
|
||
}
|
||
|
||
depth_data->one_pile_work_time = current_work_time - last_work_time;
|
||
|
||
last_work_time = current_work_time;
|
||
ESP_LOGI(TAG, "depth_data->up_down > STOP\n");
|
||
if (enc_value > last_enc_value)
|
||
{ // 下钻,深度增加
|
||
if (depth_data->up_down != 1)
|
||
{ // 方向改变
|
||
depth_data->up_down = 1;
|
||
target_sample_depth = (depth_data->depth / depth_config->sample_depth) * depth_config->sample_depth;
|
||
if (abs(depth_data->depth - target_sample_depth) < depth_config->sample_depth / 2)
|
||
{ // 小于半个采样长度的合并到下一个采样点
|
||
target_sample_depth += depth_config->sample_depth;
|
||
}
|
||
}
|
||
if (depth_data->depth >= target_sample_depth)
|
||
{ // 到达或超过目标采样点
|
||
// 由于编码器精度问题不能确保数据在采样点上，需要通过插值计算采样点
|
||
// 当使用10线编码器时每个脉冲深度达6.25cm，当采样间隔为10cm时抖动值太大
|
||
if ((prev_depth - depth_data->depth) != 0)
|
||
{
|
||
int i;
|
||
//uint32_t time = (target_sample_depth - prev_depth) * (enc_update_time - prev_update_time) / (depth_data->depth - prev_depth) + prev_update_time;
|
||
for (i = 0; i < 2; i++)
|
||
{
|
||
// int total_flow = get_total_flow_by_time(i, time);
|
||
int total_flow = get_total_flow_by_time(i, pflow[i].update_time);
|
||
int flow = total_flow - depth_data->last_total_flow[i];
|
||
if (flow < 0)
|
||
{
|
||
flow = 0;
|
||
}
|
||
depth_data->depth_flow[i] = flow;
|
||
depth_data->last_total_flow[i] = total_flow;
|
||
}
|
||
depth_data->sample_count++;
|
||
target_sample_depth += depth_config->sample_depth;
|
||
add_recod_item();
|
||
}
|
||
}
|
||
}
|
||
else if (enc_value < last_enc_value)
|
||
{
|
||
if (depth_data->up_down != 2)
|
||
{ // 方向改变
|
||
depth_data->up_down = 2;
|
||
// target_sample_depth -= depth_config->sample_depth;
|
||
target_sample_depth = (depth_data->depth / depth_config->sample_depth - 1) * depth_config->sample_depth;
|
||
if (abs(depth_data->depth - target_sample_depth) < depth_config->sample_depth / 2)
|
||
{ // 小于半个采样长度的合并到下一个采样点
|
||
target_sample_depth -= depth_config->sample_depth;
|
||
}
|
||
}
|
||
if (depth_data->depth <= target_sample_depth)
|
||
{
|
||
// 由于编码器精度问题不能确保数据在采样点上，需要通过插值计算采样点
|
||
// 当使用10线编码器时每个脉冲深度达7.6cm，当采样间隔为10cm时抖动值太大
|
||
if ((prev_depth - depth_data->depth) != 0)
|
||
{
|
||
int i;
|
||
//uint32_t time = (prev_depth - target_sample_depth) * (enc_update_time - prev_update_time) / (prev_depth - depth_data->depth) + prev_update_time;
|
||
for (i = 0; i < 2; i++)
|
||
{
|
||
// int total_flow = get_total_flow_by_time(i, time);
|
||
int total_flow = get_total_flow_by_time(i, pflow[i].update_time);
|
||
int flow = total_flow - depth_data->last_total_flow[i];
|
||
if (flow < 0)
|
||
{
|
||
flow = 0;
|
||
}
|
||
depth_data->depth_flow[i] = flow;
|
||
depth_data->last_total_flow[i] = total_flow;
|
||
}
|
||
depth_data->sample_count++;
|
||
add_recod_item();
|
||
target_sample_depth -= depth_config->sample_depth;
|
||
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// depth_data->up_down = 0;
|
||
}
|
||
|
||
if (depth_data->depth < depth_config->inc_pile_depth) // 小于指定深度时才允许行走清零
|
||
{
|
||
if (pMoveCtx->pile_inc_req == 1)
|
||
{
|
||
pMoveCtx->pile_inc_req = 0;
|
||
reset_depth();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (pMoveCtx->pile_inc_req == 1)
|
||
{
|
||
pMoveCtx->pile_inc_req = 0;
|
||
}
|
||
}
|
||
}
|
||
else{
|
||
is_work = 0;
|
||
depth_data->one_pile_work_time = 0;
|
||
}
|
||
prev_depth = depth_data->depth;
|
||
prev_update_time = enc_update_time;
|
||
last_enc_value = enc_value;
|
||
}
|
||
////////////////////////////////////////////////////////////////////
|
||
//每隔500ms向蓝牙发送一次数据
|
||
if(bt_time_count++ > 50){
|
||
ESP_LOGI(TAG, "bt_time_count++ > 50\n");
|
||
bt_time_count = 0;
|
||
send_data_to_bt();
|
||
}
|
||
////////////////////////////////////////////////////////////////////
|
||
// if (++count > 100)
|
||
// {
|
||
// count = 0;
|
||
// ESP_LOGI(TAG, "encoder:%d", encoder->get_counter_value(encoder));
|
||
// }
|
||
vTaskDelay(10);
|
||
}
|
||
}
|
||
|
||
void add_recod_item(void)
|
||
{
|
||
//每10cm计算一次
|
||
record->count = depth_data->sample_count;
|
||
memmove(&record->item[1], &record->item[0], sizeof(record->item[0]) * 9);
|
||
record->item[0].flow[0] = depth_data->depth_flow[0];
|
||
record->item[0].flow[1] = depth_data->depth_flow[1];
|
||
record->item[0].total_flow[0] = depth_data->last_total_flow[0];
|
||
record->item[0].total_flow[1] = depth_data->last_total_flow[1];
|
||
|
||
if (depth_config->move_current_channel == 2) // 1
|
||
{
|
||
record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR];
|
||
record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 1];
|
||
}
|
||
else if (depth_config->move_current_channel == 1)
|
||
{
|
||
record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR];
|
||
record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
|
||
}
|
||
else
|
||
{
|
||
record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR + 1];
|
||
record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
|
||
}
|
||
|
||
record->item[0].tilt_x = (short)gWordVar[TILT_SENSER_ADDR];
|
||
record->item[0].tilt_y = (short)gWordVar[TILT_SENSER_ADDR + 1];
|
||
|
||
record->item[0].speed = depth_data->speed; //每500ms计算一次
|
||
record->item[0].depth = depth_data->depth; //每10ms计算一次
|
||
}
|
||
extern int zero_totalflow_req;
|
||
extern void save_pile_id(void);
|
||
void reset_depth(void)
|
||
{
|
||
//uint16_t pile_id;
|
||
last_sample_depth = 0;
|
||
last_flow[0] = 0;
|
||
last_flow[1] = 0;
|
||
prev_depth = 0;
|
||
prev_update_time = 0;
|
||
target_sample_depth = 0;
|
||
zero_totalflow_req = 1;
|
||
if (record->count > 0)
|
||
{
|
||
record->count = 0;
|
||
memset(record->item, 0, sizeof(record->item));
|
||
}
|
||
memset(depth_data, 0, sizeof(*depth_data));
|
||
if (record->max_depth >= depth_config->min_valid_depth) // 上一个桩有一定深度数据
|
||
{
|
||
save_pile_id();
|
||
last_pile_id++;
|
||
gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;
|
||
}
|
||
memset(record, 0, sizeof(*record));
|
||
record->pile_id = last_pile_id;
|
||
//ec11_pcnt_clear(0);
|
||
depth_data->depth_offset = -depth_config->depth_offset;
|
||
depth_data->depth = depth_config->depth_offset;
|
||
enc1_value = 0;
|
||
enc1_update_time = 0;
|
||
depth_data->up_down = 1; // 默认工作
|
||
}
|
||
|
||
void DEPTH_init()
|
||
{
|
||
|
||
// depth_init();
|
||
|
||
pcnt_rotary_encoder_init();
|
||
capture_depth_init();
|
||
xTaskCreate(depth_task, "depth_task", 4096, NULL, 10, NULL);
|
||
}
|
||
|
||
|