rgv-kinco/applications/env.c

/*
 * @file env.c
 *
 * @brief 环境变量
 * @details config\roadinfo
 *
 * @copyright (c) 2006-2021, Huali Development Team
 *
 *             (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.
 *
 * Change Logs:
 * @date           @author       @note
 * 2021-10-16     Simon       the first version
 */

#ifndef ENV_C_
#define ENV_C_
#include "rtthread.h"
#include "env.h"
#include "stdint.h"
#include "stdbool.h"
#include "sys/socket.h"
#include "netdev.h"
#include "finsh.h"

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

/*********************************************************************************************************
 *                                              FLASH CFG
 *********************************************************************************************************/
#define FLASH_SAVED                                     0xf0f0
#define FLASH_CFG_ADDR                                  ((uint32_t)0x08100000) /* Base @ of Sector 12, 16 Kbytes */
#define FLASH_ROADINFO_ADDR                             ((uint32_t)0x08110000) /* Base @ of Sector 16, 64 Kbytes */

Cfg_t Cfg;
Set_t Set;
IN_t I;
OUT_t O;
Status_t S;


uint8_t FLASH_Write(uint32_t startAddr, const uint8_t *pBuffer, uint32_t size)
{
    stm32_flash_erase(startAddr, size);
    stm32_flash_write(startAddr, pBuffer, size);
    return 1;
}

uint16_t FLASH_ReadHalfWord(uint32_t addr)
{
    return *((uint16_t*) addr);
}
/**
 * @brief 保存配置到Flash
 * @param
 * @return
 */
uint8_t Flash_SaveCfg(void) {
    uint8_t ret = 1;
    LOG_I("Flash_SaveCfg Save cfg to flash start\r\n");
    ret = FLASH_Write(FLASH_CFG_ADDR, (uint8_t *)(&Cfg), sizeof(Cfg_t));
    LOG_I("Flash_SaveCfg Save cfg to flash end: %d\r\n", ret);
    return ret;
}
/**
 * @brief 初始化配置
 * @param
 * @return
 */
int FlashLoadCfg(void) {
    LOG_I("FlashLoadCfg init 0x%x:0x%x", FLASH_CFG_ADDR, (*((uint8_t *) FLASH_CFG_ADDR)));
    if((*((uint16_t *) FLASH_CFG_ADDR)) == FLASH_SAVED){
        // 从flash读取配置
        memcpy((uint8_t *)&Cfg, (uint8_t *) FLASH_CFG_ADDR, sizeof(Cfg_t));
        LOG_I("Read cfg from flash");
    }
    else
    {
        Cfg_Init();	
		Flash_SaveCfg();	//增加初始化时，保存flash的值
        //如果flash里面没有配置，则初始化默认配置
        LOG_I("Read cfg from default cfg");
		
    }
    if(Cfg.BtyType == CFG_BATTERY_TYPE_48V){
        Cfg.VoltFull = 5600;	//100%	5200	bug8	5200
        Cfg.VoltHigh = 5400;	//80%	5000	bug8	5100
        Cfg.VoltMid = 5200;		//60%	4800	bug8	5000
        Cfg.VoltLow = 5000;		//40%	4700	bug8	4900
        Cfg.VoltWarn = 4800;	//20%	4600	bug8	4800
        Cfg.VoltStop = 4700;	//0%	4500	bug8	4700
    }else{
        Cfg.VoltFull = 2600;
        Cfg.VoltHigh = 2500;
        Cfg.VoltMid = 2400;
        Cfg.VoltLow = 2350;
        Cfg.VoltWarn = 2300;
        Cfg.VoltStop = 2250;
    }
    Cfg.StartAct = ACT_LEFT;

//    struct netdev *netdev = RT_NULL;
//
//    netdev = netdev_get_by_name("e0");
//    if(netdev)
//    {
//        netdev_set_gw(netdev, (ip_addr_t *)&Cfg.gateway);
//        netdev_set_ipaddr(netdev, (ip_addr_t *)&Cfg.ipaddr);
//        netdev_set_netmask(netdev, (ip_addr_t *)&Cfg.netmask);
//    }
    return 0;
}
INIT_ENV_EXPORT(FlashLoadCfg);

