gcs-nf/MavLinkNode/sbusparser.cpp

#include "sbusparser.h"
#include "QDebug"

bool SBusParser::parse_char(uint8_t c)
{
    bool rslt = false;
    switch (status)
    {
    case 0u:
        if (c == 0xEB)
        {
            ++status;
        }
        break;
    case 1u:
        if (c == 0x90)
        {
            ++status;
        }
        else
        {
            status = 0u;
        }
        break;
    case 2u:
        if (c == 0x0F)
        {
            Buf[0] = c;
            idx = 1;
            sum = c;
            ++status;
        }
        else
        {
            status = 0u;
        }
        break;
    case 3u:
        Buf[idx++] = c;
        sum += c;
        if (idx == 25u)
        {
            ++status;
        }
        break;
    case 4u:
        seq = c;
        sum += c;
        ++status;
        break;
    case 5u:
        if (c == sum)
        {
            if (update())
            {
                rslt = true;
            }
        }
        status = 0u;
        break;
    default:
        status = 0u;
    }
    return rslt;
}

bool SBusParser::update(void)
{
    uint16_t temp;
    uint16_t ChannelTemp[17];

    ChannelTemp[1]  = ((Buf[2]  & 0x07) << 8) | (Buf[1]);
    ChannelTemp[2]  = ((Buf[3]  & 0x3F) << 5) | (Buf[2]  >> 3);
    ChannelTemp[3]  = ((Buf[5]  & 0x01) << 10)| (Buf[4]  << 2) | (Buf[3] >> 6);
    ChannelTemp[4]  = ((Buf[6]  & 0x0F) << 7) | (Buf[5]  >> 1);
    ChannelTemp[5]  = ((Buf[7]  & 0x7F) << 4) | (Buf[6]  >> 4);
    ChannelTemp[6]  = ((Buf[9]  & 0x03) << 9) | (Buf[8]  << 1) | (Buf[7] >> 7);
    ChannelTemp[7]  = ((Buf[10] & 0x1F) << 6) | (Buf[9]  >> 2);
    ChannelTemp[8]  = ((Buf[11] & 0xFF) << 3) | (Buf[10] >> 5);
    ChannelTemp[9]  = ((Buf[13] & 0x07) << 8) | (Buf[12]);
    ChannelTemp[10] = ((Buf[14] & 0x3F) << 5) | (Buf[13] >> 3);
    ChannelTemp[11] = ((Buf[16] & 0x01) << 10)| (Buf[15] << 2) | (Buf[14] >> 6);
    ChannelTemp[12] = ((Buf[17] & 0x0F) << 7) | (Buf[16] >> 1);
    ChannelTemp[13] = ((Buf[18] & 0x7F) << 4) | (Buf[17] >> 4);
    ChannelTemp[14] = ((Buf[20] & 0x03) << 9) | (Buf[19] << 1) | (Buf[18] >> 7);
    ChannelTemp[15] = ((Buf[21] & 0x1F) << 6) | (Buf[20] >> 2);
    ChannelTemp[16] = ((Buf[22] & 0xFF) << 3) | (Buf[21] >> 5);

    Channel[0] = seq;
    for(int i = 1; i <= 16; ++i )
    {
        temp = (uint16_t)(0.606f * ChannelTemp[i] + 879.39f);
        if (temp >=800 && temp <= 2200)
        {
            Channel_Old[FilterCount][i] = temp;

            //qDebug() <<FilterFirstIni;
            if(FilterFirstIni == 0)//先取5次数据再进行滤波处理
            {
                //去掉一个最大值和最小值
                float ChannelSum = 0;
                uint16_t MaxValue = 0,MinValue = 0xffff;
                for(int j = 0;j<DeepOfFilter;j++)
                {
                    MaxValue = (MaxValue>Channel_Old[j][i])?(MaxValue):(Channel_Old[j][i]);//fine(Channel_Old[j][i],MaxValue,0x00);//比较最大值
                    MinValue = (MinValue<Channel_Old[j][i])?(MinValue):(Channel_Old[j][i]);//fine(Channel_Old[j][i],MinValue,0x01);//比较最小值
                    ChannelSum += Channel_Old[j][i];

                    //qDebug() << j << FilterCount << i << MaxValue << MinValue <<ChannelSum;
                }
                ChannelSum = (ChannelSum - MaxValue - MinValue)/(DeepOfFilter - 2);

                //如果存在野值，那么就用均值将野值代替掉
                /*
                if((MaxValue == Channel_Old[FilterCount][i])||(MinValue == Channel_Old[FilterCount][i]))
                {
                    Channel_Old[FilterCount][i] = (uint16_t)ChannelSum;
                }
                */




                if (ChannelSum >=800 && ChannelSum <= 2200)
                {
                   Channel[i] = ChannelSum;
                }
            }
            else
            {
                if (temp >=800 && temp <= 2200)
                {
                   Channel[i] = temp;
                }

                FilterFirstIni--;
                if(FilterFirstIni <= 0)FilterFirstIni = 0;
            }

            //将计数值清零
            FilterCount++;
            if(FilterCount>=DeepOfFilter)
            {
                FilterCount = 0;
            }

            if(FilterFirstIni > 0)
            {
                return false;
            }


        }
        else
        {
            return false;
        }


    }

    if(Buf[23] & (1<<1))//bit 1
    {
      Channel[17] = 1900;
    }
    else
    {
      Channel[17] = 1100;
    }

    //DigiChannel 2
    if(Buf[23] & (1))//bit 0
    {
      Channel[18] = 1900;
    }
    else
    {
      Channel[18] = 1100;
    }

    Channel[19] = (uint16_t)(Buf[23] & 0x0C);//signal is lost/fail/ok?
    if (Channel[19])
    {
        return false;
    }
    return true;
}

//type == 0x00 比较大的值，type = 0x01,比较小的值
uint16_t SBusParser::fine(uint16_t data,uint16_t compare,uint8_t type)
{
    uint16_t Max = 0,Min = 0xffff;

    switch (type) {
        case 0x00:
        {
            Max = (data >= compare)?(data):(compare);
        }break;
        case 0x01:
        {
            Min = (data <= compare)?(data):(compare);
        }break;
    }
    return (type)?(Min):(Max);
}