hpm6750/controller_yy_app/software/soft_rc_input.c

#include "soft_rc_input.h"
#include "auto_pilot.h"
#include "common.h"
#include "control_throttle.h"
#include "hard_rc_sbus.h"
#include "soft_flash.h"
#include "soft_time.h"
#include "ver_config.h"

/* sbus 信号状态 */
typedef enum
{
    SBUS_SIGNAL_OK = 0x00,
    SBUS_SIGNAL_LOST = 0x01,
    SBUS_SIGNAL_FAILSAFE = 0x03,
} RCSbus_SignalStatus;

//******************************************
//摇杆输入
short rock_in[4] = {1500, 1500, 1500, 1500};
//摇杆按键
char rock_key = KEY_UP;

//******************************************

// PWM输入的值  单位 us  //不上电时是默认值
short rc_in[RC_INPUT_CH_NUM] = {1500, 1500, 1500, 1500, 1200, 1800, 1800};
// pwm输入的原始值  //不上电时是默认值
short raw_rc_in[RC_INPUT_CH_NUM] = {1500, 1500, 1500, 1500, 1200, 1800, 1800};

/**************************实现函数********************************************
 *函数原型:		void rc_input_initial(void)
 *功　　能:	  输入初始化
 *******************************************************************************/
void rc_input_initial(void) { rc_sbus_init(SBUS_BPS); }

/**************************实现函数********************************************
*函数原型:		void get_rc_value(void)
*功　　能:	    获取get_rc的输入值
*返 回 值：		1        正常
                0        异常
*******************************************************************************/
unsigned char subs_failsafe_status;

//遥控器是否有效
comp_status comp_rc_status = COMP_NOEXIST;

// sbus 失联记时
static uint32_t sbus_data_link_lost_time_us = 0;

//遥控器健康标志位
RcSignalHealthType rc_signal_health = RC_SIGNAL_HEALTH;

//遥控器输入中间量
short tmp_rc_in[RC_INPUT_CH_NUM] = {0};

void get_rc_value(void)
{
    uint8_t i = 0;
    static short health_counts = 0;
    static short fail_counts = 0;
    static short bad_counts = 0;

    //如果遥控器有效
    if (comp_rc_status == COMP_NORMAL)
    {
        for (i = 0; i < RC_CALIB_CH_NUM; i++)
        {
            if (raw_rc_in[i] >= rc_cal_offset[i])
            {
                tmp_rc_in[i] = (raw_rc_in[i] - rc_cal_offset[i]) *
                                   rc_cal_factor_up[i] / 1000 +
                               1500;
            }
            else if (raw_rc_in[i] < rc_cal_offset[i])
            {
                tmp_rc_in[i] = (raw_rc_in[i] - rc_cal_offset[i]) *
                                   rc_cal_factor_down[i] / 1000 +
                               1500;
            }
        }

        for (uint8_t i = RC_CH5; i < RC_INPUT_CH_NUM; ++i)
        {
            tmp_rc_in[i] = raw_rc_in[i];
        }

        //所有通道都在最大最小值得范围内,所有数据正常
        if ((tmp_rc_in[RC_CH1] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH1] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH2] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH2] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH3] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH3] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH4] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH4] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH5] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH5] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH6] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH6] <= RC_IN_MAX) &&
            (tmp_rc_in[RC_CH7] >= RC_IN_MIN &&
             tmp_rc_in[RC_CH7] <= RC_IN_MAX) &&
            (subs_failsafe_status == SBUS_SIGNAL_OK))
        {
            //正常数据，赋值给rc_in
            for (i = 0; i < RC_INPUT_CH_NUM; i++)
            {
                rc_in[i] = tmp_rc_in[i];
            }

            //出错或失控后如果400ms都正常，就恢复遥控器的值
            if ((health_counts++) >= 25)
            {
                rc_signal_health = RC_SIGNAL_HEALTH;
                health_counts = 25;
            }

            fail_counts = 0;
            bad_counts = 0;
        }
        //失控保护,futaba的遥控器的拨码开关不是直接跳变，会有中间值出现，所以要加缓冲
        else if ((tmp_rc_in[RC_CH1] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH1] <= RC_IN_MAX) &&
                 (tmp_rc_in[RC_CH2] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH2] <= RC_IN_MAX) &&
                 (tmp_rc_in[RC_CH3] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH3] <= RC_IN_MAX) &&
                 (tmp_rc_in[RC_CH4] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH4] <= RC_IN_MAX) &&
                 (tmp_rc_in[RC_CH5] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH5] <= RC_IN_MAX) &&
                 (tmp_rc_in[RC_CH6] >= RC_IN_MIN &&
                  tmp_rc_in[RC_CH6] <= RC_IN_MAX) &&
                 ((tmp_rc_in[RC_CH7] >= RC_FS_MIN &&
                   tmp_rc_in[RC_CH7] <= RC_FS_MAX) ||
                  (subs_failsafe_status != SBUS_SIGNAL_OK)))
        {
            //失控保护数据，赋值给rc_in
            for (i = 0; i < RC_INPUT_CH_NUM; i++)
            {
                rc_in[i] = tmp_rc_in[i];
            }

            if ((fail_counts++) >= 50)
            {
                rc_signal_health = RC_SIGNAL_FAIL;
                fail_counts = 50;
            }

            health_counts = 0;
            bad_counts = 0;
        }
        //跳出最大最小值的范围就认为遥控器出错,遥控器出错不更新rc_in的值，持续出错执行错误保护
        else
        { //数据错误后都赋值为默认值，赋值给rc_in
            for (i = 0; i < 4; i++)
            {
                rc_in[i] = 1500;
            }
            // GPS模式
            rc_in[RC_CH5] = 1200;
            //机头锁定
            rc_in[RC_CH6] = 1800;
            //待命
            rc_in[RC_CH7] = 1800;

            if ((bad_counts++) >= 25)
            {
                rc_signal_health = RC_SIGNAL_BAD;
                bad_counts = 25;
            }

            health_counts = 0;
            fail_counts = 0;
        }
    }
}

