rgv-kinco/applications/nav/guide.c

/*
 *********************************************************************************************************
 *                                                xmk guide
 *                                          huali xmk guide process
 *
 *                                 (c) Copyright 2016-2020, hualijidian.com
 *                                           All Rights Reserved
 *
 * File    : guide.c
 * By      : eric
 * Version : V0.0.1
 *********************************************************************************************************
 */
#include "stdio.h"
#include "ctype.h"
#include "env.h"
#include "guide.h"
#include "cargo.h"
#include "screen.h"
#include "music.h"
#include "driver.h"
#include "apptask.h"
#include "lift.h"
#include "obs.h"
#include "hardware.h"
#include "demotask.h"
#include "dm_scaner.h"
#include "task_scheduler.h"
#include "systick.h"
#include "chassis_bus.h"

#include "bms.h"

#define DBG_SECTION_NAME  "guide"
#define DBG_LEVEL         DBG_INFO
#include <rtdbg.h>

enum{
  PUBLISHTASKSTA_CARRERYING,      //ִ����
  PUBLISHTASKSTA_FINISH,          //���
  PUBLISHTASKSTA_PAUSE            //��ͣ
};

/*
 *********************************************************************************************************
 *                                            GLOBAL VARIABLES
 *********************************************************************************************************
 */
/*�����ٶ�*/
uint32_t loopTime1s_Auto_Charge;
uint32_t Timer_Auto_Charge = 0;

/**
 * @brief ������
 * @param
 * @retval
 * @note
 */
rt_inline void _batteryProcess(void);
rt_inline void _Timer_Charge(void);
rt_inline void _Test_Print(void);
/**
 * @brief ����״̬���þ�ʾ��
 * @param
 * @retval
 */
//rt_inline uint8_t _collectStatus(void);
rt_inline uint8_t _navToStation(uint32_t station);
rt_inline uint8_t _prepare(void);
rt_inline void _run(void);
rt_inline void collectTaskInfo(void);
rt_inline bool _GDReady(void);
rt_inline bool _GDActionProcess(uint8_t action);
rt_inline void _GDAddTask(void);
rt_inline void _GDPublishTaskStatus(void);
rt_inline point_t station2Point(int station_xy,int station_z);


void GDCruise(void *param)
{
    uint32_t loopTime1s;

    while(1)
    {
        loopTime1s++;
//        _Adcc();	Bug0018
        if(S.DRStatus == DR_STATUS_INIT){
			if(S.Status != STATUS_ESTOP)
            S.Status = STATUS_INIT;	//
        }
        //CanSendProcess(CAN1);
        if((loopTime1s % 10) == 0)
        {
            /* ÿ��ִ��һ�� */
            _batteryProcess();	//电池进程
            //_Auto_Charge();
            //_Timer_Charge();
            //CscSendStatusToTsj();
            _GDAddTask();	//GD
            _GDPublishTaskStatus();	//打印任务状态
        }

        Dir_Lift_Process();		//顶升进程
        Cargo_Process();	//货物的进程
        Dir_Lift_LR_Timer();	//延时到位
        Dir_Lift_FB_Timer();	
        //_Test_Print();
        //Demo_Task();
        Screen_SetCode(S.Status);	
        /* ����״̬  */
        collectTaskInfo();	//收集任务信息
        switch(S.Status){
        case STATUS_INIT:
            DRSetAction(ACT_STOP);
            if(_prepare()){
                LOG_D("Status INIT-> READY");
				if(S.Status != STATUS_ESTOP)
                S.Status = STATUS_READY;
            }
            break;
        case STATUS_READY:
            if(Task_HasTransport()){
                LOG_D("Status STOP-> RUN");
				if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
                S.Status = STATUS_RUN;
                break;
            }
    //        DRSetAction(ACT_STOP);
            Screen_Icon_Run(Icon_Stop);
            Screen_Icon_Speed(Icon_Clear);
            break;
        case STATUS_RUN:	//状态是运行
            SetSpeed(Cfg.Speed);
            _run();
            Screen_Icon_Run(Icon_Run);
            Screen_Icon_Speed(Icon_Hign_Speed);
            Screen_Icon_Warn(Icon_Clear);
            break;
        case STATUS_STOP_OBS:
            Screen_Icon_Speed(Icon_Mid_Speed);
            Screen_Icon_Warn(Icon_Warn);
            _run();
            break;
        case STATUS_RUN_OBS_LOW_SPEED:	//obs
            SetSpeed(Cfg.SpdNear);
            _run();
            Screen_Icon_Speed(Icon_Mid_Speed);
            Screen_Icon_Warn(Icon_Warn);
            break;
        case STATUS_RUN_LOW_POWER:
            _run();
            break;
        case STATUS_ESTOP:
            DRSetAction(ACT_BRAKE);
            Screen_Icon_Run(Icon_Stop);
            break;
        case STATUS_REMOTE_MANUAL:
            S.DemoTaskStatus = STATUS_INIT;
            break;
        default:
            DRSetAction(ACT_STOP);
        }
        rt_thread_mdelay(100);
    }
}

