rgv-kinco/applications/wcs/wcs.c

/*******************************************************************************************
 * @file wcs.c
 *
 * @brief wcs协议解析
 *
 *               (c) Copyright 2021, Shandong Huali electromechanical Co., Ltd..
 *             This is protected by international copyright laws. Knowledge of the
 *             source code may not be used to write a similar product. This file may
 *             only be used in accordance with a license and should not be redistributed
 *             in any way. We appreciate your understanding and fairness.
 *
 *
 * @author      Simon
 * @date        Created: 2021.06.16-T18:02:42+0800
 *
*******************************************************************************************/
#include "rtthread.h"
#include "board.h"
#include "lwip/netdb.h"
#include "wcs.h"

#define DBG_TAG                "wcs"
#define DBG_LVL           DBG_INFO
#include <rtdbg.h>

enum
{
    DEV_TYPE_NONE,
    DEV_TYPE_SHUTTLE,
    DEV_TYPE_PALLET,
    DEV_TYPE_OTHER,
};

typedef struct
{
    uint16_t tag;
    uint16_t msg_len;
    uint8_t dev_type;
    uint8_t dev_no;
    uint8_t mode;
    uint8_t map_ver;
    uint8_t task_no;
    uint8_t task_type;
}wcs_frame_head_t;

typedef struct
{
    uint16_t msg_len;
    uint16_t crc;
    uint16_t tag;
}wcs_frame_tail_t;

typedef struct __attribute__((__packed__))
{
    uint8_t seg_no;
    wcs_point_t point[1];
}wcs_seg_t;

typedef struct __attribute__((__packed__))
{
    uint8_t seg_no;
    uint8_t cmd_no;
    uint8_t cmd;
    uint32_t cmd_param;
    uint8_t area_no;
    uint16_t auth;
}wcs_cmd_req_t;

typedef struct __attribute__((__packed__))
{
    uint8_t x;
    uint8_t y;
    uint8_t z;
//	uint8_t action;
}wcs_location_t;

typedef struct __attribute__((__packed__))
{
    uint8_t task_result;
    uint8_t cmd_no;
    uint8_t cmd_exe_result;
    uint32_t cmd_exe_param;
    wcs_location_t location;
    uint8_t seg_no;
    uint16_t seg_target;
    uint32_t barcode;
    uint8_t car_status;
    uint8_t pallet_status;
    uint8_t reserve_status;
    uint8_t dir;
    uint8_t vol;
    uint8_t warning;
    uint8_t res[3];
}wcs_ack_t;

const static uint16_t wcs_polynom = 0xA001;
static uint8_t wcs_clear_ack = 0;

void ll_get_point(wcs_point_t *point);
uint8_t ll_get_seg(void);
uint16_t ll_get_target(void);
uint8_t ll_get_car_status(void);
uint8_t ll_get_dtc(void);
uint8_t ll_get_pallet(void);
uint8_t ll_get_battery(void);
uint8_t ll_get_reserve(void);
uint8_t ll_get_dir(void);

int cmd_work_result(uint8_t *cmd_no);
int cmd_parser(uint8_t cmd_no, uint8_t cmd, uint32_t *param);
void cmd_set_clear_indication(void (*clear_func)(void));

int sch_trajectory(uint8_t task_id, uint8_t seg_no, wcs_point_t *point);
int sch_target(uint8_t task_id, uint8_t seg_no);
int sch_get_status(void);
void be_send(void *buf, int sz);

static uint16_t wcs_crc16(uint8_t *ptr, uint16_t len)
{
	uint8_t i;
	uint16_t crc = 0xffff;

	if (len == 0) {
		len = 1;
	}
	while (len--) {
		crc ^= *ptr;
		for (i = 0; i<8; i++)
		{
			if (crc & 1) {
				crc >>= 1;
				crc ^= wcs_polynom;
			}
			else {
				crc >>= 1;
			}
		}
		ptr++;
	}
	return(crc);
}

static __inline wcs_frame_tail_t *wcs_get_tail(void *buf, int sz)
{
    uint8_t *pbuf = buf;
    return (wcs_frame_tail_t *)&pbuf[sz - sizeof(wcs_frame_tail_t)];
}

static __inline uint8_t *wcs_get_body(void *buf, int sz)
{
    uint8_t *pbuf = buf;
    return &pbuf[sizeof(wcs_frame_head_t)];
}

static void wcs_ack_status(wcs_ack_t *pack)
{
    ll_get_point((wcs_point_t *)&pack->location);
    pack->seg_no = ll_get_seg();
    pack->seg_target = htons(ll_get_target());
    pack->car_status = ll_get_car_status();
    pack->warning = ll_get_dtc();
    pack->pallet_status = ll_get_pallet();
    pack->reserve_status = ll_get_reserve();
    pack->dir = ll_get_dir();
    pack->vol = ll_get_battery();
//    LOG_D("ack");
}

static void wcs_ack_clear(void)
{
    wcs_clear_ack = 1;
}