static int Net_Init(void)
{
    struct netdev *netdev = RT_NULL;
	while(!netdev)
	{
		netdev = netdev_get_by_name("e0");
		rt_thread_mdelay(100);
	}
    
	netdev_set_gw(netdev, (ip_addr_t *)&Cfg.gateway);
	netdev_set_ipaddr(netdev, (ip_addr_t *)&Cfg.ipaddr);
	netdev_set_netmask(netdev, (ip_addr_t *)&Cfg.netmask);

    return RT_EOK;
}
INIT_APP_EXPORT(Net_Init);


void Cfg_Init(void) {
    Cfg.AgvId = 55;
    Cfg.SpdFctr = 50;
    Cfg.Speed = 60;
    Cfg.SpdMxDft = 10;
    Cfg.SpdNear = 10;	//Bug0018,之前为7
    Cfg.SpdMan = 20;
    Cfg.SpdRota = 10;
    Cfg.MaxRpm = 3000;

    Cfg.RfidType = CFG_RFID_TYPE_HS;
//    Cfg.CargoType = CFG_CARGO_TYPE_LIFT;
    Cfg.CargoType = CFG_CARGO_TYPE_LIFT;
    Cfg.ObsType = CFG_OBS_TYPE_KL;
    Cfg.BtyType = CFG_BATTERY_TYPE_48V;

    Cfg.NavSP = 1;
    Cfg.NavSI = 5;
    Cfg.NavSD = 5;
    Cfg.NavWP = 25;
    Cfg.DftWP = 7;
    Cfg.MtsAglAcy = 30;
    Cfg.FWlkAcc = 2000;
    Cfg.FWlkDcc = 2000;
    Cfg.BWlkAcc = 10;
    Cfg.BWlkDcc = 10;

    Cfg.StrMcType = CFG_StrMcType_SenChuang;
    Cfg.WlkMcType = CFG_WlkMcType_SenChuang;

    if(Cfg.StrMcType == CFG_StrMcType_Motec){
        Cfg.FStrPlsDeg = 964;
        Cfg.BStrPlsDeg = 964;

        Cfg.RoteAngle = -713;
    }else{
//        Cfg.FStrPlsDeg = 458;   //5.5*10000*30/3600-458
//        Cfg.BStrPlsDeg = 458;
              Cfg.FStrPlsDeg = 420;  //5*10000*30.25/3600=416
        Cfg.BStrPlsDeg = 420;
        Cfg.RoteAngle = 599;
    }
    Cfg.FZeroAng = 200;
    Cfg.BZeroAng = 200;

    Cfg.ipaddr = 0x866fa8c0;
    Cfg.netmask = 0x00ffffff;
    Cfg.gateway = 0x016fa8c0;	
	Cfg.stop_dist = 20;
	Cfg.reduce_dist = 100;
    Cfg.Saved = FLASH_SAVED;
}

void Stat_Init(void) {
    S.AgvId = Cfg.AgvId;
    S.LogSwMain = 1;
    S.LogSwGuide = 1;
    S.LogSwDriver = 1;
    //S.LogSwCan = 1;
    S.Led0Status = LIGHT_STATUS_TOGGLE_1S;
    S.FR = FR_FORWARD;
    S.Status = STATUS_INIT;
    S.Point = NULL_POINT;
    S.Station = NULL_POINT;
    S.TgtAction = ACT_NULL;
    S.DRAction = ACT_NULL;
    S.TgtStation = NULL_POINT;
    S.LiftStatus = LIFT_STATUS_CENTER;
    S.CargoStatus = CARGO_STATUS_INIT;
    S.IdleSecond = IDLE_TIMEOUT;
    S.BatteryVolt = Cfg.VoltFull;
    S.BatteryStatus = BATTERY_STATUS_FULL;
    S.BPos = -20000000;
    S.FPos = -20000000;
    S.FMgsOffset = MgsNA;
    S.BMgsOffset = MgsNA;
    S.LMgsOffset = MgsNA;
    S.RMgsOffset = MgsNA;
    S.TaskId = "";
    S.OBSArea = OBS_AREA_ON;
	S.near_time_out = 160;
	S.motor_err = 0;//Bug107
//    S.RoadNum = 2;
}
INIT_ENV_EXPORT(Stat_Init);