int GDInit(void)
{
    rt_thread_t tid;
	if(S.Status != STATUS_ESTOP)
    S.Status = STATUS_INIT;
    Set_Init();
//    RoadInfo_Init();
    Cargo_Init();
    ObsInit();
    Lift_Init();

    tid = rt_thread_create("nav", GDCruise, RT_NULL, 4096, 10, 5);
    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
    }
    LOG_D("Guide inited!");
    return 0;
}
INIT_APP_EXPORT(GDInit);

rt_inline uint8_t _prepare(void) {
    if(S.DRStatus == DR_STATUS_RUN){
        return 1;
    }
    return 0;
}
rt_inline uint8_t _transportProcess(TASK_Transport_t *task) {
    static TASK_Transport_t *preTask = NULL;
    if(task != preTask){
        LOG_D("transport to %d", task->Dst);
        preTask = task;
    }
    switch(Trans.Status){
    case TRANS_STATUS_INIT:{
            int target_PointXY = (task->Dst);
            //if(Cargo_ToHomePos())
            /*���Ŀ��㲻��������������Ŀ��������������ҵ�ǰ�������������ڵ�ǰС������*/
            if((target_PointXY != Set.TsjStation) || ((target_PointXY == Set.TsjStation) && S.cValue == S.TsjFloorStatus))
                Trans.Status = TRANS_STATUS_TO_DST;

        }
        break;
    case TRANS_STATUS_TO_DST:
        if(_navToStation(task->Dst)){
            Trans.Status = TRANS_STATUS_ACTION;
        }
        break;
    case TRANS_STATUS_ACTION:
        if(_GDActionProcess(task->Act)){	//执行动作
            LOG_I("guide task action type:%d\r\n",task->Act);	//打印动作
            Trans.Status = TRANS_STATUS_INIT;
            Task_FinishCurTransport();
            S.Status = STATUS_READY;
            if(!Task_HasTransport()){
                task_set_status(PUBLISHTASKSTA_FINISH, station2Point(S.Point,S.cValue));
                LOG_I("guide task finish\r\n");
            }
            return 1;
        }
        break;
    default:
        Trans.Status = TRANS_STATUS_INIT;
    }
    return 0;
}
rt_inline void _run(void) {
    TASK_Transport_t *curTransport = NULL;
    /* ����������� */
    curTransport = Task_GetCurTransport();	//获取当前运输值
    if(curTransport != NULL){
        _transportProcess(curTransport);	//根据运输值做动作
        return;
    }
    return;
}
rt_inline void collectTaskInfo(void) {
    TASK_Transport_t *curTransport = Task_GetCurTransport();
    if(curTransport != NULL){
        S.TaskId = curTransport->Id;
        S.TaskStatus = Trans.Status;
    }else{
        S.TaskStatus = TRANS_STATUS_NULL;
    }
}
rt_inline bool navToStationNav(uint32_t station) {
	S.TgtStationXvalue = (S.TgtStation / 1000) % 1000;
	S.TgtStationYvalue = S.TgtStation % 1000;
//	S.TgtCvalue = (S.TgtStation / 1000 /1000) % 1000;
//	LOG_I("S.TgtStationXvalue: %d\r\n",S.TgtStationXvalue);
//	LOG_I("S.TgtStationYvalue: %d\r\n",S.TgtStationYvalue);
//  S.AtStation = station;
//********************************1Cto1C*********************************************************************************
//    if(S.TgtCvalue == S.cValue){
        if (S.Point == S.TgtStation ){
        	switch(S.Action){
        	case ACT_FORWARD:
        		DRSetAction(ACT_FORWARD_STOP_CROSS);
        		return 0;
        	case ACT_BACKWARD:
        		DRSetAction(ACT_BACKWARD_STOP_CROSS);
        		return 0;
        	case ACT_LEFT:
        		DRSetAction(ACT_LEFT_STOP_CROSS);
        		return 0;
        	case ACT_RIGHT:
        		DRSetAction(ACT_RIGHT_STOP_CROSS);
        		return 0;
            case ACT_RESET:
        	case ACT_STOP:
        		return 1;
        	}
        }
//        if (S.yValue == S.RoadNum)
        {
            if (S.TgtStationYvalue > S.yValue)	//目标值大于y值
			{
            	if (S.DirStatus == DIR_STATUS_LR){
            		if (Dir_Lift_LR()){
                		DRSetAction(ACT_LEFT);
                		S.DirPauseStatus = DIR_RUN_STATUS;
            		}
            	}else{
            		if (drDirFbNav()){
            			Dir_Lift_LR();
            		}
            	}
            }
            if (S.TgtStationYvalue < S.yValue)	//目标值大于y值
			{
            	if (S.DirStatus == DIR_STATUS_LR){
            		if (Dir_Lift_LR()){
                		DRSetAction(ACT_RIGHT);
                		S.DirPauseStatus = DIR_RUN_STATUS;
            		}
            	}else{
            		if (drDirFbNav()){
            			Dir_Lift_LR();
            		}
            	}
            }
            if (S.TgtStationYvalue == S.yValue && S.TgtStationXvalue < S.xValue){
            	if (S.DirStatus == DIR_STATUS_FB){
            		if (Dir_Lift_FB()){
            			DRSetAction(ACT_BACKWARD);
                        S.DirPauseStatus = DIR_RUN_STATUS;
            		}
            	}else{
            		if (drDirLrNav()){
            			Dir_Lift_FB();
            		}
            	}
            }
            if (S.TgtStationXvalue == S.yValue && S.TgtStationXvalue > S.xValue){
            	if (S.DirStatus == DIR_STATUS_FB){
            		if (Dir_Lift_FB()){
            			DRSetAction(ACT_FORWARD);
                        S.DirPauseStatus = DIR_RUN_STATUS;
            		}
            	}else{
            		if (drDirLrNav()){
            			Dir_Lift_FB();
            		}
            	}
            }
        }
        if (S.Point != S.TgtStation)
        {
        	if(S.yValue != S.TgtStationYvalue){
        		if (S.DirStatus == DIR_STATUS_FB){
        			DRSetAction(ACT_BACKWARD);
        		}
        		S.DirPauseStatus = DIR_RUN_STATUS;
        	}
        	if(S.yValue == S.TgtStationYvalue){
        		if(S.xValue < S.TgtStationXvalue){
        			if (S.DirStatus == DIR_STATUS_FB){
        				DRSetAction(ACT_FORWARD);
        			}
            		S.DirPauseStatus = DIR_RUN_STATUS;
        		}else{
        			if (S.DirStatus == DIR_STATUS_FB){
        				DRSetAction(ACT_BACKWARD);
        			}
            		S.DirPauseStatus = DIR_RUN_STATUS;
        		}
        	}
        }
//    }
    return 0;
}

