File content as of revision 18:a21199781d20:

#include "Utility.hpp"
#include "mbed.h"
#include <stdlib.h>
#include <list>
#include <cmath>

namespace utility
{
    bool is_almost_equal(double a, double b, double tolerance)
    {
        double difference = std::abs(a-b);
        return (difference <= tolerance);
    }
    
    //Return angle between 0 and 360 degree
    double wrap_angle(double angle)
    {
        return angle - 360 * std::floor( angle / 360 );
    } 
    
    double degree_from_radian(const double angle_radian)
    {
        const double angle_degree = angle_radian * 180.0 / PI;
        return wrap_angle(angle_degree);
    }
    
    unsigned int update_bit(const unsigned int previous_4_bytes, const int position, const bool new_bit_value)
    {
        return update_bits(previous_4_bytes, position, position, 0xFFFFFFFF, new_bit_value);
    }
    
    unsigned int update_bits(const unsigned int previous_4_bytes, const int start_bit, const int stop_bit, const unsigned int reserved_bits_mask, const unsigned int new_bits)
    {
        const unsigned int all_ones_but_n_zeros_right_shifted = (0xFFFFFFFF << (stop_bit - start_bit + 1));
        const unsigned int all_zeros_but_n_ones_right_shifted = ~all_ones_but_n_zeros_right_shifted;
        
        const unsigned int all_zeros_but_ones_at_position = all_zeros_but_n_ones_right_shifted << start_bit;
        const unsigned int all_ones_but_zeros_at_position = ~all_zeros_but_ones_at_position;
        
        const unsigned int all_zeros_but_new_bits_at_position = new_bits << start_bit;
        const unsigned int all_unchanged_but_zeros_at_position = previous_4_bytes & all_ones_but_zeros_at_position;
        const unsigned int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zeros_at_position | all_zeros_but_new_bits_at_position;
        
        const unsigned int updated_bits_with_reserved_bits_at_zero = all_unchanged_but_new_bit_value_at_position & reserved_bits_mask;
        
        return updated_bits_with_reserved_bits_at_zero;
    }
    
    void blink()
    {
        DigitalOut led(LED1);
        while(1) 
        {
            led = 1;
            wait(0.2);
            led = 0;
            wait(0.2);
        }
    }
    
    MovingAverageFilter::MovingAverageFilter(const int subSize):
        subsetSize(subSize)
    {
        for(int i=0; i<subSize; i++)
        {
            subset.push_front(0);
        }
    }
    
    int MovingAverageFilter::calculate(int newValue)
    {
        subset.push_front(newValue);
        subset.pop_back();
        int result = 0;
        
        for (std::list<int>::iterator it=subset.begin(); it != subset.end(); ++it)
        {
            result += *it;
        }
        result = result / subsetSize;
        
        return result;
    }
}