rgv-kinco/applications/screen.c

#include "rtthread.h"
#include "stdio.h"
#include "screen.h"
#include "stdio.h"
#include "env.h"

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

#define SCREEN_TARGET_LEN   8
#define TARGET_ID           7

#define SCREEN_DATA_LEN   8
#define SWITCH_ID         5
#define ICON_CONTROL      7

#define SCREEN_TYPE_ID_START        1
#define SCREEN_TYPE_ID_STOP         2
#define SCREEN_TYPE_ID_HIGH_SPEED   3
#define SCREEN_TYPE_ID_MOD_SPEED    4
#define SCREEN_TYPE_ID_LOW_SPEED    5
#define SCREEN_TYPE_ID_UP           6
#define SCREEN_TYPE_ID_DOWN         7
#define SCREEN_TYPE_KEY_STOP        0


typedef struct
{
    rt_device_t device;

    int status;
    int msg_sz_idx;

    char *recv_buffer;
    rt_size_t recv_bufsz;
    rt_size_t cur_recv_len;
    rt_sem_t rx_notice;
}Screen_Parser_t;

uint16_t SCREEN_RecvStatus = SCREEN_STATUS_INIT;
uint8_t ScreenRecvData[SCREEN_RECV_DATA_LEN];
int ScreenRecvIdx = 0;
int ScreenRecvLen = 7;

uint8_t INI[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0x20, 0xCE, 0xB4, 0xC9, 0xE8, 0xD6, 0xC3, 0x20, 0x20};
uint8_t READY[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0x20, 0x20, 0xBE, 0xCD, 0xD0, 0xF7, 0x20, 0x20, 0x20};
uint8_t RUN[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0x20, 0x20, 0xD4, 0xCB, 0xD0, 0xD0, 0x20, 0x20, 0x20};
uint8_t STOP[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0x20, 0x20, 0xCD, 0xA3, 0xD6, 0xB9, 0x20, 0x20, 0x20};
uint8_t ESTOP[16]= {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0x20, 0x20, 0xBC, 0xB1, 0xCD, 0xA3, 0x20, 0x20, 0x20};
uint8_t DRIVER_ERROR    [16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0xC7, 0xFD, 0xB6, 0xAF, 0xC6, 0xF7, 0xB4, 0xED, 0xCE, 0xF3};
uint8_t TARGET_ST_ERROR[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xB5, 0xBC, 0xBA, 0xBD, 0xB4, 0xED, 0xCE, 0xF3, 0x20};
uint8_t TASK_ERROR[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xC8, 0xCE, 0xCE, 0xF1, 0xB4, 0xED, 0xCE, 0xF3, 0x20};
uint8_t REMOTE_MANUAL[16]= {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xCA, 0xD6, 0xB6, 0xAF, 0xBF, 0xD8, 0xD6, 0xC6, 0x20};
uint8_t STOP_OBS [16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xC7, 0xB0, 0xB7, 0xBD, 0xD5, 0xCF, 0xB0, 0xAD, 0x20};
uint8_t STOP_BUMPER[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xB7, 0xC0, 0xD7, 0xB2, 0xCD, 0xA3, 0xD6, 0xB9, 0x20};
uint8_t STOP_LOW_POWER[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xB5, 0xCD, 0xB5, 0xE7, 0xD1, 0xB9,0xCD, 0xA3, 0xD6, 0xB9};
uint8_t RUN_LOW_POWER[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xB5, 0xCD, 0xB5, 0xE7, 0xD1, 0xB9, 0xD4, 0xCB, 0xD0, 0xD0};
uint8_t RUN_OBS_LOW_SPEED[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xD5, 0xCF, 0xB0, 0xAD, 0xBC, 0xF5, 0xCB, 0xD9};
uint8_t FMGS_OFFLINE[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xC7, 0xB0, 0xB4, 0xC5, 0xB5, 0xBC, 0xCD, 0xD1, 0xCF, 0xDF};
uint8_t BMGS_OFFLINE[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xBA, 0xF3, 0xB4, 0xC5, 0xB5, 0xBC, 0xCD, 0xD1, 0xCF, 0xDF};
uint8_t LMGS_OFFLINE[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xD7, 0xF3, 0xB4, 0xC5, 0xB5, 0xBC, 0xCD, 0xD1, 0xCF, 0xDF};
uint8_t RMGS_OFFLINE[16] = {0x5A, 0xA5, 0x0D, 0x82, 0x40, 0x00, 0xD3, 0xD2, 0xB4, 0xC5, 0xB5, 0xBC, 0xCD, 0xD1, 0xCF, 0xDF};
uint8_t CARGO_FULL[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xC8, 0xA1, 0xBB, 0xF5, 0xCA, 0xA7, 0xB0, 0xDC, 0x31};
uint8_t CARGO_PICKUP_FAIL[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20,0xC8, 0xA1, 0xBB, 0xF5, 0xCA, 0xA7, 0xB0, 0xDC, 0x32};
uint8_t CARGO_RELEASE_FAIL[16] = {0x5A, 0XA5, 0x0D, 0x82, 0x40, 0x00, 0x20, 0xB7, 0xC5, 0xBB, 0xF5, 0xCA, 0xA7, 0xB0, 0xDC, 0x20};

