From fb1bb7a769f0dbc4d3710fdf4d9b2280f986bbaa Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 10 Apr 2018 09:57:03 +0200 Subject: [PATCH] Move the SimpleAnalyzer to a separate file --- src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_messages.cpp | 124 ++------------ src/modules/mavlink/mavlink_messages.h | 54 ------- .../mavlink/mavlink_simple_analyzer.cpp | 148 +++++++++++++++++ src/modules/mavlink/mavlink_simple_analyzer.h | 153 ++++++++++++++++++ 5 files changed, 311 insertions(+), 169 deletions(-) create mode 100644 src/modules/mavlink/mavlink_simple_analyzer.cpp create mode 100644 src/modules/mavlink/mavlink_simple_analyzer.h diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 7dc39e8364..3ea6d3bf31 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -56,6 +56,7 @@ px4_add_module( mavlink_rate_limiter.cpp mavlink_receiver.cpp mavlink_shell.cpp + mavlink_simple_analyzer.cpp mavlink_stream.cpp mavlink_ulog.cpp DEPENDS diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a989c8c491..3a0f115764 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -47,6 +47,7 @@ #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_command_sender.h" +#include "mavlink_simple_analyzer.h" #include #include @@ -105,9 +106,9 @@ #include static uint16_t cm_uint16_from_m_float(float m); -static void get_mavlink_custom_mode(struct vehicle_status_s *status, uint8_t *mavlink_base_mode, - union px4_custom_mode *custom_mode); -static void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, +static void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode, + union px4_custom_mode *custom_mode); +static void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t @@ -123,8 +124,8 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } -void get_mavlink_custom_mode(struct vehicle_status_s *status, uint8_t *mavlink_base_mode, - union px4_custom_mode *custom_mode) +void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode, + union px4_custom_mode *custom_mode) { custom_mode->data = 0; *mavlink_base_mode = 0; @@ -249,7 +250,7 @@ void get_mavlink_custom_mode(struct vehicle_status_s *status, uint8_t *mavlink_b } } -void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_state, +void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; @@ -257,7 +258,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st *mavlink_custom_mode = 0; union px4_custom_mode custom_mode; - get_mavlink_custom_mode(status, mavlink_base_mode, &custom_mode); + get_mavlink_navigation_mode(status, mavlink_base_mode, &custom_mode); *mavlink_custom_mode = custom_mode.data; /* set system state */ @@ -4389,7 +4390,7 @@ protected: // flight mode union px4_custom_mode custom_mode; uint8_t mavlink_base_mode; - get_mavlink_custom_mode(&status, &mavlink_base_mode, &custom_mode); + get_mavlink_navigation_mode(&status, &mavlink_base_mode, &custom_mode); msg->custom_mode = custom_mode.custom_mode_hl; } @@ -4743,110 +4744,3 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink) return nullptr; } - -SimpleAnalyzer::SimpleAnalyzer(Mode mode, float window) : - _window(window), - _mode(mode) -{ - reset(); -} - -SimpleAnalyzer::~SimpleAnalyzer() -{ -} - -void SimpleAnalyzer::reset() -{ - _n = 0; - - switch (_mode) { - case AVERAGE: - _result = 0.0f; - - break; - - case MIN: - _result = FLT_MAX; - - break; - - case MAX: - _result = FLT_MIN; - - break; - - default: - PX4_ERR("SimpleAnalyzer: Unknown mode."); - } -} - -void SimpleAnalyzer::add_value(float val, float update_rate) -{ - switch (_mode) { - case AVERAGE: - _result = (_result * _n + val) / (_n + 1u); - - break; - - case MIN: - if (val < _result) { - _result = val; - } - - break; - - case MAX: - if (val > _result) { - _result = val; - } - - break; - } - - // if we get more measurements than n_max so the exponential moving average - // is computed - if ((_n < update_rate * _window) && (update_rate > 1.0f)) { - _n++; - } - - // value sanity checks - if (!PX4_ISFINITE(_result)) { - PX4_ERR("SimpleAnalyzer: Result is not finite, reset the analyzer."); - reset(); - } -} - -bool SimpleAnalyzer::valid() const -{ - return _n > 0u; -} - -float SimpleAnalyzer::get() const -{ - return _result; -} - -float SimpleAnalyzer::get_scaled(float scalingfactor) const -{ - return get() * scalingfactor; -} - -void SimpleAnalyzer::check_limits(float &x, float min, float max) const -{ - if (x > max) { - x = max; - - } else if (x < min) { - x = min; - } -} - -void SimpleAnalyzer::int_round(float &x) const -{ - if (x < 0) { - x -= 0.5f; - - } else { - x += 0.5f; - } -} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index e02778ee93..0855aadaf0 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -43,8 +43,6 @@ #include "mavlink_stream.h" -#include - class StreamListItem { @@ -63,56 +61,4 @@ public: const char *get_stream_name(const uint16_t msg_id); MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink); -class SimpleAnalyzer -{ -public: - enum Mode { - AVERAGE = 0, - MIN, - MAX, - }; - - SimpleAnalyzer(Mode mode, float window = 60.0f); - virtual ~SimpleAnalyzer(); - - void reset(); - void add_value(float val, float update_rate); - bool valid() const; - float get() const; - float get_scaled(float scalingfactor) const; - - template - void get_scaled(T &ret, float scalingfactor) const - { - float avg = get_scaled(scalingfactor); - int_round(avg); - check_limits(avg, std::numeric_limits::min(), std::numeric_limits::max()); - - ret = static_cast(avg); - } - -private: - unsigned int _n = 0; - float _window = 60.0f; - Mode _mode = AVERAGE; - float _result = 0.0f; - - void check_limits(float &x, float min, float max) const; - void int_round(float &x) const; -}; - -template -void convert_limit_safe(Tin in, Tout *out) -{ - if (in > std::numeric_limits::max()) { - *out = std::numeric_limits::max(); - - } else if (in < std::numeric_limits::min()) { - *out = std::numeric_limits::min(); - - } else { - *out = static_cast(in); - } -} - #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_simple_analyzer.cpp b/src/modules/mavlink/mavlink_simple_analyzer.cpp new file mode 100644 index 0000000000..670af116fe --- /dev/null +++ b/src/modules/mavlink/mavlink_simple_analyzer.cpp @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_simple_analyzer.cpp + * + * @author Achermann Florian + */ + +#include "mavlink_simple_analyzer.h" + +#include + +#include +#include + +SimpleAnalyzer::SimpleAnalyzer(Mode mode, float window) : + _window(window), + _mode(mode) +{ + reset(); +} + +void SimpleAnalyzer::reset() +{ + _n = 0; + + switch (_mode) { + case AVERAGE: + _result = 0.0f; + + break; + + case MIN: + _result = FLT_MAX; + + break; + + case MAX: + _result = FLT_MIN; + + break; + + default: + PX4_ERR("SimpleAnalyzer: Unknown mode."); + } +} + +void SimpleAnalyzer::add_value(float val, float update_rate) +{ + switch (_mode) { + case AVERAGE: + _result = (_result * _n + val) / (_n + 1u); + + break; + + case MIN: + if (val < _result) { + _result = val; + } + + break; + + case MAX: + if (val > _result) { + _result = val; + } + + break; + } + + // if we get more measurements than n_max so the exponential moving average + // is computed + if ((_n < update_rate * _window) && (update_rate > 1.0f)) { + _n++; + } + + // value sanity checks + if (!PX4_ISFINITE(_result)) { + PX4_DEBUG("SimpleAnalyzer: Result is not finite, reset the analyzer."); + reset(); + } +} + +bool SimpleAnalyzer::valid() const +{ + return _n > 0u; +} + +float SimpleAnalyzer::get() const +{ + return _result; +} + +float SimpleAnalyzer::get_scaled(float scalingfactor) const +{ + return get() * scalingfactor; +} + +void SimpleAnalyzer::check_limits(float &x, float min, float max) const +{ + if (x > max) { + x = max; + + } else if (x < min) { + x = min; + } +} + +void SimpleAnalyzer::int_round(float &x) const +{ + if (x < 0) { + x -= 0.5f; + + } else { + x += 0.5f; + } +} diff --git a/src/modules/mavlink/mavlink_simple_analyzer.h b/src/modules/mavlink/mavlink_simple_analyzer.h new file mode 100644 index 0000000000..6600ab38bc --- /dev/null +++ b/src/modules/mavlink/mavlink_simple_analyzer.h @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_simple_analyzer.h + * + * @author Achermann Florian + */ + +#ifndef MAVLINK_SIMPLE_ANALYZER_H_ +#define MAVLINK_SIMPLE_ANALYZER_H_ + +#include + +/** + * SimpleAnalyzer + * + * Class used for simple analysis of data streams. + * The data can be analyzed in the following three modes: + * + * AVERAGE: + * The average value is computed at the beginning. Based on the number of analyzed values, + * the update rate and the window size the switch to the moving average is determined. + * + * MIN: + * The minimum value is tracked. + * + * MAX: + * The maximum value is tracked. + */ +class SimpleAnalyzer +{ +public: + enum Mode { + AVERAGE = 0, + MIN, + MAX, + }; + + /** + * Constructor + * + * Defines the mode of the analyzer and the window size in case of the + * averaging mode. + * + * @param[in] mode: The mode of the analyzer + * @param[in] window: The window size in seconds. Only used in the averaging mode. + */ + SimpleAnalyzer(Mode mode, float window = 60.0f); + + /** + * Reset the analyzer to the initial state. + */ + void reset(); + + /** + * Add a new value to the analyzer and update the result according to the mode. + * + * @param[in] val: The value to process + * @param[in] update_rate: The update rate in [1/s] for which new value are added. + * Used in the averaging mode to determine when to switch from averaging to the moving average. + */ + void add_value(float val, float update_rate); + + /** + * Returns true if at least one value has been added to the analyzer. + */ + bool valid() const; + + /** + * Get the current result of the analyzer. + */ + float get() const; + + /** + * Get the scaled value of the current result of the analyzer. + * + * @param[in] scalingfactor: The factor used to scale the result. + */ + float get_scaled(float scalingfactor) const; + + /** + * Get the rounded scaled value casted to the input template type. + * Should only be used to return integer types. + * + * @param[out] ret: The scaled and rounded value of the current analyzer result. + * @parma[in] scalingfactor: The factor which is used to scale the result. + */ + template + void get_scaled(T &ret, float scalingfactor) const + { + float avg = get_scaled(scalingfactor); + int_round(avg); + check_limits(avg, std::numeric_limits::min(), std::numeric_limits::max()); + + ret = static_cast(avg); + } + +private: + unsigned int _n = 0; /**< The number of added samples */ + float _window = 60.0f; /**< The window size for the moving average filter [s] */ + Mode _mode = AVERAGE; /**< The mode of the simple analyzer */ + float _result = 0.0f; /**< The result of the analyzed data. */ + + void check_limits(float &x, float min, float max) const; + void int_round(float &x) const; +}; + +template +void convert_limit_safe(Tin in, Tout *out) +{ + if (in > std::numeric_limits::max()) { + *out = std::numeric_limits::max(); + + } else if (in < std::numeric_limits::min()) { + *out = std::numeric_limits::min(); + + } else { + *out = static_cast(in); + } +} + +#endif /* MAVLINK_SIMPLE_ANALYZER_H_ */