Bruno Allaire-Lemay / Mbed 2 deprecated APP1test

df

Dependencies:   mbed

Fork of APP1 by Team APP

Utility.cpp@18:a21199781d20, 2017-01-17 (annotated)

Committer:
dupm2216
Date:
Tue Jan 17 19:45:02 2017 +0000
Revision:
18:a21199781d20
Parent:
15:b38d9d210e32
Child:
21:a111be2582be
Add MovingAverageFilter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dupm2216 6:3facf0329142 1 #include "Utility.hpp"
dupm2216 15:b38d9d210e32 2 #include "mbed.h"
dupm2216 18:a21199781d20 3 #include <stdlib.h>
dupm2216 18:a21199781d20 4 #include <list>
dupm2216 6:3facf0329142 5 #include <cmath>
dupm2216 6:3facf0329142 6
dupm2216 6:3facf0329142 7 namespace utility
dupm2216 6:3facf0329142 8 {
dupm2216 6:3facf0329142 9 bool is_almost_equal(double a, double b, double tolerance)
dupm2216 6:3facf0329142 10 {
dupm2216 6:3facf0329142 11 double difference = std::abs(a-b);
dupm2216 6:3facf0329142 12 return (difference <= tolerance);
dupm2216 6:3facf0329142 13 }
dupm2216 6:3facf0329142 14
dupm2216 6:3facf0329142 15 //Return angle between 0 and 360 degree
dupm2216 6:3facf0329142 16 double wrap_angle(double angle)
dupm2216 6:3facf0329142 17 {
dupm2216 6:3facf0329142 18 return angle - 360 * std::floor( angle / 360 );
dupm2216 6:3facf0329142 19 }
dupm2216 6:3facf0329142 20
dupm2216 6:3facf0329142 21 double degree_from_radian(const double angle_radian)
dupm2216 6:3facf0329142 22 {
dupm2216 6:3facf0329142 23 const double angle_degree = angle_radian * 180.0 / PI;
dupm2216 6:3facf0329142 24 return wrap_angle(angle_degree);
dupm2216 6:3facf0329142 25 }
GaiSensei 12:1c341b119b23 26
GaiSensei 13:bb9669053eb3 27 unsigned int update_bit(const unsigned int previous_4_bytes, const int position, const bool new_bit_value)
GaiSensei 13:bb9669053eb3 28 {
dupm2216 15:b38d9d210e32 29 return update_bits(previous_4_bytes, position, position, 0xFFFFFFFF, new_bit_value);
GaiSensei 13:bb9669053eb3 30 }
GaiSensei 13:bb9669053eb3 31
dupm2216 15:b38d9d210e32 32 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)
GaiSensei 12:1c341b119b23 33 {
dupm2216 15:b38d9d210e32 34 const unsigned int all_ones_but_n_zeros_right_shifted = (0xFFFFFFFF << (stop_bit - start_bit + 1));
dupm2216 15:b38d9d210e32 35 const unsigned int all_zeros_but_n_ones_right_shifted = ~all_ones_but_n_zeros_right_shifted;
dupm2216 15:b38d9d210e32 36
dupm2216 15:b38d9d210e32 37 const unsigned int all_zeros_but_ones_at_position = all_zeros_but_n_ones_right_shifted << start_bit;
dupm2216 15:b38d9d210e32 38 const unsigned int all_ones_but_zeros_at_position = ~all_zeros_but_ones_at_position;
dupm2216 15:b38d9d210e32 39
dupm2216 15:b38d9d210e32 40 const unsigned int all_zeros_but_new_bits_at_position = new_bits << start_bit;
dupm2216 15:b38d9d210e32 41 const unsigned int all_unchanged_but_zeros_at_position = previous_4_bytes & all_ones_but_zeros_at_position;
dupm2216 15:b38d9d210e32 42 const unsigned int all_unchanged_but_new_bit_value_at_position = all_unchanged_but_zeros_at_position | all_zeros_but_new_bits_at_position;
dupm2216 15:b38d9d210e32 43
dupm2216 15:b38d9d210e32 44 const unsigned int updated_bits_with_reserved_bits_at_zero = all_unchanged_but_new_bit_value_at_position & reserved_bits_mask;
GaiSensei 13:bb9669053eb3 45
dupm2216 15:b38d9d210e32 46 return updated_bits_with_reserved_bits_at_zero;
dupm2216 15:b38d9d210e32 47 }
dupm2216 15:b38d9d210e32 48
dupm2216 15:b38d9d210e32 49 void blink()
dupm2216 15:b38d9d210e32 50 {
dupm2216 15:b38d9d210e32 51 DigitalOut led(LED1);
dupm2216 15:b38d9d210e32 52 while(1)
dupm2216 15:b38d9d210e32 53 {
dupm2216 15:b38d9d210e32 54 led = 1;
dupm2216 15:b38d9d210e32 55 wait(0.2);
dupm2216 15:b38d9d210e32 56 led = 0;
dupm2216 15:b38d9d210e32 57 wait(0.2);
dupm2216 15:b38d9d210e32 58 }
GaiSensei 12:1c341b119b23 59 }
dupm2216 18:a21199781d20 60
dupm2216 18:a21199781d20 61 MovingAverageFilter::MovingAverageFilter(const int subSize):
dupm2216 18:a21199781d20 62 subsetSize(subSize)
dupm2216 18:a21199781d20 63 {
dupm2216 18:a21199781d20 64 for(int i=0; i<subSize; i++)
dupm2216 18:a21199781d20 65 {
dupm2216 18:a21199781d20 66 subset.push_front(0);
dupm2216 18:a21199781d20 67 }
dupm2216 18:a21199781d20 68 }
dupm2216 18:a21199781d20 69
dupm2216 18:a21199781d20 70 int MovingAverageFilter::calculate(int newValue)
dupm2216 18:a21199781d20 71 {
dupm2216 18:a21199781d20 72 subset.push_front(newValue);
dupm2216 18:a21199781d20 73 subset.pop_back();
dupm2216 18:a21199781d20 74 int result = 0;
dupm2216 18:a21199781d20 75
dupm2216 18:a21199781d20 76 for (std::list<int>::iterator it=subset.begin(); it != subset.end(); ++it)
dupm2216 18:a21199781d20 77 {
dupm2216 18:a21199781d20 78 result += *it;
dupm2216 18:a21199781d20 79 }
dupm2216 18:a21199781d20 80 result = result / subsetSize;
dupm2216 18:a21199781d20 81
dupm2216 18:a21199781d20 82 return result;
dupm2216 18:a21199781d20 83 }
dupm2216 9:12519f9dd3cd 84 }