#include "rc_rock.h"

/* 摇杆失联判定 2s */
#define RC_ROCK_LINK_LOST_TIME_US 2000000


/**
 * @brief 摇杆数据初始化, 上电时调用
 *
 */
void RcRock_Struct_Init(RC_Rock_Data *prc_rock)
{
    prc_rock->rc.signal_health = RC_SIGNAL_HEALTH;
    prc_rock->rc.link_status = COMP_NOEXIST;
    for (uint8_t ch = 0; ch < RC_INPUT_CH_NUM; ch++)
    {
        prc_rock->rc.ch_value[ch] = 1500;
        prc_rock->rc.raw_ch_value[ch] = 1500;

        prc_rock->rc.calib_offset[ch] = 1500;
        prc_rock->rc.calib_factor_up[ch] = 1000;
        prc_rock->rc.calib_factor_down[ch] = 1000;
    }
}

/**
 * @brief 监测 rc_rock 通讯连接状态, 本函数至少 5 hz 频率调用
 *
 */
void RcRock_CheckLinkStatus(float dt_s)
{
    RC_Rock_Data *prock = &rc_rock;

    if (prock->rc.link_status == COMP_NORMAL)
    {
        prock->_link_lost_time_us += dt_s * 1000000;

        if (prock->_link_lost_time_us > RC_ROCK_LINK_LOST_TIME_US)
        {
            prock->rc.link_status = COMP_LOST;
        }
    }
}