482 lines
15 KiB
C
482 lines
15 KiB
C
#include <string.h>
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "freertos/queue.h"
|
||
#include "freertos/semphr.h"
|
||
#include "esp_system.h"
|
||
#include "esp_timer.h"
|
||
#include "driver/mcpwm.h"
|
||
#include "driver/gpio.h"
|
||
#include "esp_err.h"
|
||
#include "esp_log.h"
|
||
#include "esp_check.h"
|
||
#include "soc/rtc.h"
|
||
|
||
#include "depth.h"
|
||
#include "rotary_encoder.h"
|
||
#include "gwordvar.h"
|
||
#include "flow.h"
|
||
#include "bl0939.h"
|
||
#include "utils.h"
|
||
|
||
static const char *TAG = "depth";
|
||
|
||
extern uint16_t last_pile_id;
|
||
int32_t enc1_value; // 捕获中更新的值
|
||
uint32_t enc1_update_time;
|
||
uint32_t _enc1_update_time;
|
||
|
||
uint32_t enc2_update_time;
|
||
uint32_t _enc2_update_time;
|
||
int32_t enc2_value;
|
||
|
||
extern rotary_encoder_t *encoder;
|
||
|
||
depth_config_t *depth_config = (depth_config_t *)&gWordVar[DEPTH_CONFIG_ADDR];
|
||
depth_t *depth_data = (depth_t *)&gWordVar[DEPTH_REG_ADDR];
|
||
record_t *record = (record_t *)&gWordVar[RECORD_REG_ADDR];
|
||
|
||
rotary_encoder_t *encoder = NULL; // 编码器测量深度参数
|
||
|
||
|
||
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));
|
||
}
|
||
|
||
|
||
/**
|
||
* @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 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->pile_work_state_and_direction = (PILE_STATE_STOP | PILE_DIR_NONE); // 默认不工作
|
||
}
|
||
|
||
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;
|
||
|
||
extern bool is_connected;
|
||
// extern uint16_t spp_handle_table[IDX_NB];
|
||
extern uint16_t spp_conn_id;
|
||
extern flow_t *pflow;
|
||
|
||
|
||
void calculate_depth_speed(int speed_enc_value)
|
||
{
|
||
static int speed_last_enc_value = -1;
|
||
static int64_t speed_last_time = -1;
|
||
int64_t speed_time = esp_timer_get_time();
|
||
|
||
if (speed_last_enc_value == -1) // 第一次，记录值，并不计算
|
||
{
|
||
goto exit;
|
||
}
|
||
|
||
depth_data->speed = (speed_enc_value - speed_last_enc_value) * depth_config->N / depth_config->M * 600000ll / (speed_time - speed_last_time);
|
||
// ESP_LOGI(TAG, "speed:%d", depth_data->speed);
|
||
exit:
|
||
speed_last_enc_value = speed_enc_value;
|
||
speed_last_time = speed_time;
|
||
}
|
||
|
||
|
||
|
||
|
||
/**
|
||
* _enc1_update_time 编码器1上一次更新时间
|
||
* enc1_update_time 编码器1当前更新时间
|
||
* time_diff 两次编码器更新时间的绝对差值
|
||
* depth_config 预设好的深度数据
|
||
* enc1_value 编码器1值，在深度中断中获得
|
||
* prev_depth 上一次的深度
|
||
*/
|
||
void depth_task(void *arg)
|
||
{
|
||
ESP_LOGI(TAG, "depth_task start\n");
|
||
|
||
int time_diff = 0;
|
||
int speed_calc_count = 0;
|
||
int enc_value = 0;
|
||
int64_t work_start_time = 0;
|
||
uint16_t last_work_state = 0;
|
||
|
||
depth_data->depth_offset = -depth_config->depth_offset;
|
||
depth_data->pile_work_state_and_direction = 0x0300;
|
||
last_work_state = depth_data->pile_work_state_and_direction;
|
||
record->pile_id = ++last_pile_id;
|
||
gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;
|
||
|
||
while (1)
|
||
{
|
||
if (_enc1_update_time != enc1_update_time) /* 深度数据更新 */
|
||
{
|
||
enc_update_time = enc1_update_time;
|
||
enc_value = enc1_value;
|
||
time_diff = abs_sub_uint32(enc_update_time, _enc1_update_time);
|
||
_enc1_update_time = enc_update_time;
|
||
}
|
||
//ESP_LOGI(TAG, "time_diff:%d", time_diff);
|
||
|
||
if (time_diff != 0)
|
||
{
|
||
// ESP_LOGI(TAG, "time_diff = %d\n",time_diff);
|
||
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;
|
||
}
|
||
|
||
// ESP_LOGI(TAG, "depth_data->depth:0x%x", (unsigned int)depth_data->depth);
|
||
|
||
uint16_t pile_work_state = (depth_data->pile_work_state_and_direction & 0xff00);
|
||
if (pile_work_state == PILE_STATE_WORK)
|
||
{
|
||
// ESP_LOGI(TAG, "pile_work_state == PILE_STATE_WORK\n");
|
||
/*如果机器从停止状态->工作状态，则重新记录工作开始时间*/
|
||
if(last_work_state == PILE_STATE_STOP){
|
||
work_start_time = esp_timer_get_time();
|
||
depth_data->one_pile_work_time = 0;
|
||
}
|
||
int64_t current_work_time = esp_timer_get_time();
|
||
if(work_start_time != 0){
|
||
depth_data->one_pile_work_time = (uint16_t)((current_work_time - work_start_time)/1000000); // s
|
||
// ESP_LOGI(TAG, "time : %ud", (int)depth_data->one_pile_work_time);
|
||
}
|
||
|
||
/*下钻，深度增加 计算采样深度流量*/
|
||
if (enc_value > last_enc_value)
|
||
{
|
||
// ESP_LOGI(TAG, "enc_value > last_enc_value\n");
|
||
uint8_t pile_work_dir = (depth_data->pile_work_state_and_direction & 0xff);
|
||
if(pile_work_dir != PILE_DIR_DOWN)
|
||
{
|
||
// 方向改变，更新目标采样深度
|
||
depth_data->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_DOWN);
|
||
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)
|
||
{
|
||
// ESP_LOGI(TAG, "enc_value < last_enc_value\n");
|
||
uint8_t pile_work_dir = (depth_data->pile_work_state_and_direction & 0xff);
|
||
if (pile_work_dir != PILE_DIR_UP)
|
||
{
|
||
// 方向改变
|
||
depth_data->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_UP);
|
||
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->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_NONE);
|
||
}
|
||
|
||
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 if((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_PAUSE)
|
||
{
|
||
|
||
}
|
||
else if((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_STOP)
|
||
{
|
||
work_start_time = 0;
|
||
}
|
||
last_work_state = pile_work_state;
|
||
prev_depth = depth_data->depth;
|
||
prev_update_time = enc_update_time;
|
||
last_enc_value = enc_value;
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
||
// 500ms计算一次速度
|
||
if (++speed_calc_count >= 5)
|
||
{
|
||
calculate_depth_speed(enc1_value);
|
||
speed_calc_count = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
void add_recod_item(void)
|
||
{
|
||
// 每10cm计算一次
|
||
record->count = depth_data->sample_count;
|
||
ESP_LOGI(TAG, "add_recod_item:id:%d count:%d", record->pile_id, record->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 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;
|
||
flow_zero();
|
||
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->pile_work_state_and_direction = PILE_STATE_STOP | PILE_DIR_NONE; // 默认不工作
|
||
}
|
||
|
||
void DEPTH_init(void)
|
||
{
|
||
pcnt_rotary_encoder_init();
|
||
capture_depth_init();
|
||
|
||
xTaskCreate(depth_task, "depth_task", 4096, NULL, 10, NULL);
|
||
}
|