/**
 * @brief ������վ��
 * @param
 * @retval 1������վ�㣬0��δ����վ��
 */
rt_inline uint8_t _navToStation(uint32_t station) {
    if(S.TgtStation != station){
        LOG_D("Nav to station %d", station);
        S.TgtStation = station;
        Screen_SetTarget(station);
    }
    if(S.Point == NULL_POINT){
        DRSetAction(ACT_FORWARD);
        S.NavStatus = NAV_STATUS_NAV;
        return 0;
    }
    /* ����Ҫ��վʱ ����վ�㼴�� */
//    if(inSt == 0){
        if(navToStationNav(station)){
            S.NavStatus = NAV_STATUS_STATION_IN;
            return 1;
        }
        S.NavStatus = NAV_STATUS_NAV;
        return 0;
}

static void CanSendByte(uint32_t id, uint8_t len, uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4, uint8_t d5,
        uint8_t d6, uint8_t d7)
{
    struct rt_can_msg msg;
    msg.id = id;
    msg.len = 8;
    msg.data[0] = d0;
    msg.data[1] = d1;
    msg.data[2] = d2;
    msg.data[3] = d3;
    msg.data[4] = d4;
    msg.data[5] = d5;
    msg.data[6] = d6;
    msg.data[7] = d7;
    chassis_bus_write(&msg, sizeof(msg));
}