/**
 * @brief sbus 信号转 pwm
 */
void sbus2pwm(int16_t *pwm, int16_t *sbus_ch)
{
    uint8_t i = 0;

    for (i = 0; i < RC_INPUT_CH_NUM; i++)
    {
        pwm[i] = (sbus_ch[i] - 1024) * 420 / 672 + 1500;
    }
}

/**
 * @brief sbus 信号解析
 */
void sbus_channel_anslysis(uint8_t *sbus_data)
{
    static int16_t channels[SBUS_CH] = {1023, 1023, 1023, 1023, 1023, 1023,
                                        1023, 1023, 1023, 1023, 1023, 1023,
                                        1023, 1023, 1023, 1023, 0,    0};

    // buf1的8位作为ch1的低8位，buf2的低3位作为ch1的高3位，组成ch1的11位。
    channels[0] = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF);
    // buf2的高5位作为ch2的低5位，buf3的低6位作为ch2的高6位，组成ch2的11位。
    channels[1] = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF);
    //以此类推，每个通道占11位
    channels[2] =
        ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF);
    channels[3] = ((sbus_data[5] >> 1 | sbus_data[6] << 7) & 0x07FF);
    channels[4] = ((sbus_data[6] >> 4 | sbus_data[7] << 4) & 0x07FF);
    channels[5] =
        ((sbus_data[7] >> 7 | sbus_data[8] << 1 | sbus_data[9] << 9) & 0x07FF);
    channels[6] = ((sbus_data[9] >> 2 | sbus_data[10] << 6) & 0x07FF);
    channels[7] = ((sbus_data[10] >> 5 | sbus_data[11] << 3) & 0x07FF);

#ifdef ALL_CHANNELS
    // & the other 8 + 2 channels if you need them
    channels[8] = ((sbus_data[12] | sbus_data[13] << 8) & 0x07FF);
    channels[9] = ((sbus_data[13] >> 3 | sbus_data[14] << 5) & 0x07FF);
    channels[10] =
        ((sbus_data[14] >> 6 | sbus_data[15] << 2 | sbus_data[16] << 10) &
         0x07FF);
    channels[11] = ((sbus_data[16] >> 1 | sbus_data[17] << 7) & 0x07FF);
    channels[12] = ((sbus_data[17] >> 4 | sbus_data[18] << 4) & 0x07FF);
    channels[13] =
        ((sbus_data[18] >> 7 | sbus_data[19] << 1 | sbus_data[20] << 9) &
         0x07FF);
    channels[14] = ((sbus_data[20] >> 2 | sbus_data[21] << 6) & 0x07FF);
    channels[15] = ((sbus_data[21] >> 5 | sbus_data[22] << 3) & 0x07FF);

    // DigiChannel 1
    if (sbus_data[23] & (1 << 0))
    {
        channels[16] = 1;
    }
    else
    {
        channels[16] = 0;
    }
    // DigiChannel 2
    if (sbus_data[23] & (1 << 1))
    {
        channels[17] = 1;
    }
    else
    {
        channels[17] = 0;
    }