void Set_Init(void) {
    SetSpeed(Cfg.Speed);
    Set.RpmNear = Cfg.SpdNear * Cfg.SpdFctr;
    Set.RpmManual = Cfg.SpdMan * Cfg.SpdFctr;
    Set.RpmRotate = Cfg.SpdRota * Cfg.SpdFctr;
    Set.Branch = BRANCH_CENTER;
    Set.RotateAct = ACT_NULL;
    Set.FWlkRpm = 0;
    Set.BWlkRpm = 0;
    Set.FAngle = AngleNA;
    Set.BAngle = AngleNA;
    Set.LiftRpmUp = 300;
    Set.LiftRpmDown = -300;
    Set.LiftPos = PosNA;
    Set.LiftUpPos = 4500000;
    Set.LiftDownPos = 300000;

    Set.FWlkAAcc = 500;		//300
    Set.FWlkADcc = 800;	//2000
    Set.FWlkMAcc = 1000;
    Set.FWlkMDcc = 1000;
    Set.StationDcc = 0;

//    Set.FWlkAAcc = 2000;
//    Set.FWlkADcc = 700;
//    Set.FWlkMAcc = 2000;
//    Set.FWlkMDcc = 1000;
//dmklift
    Set.LiftRpm = 0;
    Set.ChargeStation = 1008003;	//BUG4
    Set.TsjStation = 2003011;		//BUG4
	//专为唐山修改,2排11列与3排11列不起作用
	Set.TsjPoint = Set.TsjStation % 1000000;
}

void SetSpeed(uint16_t speed) {
    Set.Speed = speed;
    Set.RpmNear = Cfg.SpdNear * Cfg.SpdFctr;
    Set.RpmManual = Cfg.SpdMan * Cfg.SpdFctr;
    Set.RpmRotate = Cfg.SpdRota * Cfg.SpdFctr;
    Set.SpeedDrift = (Set.Speed <= Cfg.SpdMxDft)?Set.Speed:Cfg.SpdMxDft;
    Set.DRRpmNav = Set.RpmNav = Set.Speed * Cfg.SpdFctr;
    Set.DRRpmDft = Set.RpmDft = Set.SpeedDrift * Cfg.SpdFctr;
}

