From fd48d9c19032500fef4d5394dd7731dfdcbf85a5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 19 Jan 2017 08:01:29 +0100
Subject: [PATCH] sensors: print status of temp compensation with 'sensors
 status'

---
 .../sensors/temperature_compensation.cpp      | 42 ++++++++++++++++++-
 .../sensors/temperature_compensation.h        |  2 +
 src/modules/sensors/voted_sensors_update.cpp  |  2 +
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp
index b35e57825e..21eeaf789a 100644
--- a/src/modules/sensors/temperature_compensation.cpp
+++ b/src/modules/sensors/temperature_compensation.cpp
@@ -41,8 +41,8 @@
 
 #include "temperature_compensation.h"
 #include <systemlib/param/param.h>
-#include <stdio.h>
 #include <px4_defines.h>
+#include <px4_log.h>
 
 namespace sensors
 {
@@ -422,4 +422,44 @@ int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &s
 	return 1;
 }
 
+void TemperatureCompensation::print_status()
+{
+	PX4_INFO("Temperature Compensation:");
+	PX4_INFO(" gyro: enabled: %i", _parameters.gyro_tc_enable);
+
+	if (_parameters.gyro_tc_enable == 1) {
+		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+			uint8_t mapping = _gyro_data.device_mapping[i];
+
+			if (_gyro_data.device_mapping[i] != 255) {
+				PX4_INFO("  using device ID %i for topic instance %i", _parameters.gyro_cal_data[mapping].ID, i);
+			}
+		}
+	}
+
+	PX4_INFO(" accel: enabled: %i", _parameters.accel_tc_enable);
+
+	if (_parameters.accel_tc_enable == 1) {
+		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+			uint8_t mapping = _accel_data.device_mapping[i];
+
+			if (_accel_data.device_mapping[i] != 255) {
+				PX4_INFO("  using device ID %i for topic instance %i", _parameters.accel_cal_data[mapping].ID, i);
+			}
+		}
+	}
+
+	PX4_INFO(" baro: enabled: %i", _parameters.baro_tc_enable);
+
+	if (_parameters.baro_tc_enable == 1) {
+		for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
+			uint8_t mapping = _baro_data.device_mapping[i];
+
+			if (_baro_data.device_mapping[i] != 255) {
+				PX4_INFO("  using device ID %i for topic instance %i", _parameters.baro_cal_data[mapping].ID, i);
+			}
+		}
+	}
+}
+
 }
diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h
index 2daa065680..1a14c6db02 100644
--- a/src/modules/sensors/temperature_compensation.h
+++ b/src/modules/sensors/temperature_compensation.h
@@ -91,6 +91,8 @@ public:
 	int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
 				   float *offsets, float *scales);
 
+	/** output current configuration status to console */
+	void print_status();
 private:
 
 	/* Struct containing parameters used by the single axis 5th order temperature compensation algorithm
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index 3f809094b1..4b6969c284 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -873,6 +873,8 @@ void VotedSensorsUpdate::print_status()
 	_mag.voter.print();
 	PX4_INFO("baro status:");
 	_baro.voter.print();
+
+	_temperature_compensation.print_status();
 }
 
 bool