ctnd/platform/device/pcs/n9/n9_sm.c

#include "plt.h"

static struct state_t n9_states[] = {
    {SMST_LAUNCH, "launch"},
    {SMST_STDBY, "stdby"},
    {SMST_STOP, "stop"},
    {SMST_READY, "ready"},
    {SMST_DHG, "dhg"},
    {SMST_CHG, "chg"},
    {SMST_OFFGRID, "offgrid"},
    {SMST_ERR, "err"},
};

static struct err_t n9_errs[] = {
    {N9ERR_NONE, "none"},

    // launch IES1000ERR_LAUNCH_COMMERR
    {N9ERR_LAUNCH_COMMERR, "launch, comm err"},

    // err
    {N9ERR_ERR_COMMERR, "err, comm err"},
    {N9ERR_ERR_PWRUP, "err, pwrup"},
    {N9ERR_ERR_PWROFF, "err, power off"},

    // stdby
    {N9ERR_STDBY_COMMERR, "stdby, comm err"},
    {N9ERR_STDBY_CHK_DEVAPS0_FAIL_AFTER_SET, "stdby, chk aps = 0 fail"},
    {N9ERR_STDBY_WAIT_STOP_TIMEOUT, "stdby, chk stop fail"},

    // stop
    {N9ERR_STOP_COMMERR, "stop, comm err"},
    {N9ERR_STOP_NONE_STOP_DETECTED, "stop, none stop run state detected"},
    {N9ERR_STOP_CHK_DEVAPS0_FAIL_AFTER_SET, "stop, chk dev aps = 0 fail"},
    {N9ERR_STOP_WAIT_START_TIMEOUT, "stop, wait start timeout"},

    // ready
    {N9ERR_READY_COMMERR, "ready, comm err"},
    {N9ERR_READY_NOSTART_DETECTED, "ready, no start detected"},
    {N9ERR_READY_WAIT_STOP_TIMEOUT, "ready, wait stop timeout"},
    {N9ERR_READY_WAIT_AP_OVER_0_TIMEOUT, "ready, wait ap over zero time out"},
    {N9ERR_READY_WAIT_AP_UNDER_0_TIMEOUT, "ready, wait ap under zero time out"},

    // dhg
    {N9ERR_DHG_COMMERR, "dhg, comm err"},
    {N9ERR_DHG_NONDHG_DETECTED, "dhg, none dhg detected"},
    {N9ERR_DHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT, "dhg, wait set dev aps = 0 time out"},
    {N9ERR_DHG_WAIT_AP0_FOR_READYCMD_TIMEOUT, "dhg, wait ap = 0 for ready cmd time out"},
    {N9ERR_DHG_TICK_TIMEOUT_DETECTED, "dhg, tick timeout"},

    // chg
    {N9ERR_CHG_COMMERR, "chg, comm err"},
    {N9ERR_CHG_NONCHG_DETECTED, "chg, none dhg detected"},
    {N9ERR_CHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT, "chg, wait set dev aps = 0 time out"},
    {N9ERR_CHG_WAIT_AP0_FOR_READYCMD_TIMEOUT, "chg, wait ap = 0 for ready cmd time out"},
    {N9ERR_CHG_TICK_TIMEOUT_DETECTED, "chg, tick timeout"},

    // offgrid
    {N9ERR_OFFGRID_ERRSTAT_DETECTED, "offgrid, err state detected"},
    {N9ERR_OFFGRID_NON_DHGCHG_DETECTED, "offgrid, non dhg chg state detected"},
};

int n9_sm_init(int idx)
{
    struct statemachine_t *sm = &n9[idx].sm;

    sm_reset_timing(sm, 10, 10);
    sm->states = n9_states;
    sm->state_nbr = sizeof(n9_states) / sizeof(struct state_t);
    sm->errs = n9_errs;
    sm->err_nbr = sizeof(n9_errs) / sizeof(struct err_t);
    sm_set_state(sm, SMST_LAUNCH, N9ERR_NONE);

    return 0;
}