static Screen_Parser_t Screen_Parser = {0};

rt_inline int Screen_Send(void *buf, int len)
{
    return rt_device_write(Screen_Parser.device, 0, buf, len);
}

rt_err_t Screen_RxInd(rt_device_t dev, rt_size_t size)
{
    if(Screen_Parser.rx_notice)
    {
        rt_sem_release(Screen_Parser.rx_notice);
        return RT_EOK;
    }
    return -RT_ENOMEM;
}

static rt_err_t Screen_Getchar(Screen_Parser_t *parser, char *ch, rt_int32_t timeout)
{
    rt_err_t result = RT_EOK;

    while (rt_device_read(parser->device, 0, ch, 1) == 0)
    {
        rt_sem_control(parser->rx_notice, RT_IPC_CMD_RESET, RT_NULL);

        result = rt_sem_take(parser->rx_notice, timeout);
        if (result != RT_EOK)
        {
            return result;
        }
    }

    return RT_EOK;
}

static int Screen_Readline(Screen_Parser_t *parser, uint32_t timeout)
{
    rt_size_t read_len = 0;
    char ch = 0;
    rt_bool_t is_full = RT_FALSE;
    rt_bool_t is_newline = RT_FALSE;
    rt_bool_t is_head = RT_FALSE;
    int frame_sz = 0;
    int rc = RT_EOK;

    rt_memset(parser->recv_buffer, 0x00, parser->recv_bufsz);
    parser->cur_recv_len = 0;

    while (parser->device)
    {
        rc = Screen_Getchar(parser, &ch, timeout);
        if(rc != RT_EOK)
        {
            rt_memset(parser->recv_buffer, 0x00, parser->recv_bufsz);
            parser->cur_recv_len = 0;
            return rc;
        }
        /* is newline */
        if(ch == 0x5a)
        {
            is_newline = RT_TRUE;
        }


        if(is_newline)
        {
            if (read_len < parser->recv_bufsz)
            {
                if(!is_head)
                {
                    if(read_len == 1 && ch == 0xa5)
                    {
                        is_head = RT_TRUE;
                    }
                }
                else
                {
                    if(read_len == 2)
                    {
                        frame_sz = ch + 3;;
                    }
                }
                parser->recv_buffer[read_len++] = ch;
                parser->cur_recv_len = read_len;
            }
            else
            {
                is_full = RT_TRUE;
                return -RT_EFULL;
            }
        }

        /* is end */
        if (read_len == frame_sz && is_head)
        {
            if (is_full)
            {
                LOG_E("read line failed. The line data length is out of buffer size(%d)!", parser->recv_bufsz);
                rt_memset(parser->recv_buffer, 0x00, parser->recv_bufsz);
                parser->cur_recv_len = 0;
                return -RT_EFULL;
            }
            break;
        }
    }

    return read_len;
}

static int Screen_Init(void)
{
    Screen_Parser.device = rt_device_find("uart4");
    Screen_Parser.rx_notice = rt_sem_create("screen", 0, RT_IPC_FLAG_FIFO);
    rt_device_set_rx_indicate(Screen_Parser.device, Screen_RxInd);
    if(Screen_Parser.device)
        rt_device_open(Screen_Parser.device, 0);
    return RT_EOK;
}
INIT_ENV_EXPORT(Screen_Init);

void Screen_SetTarget(uint16_t Target_Site)
{
    //5A A5 05 82 20 00 00 0F
    uint8_t Target[SCREEN_TARGET_LEN] = {0x5A, 0xA5, 0x05, 0x82, 0x20, 0x00, 0x00, 0x00};
    Target[TARGET_ID] = Target_Site;
    Screen_Send(Target, SCREEN_TARGET_LEN);
}