void _Reset() {
    DRSetAction(ACT_RESET);
    LOG_D("DR_ACT_RESET");
    S.Status = STATUS_READY;
//    CanSendByte(0x171, 8, 0, 0x21, 0, 0x11,0,0x01, 0, 0x01);
//    CanSendByte(0x172, 8, 0, 0x21, 0, 0x11,0,0x01, 0, 0x01);
    CanSendByte(0x17a, 8, 0, 0x21, 0, 0x11,0,0x01, 0, 0x01);
//    CanSendByte(0x17b, 8, 0, 0x21, 0, 0x11,0,0x01, 0, 0x01);
//    CanSendByte(0x175, 8, 0, 0x21, 0, 0x11,0,0x01, 0, 0x01);
}
void GDStart(uint8_t FR) {
    S.FR = FR;
//	if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
    S.Status = STATUS_RUN;
    _Reset();
}
void GDStop(void) {
	if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
    S.Status = STATUS_READY;
    DRSetAction(ACT_STOP);
}
void GDPause(void) {
    LOG_D("GD_Pause");
    if((S.Status != STATUS_STOP_OBS) && (S.Status != STATUS_RUN_OBS_LOW_SPEED)){
        S.PreStatus = S.Status;
    }
	if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
    S.Status = STATUS_STOP_OBS;
}
void GDLowSpeed(void) {
    LOG_I("GD_LowSpeed");
    switch(S.Status){
    case STATUS_RUN:
    case STATUS_RUN_LOW_POWER:
        S.PreStatus = S.Status;
		if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
        S.Status = STATUS_RUN_OBS_LOW_SPEED;
    }
}
/* ȥվ�� */
void GDToStation(uint16_t station){
    S.TgtStation = station;
}
void GDResume(void) {
    if(S.Status == STATUS_STOP_OBS || S.Status == STATUS_RUN_OBS_LOW_SPEED){
        if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
		S.Status = S.PreStatus;
    }
}
void GDEStop(void) {
    S.Status = STATUS_ESTOP;
    DRSetAction(ACT_BRAKE);
}
//void GDBreak(void) {
//    S.Status = STATUS_BREAK;
//    DRSetAction(ACT_BRAKE);
//}
uint8_t GDSetAction(uint8_t action) {
    DRSetAction(action);
    return true;
}
uint8_t GDSetStation(uint16_t station) {
    S.TgtStation = station;
    return true;
}
uint8_t GDSetSpeed(uint16_t speed) {
    SetSpeed(speed);
    return true;
}
void GD_ManualStop(void) {
    S.Status = STATUS_REMOTE_MANUAL;
    DRSetAction(ACT_MANUAL_STOP);
}
void GD_ManualForward(void) {
	S.Status = STATUS_REMOTE_MANUAL;
	if (S.DirStatus == DIR_STATUS_FB){
		DRSetAction(ACT_MANUAL_FORWARD);
	}
}
void GD_ManualBackward(void) {
    S.Status = STATUS_REMOTE_MANUAL;
	if (S.DirStatus == DIR_STATUS_FB){
		DRSetAction(ACT_MANUAL_BACKWARD);
	}
}
void GD_ManualLeftward(void) {
    S.Status = STATUS_REMOTE_MANUAL;
    DRSetAction(ACT_MANUAL_DRIFT_LEFT);
}
void GD_ManualRightward(void) {
    S.Status = STATUS_REMOTE_MANUAL;
    DRSetAction(ACT_MANUAL_DRIFT_RIGHT);
}
void GD_ManualDriftLeft(void) {
    S.Status = STATUS_REMOTE_MANUAL;
	if (S.DirStatus == DIR_STATUS_LR){
		DRSetAction(ACT_MANUAL_DRIFT_LEFT);
	}
}
void GD_ManualDriftRight(void) {
    S.Status = STATUS_REMOTE_MANUAL;
	if (S.DirStatus == DIR_STATUS_LR){
		DRSetAction(ACT_MANUAL_DRIFT_RIGHT);
	}
}
void GD_ManualRoteLeft(void) {
    S.Status = STATUS_REMOTE_MANUAL;
    DRSetAction(ACT_MANUAL_ROTATE_LEFT);
}
void GD_ManualRoteRight(void) {
    S.Status = STATUS_REMOTE_MANUAL;
    DRSetAction(ACT_MANUAL_ROTATE_RIGHT);
}
#define ASCII_TO_NUM3(a, b, c) TO_DIGIT((a))* 100+ TO_DIGIT((b)) * 10 + TO_DIGIT((c));
uint8_t GDAddAsciiTrans(uint8_t* data, uint8_t len) {
    TASK_Transport_t task;
    // �ж������
    if(len != 6){
        LOG_E("TASK_ERROR_LEN %d", len);
        return TASK_ERROR_LEN;
    }
    // �ж������Ƿ�ȫ������
    if(!isdigit(
            data[0]) || !isdigit(data[1]) || !isdigit(data[2])){
        LOG_E("TASK_ERROR_SRC_NOT_NUM");
        return TASK_ERROR_SRC_NOT_NUM;
    }
    task.Src = ASCII_TO_NUM3(data[0], data[1], data[2])
    ;
    if(!isdigit(
            data[3]) || !isdigit(data[4]) || !isdigit(data[5])){
        LOG_E("TASK_ERROR_DST_NOT_NUM");
        return TASK_ERROR_DST_NOT_NUM;
    }
    task.Dst = ASCII_TO_NUM3(data[3], data[4], data[5])
    ;
		task.Id[0]='\0';
    return Task_AddTransport(task);
}
uint8_t GDClearTask(void) {
    //JsonPrintfToBuff("{\"t\":\"ClearTask\", \"r\":\"ok\"}");
    return Task_ClearTransport();
}