static void n9_sm_launch(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &dev->comm;
    struct statemachine_t *sm = &dev->sm;

    if (sm_get_step(sm) == 0)
    { // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_reset_cmd(idx);
        n9_comm_reset(idx);
        sm_set_step(sm, 20);
    }
    else if (sm_get_step(sm) == 20)
    {
        if (comm_get_state(comm) == COMMST_NORMAL)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, comm ok, goto stdby",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STDBY, N9ERR_NONE);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, N9ERR_LAUNCH_COMMERR);
        }
    }
}

static void n9_sm_err(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &n9[idx].comm;
    struct statemachine_t *sm = &n9[idx].sm;
    static double ts_last_try;
    double ts;

    if (sm_get_step(sm) == 0)
    { // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_reset_cmd(idx);
        n9_set_dev_stopcmd(idx); // if err, stop device immediately
        sm_set_step(sm, 10);
        ts_last_try = sm_get_timeofday();
    }
    else if (sm_get_step(sm) == 10)
    { // wait cmd
        ts = sm_get_timeofday();
        if (n9_get_cmd(idx) == CMD_SM_STDBY)
        { // stdby cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stdby cmd, reset comm then chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            n9_comm_reset(idx);
            sm_set_step(sm, 20);
        }
        else if (comm_get_state(comm) != COMMST_NORMAL && (ts - ts_last_try > 60000))
        { // 60s
            log_dbg("%s, idx:%d, state:%s, step:%d,comm:%s,reset comm",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), comm_get_state_str(comm));
            ts_last_try = ts;
            n9_comm_reset(idx);
        }
    }
    else if (sm_get_step(sm) == 20)
    { /* chk comm state */
        if (comm_get_state(comm) == COMMST_NORMAL)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, comm ok, goto stdby",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STDBY, N9ERR_NONE);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, stay err",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, N9ERR_ERR_COMMERR);
        }
    }
}

static void n9_sm_stdby(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &n9[idx].comm;
    struct statemachine_t *sm = &dev->sm;

    /* chk comm state */
    if (comm_get_state(comm) != COMMST_NORMAL)
    {
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, N9ERR_STDBY_COMMERR);
        return;
    }

    if (sm_get_step(sm) == 0)
    { /* entry */
        n9_reset_cmd(idx);
        // n9_set_dev_resetcmd(idx);  // cyx : stdby : 各种设备保持原样
        // n9_set_dev_stopcmd(idx);
        sm_set_step(sm, 10);
    }
    else if (sm_get_step(sm) == 10)
    { /* wait cmd */
        if (n9_get_cmd(idx) == CMD_SM_STOP)
        { /* stop cmd */
            log_dbg("%s, idx:%d, state:%s, step:%d, get stop cmd, chk devaps = 0 ?", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            sm_set_step(sm, 20);
        }
    }
    else if (sm_get_step(sm) == 20)
    { /* chk devaps = 0 ? */
        if (n9_get_dev_aps(idx) != 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps != 0 detected, set it to 0, wait and chk", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps = 0 detected, chk stop state", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }
    }
    else if (sm_get_step(sm) == 30)
    { /* wait and chk devaps = 0 */
        if (n9_get_dev_aps(idx) == 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, devaps = 0 detected, chk stop state", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk devaps = 0 fail after set, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, N9ERR_STDBY_CHK_DEVAPS0_FAIL_AFTER_SET);
        }
    }
    else if (sm_get_step(sm) == 40)
    { /* chk runstate = stop ? */
        if (N9_RUNSTAT_IDLE != n9_get_runstat(idx))
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, non-stop run state detected, send stop cmd, wait and chk", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_dev_stopcmd(idx); // when stdby->stop, stop device
            sm_set_step(sm, 50);
            sm_set_count(sm, 0);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, stop run state detected, goto stop",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, N9ERR_NONE);
        }
    }
    else if (sm_get_step(sm) == 50)
    { /* wait run state = stop after stopcmd*/
        if (N9_RUNSTAT_IDLE == n9_get_runstat(idx))
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, stop run state detected, goto stop", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, N9ERR_NONE);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                log_dbg("%s, idx:%d, state:%s, step:%d, wait stop run state timeout, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, N9ERR_STDBY_WAIT_STOP_TIMEOUT);
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting stop run state, count:%d", __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
}

