101 lines
2.7 KiB
C
101 lines
2.7 KiB
C
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "esp_system.h"
|
|
#include "esp_log.h"
|
|
|
|
#include "pile_id.h"
|
|
#include "config.h"
|
|
#include "modbusS.h"
|
|
#include "flow.h"
|
|
#include "depth.h"
|
|
#include "../communication_pad/inc/communication_pad.h"
|
|
#include "ads1220.h"
|
|
|
|
#define TAG "pile_id"
|
|
|
|
uint16_t *cur_pile_id = &gWordVar[LAST_PILE_ID_ADDR];
|
|
gps_message_t *gps_message = (gps_message_t *)&gWordVar[REG_GPS_MESSAGE];
|
|
double gps_xy[2];
|
|
int none_flag = 1;
|
|
|
|
extern move_t *pMoveCtx;
|
|
extern int depth_over_inc_pile_flag;
|
|
|
|
static double double_abs(double a, double b)
|
|
{
|
|
if (a > b) return a - b;
|
|
else return b - a;
|
|
}
|
|
|
|
int gps_distance_check(void)
|
|
{
|
|
if (gps_message->gps_status != 4) // gps信号不是精准定位则不启用
|
|
{
|
|
none_flag = 1;
|
|
return 0;
|
|
}
|
|
if (none_flag) return 0;
|
|
if (double_abs(gps_message->x, gps_xy[0]) > GPS_DISTANCE_TH) return 1;
|
|
if (double_abs(gps_message->y, gps_xy[1]) > GPS_DISTANCE_TH) return 1;
|
|
return 0;
|
|
}
|
|
|
|
void inc_pile_fun(void)
|
|
{
|
|
ESP_LOGI(TAG, "inc pile");
|
|
// 增加桩号
|
|
(*cur_pile_id)++;
|
|
save_pile_id();
|
|
// 更新判断的标志数据
|
|
depth_over_inc_pile_flag = 0;
|
|
pMoveCtx->work_time = 0;
|
|
pMoveCtx->work_stop_state_flag = 0;
|
|
gps_xy[0] = gps_message->x;
|
|
gps_xy[1] = gps_message->y;
|
|
none_flag = 0;
|
|
// 清空数据
|
|
zero_flow1();
|
|
zero_flow2();
|
|
zero_depth();
|
|
}
|
|
|
|
// 轮询检查是否换桩
|
|
void pile_id_check_task(void)
|
|
{
|
|
static uint16_t last_pile_work_state = PILE_STATE_STOP;
|
|
while (1)
|
|
{
|
|
// 自动更新工作状态
|
|
if (last_pile_work_state == PILE_STATE_STOP) // 判断是否开始
|
|
{
|
|
if (gWordVar[AC_CURRENT_REG_ADDR + 0] > CURRENT0_TH && depth_over_inc_pile_flag == 1) // 钻头电流开 && 深度超过阈值
|
|
{
|
|
depth_data->pile_work_state_and_direction = (depth_data->pile_work_state_and_direction & 0XFF) | PILE_STATE_WORK;
|
|
}
|
|
}
|
|
|
|
if (last_pile_work_state == PILE_STATE_WORK) // 判断是否结束
|
|
{
|
|
if (pMoveCtx->work_stop_state_flag == 1 || gps_distance_check())
|
|
{
|
|
depth_data->pile_work_state_and_direction = (depth_data->pile_work_state_and_direction & 0XFF) | PILE_STATE_STOP;
|
|
}
|
|
}
|
|
|
|
|
|
// 开始则换桩
|
|
if ((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_WORK && last_pile_work_state == PILE_STATE_STOP)
|
|
{
|
|
inc_pile_fun();
|
|
}
|
|
last_pile_work_state = depth_data->pile_work_state_and_direction & 0xff00;
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
}
|
|
}
|
|
|
|
void pile_id_init(void)
|
|
{
|
|
xTaskCreate(pile_id_check_task, "ads1220_task", 4096, NULL, 10, NULL);
|
|
}
|