File content as of revision 15:b38d9d210e32:

#include "Utility.hpp"
#include "mbed.h"

#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);
        }
    }
}