void GDClearPauseTask(void) {
    if(S.Status != STATUS_RUN
        && S.Status != STATUS_RUN_OBS_LOW_SPEED
        && S.Status != STATUS_RUN_LOW_POWER)
    {
        Task_ForceClear();
		if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
        S.Status = STATUS_READY;
        Trans.Status = TRANS_STATUS_INIT;
        task_set_status(PUBLISHTASKSTA_FINISH, station2Point(S.Point,S.cValue));
        LOG_I("guide task finish\r\n");
    }
}

rt_inline void _batteryProcess(void) {
	
	if(get_bat_enable())
	{
		S.BatteryStatus = get_bat_rsoc();
		if(S.BatteryStatus > 95)	Screen_Icon_Battery(Icon_Battery_100);
		else	
		if(83 < S.BatteryStatus && S.BatteryStatus < 90)	Screen_Icon_Battery(Icon_Battery_80);
		else
		if(63 < S.BatteryStatus && S.BatteryStatus < 80)	Screen_Icon_Battery(Icon_Battery_60);
		else
		if(43 < S.BatteryStatus && S.BatteryStatus < 60)	Screen_Icon_Battery(Icon_Battery_40);
		else
		if(23 < S.BatteryStatus && S.BatteryStatus < 40)	
		{
			Screen_Icon_Battery(Icon_Battery_20);
			if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
			S.Status = STATUS_RUN_LOW_POWER;
		}
		else
		if(S.BatteryStatus < 20)	
		{
			Screen_Icon_Battery(Icon_Battery_20);
			if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
			S.Status = STATUS_STOP_LOW_POWER;
		}					
	}
	else
	{
		if(S.BatteryVolt >= Cfg.VoltFull)
		{
			S.BatteryStatus = BATTERY_STATUS_FULL;
			Screen_Icon_Battery(Icon_Battery_100);
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltHigh && S.BatteryVolt < Cfg.VoltFull-30)	//bug8
		{
			S.BatteryStatus = BATTERY_STATUS_HIGH;
			Screen_Icon_Battery(Icon_Battery_80);
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltMid && S.BatteryVolt < Cfg.VoltHigh-30)	//bug8
		{
			S.BatteryStatus = BATTERY_STATUS_MID;
			Screen_Icon_Battery(Icon_Battery_60);
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltLow && S.BatteryVolt < Cfg.VoltMid-30)	//bug8
		{
			S.BatteryStatus = BATTERY_STATUS_LOW;
			Screen_Icon_Battery(Icon_Battery_40);
		}
		else 
		if(S.BatteryVolt >= Cfg.VoltWarn && S.BatteryVolt < Cfg.VoltLow-30)	//bug8
		{
			S.BatteryStatus = BATTERY_STATUS_WARN;
			Screen_Icon_Battery(Icon_Battery_20);
			if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
			S.Status = STATUS_RUN_LOW_POWER;
		}
		else
		if(S.BatteryVolt < Cfg.VoltWarn-30)	//bug8
		{
			S.BatteryStatus = BATTERY_STATUS_STOP;
			Screen_Icon_Battery(Icon_Battery_00);
			if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL){
				
				S.Status = STATUS_STOP_LOW_POWER;
			}
		}
	}	//bms不行

	if((S.Status == STATUS_RUN_LOW_POWER || S.Status == STATUS_STOP_LOW_POWER) && (S.BatteryStatus != BATTERY_STATUS_WARN)){
	    if(S.Status != STATUS_ESTOP && S.Status != STATUS_REMOTE_MANUAL)
		S.Status = STATUS_READY;
	}
    Screen_SetBatteryStatus(S.BatteryStatus);
}