static void n9_sm_offgrid(int idx)
{
    // struct ies1000_t* dev = &ies1000[idx];
    // struct comm_t* comm = &ies1000[idx].comm;
    // struct statemachine_t* sm = &ies1000[idx].sm;

    // // chk comm state
    // if( comm_get_state(comm) != COMMST_NORMAL){
    //     log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
    //                             __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
    //     sm_set_state(sm, SMST_ERR, IES1000ERR_STOP_COMMERR);
    //     return;
    // }

    // if( sm_get_step(sm) == 0 ){ // entry
    //     ies1000_reset_cmd(idx);
    //     sm_set_step(sm, 10);
    // }else if( sm_get_step(sm) == 10 ){ // wait cmd and chk
    //     if( ies1000_get_cmd(idx) == CMD_SM_READY ){ // ready cmd
    //         log_dbg("%s, idx:%d, state:%s, step:%d, get ready cmd, set dev idlecmd, goto ready",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
    //         ies1000_reset_cmd(idx);
    //         ies1000_set_dev_idlecmd(idx);
    //         sm_set_state(sm, SMST_READY, IES1000ERR_NONE);
    //     }else if( ies1000_get_errstat(idx) != 0){
    //         log_dbg("%s, idx:%d, state:%s, step:%d, err state detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
    //         sm_set_state(sm, SMST_ERR, IES1000ERR_OFFGRID_ERRSTAT_DETECTED);
    //     }else if( ies1000_get_runstat(idx) != IES1000_RUNSTAT_CHG && ies1000_get_runstat(idx) != IES1000_RUNSTAT_DHG){
    //         log_dbg("%s, idx:%d, state:%s, step:%d, non dhg chg run state detected, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
    //         sm_set_state(sm, SMST_ERR, IES1000ERR_OFFGRID_NON_DHGCHG_DETECTED);
    //     }else{
    //     }
    // }
}