static void wcs_ack(uint8_t result, wcs_frame_head_t *head, wcs_cmd_req_t *cmd)
{
    static uint8_t buf[sizeof(wcs_ack_t) + sizeof(wcs_frame_head_t) + sizeof(wcs_frame_tail_t)] = {0};
    wcs_frame_tail_t *ptail = wcs_get_tail(buf, sizeof(buf));
    wcs_ack_t *pack = (wcs_ack_t *)wcs_get_body(buf, sizeof(buf));
    wcs_frame_head_t *phead = (wcs_frame_head_t *)buf;

    if(wcs_clear_ack)
    {
        wcs_clear_ack = 0;
        memset(buf, 0, sizeof(buf));
    }
	/* 填充发送信息 */
    phead->tag = htons(0x02fd);
    phead->msg_len = htons(sizeof(buf));
    phead->map_ver = 0;
    if((cmd && cmd->cmd_no) || head->task_no)
    {
        memcpy(&phead->dev_type, &head->dev_type, sizeof(wcs_frame_head_t) - 4);
    }
    if(head->task_no)
    {
        phead->task_type = head->task_type;
        pack->task_result = result;
        pack->cmd_no = 0;
        pack->cmd_exe_result = 0;
        pack->cmd_exe_param = 0;
    }
    else if(cmd)
    {
        pack->task_result = 0;
        pack->cmd_no = cmd->cmd_no;
        pack->cmd_exe_result = result;
        pack->cmd_exe_param = cmd->cmd_param;   /* 延时指令不会返回参数 */
    }
    /* 任务执行完毕 */
    else if(phead->task_no && phead->task_type == 2)
    {
        if(ll_get_car_status() == 3)
        {
            pack->task_result = 0;
        }
    }
    else
    {
        int rc;
        uint8_t cmd_exe_no = 0;

        rc = cmd_work_result(&cmd_exe_no);
        if(rc >= 0)
        {
            pack->cmd_no = cmd_exe_no;
            pack->cmd_exe_result = rc;
        }
    }

    wcs_ack_status(pack);

    ptail->tag = htons(0x03fc);
    ptail->msg_len = htons(sizeof(buf));
    ptail->crc = wcs_crc16(buf, sizeof(buf) -4);

    be_send(buf, sizeof(buf));
    if((cmd && cmd->cmd_no) || head->task_no)
        LOG_HEX(DBG_TAG, 16, buf, sizeof(buf));
}
/****************************************
 *        wcs帧解析   
 *函数功能 : 
 *参数描述 : 无
 *返回值   : 无
 ****************************************/
static int wcs_frame_parser(void *buf, int sz)
{
    wcs_frame_head_t *phead = (wcs_frame_head_t *)buf;
    wcs_frame_tail_t *ptail = wcs_get_tail(buf, sz);
    int rc = 0;
    if(phead->msg_len == ptail->msg_len && ntohs(phead->msg_len) == sz)	/* 长度一致 */
    {
//        LOG_D("recv frame");
        uint16_t cal_crc = wcs_crc16(buf, sz -4);
        if(ptail->crc == cal_crc)	/* 校验通过 */
        {
//            LOG_D("check ok");
            if(phead->dev_type == DEV_TYPE_SHUTTLE)	/* 对象四向车 */
            {
                if(phead->task_no)	/* 任务编号 */
                {
                    LOG_I("task id[%u]", phead->task_no);
                    LOG_HEX(DBG_TAG, 16, buf, sz);
                    if(phead->task_type == 1)	/* 任务类型 */
                    {
                        if(ll_get_car_status() != 3)
                        {
                            rc = 0;
                        }
                        else	//就绪，就获取身体信息
                        {
                            uint8_t seg_cnt;
                            wcs_seg_t *ptask = (wcs_seg_t *)wcs_get_body(buf, sz);
                            //LOG_I("add trajectory");
                            seg_cnt = (sz - sizeof(wcs_frame_head_t) - sizeof(wcs_cmd_req_t) - sizeof(wcs_frame_tail_t)) / sizeof(wcs_point_t);
                            rc = sch_trajectory(phead->task_no, seg_cnt, ptask->point);
                        }
                        wcs_ack(rc? rc: 1, phead, NULL);	/* 回应信息 */
                    }
                    else if(phead->task_type == 2)	//任务类型：1：路径中各段节点； 2：RES能行驶的分段动作序号

                    {
                        if(ll_get_car_status() != 11)	//11：节点待命
                        {
                            rc = 0;
                        }
                        else
                        {
                            wcs_seg_t *ptask = (wcs_seg_t *)wcs_get_body(buf, sz);
                            LOG_I("schedule task");
                            if(cmd_work_result(NULL) < 0)
                            {
                                rc = sch_target(phead->task_no, ptask->seg_no);
                            }
                            else
                            {
                                rc = ERR_C_RES_TASK_DOING;
                            }
                        }
                        wcs_ack(rc? rc: 1, phead, NULL);
                    }
                }
                else
                {
                    wcs_cmd_req_t *pcmd = (wcs_cmd_req_t *)wcs_get_body(buf, sz);
                    if(pcmd->cmd_no || pcmd->cmd)
                    {
                        LOG_I("commands[%#x]", pcmd->cmd);
                        LOG_HEX(DBG_TAG, 16, buf, sz);
                        rc = cmd_parser(pcmd->cmd_no, pcmd->cmd, (uint32_t *)&pcmd->cmd_param);
                        wcs_ack(rc, phead, pcmd);
                    }
                    else
                        wcs_ack(rc, phead, NULL);
                }
            }
        }
    }
    return 0;
}


static int wcs_init(void)
{
    extern void be_set_parser(int (*parser_fun)(void *, int));
    be_set_parser(wcs_frame_parser);
    cmd_set_clear_indication(wcs_ack_clear);
    return 0;
}
INIT_APP_EXPORT(wcs_init);