#endif

    // Failsafe
    subs_failsafe_status = SBUS_SIGNAL_OK;
    //信号丢失
    if (sbus_data[23] & (1 << 2))
    {
        subs_failsafe_status = SBUS_SIGNAL_LOST;
    }
    //失控保护激活
    if (sbus_data[23] & (1 << 3))
    {
        subs_failsafe_status = SBUS_SIGNAL_FAILSAFE;
    }

    sbus2pwm(raw_rc_in, channels);
}

/**
 * @brief 串口接收到sbus信号的钩子函数
 */
void recv_rcsbus_data_hookfunction(unsigned int len)
{
    //中断处理函数
    if (len == 25 && uart2_dma_rx_buf[0] == 0x0f &&
        (uart2_dma_rx_buf[24] == 0x00 || uart2_dma_rx_buf[24] == 0x04 ||
         uart2_dma_rx_buf[24] == 0x14 || uart2_dma_rx_buf[24] == 0x24 ||
         uart2_dma_rx_buf[24] == 0x34))
    {

        comp_rc_status = COMP_NORMAL;
        sbus_data_link_lost_time_us = 0;

        sbus_channel_anslysis(uart2_dma_rx_buf);
    }
}

/**
 * @brief sbus 输入信号失联判断
 * @param dt_s 运行间隔 s
 */
void check_sbus_link_status(float dt_s)
{
    if (COMP_NORMAL == comp_rc_status)
    {
        sbus_data_link_lost_time_us += dt_s * 1000000;

        if (sbus_data_link_lost_time_us > 500 * 1000)
        {
            comp_rc_status = COMP_LOST;
        }
    }
}

unsigned int rc_cal_time = 0;

//用来记录遥控器输入的最大最小值,全部初始化为1500.。。。
int rc_cal_max[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500},
    rc_cal_min[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500};

//开始遥控器校准的参数
RCCalibStatusType rc_cal_flag = RC_CALIB_NO;
bool rc_offset_capture_flag = false;

// 遥控器各通道的中立位
short rc_cal_offset[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500};

void rc_input_calib_start(void)
{
    if (rc_cal_flag == RC_CALIB_NO)
    {
        //赋值进入遥控器校准模式
        pilot_mode = PILOT_RC_CLB;
        rc_cal_flag = RC_CALIB_START;
        rc_offset_capture_flag = true;
        //点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，，
        rc_cal_time = micros();
    }
}

void rc_input_calib_end(void)
{
    //开始校准并且上锁状态下
    if ((rc_cal_flag == RC_CALIB_START) && (thr_lock_status == LOCKED))
    {
        rc_cal_flag = RC_CALIB_NO;

        write_rcfactor_flag = true;
        //点击校准结束后，绿灯灭，只剩红灯
    }
}

void rc_input_calibration(void)
{
    uint8_t i = 0;

    //捕获中立位同时清除以前记录的最大最小值
    if (rc_offset_capture_flag == true)
    {
        for (i = 0; i < RC_CALIB_CH_NUM; i++)
        {
            //记录中立位
            rc_cal_offset[i] = raw_rc_in[i];

            //初始化最大最小值
            rc_cal_min[i] = 1500;
            rc_cal_max[i] = 1500;
        }

        //如果校准时油门没放在中间，就不记录油门的中立位
        if (rc_cal_offset[RC_CH3] < 1350 || rc_cal_offset[RC_CH3] > 1650)
        {
            rc_cal_offset[RC_CH3] = 1500;
        }

        rc_offset_capture_flag = false;
    }

    for (i = 0; i < RC_CALIB_CH_NUM; i++)
    {
        //记录CH的最大最小值
        if (rc_cal_max[i] < raw_rc_in[i])
        {
            rc_cal_max[i] = raw_rc_in[i];
        }
        if (rc_cal_min[i] > raw_rc_in[i])
        {
            rc_cal_min[i] = raw_rc_in[i];
        }
    }
}

//比例系数,,实际的比例系数放大1000倍后的值，便于flash存储
short rc_cal_factor_up[RC_CALIB_CH_NUM] = {1000, 1000, 1000, 1000};
short rc_cal_factor_down[RC_CALIB_CH_NUM] = {1000, 1000, 1000, 1000};

/**
 * @brief 计算遥控器校准系数
 */
void calc_rcfactor_calibration(void)
{
    unsigned char i = 0;

    for (i = 0; i < RC_CALIB_CH_NUM; i++)
    {
        rc_cal_factor_up[i] =
            (short)((1000 * 500) / (rc_cal_max[i] - rc_cal_offset[i]));
        rc_cal_factor_down[i] =
            (short)((1000 * 500) / (rc_cal_offset[i] - rc_cal_min[i]));
    }
}