void Screen_SetCode(uint8_t Error_Status)
{
    switch(S.Status)
    {
    case STATUS_INIT:
    	Screen_Send(INI, 16);
		  break;
		case STATUS_READY:
			Screen_Send(READY, 16);
      break;
		case STATUS_RUN:
			Screen_Send(RUN, 16);
		  break;		
		case STATUS_ESTOP:
			Screen_Send(ESTOP, 16);
		  break;
		case STATUS_DRIVER_ERROR:
			Screen_Send(DRIVER_ERROR, 16);
		  break;
		case STATUS_TARGET_ST_ERROR:
			Screen_Send(TARGET_ST_ERROR, 16);
		  break;
		case STATUS_TASK_ERROR:
			Screen_Send(TASK_ERROR, 16);
		  break;
		case STATUS_REMOTE_MANUAL:
			Screen_Send(REMOTE_MANUAL, 16);
		  break;
		case STATUS_STOP_OBS:
			Screen_Send(STOP_OBS, 16);
		  break;
		case STATUS_STOP_BUMPER:
			Screen_Send(STOP_BUMPER, 16);
		  break;
		case STATUS_STOP_LOW_POWER:
			Screen_Send(STOP_LOW_POWER, 16);
		  break;	
		case STATUS_RUN_LOW_POWER:
			Screen_Send(RUN_LOW_POWER, 16);
		  break;
		case STATUS_RUN_OBS_LOW_SPEED:
			Screen_Send(RUN_OBS_LOW_SPEED, 16);
		  break;
		case STATUS_ERROR_FMGS_OFFLINE:
			Screen_Send(FMGS_OFFLINE, 16);
		  break;
		case STATUS_ERROR_BMGS_OFFLINE:
			Screen_Send(BMGS_OFFLINE, 16);
		  break;
		case STATUS_ERROR_LMGS_OFFLINE:
			Screen_Send(LMGS_OFFLINE, 16);
		  break;
		case STATUS_ERROR_RMGS_OFFLINE:
			Screen_Send(RMGS_OFFLINE, 16);
		  break;
		case STATUS_ERROR_CARGO_START_FULL:
			Screen_Send(CARGO_FULL, 16);
		 break;
		case STATUS_ERROR_CARGO_PICKUP_FAIL:
			Screen_Send(CARGO_PICKUP_FAIL, 16);
		  break;				
		case STATUS_ERROR_CARGO_RELEASE_FAIL:
		  Screen_Send(CARGO_RELEASE_FAIL, 16);
		   break;
    default:
    	Screen_Send(INI, 16);
    }
}

void Screen_Icon_Speed(uint8_t Speed_Status)
{
    uint8_t Screen_Speed_Btn[8] = {0x5A, 0xA5, 0x05, 0x82, 0x10, 0x01, 0x00, 0x00};
    Screen_Speed_Btn[7] = Speed_Status;
    Screen_Send(Screen_Speed_Btn, 8);
}

void Screen_Icon_Run(uint8_t Icon_Status)
{
    uint8_t Icon[8] = {0x5A, 0xA5, 0x05, 0x82, 0x10, 0x00, 0x00, 0x00};
    Icon[7] = Icon_Status;
    Screen_Send(Icon, 8);
}

void Screen_Icon_Lift(uint8_t Icon_Status)
{
    uint8_t Icon[8] = {0x5A, 0xA5, 0x05, 0x82, 0x50, 0x01, 0x00, 0x00};
    Icon[7] = Icon_Status;
    Screen_Send(Icon, 8);
//	LogHex(Icon,8);
}

void Screen_Icon_Battery(uint8_t Icon_Status)
{
    uint8_t Icon[8] = {0x5A, 0xA5, 0x05, 0x82, 0x50, 0x00, 0x00, 0x00};
    Icon[7] = Icon_Status;
    Screen_Send(Icon, 8);
//	LogHex(Icon, 8);
}

void Screen_Icon_Warn(uint8_t Icon_Status)
{
    uint8_t Icon[8] = {0x5A, 0xA5, 0x05, 0x82, 0x30, 0x01, 0x00, 0x00};
    Icon[7] = Icon_Status;
    Screen_Send(Icon, 8);
}

void Screen_SwitchPlay(uint8_t SwitchID, uint8_t icon)
{
    uint8_t Switch[SCREEN_DATA_LEN] = {0x5A, 0xA5, 0x05, 0x82, 0x10, 0x00, 0x00, 0x00};
    Switch[SWITCH_ID] = SwitchID;
    Switch[ICON_CONTROL] = icon;
    Screen_Send(Switch, SCREEN_DATA_LEN);
}

void Screen_SetBatteryStatus(uint8_t batteryLevel)
{
    static uint8_t Electricity[8] = {0x5A, 0xA5, 0x05, 0x82, 0x30, 0x02, 0x00, 0x00};
    Electricity[7] = batteryLevel;
    Screen_Send(Electricity, 8);
//		LogHex(Electricity, 8);
}