PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_simple_analyzer.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_simple_analyzer.cpp
36  *
37  * @author Achermann Florian <acfloria@ethz.ch>
38  */
39 
41 
42 #include <float.h>
43 
44 #include <px4_platform_common/log.h>
45 #include <px4_platform_common/defines.h>
46 
48  _window(window),
49  _mode(mode)
50 {
51  reset();
52 }
53 
55 {
56  _n = 0;
57 
58  switch (_mode) {
59  case AVERAGE:
60  _result = 0.0f;
61 
62  break;
63 
64  case MIN:
65  _result = FLT_MAX;
66 
67  break;
68 
69  case MAX:
70  _result = FLT_MIN;
71 
72  break;
73 
74  default:
75  PX4_ERR("SimpleAnalyzer: Unknown mode.");
76  }
77 }
78 
79 void SimpleAnalyzer::add_value(float val, float update_rate)
80 {
81  switch (_mode) {
82  case AVERAGE:
83  _result = (_result * _n + val) / (_n + 1u);
84 
85  break;
86 
87  case MIN:
88  if (val < _result) {
89  _result = val;
90  }
91 
92  break;
93 
94  case MAX:
95  if (val > _result) {
96  _result = val;
97  }
98 
99  break;
100  }
101 
102  // if we get more measurements than n_max so the exponential moving average
103  // is computed
104  if ((_n < update_rate * _window) && (update_rate > 1.0f)) {
105  _n++;
106  }
107 
108  // value sanity checks
109  if (!PX4_ISFINITE(_result)) {
110  PX4_DEBUG("SimpleAnalyzer: Result is not finite, reset the analyzer.");
111  reset();
112  }
113 }
114 
116 {
117  return _n > 0u;
118 }
119 
120 float SimpleAnalyzer::get() const
121 {
122  return _result;
123 }
124 
125 float SimpleAnalyzer::get_scaled(float scalingfactor) const
126 {
127  return get() * scalingfactor;
128 }
129 
130 void SimpleAnalyzer::check_limits(float &x, float min, float max) const
131 {
132  if (x > max) {
133  x = max;
134 
135  } else if (x < min) {
136  x = min;
137  }
138 }
139 
140 void SimpleAnalyzer::int_round(float &x) const
141 {
142  if (x < 0) {
143  x -= 0.5f;
144 
145  } else {
146  x += 0.5f;
147  }
148 }
149 
150 void convert_limit_safe(float in, uint16_t &out)
151 {
152  if (in > UINT16_MAX) {
153  out = UINT16_MAX;
154 
155  } else if (in < 0) {
156  out = 0;
157 
158  } else {
159  out = static_cast<uint16_t>(in);
160  }
161 }
162 
163 void convert_limit_safe(float in, int16_t &out)
164 {
165  if (in > INT16_MAX) {
166  out = INT16_MAX;
167 
168  } else if (in < INT16_MIN) {
169  out = INT16_MIN;
170 
171  } else {
172  out = static_cast<int16_t>(in);
173  }
174 }
SimpleAnalyzer(Mode mode, float window=60.0f)
Constructor.
static Mode _mode
Definition: motor_ramp.cpp:81
float get() const
Get the current result of the analyzer.
void reset()
Reset the analyzer to the initial state.
float get_scaled(float scalingfactor) const
Get the scaled value of the current result of the analyzer.
void check_limits(float &x, float min, float max) const
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
bool valid() const
Returns true if at least one value has been added to the analyzer.
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
unsigned int _n
The number of added samples.
void int_round(float &x) const
mode
Definition: vtol_type.h:76
float _result
The result of the analyzed data.
void add_value(float val, float update_rate)
Add a new value to the analyzer and update the result according to the mode.
Mode _mode
The mode of the simple analyzer.
float _window
The window size for the moving average filter [s].