Move the SimpleAnalyzer to a separate file

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

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 <commander/px4_custom_mode.h>
 #include <drivers/drv_pwm_output.h>
@@ -105,9 +106,9 @@
 #include <uORB/uORB.h>
 
 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,
+static void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode,
-				    union px4_custom_mode *custom_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_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,
+void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, uint8_t *mavlink_base_mode,
-			     union px4_custom_mode *custom_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;
-	}
-}

src/modules/mavlink/mavlink_messages.h

@@ -43,8 +43,6 @@
 
 #include "mavlink_stream.h"
 
-#include <limits>
-
 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 <typename T>
-	void get_scaled(T &ret, float scalingfactor) const
-	{
-		float avg = get_scaled(scalingfactor);
-		int_round(avg);
-		check_limits(avg, std::numeric_limits<T>::min(), std::numeric_limits<T>::max());
-
-		ret = static_cast<T>(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<typename Tin, typename Tout>
-void convert_limit_safe(Tin in, Tout *out)
-{
-	if (in > std::numeric_limits<Tout>::max()) {
-		*out = std::numeric_limits<Tout>::max();
-
-	} else if (in < std::numeric_limits<Tout>::min()) {
-		*out = std::numeric_limits<Tout>::min();
-
-	} else {
-		*out = static_cast<Tout>(in);
-	}
-}
-
 #endif /* MAVLINK_MESSAGES_H_ */

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 <acfloria@ethz.ch>
+ */
+
+#include "mavlink_simple_analyzer.h"
+
+#include <float.h>
+
+#include <px4_log.h>
+#include <px4_defines.h>
+
+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;
+	}
+}

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 <acfloria@ethz.ch>
+ */
+
+#ifndef MAVLINK_SIMPLE_ANALYZER_H_
+#define MAVLINK_SIMPLE_ANALYZER_H_
+
+#include <limits>
+
+/**
+ * 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 <typename T>
+	void get_scaled(T &ret, float scalingfactor) const
+	{
+		float avg = get_scaled(scalingfactor);
+		int_round(avg);
+		check_limits(avg, std::numeric_limits<T>::min(), std::numeric_limits<T>::max());
+
+		ret = static_cast<T>(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<typename Tin, typename Tout>
+void convert_limit_safe(Tin in, Tout *out)
+{
+	if (in > std::numeric_limits<Tout>::max()) {
+		*out = std::numeric_limits<Tout>::max();
+
+	} else if (in < std::numeric_limits<Tout>::min()) {
+		*out = std::numeric_limits<Tout>::min();
+
+	} else {
+		*out = static_cast<Tout>(in);
+	}
+}
+
+#endif /* MAVLINK_SIMPLE_ANALYZER_H_ */