static void n9_sm_stop(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &dev->comm;
    struct statemachine_t *sm = &dev->sm;

    /* chk comm state */
    if (comm_get_state(comm) != COMMST_NORMAL)
    {
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, N9ERR_STOP_COMMERR);
        return;
    }

    if (sm_get_step(sm) == 0)
    { // entry
        n9_reset_cmd(idx);
        n9_set_aps(idx, 0);
        sm_set_step(sm, 10);
    }
    else if (sm_get_step(sm) == 10)
    { // wait cmd and chk
        if (n9_get_cmd(idx) == CMD_SM_STDBY)
        { // stdby cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stdby cmd, goto stdby", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            sm_set_state(sm, SMST_STDBY, N9ERR_NONE);
        }
        else if (n9_get_cmd(idx) == CMD_SM_READY)
        { // ready cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get ready cmd, set dev resetcmd, chk dev aps", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            sm_set_step(sm, 20);
            n9_set_dev_resetcmd(idx);
        }
        else
        { /* no cmd, do chking */
            if (N9_RUNSTAT_IDLE != n9_get_runstat(idx))
            {
                log_dbg("%s, idx:%d, state:%s, step:%d, run state != stop detected, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, N9ERR_STOP_NONE_STOP_DETECTED);
            }
        }
    }
    else if (sm_get_step(sm) == 20)
    { /* chk devaps = 0 ? */
        if (n9_get_dev_aps(idx) != 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps != 0 detected, set it to 0, then chk", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
        }
        else
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps = 0 detected, goto set pcs start cmd", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }
    }
    else if (sm_get_step(sm) == 30)
    { /* wait and chk devaps = 0 */
        if (n9_get_dev_aps(idx) == 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, dev aps = 0 detected,got set pcs start cmd", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 40);
        }
        else
        {

            log_dbg("%s, idx:%d, state:%s, step:%d, chk dev aps = 0 fail, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_ERR, N9ERR_STOP_CHK_DEVAPS0_FAIL_AFTER_SET);
        }
    }
    else if (sm_get_step(sm) == 40)
    {

        log_dbg("%s, idx:%d, state:%s, step:%d, set pcs start cmd and goto wait pcs start", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_set_dev_startcmd(idx);
        sm_set_step(sm, 50);
    }
    else if (sm_get_step(sm) == 50)
    { // wait pcs idle
        if (N9_RUNSTAT_RUN == n9_get_runstat(idx))
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk pcs start ok, goto ready", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_READY, N9ERR_NONE);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 200)
            { /* should over 10s  */
                sm_set_state(sm, SMST_ERR, N9ERR_STOP_WAIT_START_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait pcs start timeout, goto err",
                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting wait pcs start, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
}

static void n9_sm_ready(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &dev->comm;
    struct statemachine_t *sm = &dev->sm;

    int aps = n9_get_aps(idx);
    int ap = n9_get_ap(idx);

    /* chk comm state */
    if (comm_get_state(comm) != COMMST_NORMAL)
    {
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, N9ERR_READY_COMMERR);
        return;
    }

    if (sm_get_step(sm) == 0)
    { // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_reset_cmd(idx);
        n9_reset_aps(idx);
        sm_set_step(sm, 10);
    }
    else if (sm_get_step(sm) == 10)
    { // wait and chk
        if (n9_get_cmd(idx) == CMD_SM_STOP)
        { // stop cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, get stop cmd, dev stop cmd sent, wait and chk", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            n9_set_dev_stopcmd(idx);
            sm_set_step(sm, 20);
            sm_set_count(sm, 0);
        }
        else if (aps > 0)
        { // new aps, prepare to dhg
            log_dbg("%s, idx:%d, state:%s, step:%d, aps > 0 detected, %d, set dev aps, wait and chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            n9_set_dev_aps(idx, aps);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);
        }
        else if (aps < 0)
        { // new aps, prepare to chg
            log_dbg("%s, idx:%d, state:%s, step:%d, aps < 0 detected, %d, set dev aps, wait and chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            n9_set_dev_aps(idx, aps);
            sm_set_step(sm, 40);
            sm_set_count(sm, 0);
        } /*else if( ies1000_get_cmd(idx) == CMD_SM_OFFGRID ){ // offgrid cmd
             log_dbg("%s, idx:%d, state:%s, step:%d, get offgrid cmd, set dev run mode = offgrid, wait and chk",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
             ies1000_reset_cmd(idx);
             ies1000_set_dev_runmod(idx, IES1000_RUNMOD_OFFGRID);
             sm_set_step(sm, 500);
             sm_set_count(sm, 0);
         }*/
        else if (n9_get_runstat(idx) != N9_RUNSTAT_RUN)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != idle detected, %d, goto err",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), n9_get_runstat(idx));
            sm_set_state(sm, SMST_ERR, N9ERR_READY_NOSTART_DETECTED);
        }
    }
    else if (sm_get_step(sm) == 20)
    { /* wait and chk run state = stop */
        if (n9_get_runstat(idx) == N9_RUNSTAT_IDLE)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, dev run state = stop detected, goto stop",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_state(sm, SMST_STOP, N9ERR_NONE);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev run state = stop timeout, goto err",
                        __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                sm_set_state(sm, SMST_ERR, N9ERR_READY_WAIT_STOP_TIMEOUT);
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev run state = stop, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
    else if (sm_get_step(sm) == 30)
    { // chk ap > 0
        if (ap > 0)
        { /* chk ok */
            log_dbg("%s, idx:%d, state:%s, step:%d, ap > 0 detected, %d, goto dhg",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
            sm_set_state(sm, SMST_DHG, N9ERR_NONE);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                sm_set_state(sm, SMST_ERR, N9ERR_READY_WAIT_AP_OVER_0_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap > 0 for dhg timeout, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap > 0 for dhg, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
    else if (sm_get_step(sm) == 40)
    { // chk ap for chg
        if (ap < 0)
        { /* chk ok */
            log_dbg("%s, idx:%d, state:%s, step:%d, ap = %d < 0 detected, goto chg",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
            sm_set_state(sm, SMST_CHG, N9ERR_NONE);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                sm_set_state(sm, SMST_ERR, N9ERR_READY_WAIT_AP_UNDER_0_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap < 0 for chg timeout, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap < 0 for chg, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    } /*else if( sm_get_step(sm) == 500 ){ // wait run mode = offgrid for offgrid
         if( IES1000_RUNMOD_OFFGRID == ies1000_get_runmod(idx) ){
             log_dbg("%s, idx:%d, state:%s, step:%d, chk run mode = offgrid ok, set dev startcmd, wait and chk voltage",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
             ies1000_set_dev_startcmd(idx);
             sm_set_step(sm, 501);
             sm_set_count(sm, 0);
         }else{
             sm_inc_count(sm);
             if( sm_get_count(sm) >= 100 ){ // 5s
                 sm_set_state(sm, SMST_ERR, IES1000ERR_READY_WAIT_RUNMOD_OFFGRID_FOR_OFFGRID_TIMEOUT);
                 log_dbg("%s, idx:%d, state:%s, step:%d, wait run mode = offgrid timeout, goto err",
                                 __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
             }else{
                 log_dbg("%s, idx:%d, state:%s, step:%d, waiting run mode = offgrid, count:%d",
                                 __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
             }
         }
     }else if( sm_get_step(sm) == 501 ){ // chk voltage for offgrid
         if( ies1000_get_ua(idx) > 340.0 && ies1000_get_ua(idx) < 420.0){
             log_dbg("%s, idx:%d, state:%s, step:%d, chk voltage ok, %.1f, goto offgrid",__func__, idx, sm_get_szstate(sm), sm_get_step(sm), ies1000_get_ua(idx));
             sm_set_state(sm, SMST_OFFGRID, IES1000ERR_NONE);
         }else{
             sm_inc_count(sm);
             if( sm_get_count(sm) >= 300 ){ // 15s
                 log_dbg("%s, idx:%d, state:%s, step:%d, wait voltage to 380 for offgrid timeout, goto err",__func__, idx, sm_get_szstate(sm), sm_get_step(sm));
                 sm_set_state(sm, SMST_ERR, IES1000ERR_READY_WAIT_TARGET_VOLTAGE_FOR_OFFGRID_TIMEOUT);
             }else{
                 log_dbg("%s, idx:%d, state:%s, step:%d, wait target voltage for offgrid, count:%d",
                                 __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
             }
         }
     }*/
}

static void n9_sm_dhg(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &dev->comm;
    struct statemachine_t *sm = &dev->sm;
    int aps = n9_get_aps(idx);
    int last_aps = n9_get_last_aps(idx);
    int ap = n9_get_ap(idx);

    /* chk comm state */
    if (comm_get_state(comm) != COMMST_NORMAL)
    {
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, N9ERR_DHG_COMMERR);
        return;
    }

    if (sm_get_step(sm) == 0)
    { // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_reset_cmd(idx);
        sm_set_step(sm, 10);
        n9_reset_bsytikchk(idx);
        gt_reset_bsytikchk(idx);
    }
    else if (sm_get_step(sm) == 10)
    { // wait and chk
        if (n9_get_cmd(idx) == CMD_SM_READY)
        { // ready cmd
            log_dbg("%s, idx:%d, state:%s, step:%d, got ready cmd, set dev aps = 0, wait and chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);
        }
        else if (n9_get_runstat(idx) != N9_RUNSTAT_RUN)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != dhg detected, set dev aps to 0, goto err",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_dev_aps(idx, 0);
            sm_set_state(sm, SMST_ERR, N9ERR_DHG_NONDHG_DETECTED);
        }
        else if (aps < 0)
        { // aps <= 0
            log_dbg("%s, idx:%d, state:%s, step:%d, aps <= 0 detected, %d, set dev aps = 0, wait and chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);
        }
        else if (n9_is_aps_changed(idx))
        { // new aps, set devaps and chk
            log_dbg("%s, idx:%d, state:%s, step:%d, new aps detected, %d -> %d, set dev aps",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), last_aps, aps);
            n9_set_aps(idx, aps); // update last aps
            n9_set_dev_aps(idx, aps);
        }
        else if (n9_is_bsytikchk_timeout(idx))
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, bsytik timeout detected, set dev aps to 0",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_aps(idx, 0);
        }
    }
    else if (sm_get_step(sm) == 30)
    { // chk dev aps = 0 for readycmd
        if (n9_get_dev_aps(idx) == 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk dev aps = 0 ok, wait and chk ap",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 31);
            sm_set_count(sm, 0);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                sm_set_state(sm, SMST_ERR, N9ERR_DHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev aps = 0 for readycmd timeout, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev aps = 0 for readycmd, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
    else if (sm_get_step(sm) == 31)
    { // chk ap = 0  for readycmd

        if (ap < 2 && ap > -2)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk ap ok for readycmd, ap now: %d, goto ready",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);

            if (n9_is_bsytikchk_timeout(idx))
            {
                sm_set_state(sm, SMST_ERR, N9ERR_DHG_TICK_TIMEOUT_DETECTED);
            }
            else
            {
                sm_set_state(sm, SMST_READY, N9ERR_NONE);
            }
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap = 0 for readycmd timeout, ap still: %d, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
                sm_set_state(sm, SMST_ERR, N9ERR_DHG_WAIT_AP0_FOR_READYCMD_TIMEOUT);
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap = 0 for readycmd, count:%d, ap still:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm), ap);
                }
            }
        }
    }
}

static void n9_sm_chg(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct comm_t *comm = &dev->comm;
    struct statemachine_t *sm = &dev->sm;
    int aps = n9_get_aps(idx);
    int last_aps = n9_get_last_aps(idx);
    int ap = n9_get_ap(idx);

    /* chk comm state */
    if (comm_get_state(comm) != COMMST_NORMAL)
    {
        log_dbg("%s, idx:%d, state:%s, step:%d, comm err detected, goto err",
                __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        sm_set_state(sm, SMST_ERR, N9ERR_CHG_COMMERR);
        return;
    }

    if (sm_get_step(sm) == 0)
    { // entry
        log_dbg("%s, idx:%d, state:%s, step:%d, entry", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
        n9_reset_cmd(idx);
        sm_set_step(sm, 10);
        n9_reset_bsytikchk(idx);
        gt_reset_bsytikchk(idx);
    }
    else if (sm_get_step(sm) == 10)
    { // wait and chk
        if (n9_get_cmd(idx) == CMD_SM_READY)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, got ready cmd, set dev aps = 0",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_reset_cmd(idx);
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);
        }
        else if (n9_get_runstat(idx) != N9_RUNSTAT_RUN)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, run state != chg detected, set dev aps to 0, goto err",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_dev_aps(idx, 0);
            sm_set_state(sm, SMST_ERR, N9ERR_CHG_NONCHG_DETECTED);
        }
        else if (aps > 0)
        { // aps => 0
            log_dbg("%s, idx:%d, state:%s, step:%d, aps >= 0 detected, %d, set dev aps = 0, wait and chk",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), aps);
            n9_set_dev_aps(idx, 0);
            sm_set_step(sm, 30);
            sm_set_count(sm, 0);
        }
        else if (n9_is_aps_changed(idx))
        { /* new aps, set devaps and chk */
            log_dbg("%s, idx:%d, state:%s, step:%d, new aps detected, %d -> %d, set dev aps",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), last_aps, aps);
            n9_set_aps(idx, aps); // update last aps
            n9_set_dev_aps(idx, aps);
        }
        else if (n9_is_bsytikchk_timeout(idx))
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, bsytik timeout detected, set dev aps to 0",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            n9_set_aps(idx, 0);
        }
    }
    else if (sm_get_step(sm) == 30)
    { // chk dev aps = 0 for readycmd
        if (n9_get_dev_aps(idx) == 0)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk dev aps = 0 ok, wait and chk ap",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            sm_set_step(sm, 31);
            sm_set_count(sm, 0);
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                sm_set_state(sm, SMST_ERR, N9ERR_CHG_WAIT_DEVAPS0_FOR_READYCMD_TIMEOUT);
                log_dbg("%s, idx:%d, state:%s, step:%d, wait dev aps = 0 for readycmd timeout, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm));
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting dev aps = 0 for readycmd, count:%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm));
                }
            }
        }
    }
    else if (sm_get_step(sm) == 31)
    { // chk ap = 0  for readycmd
        if (ap < 2 && ap > -2)
        {
            log_dbg("%s, idx:%d, state:%s, step:%d, chk ap ok for readycmd, ap now: %d, goto ready",
                    __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
            if (n9_is_bsytikchk_timeout(idx))
            {
                sm_set_state(sm, SMST_ERR, N9ERR_CHG_TICK_TIMEOUT_DETECTED);
            }
            else
            {
                sm_set_state(sm, SMST_READY, N9ERR_NONE);
            }
        }
        else
        {
            sm_inc_count(sm);
            if (sm_get_count(sm) >= 100)
            {
                log_dbg("%s, idx:%d, state:%s, step:%d, wait ap = 0 for readycmd timeout, ap still :%d, goto err", __func__, idx, sm_get_szstate(sm), sm_get_step(sm), ap);
                sm_set_state(sm, SMST_ERR, N9ERR_CHG_WAIT_AP0_FOR_READYCMD_TIMEOUT);
            }
            else
            {
                if (sm_get_count(sm) % 50 == 0)
                {
                    log_dbg("%s, idx:%d, state:%s, step:%d, waiting ap = 0 for readycmd, count:%d, ap still :%d",
                            __func__, idx, sm_get_szstate(sm), sm_get_step(sm), sm_get_count(sm), ap);
                }
            }
        }
    }
}

void n9_sm(int idx)
{
    struct n9_t *dev = &n9[idx];
    struct statemachine_t *sm = &dev->sm;

    sm_cal_timing(&dev->sm);

    if (device_power_on == 0)
    {
        n9_set_dev_aps(idx, 0);
        n9_set_dev_stopcmd(idx);
        sm_set_state(sm, SMST_ERR, N9ERR_ERR_PWROFF);
    }
    switch (sm_get_state(sm))
    {
    case SMST_LAUNCH:
        n9_sm_launch(idx);
        break;

    case SMST_STDBY:
        n9_sm_stdby(idx);
        break;

    case SMST_STOP:
        n9_sm_stop(idx);
        break;

    case SMST_OFFGRID:
        n9_sm_offgrid(idx);
        break;

    case SMST_READY:
        n9_sm_ready(idx);
        break;

    case SMST_DHG:
        n9_sm_dhg(idx);
        break;

    case SMST_CHG:
        n9_sm_chg(idx);
        break;

    case SMST_ERR:
        n9_sm_err(idx);
        break;

    default:
        log_dbg("%s, unknown state, %d", __func__, n9_get_state(idx));
        break;
    }
}