CablePositioningV1.0/CablePositioning_APP_V1.0/Core/Src/position.c

/******************************************************************************
 * file    Core/Src/position.c 
 * author  Changjiale
 * version 1.0.0
 * date    18-Sep-2023
 * brief   This file provides all the position related operation functions.
 ******************************************************************************
 * Attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2023 LandPower</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of WTOE nor the names of its contributors may be used to 
 *      endorse or promote products derived from this software without specific
 *      prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "stdio.h"
#include "string.h"

#include "usart.h"
#include "dev_config.h"
#include "ADC_collect.h"
#include "position.h"
#include "flash_if.h"

/* Private define ------------------------------------------------------------*/
#define POSITION_THRESHOLD_MIN 5        //5mv
#define POSITION_THRESHOLD_MAX 995      //995mv
#define POSITION_RX_TIMEOUT 500         //500ms
#define POSITION_TX_DELAY 100        //两条命令之间需要间隔50ms以上
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
position_buf_list_t position_buf_list_rx;
position_ctrl_t position_ctrl;
static position_data_t position_data;
static _position_ctrl_t _position_ctrl;
UART_HandleTypeDef *position_usart = &huart1;      //定位模块使用串口

/* Private function prototypes -----------------------------------------------*/
/* Internal functions --------------------------------------------------------*/
/*发送命令消息函数*/
static HAL_StatusTypeDef _position_transmit(uint8_t *data, uint16_t len)
{
    DBG(DBG_M_POSITION, "Send(%d:%d):\r\n", len, HAL_GetTick());
    if (dbg_stat_get(DBG_M_POSITION))
    {
        buf_print(data, POSITION_MIN(len, 32));
        vty_print("\r\n");
    }

    /* 开启串口空闲接收 */
    HAL_UARTEx_ReceiveToIdle_DMA(position_usart, position_ctrl.dma_rx_buf, POSITION_CMD_BUF_LEN);
    osDelay(POSITION_TX_DELAY);
    /* 打开485Tx引脚 */
    HAL_GPIO_WritePin(RS485_A_DE_GPIO_Port, RS485_A_DE_Pin, GPIO_PIN_SET);
    return HAL_UART_Transmit_DMA(position_usart, data, len);
}