rt_inline void _Timer_Charge(void) {
//	LOG_I("Time: %d:%d:%d",S.HH,S.MM,S.SS);
	switch(S.HH){
	case 11:
	case 17:
		if (S.MM >30){
			S.TimerChargeStatus = TIMER_STATUS_TO_CHARGE;
		}
		return;
	case 13:
	case 7:
		if (S.MM >30){
			S.TimerChargeStatus = TIMER_STATUS_TO_TASK;
		}
		return;
	}
}

rt_inline void _Test_Print(void){
	LOG_I("++++++++++++++++CscStatus+++++++++++++++++ \r\n");
	LOG_I("S.CscStatus:%d \r\n",S.Status);
	LOG_I("S.CscCvalue:%d \r\n",S.cValue);
	LOG_I("S.TgtCvalue:%d \r\n",S.TgtCvalue);
	LOG_I("++++++++++++++++TsjStatus+++++++++++++++++ \r\n");
	LOG_I("TsjId:%d \r\n",S.TsjId);
	LOG_I("TsjStatus:%d \r\n",S.TsjStatus);
	LOG_I("TsjFloorStatus:%d \r\n",S.TsjFloorStatus);
}



/**
 * @funtion point2Station
 * @brief ������ʽת��
 * @Author Deman
 * @DateTime 2021.06.21-T15:21:33+0800
 *
 * @param   point  �ڵ�ṹ��
 * @return  ����     zxy
 */
rt_inline uint32_t point2Station(point_t point)
{
    return (point.x*1000+point.y);
}

/**
 * @funtion station2Point
 * @brief ������ʽת�� xy z -> point_t
 * @Author Deman
 * @DateTime 2021.06.21-T15:17:53+0800
 *
 * @param   station_xy  ����x,y
 * @param   station_z  ����z
 * @return  �ڵ�ṹ��
 */