void Cfg_Cmd(rt_uint8_t argc, char **argv)
{
    int rc = 0;

    const char* help_info[] =
    {
            [0]     = "config param [value]   - config param(eg. id) with value, query param value if [value] is null",
            [1]     = "  config id            - config agv id",
            [2]     = "  config ip            - set ip address",
            [3]     = "  config netmask       - set net mask",
            [4]     = "  config gateway       - set gateway",
            [5]     = "  config speed         - set wheel speed",
            [6]     = "  config roterpm       - set rote rpm speed",
            [7]     = "  config maxrpm        - set max rpm speed",
            [8]     = "  config reset         - set config to factory",
            [9]     = "  config chargetag     - set config charge station tag number",
            [10]    = "  config lifttag       - set config lift station tag number",
			[11]    = "  config tfmini        - set config tfmini reduce and stop dist",
    };

    if (argc < 2)
    {
        printf("Usage:\n");
        for (int i = 0; i < sizeof(help_info) / sizeof(char*); i++)
        {
            printf("%s\n", help_info[i]);
        }
        printf("\n");
    }
    else
    {
        const char *operator = argv[1];
        ip_addr_t ipaddr;
        struct netdev *netdev = RT_NULL;

        netdev = netdev_get_by_name("e0");

        if(!strcmp(operator, "reset"))
        {
            Cfg_Init();
            rc = 1;
        }
        else if (!strcmp(operator, "chargetag"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%u", &Set.ChargeStation);
            }
            else
            {
                printf("%s: %08u\n", operator, Set.ChargeStation);
            }

        }
        else if (!strcmp(operator, "lifttag"))
        {
            if(argc == 3)
            {
                uint32_t lift_tag = 0;
                rc = sscanf(argv[2], "%u", &lift_tag);
                if(rc == 1)
                {
                    Set.TsjStation = lift_tag ;	//BUG4
                }
            }
            else
            {
                 printf("%s: %08u\n", operator, Set.TsjStation);	////BUG4
            }

        }
        else if (!strcmp(operator, "netmask"))
        {
            if(argc == 3)
            {
                rc = inet_aton((const char *)argv[2], &ipaddr);
                if(rc)
                {
                    Cfg.netmask = ipaddr.addr;
                    if(netdev)
                        netdev_set_netmask(netdev, &ipaddr);
                }
            }
            else
            {
                printf("%s: %s\n", operator, inet_ntoa(Cfg.netmask));
            }
        }
        else if (!strcmp(operator, "gateway"))
        {
            if(argc == 3)
            {
                rc = inet_aton((const char *)argv[2], &ipaddr);
                if(rc)
                {
                    Cfg.gateway = ipaddr.addr;
                    if(netdev)
                        netdev_set_gw(netdev, &ipaddr);
                }
            }
            else
            {
                printf("%s: %s\n", operator, inet_ntoa(Cfg.gateway));
            }
        }
        else if (!strcmp(operator, "speed_factor"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%hu", &Cfg.SpdFctr);
            }
            else
            {
                printf("%s: %hu\n", operator, Cfg.SpdFctr);
            }

        }
        else if (!strcmp(operator, "speed"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%hu", &Cfg.Speed);
            }
            else
            {
                printf("%s: %hu\n", operator, Cfg.Speed);
            }

        }
        else if (!strcmp(operator, "maxrpm"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%hu", &Cfg.MaxRpm);
            }
            else
            {
                printf("%s: %hu\n", operator, Cfg.MaxRpm);
            }

        }
        else if (!strcmp(operator, "roterpm"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%hu", &Cfg.SpdRota);
            }
            else
            {
                printf("%s: %hu\n", operator, Cfg.SpdRota);
            }

        }
        else if (!strcmp(operator, "ip"))
        {
            if(argc == 3)
            {
                rc = inet_aton((const char *)argv[2], &ipaddr);
                if(rc)
                {
                    if(netdev)
                    {
                        netdev_set_ipaddr(netdev, &ipaddr);
                    }
                    Cfg.ipaddr = ipaddr.addr;
                }
            }
            else
            {
                printf("ip address: %s\n", inet_ntoa(Cfg.ipaddr));
            }
        }
        else if (!strcmp(operator, "id"))
        {
            if(argc == 3)
            {
                rc = sscanf(argv[2], "%hu", &Cfg.AgvId);
            }
            else
            {
                printf("agv id: %hu\n", Cfg.AgvId);
            }

        }
		else if (!strcmp(operator, "tfmini"))
        {
            if(argc == 4)
            {
				rc = sscanf(argv[2], "%hu", &Cfg.reduce_dist);
                rc = sscanf(argv[3], "%hu", &Cfg.stop_dist);
            }
            else
            {
                printf("%s: %hu %hu\n", operator, Cfg.reduce_dist,Cfg.stop_dist);
            }

        }
        else
        {
            printf("Usage:\n");
            for (int i = 0; i < sizeof(help_info) / sizeof(char*); i++)
            {
                printf("%s\n", help_info[i]);
            }
            printf("\n");
        }
        if(rc)
        {
            Flash_SaveCfg();
            printf("config ok!\n");
        }
    }
}
MSH_CMD_EXPORT_ALIAS(Cfg_Cmd, config, config terminal parameter);

#endif /* ENV_C_ */