From 75ed9b002bdf67420b9f8f664dc8d8a698a8010c Mon Sep 17 00:00:00 2001
From: acfloria <florian.achermann@mavt.ethz.ch>
Date: Fri, 26 Jan 2018 13:17:08 +0100
Subject: [PATCH] Add the SimpleAnalyzer to generate messages for MAVLink
 streams

---
 src/modules/mavlink/mavlink_messages.cpp | 110 ++++++++++++++++++++++-
 src/modules/mavlink/mavlink_messages.h   |  40 +++++++++
 2 files changed, 149 insertions(+), 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 37522edae1..b1e9ed05ab 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -41,6 +41,8 @@
 
 #include <stdio.h>
 #include <errno.h>
+#include <math.h>
+#include <float.h>
 
 #include "mavlink_main.h"
 #include "mavlink_messages.h"
@@ -4281,7 +4283,7 @@ static const StreamListItem streams_list[] = {
 	StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static),
 	StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
 	StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
-	StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamWind::get_id_static),
+	StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamHighLatency::get_id_static),
 	StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static)
 };
 
@@ -4310,3 +4312,109 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink)
 
 	return nullptr;
 }
+
+SimpleAnalyzer::SimpleAnalyzer(Mode mode, unsigned int n_max) :
+	_n_max(n_max),
+	_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)
+{
+	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 < _n_max) {
+		_n++;
+	}
+
+	// value sanity checks
+	if (!PX4_ISFINITE(_result)) {
+		PX4_ERR("SimpleAnalyzer: Result is not finite, reset the analyzer.");
+		reset();
+	}
+}
+
+bool SimpleAnalyzer::valid()
+{
+	return _n > 0u;
+}
+
+float SimpleAnalyzer::get()
+{
+	return _result;
+}
+
+float SimpleAnalyzer::get_scaled(float scalingfactor)
+{
+	return get() * scalingfactor;
+}
+
+void SimpleAnalyzer::check_limits(float &x, float min, float max)
+{
+	if (x > max) {
+		x = max;
+
+	} else if (x < min) {
+		x = min;
+	}
+}
+
+void SimpleAnalyzer::int_round(float &x)
+{
+	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 0855aadaf0..0aa8389acc 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -43,6 +43,8 @@
 
 #include "mavlink_stream.h"
 
+#include <limits>
+
 class StreamListItem
 {
 
@@ -61,4 +63,42 @@ 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, unsigned int n_max = 1000);
+	virtual ~SimpleAnalyzer();
+
+	void reset();
+	void add_value(float val);
+	bool valid();
+	float get();
+	float get_scaled(float scalingfactor);
+
+	template <typename T>
+	void get_scaled(T &ret, float scalingfactor)
+	{
+		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;
+	unsigned int _n_max = 1000;
+	Mode _mode = AVERAGE;
+	float _result = 0.0f;
+
+	void check_limits(float &x, float min, float max);
+	void int_round(float &x);
+};
+
 #endif /* MAVLINK_MESSAGES_H_ */