rt_inline point_t station2Point(int station_xy,int station_z)
{
    point_t point;
    point.x = (station_xy / 1000) % 1000;
    point.y = station_xy % 1000;
    point.z = station_z;
    point.action = 0;
    return point;
}
/**
 * @funtion _GDReady
 * @brief �ۺϴ���״̬���ж��Ƿ���׼���û�ȡ�ڵ�켣������
 * @Author Deman
 * @DateTime 2021.06.21-T16:03:10+0800
 *
 * @return  true ��׼���� false δ׼����
 */
rt_inline bool _GDReady(void)
{
    //TODO �ۺϸ�״̬�ж�
    //����������Ϊ�գ�����վ����Ч������״̬����
    if(!Task_HasTransport() && S.Point != NULL_POINT
        && (S.Status == STATUS_READY || S.Status == STATUS_RUN ||S.Status == STATUS_RUN_OBS_LOW_SPEED ||S.Status == STATUS_RUN_LOW_POWER)){
        return true;
        }
    return false;
}

/**
 * @funtion _GDActionProcess
 * @brief �ڵ㶯��ִ��
 * @Author Deman
 * @DateTime 2021.06.21-T15:15:53+0800
 *
 * @param   action  ��������
 * @return  true �ɹ� falseʧ��
 */
rt_inline bool _GDActionProcess(uint8_t action)
{
    bool result = false;
    switch(action){
    case TASK_ACT_NULL:
        result = true;
        break;
    case TASK_ACT_PICKUP:
        result = Cargo_Pickup();
        break;
    case TASK_ACT_RELEASE:
        result = Cargo_Release();
        break;
    case TASK_ACT_STEER_RAMP:
//			LOG_I("do act chang lr");
            result = Dir_Lift_LR();
        break;
    case TASK_ACT_TUNNEL:
        result = Dir_Lift_FB();
        break;
    case TASK_ACT_OPEN_CHARGE:
        BTY_AUTO_ON;
        result = true;
        break;
    case TASK_ACT_CLOSE_CHARGE:
        BTY_AUTO_OFF;
        result = true;
        break;
    default:
        result = true;
    }
    return result;
}


/**
 * @funtion _GDPublishTaskStatus
 * @brief ��������״̬���ϲ�
 * @Author Deman
 * @DateTime 2021.06.21-T15:14:31+0800
 *
 */
rt_inline void _GDPublishTaskStatus(void)
{
    uint8_t status;
    point_t cur_point;
    cur_point = station2Point(S.Point,S.cValue);
    switch(S.Status){
        case STATUS_INIT:
            status = PUBLISHTASKSTA_PAUSE;
            break;
        case STATUS_READY:
            status = PUBLISHTASKSTA_FINISH;
            break;
        case STATUS_RUN:
        case STATUS_RUN_OBS_LOW_SPEED:
        case STATUS_RUN_LOW_POWER:
            if(Task_HasTransport()){
                status = PUBLISHTASKSTA_CARRERYING;
                task_set_status(status, cur_point);
            }
            break;
        case STATUS_STOP_OBS:
        case STATUS_ESTOP:
        case STATUS_REMOTE_MANUAL:
        default:
            status = PUBLISHTASKSTA_PAUSE;
            task_set_status(status, cur_point);
    }

}


/**
 * @funtion _GDAddTask
 * @brief ���ϲ��ȡ�Ĺ켣����ӵ���������
 * @Author Deman
 * @DateTime 2021.06.21-T16:04:29+0800
 *
 */
rt_inline void _GDAddTask(void)		/* 导航增加任务 */
{
    point_t trajectory[TASK_MAX_POINT];
    point_t cur_point;
    uint8_t point_num;
    uint8_t i;
    cur_point = station2Point(S.Point,S.cValue);
    if(_GDReady()){
        point_num = task_get(cur_point, trajectory);
        if(point_num > 0 && point_num <= TASK_MAX_POINT){
            for(i = 0;i < point_num;i++){
                TASK_Transport_t task;
                sprintf(task.Id,"%3d%3d%3d%d",trajectory[i].z, trajectory[i].x, trajectory[i].y,trajectory[i].action);
                task.Dst = point2Station(trajectory[i]);
                task.DstFloor = trajectory[i].z;
                task.Act = trajectory[i].action;
                Task_AddTransport(task);
            }
        }
   }
}