/* 检查校验值 */
static uint8_t _position_pkt_chk(void)
{
    uint16_t chk_len = _position_ctrl.cmd_buf_index - 2;     //数据长度
    uint8_t *buf = _position_ctrl.cmd_receive_buf + 1;
    uint8_t chk = 0;
    uint16_t i = 0;

    /* 设备进入复位状态后不再处理报文. */
    if (IS_MONITOR_BIT_SET(system_init_flag, SYS_INIT_RESET))
    {
        vty_print("@0\r\n");
        /* 这里挂住线程是因为重启时如果有收包, 会导致收包出错, 从而让设备进入休眠状态. */
        osDelay(portMAX_DELAY);
        return FALSE;
    }

    if (buf[3] != _position_ctrl.pkt_id)
    {
        vty_print("POSITION ID: %d %d\r\n", buf[3], _position_ctrl.pkt_id);
        return FALSE;
    }

    for (i = 0; i < chk_len; i++)
    {
        chk += buf[i];
    }

    if (chk == buf[chk_len])
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

/* 收包函数 */
static HAL_StatusTypeDef _position_pkt_wait(void)
{
    uint8_t *buf = NULL;
    uint8_t *pkt_head = &_position_ctrl.cmd_receive_buf[0];
    uint16_t len = 0;
    uint16_t *pkt_len = (uint16_t*)(&_position_ctrl.cmd_receive_buf[1]);
    uint32_t notify_value = 0;
    uint32_t time = HAL_GetTick();
    position_buf_list_t *buf_l = &position_buf_list_rx;

    while(1)
    {
        /* 收包超时. */
        if ((HAL_GetTick() - time) > POSITION_RX_TIMEOUT)
        {
            return HAL_TIMEOUT;
        }

        /* 等待 DMA 空闲收包中断. */
        notify_value = ulTaskNotifyTake(pdTRUE, POSITION_RX_TIMEOUT);
        if (0 == notify_value)
        {
            continue;
        }

        /* 遍历所有有效的 buf. */
        while(buf_l->current != buf_l->valid)
        {
            buf = buf_l->buf[buf_l->current];
            len = buf_l->buf_cnt[buf_l->current];

            /* 更新 buf index. */
            buf_l->current++;
            if (POSITION_BUF_LIST_LEN == buf_l->current)
            {
                buf_l->current = 0;
            }

            /* 收包总长大于报文长度 */
            if (_position_ctrl.cmd_buf_index >= 3 && (_position_ctrl.cmd_buf_index + len) > (*pkt_len + 3))
            {
                _position_ctrl.cmd_buf_index = 0;
                _position_ctrl.pkt_state = POSITION_PKT_NONE;

                /* 如果收包状态为完成, 舍弃之前的包后接收最新的包 */
                if (POSITION_PKT_COMPLETE != _position_ctrl.pkt_state)
                {
                    continue;
                }
            }

            /* 如果收包大于协议报文长度, 认为报文错误. */
            if (_position_ctrl.cmd_buf_index + len > POSITION_CMD_BUF_LEN)
            {
                _position_ctrl.cmd_buf_index = 0;
                _position_ctrl.pkt_state = POSITION_PKT_NONE;
                continue;
            }

            memcpy(&_position_ctrl.cmd_receive_buf[_position_ctrl.cmd_buf_index], buf, len);
            _position_ctrl.cmd_buf_index += len;

            if (_position_ctrl.cmd_buf_index < 3)
            {
                continue;
            }

            if (POSITION_PKT_NONE == _position_ctrl.pkt_state)
            {
                if (*pkt_head != 0xa5 || (*pkt_len + 3 > POSITION_CMD_BUF_LEN))
                {
                    _position_ctrl.cmd_buf_index = 0;
                    continue;
                }

                if (_position_ctrl.cmd_buf_index < *pkt_len + 3)
                {
                    _position_ctrl.pkt_state = POSITION_PKT_HALF;
                    continue;
                }
            }
            else if (POSITION_PKT_HALF == _position_ctrl.pkt_state)
            {
                if (_position_ctrl.cmd_buf_index < *pkt_len + 3)
                {
                    continue;
                }
            }

            if (_position_pkt_chk())
            {
                _position_ctrl.pkt_state = POSITION_PKT_COMPLETE;
            }
            else
            {
                _position_ctrl.cmd_buf_index = 0;
                _position_ctrl.pkt_state = POSITION_PKT_NONE;
            }
        }

        if (POSITION_PKT_COMPLETE == _position_ctrl.pkt_state)
        {
            DBG(DBG_M_POSITION, "Recv(%d):\r\n", _position_ctrl.cmd_buf_index);
            if (dbg_stat_get(DBG_M_POSITION))
            {
                buf_print(_position_ctrl.cmd_receive_buf, POSITION_MIN(_position_ctrl.cmd_buf_index, 32));
                vty_print("\r\n");
            }
            _position_ctrl.cmd_buf_index = 0;
            _position_ctrl.pkt_state = POSITION_PKT_NONE;
            return HAL_OK;
        }
    }
}

/* 填充报文 */
static void _position_packed(uint8_t cmd, uint16_t len)
{
    uint8_t *buf = _position_ctrl.cmd_send_buf;
    uint16_t pkt_len = len - 3;
    uint8_t *chk = buf + pkt_len + 2;
    uint8_t i = 0;

    buf[0] = 0xa5;                      // 报文头.
    *(uint16_t*)(&buf[1]) = pkt_len;    // 报文长度.
    buf[3] = 0;                         // 设备 id, 单板为 0.
    buf[4] = _position_ctrl.pkt_id;     // 报文序列号.
    buf[5] = cmd;                       // 报文命令字.

    /* 填充校验值 */
    *chk = 0;
    for (i = 1; i <= pkt_len + 1; i++)
    {
        *chk += buf[i];
    }
}

/* 命令0的收包处理函数 */
static HAL_StatusTypeDef _position_pkt_state(void)
{
    uint8_t *buf = _position_ctrl.cmd_receive_buf;
    uint8_t *data = buf + POSITION_HEAD_LEN;

    if (buf[1] != 0x29
     || buf[2] != 0
     || buf[5] != POSITION_STATE)
    {
        return HAL_ERROR;
    }
    
    /* 新行波数据 */
    if (IS_MONITOR_BIT_SET(*data, 2) || IS_MONITOR_BIT_SET(*data, 1))
    {
        position_ctrl.new = TRUE;
    }

    data++;
    position_data.vol = *(uint16_t*)data;	//设备电压
    data += 2;
    position_data.temperature = *(uint16_t*)data;	//设备温度
    
    _position_ctrl.pkt_id++;
    return HAL_OK;
}

/* 命令1的收包处理函数 */
static HAL_StatusTypeDef _position_pkt_cfg(void)
{
    uint8_t *buf = _position_ctrl.cmd_receive_buf;

    if (buf[1] != 0x05
     || buf[2] != 0 
     || buf[5] != POSITION_CFG 
     || buf[6] != 0)
    {
        return HAL_ERROR;
    }
    
    _position_ctrl.pkt_id++;
    return HAL_OK;
}

/* 命令2的收包处理函数 */
static HAL_StatusTypeDef _position_pkt_query(void)
{
    uint8_t *buf = _position_ctrl.cmd_receive_buf;
    _position_data_t *data = (_position_data_t *)(buf + POSITION_HEAD_LEN);

    if (buf[1] != 0x34
     || buf[2] != 0
     || buf[5] != POSITION_QUERY)
    {
        return HAL_ERROR;
    }

    /* 新行波数据 */
	if (position_data.wave_info.file_s != data->file_s || position_data.wave_info.file_ns != data->file_ns)
    {
        for (uint8_t i = 0; i < POSITION_SUM; i++)
        {
            if (((data->wave_info[i].s == data->file_s) && (data->wave_info[i].ns >= data->file_ns))
                  || (data->wave_info[i].s > data->file_s))
            {
                position_data.wave_info.v[i] = data->wave_info[i].v;
                position_data.wave_info.s[i] = data->wave_info[i].s;
                position_data.wave_info.ns[i] = data->wave_info[i].ns;
            }
            else
            {
                position_data.wave_info.v[i] = 0.0;
                position_data.wave_info.s[i] = 0;
                position_data.wave_info.ns[i] = 0;
            }
        }
        position_data.wave_info.file_s = data->file_s;
        position_data.wave_info.file_ns = data->file_ns;
        position_data.wave_info.file_feq = data->file_feq;
		/* 写 FLASH */
        position_write(&position_data);

        /* 写日志 */
		flash_log_write(FLASH_LOG_TYPE_INFO, "\r%f %d %d\r %f %d %d\r %f %d %d\r %d %d %d\r\n",
            position_data.wave_info.v[0], position_data.wave_info.s[0], position_data.wave_info.ns[0],
            position_data.wave_info.v[1], position_data.wave_info.s[1], position_data.wave_info.ns[1],
            position_data.wave_info.v[2], position_data.wave_info.s[2], position_data.wave_info.ns[2],
            position_data.wave_info.file_s, position_data.wave_info.file_ns, position_data.wave_info.file_feq);
    }

    _position_ctrl.pkt_id++;
    return HAL_OK;
}

/* 命令3的收包处理函数 */
static int16_t _position_pkt_wave(uint8_t *data)
{
    uint8_t *buf = _position_ctrl.cmd_receive_buf;
    uint8_t *wave_data = buf + POSITION_HEAD_LEN;
    uint16_t pkt_len = 0;

    if (buf[5] != POSITION_WAVE)
    {
        return -1;
    }

    pkt_len = *(uint16_t*)(&_position_ctrl.cmd_receive_buf[1]);
    _position_ctrl.pkt_id++;
    memcpy(data, wave_data, pkt_len - 4);
    return pkt_len - 4;
}

/* 发送命令0函数 */
static HAL_StatusTypeDef _position_state(void)
{
    uint8_t *buf = _position_ctrl.cmd_send_buf;
    _position_packed(POSITION_STATE, POSITION_STATE_LEN);

    if (_position_transmit(buf, POSITION_STATE_LEN) != HAL_OK)
    {
        return HAL_ERROR;
    }

    if (_position_pkt_wait() == HAL_OK)
    {
        if (_position_pkt_state() != HAL_OK)
        {
            return HAL_ERROR;
        }
    }
    else
    {
        return HAL_TIMEOUT;
    }

    return HAL_OK;
}

/* 发送命令1函数 */
static HAL_StatusTypeDef _position_cfg(void)
{
    uint8_t *buf = _position_ctrl.cmd_send_buf;

    uint8_t *data = buf + POSITION_HEAD_LEN;
    float threshold = 0;
    if (dev_config.wave_threshold < POSITION_THRESHOLD_MIN)
    {
        threshold = POSITION_THRESHOLD_MIN;
    }
    else if (dev_config.wave_threshold > POSITION_THRESHOLD_MAX)
    {
        threshold = POSITION_THRESHOLD_MAX;
    }
    else
    {
        threshold = (float)dev_config.wave_threshold;
    }
    memcpy(data, &threshold, sizeof(float));

    data += 4;
    *(uint32_t*)data = 0;

    _position_packed(POSITION_CFG, POSITION_CFG_LEN);

    if (_position_transmit(buf, POSITION_CFG_LEN) != HAL_OK)
    {
        return HAL_ERROR;
    }

    if (_position_pkt_wait() == HAL_OK)
    {
        if (_position_pkt_cfg() != HAL_OK)
        {
            return HAL_ERROR;
        }
    }
    else
    {
        return HAL_TIMEOUT;
    }

    return HAL_OK;
}

/* 发送命令2函数 */
static HAL_StatusTypeDef _position_query(void)
{
    uint8_t *buf = _position_ctrl.cmd_send_buf;
    _position_packed(POSITION_QUERY, POSITION_QUERY_LEN);

    if (_position_transmit(buf, POSITION_QUERY_LEN) != HAL_OK)
    {
        return HAL_ERROR;
    }

    if (_position_pkt_wait() == HAL_OK)
    {
        if (_position_pkt_query() != HAL_OK)
        {
            return HAL_ERROR;
        }
    }
    else
    {
        return HAL_TIMEOUT;
    }

    return HAL_OK;
}

static void _position_err(void)
{
    if (position_ctrl.position_wave_up)
    {
        position_ctrl.position_wave_up = FALSE;
    }
    else if (position_ctrl.cfg)
    {
        position_ctrl.cfg = FALSE;
    }
    else if (position_ctrl.new)
    {
        position_ctrl.new = FALSE;
    }
    /* 串口通讯错误 */
    else
    {
        position_ctrl.offline = TRUE;
    }
    position_ctrl.send_cnt = 0;
    position_ctrl.err_cnt++;
}

/* Interface functions -------------------------------------------------------*/
/* 故障定位轮询函数 */
HAL_StatusTypeDef position_start(void)
{
    HAL_StatusTypeDef rv = HAL_OK;

    while(1)
    {
        if (position_ctrl.send_cnt > POSITION_ERR_MAX)
        {
            _position_err();
            break;
        }

        if (position_ctrl.cfg)
        {
            rv = _position_cfg();
            if (HAL_OK == rv)
            {
                position_ctrl.cfg = FALSE;
            }
            else
            {
                position_ctrl.send_cnt++;
                continue;
            }
        }

        rv = _position_state();
        if (HAL_OK != rv)
        {
            position_ctrl.send_cnt++;
            continue;
        }

        if (position_ctrl.new)
        {
            rv = _position_query();
            if (HAL_OK == rv)
            {
                position_ctrl.new = FALSE;
            }
            else
            {
                position_ctrl.send_cnt++;
                continue;
            }
        }
        else
        {
            break;
        }
    }

    position_ctrl.complete = TRUE;

    if (HAL_OK == rv)
    {
        position_ctrl.offline = FALSE;
        position_ctrl.send_cnt = 0;
        position_ctrl.err_cnt = 0;
        return HAL_OK;
    }
    else
    {
        return HAL_ERROR;
    }
}

/* 发送命令3函数 */
HAL_StatusTypeDef position_wave(uint8_t *wl_data, uint16_t index, uint16_t *data_len, uint32_t file_s, uint32_t file_ns)
{
    if (position_ctrl.send_cnt > POSITION_ERR_MAX)
    {
        _position_err();
        return HAL_ERROR;
    }

    uint8_t *buf = _position_ctrl.cmd_send_buf;
    int16_t ret = 0;

    uint8_t *data = buf + POSITION_HEAD_LEN;
    *(uint32_t*)data = file_s;
    data += 4;
    *(uint32_t*)data = file_ns;
    data += 4;
    *(uint32_t*)data = index * DEBUG_DATA_SIZE;

    _position_packed(POSITION_WAVE, POSITION_WAVE_LEN);

    if (_position_transmit(buf, POSITION_WAVE_LEN) != HAL_OK)
    {
        return HAL_ERROR;
    }
    if (_position_pkt_wait() == HAL_OK)
    {
        ret = _position_pkt_wave(wl_data);
        if (ret != -1)
        {
            *data_len = ret;
            return HAL_OK;
        }
        else
        {
            return HAL_BUSY;
        }
    }
    else
    {
        return HAL_TIMEOUT;
    }
}

/* 故障定位初始化 */
void position_init(void)
{
    position_ctrl.complete = FALSE;
    position_ctrl.cfg = TRUE;
    if (dev_config.wave_threshold == 0)
    {
        dev_config.wave_threshold = POSITION_THRESHOLD_MAX;
    }

    position_read(position_id, &position_data);
    
    /* 重新初始化串口波特率为115200 */
    HAL_UARTEx_ReceiveToIdle_DMA(position_usart, position_ctrl.dma_rx_buf, POSITION_CMD_BUF_LEN);
    MX_USART1_UART_ReInit();
    /* 开启485芯片电源 */
    HAL_GPIO_WritePin(SENSOR_EN_GPIO_Port, SENSOR_EN_Pin, GPIO_PIN_SET);
}