Кибернетика, автоматика, протоколы

/*******************************************************************************
    Зыс из псевдокоде. Логика выдрана кусками из проекта и упрощена.
    Мог что-то важное упустить. Хотя может и заработает :)
    Ограничение: за время усреднения значение не должно изменяться более, чем на половину диапазона.
*/

bool        canCrossUp;         // Хитрый флаг. Показывает на возможность пересечения границы max/0 во время усреднения.
bool        canCrossDown;       // Хитрый флаг. Показывает на возможность пересечения границы 0/max во время усреднения.
uint16_t    averageSummator;

/*******************************************************************************
*/
void AS5048A_averagerStart( uint16_t data )
{
    // Т.к. данные для AS5048A это 14 бит без знака, на всякий случай обнуляем два старших бита.
    data = data & 0x3FFF;
    // Eсли значение от 3/4*2Пи до 2Пи, то возможно пересечение границ диапазона [0x00000, x3FFF] вверх.
    if( data > 0x2FFF )
        canCrossUp = true;
    else
        canCrossUp = false;
    // Eсли значение от 0 до 1/4*2Пи, то возможно пересечение границы диапазона [0x00000, x3FFF] вниз.
    if( data < 0x1000 )
        canCrossDown = true;
    else
        canCrossDown = false;
    
    averageSummator = 0;
    
}    

/*******************************************************************************
    Добавляем очередные данные к усреднителю.
*/
void AS5048A_averagerUpdate( uint16_t data )
{
    // Т.к. данные для AS5048A это 14 бит без знака, на всякий случай обнуляем два старших бита.
    data = data & 0x3FFF;
    // @AveragerAdd. Данные для усреднителя сдвигаем от нуля на полный диапазон, чтобы при обработке пересечения границ не связываться со знаковыми числами (а у нас числа разрядностью 14 бит!). После усреднения это значение будет вычтено обратно. А разрядности uint16_t для 14-битных данных хватит.
    data += 0x4000;
    // Проверяем условие пересечения границ. Если пересечение границы возможно, и текущее считанное значение лежит далеко от усредняемого, то его необходимо привести к окрестности усредняемого значения.
    // Приведение вверх.
    if( canCrossUp && (data < 0x6000) )
        data += 0x4000;
    // Приведение вниз.
    if( (canCrossDown && (data >= 0x6000) )
        data -= 0x4000;
    
    // Добавляем значение к усреднителю.
    averageSummator += data;
}

/*******************************************************************************
    Получаем данные, усредняя их за несколько чтений датчика подряд.
*/
uint16_t AS5048A_getAveraged( uint8_t requestedNumberOfRead )
{
    if( requestedNumberOfRead == 0 ) return( 0 );
    
    // Читаем данные с датчика.
    uint16_t data = AS5048A_read();
    // Подготавливаем усреднитель. Данные нужны для нахождения флагов пересечения границ.
    AS5048A_averagerStart( data );
    // После запуска усреднителя отправляем в него первые считанные данные.
    AS5048A_averagerUpdate( data );
    // Последующие данные просто в усреднитель.
    for( uint8_t i = 0; i < (requestedNumberOfRead-1); ++i )
        AS5048A_averagerUpdate( AS5048A_read() );
        
    // Для читабельности.
    uint16_t result = AS5048A_handler->averageSummator;
    // Делим сумму на количество циклов усреднения.
    result /= requestedNumberOfRead;
    // Теперь надо привести значение к диапазону [0..0x3FFF].
    if( result >= 0x4000 ) result -= 0x4000; // Это компенсирует "добавку". См. @AveragerAdd. Вроде условие здесь можно выкинуть.
    if( result >= 0x4000 ) result -= 0x4000; // Повторный вычет - не ошибка. Сначала "безусловно" компенсировали добавку, а теперь проверяем, укладываемся ли в диапазон [0, 0x3FFF].
    
    return( result );
}