From b5b4769d1f46be670913289b6103f3e80703692d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 1 May 2016 14:06:09 +0200
Subject: [PATCH] Enable RSSI sampling on Pixracer

---
 msg/rc_channels.msg                         |  2 +-
 src/drivers/boards/px4fmu-v4/board_config.h |  1 +
 src/modules/sensors/sensors.cpp             | 33 +++++++++++++++++----
 3 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index 3171fbc106..b8bcc5f536 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -25,6 +25,6 @@ uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
 int8[21] function						# Functions mapping
-uint8 rssi							# Receive signal strength index
+uint8 rssi							# Receive signal strength indicator (0-100)
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames
diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h
index 6713524f87..c1e214f7d3 100644
--- a/src/drivers/boards/px4fmu-v4/board_config.h
+++ b/src/drivers/boards/px4fmu-v4/board_config.h
@@ -164,6 +164,7 @@ __BEGIN_DECLS
 #define ADC_BATTERY_VOLTAGE_CHANNEL		2
 #define ADC_BATTERY_CURRENT_CHANNEL		3
 #define ADC_5V_RAIL_SENSE				4
+#define ADC_RC_RSSI_CHANNEL			11
 
 /* User GPIOs
  *
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 275200ec32..5cd33dd6f1 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -115,15 +115,15 @@ using namespace DriverFramework;
  * IN3 - battery current
  * IN4 - 5V sense
  * IN10 - spare (we could actually trim these from the set)
- * IN11 - spare (we could actually trim these from the set)
+ * IN11 - spare on FMUv2 & v3, RC RSSI on FMUv4
  * IN12 - spare (we could actually trim these from the set)
- * IN13 - aux1
- * IN14 - aux2
- * IN15 - pressure sensor
+ * IN13 - aux1 on FMUv2, unavaible on v3 & v4
+ * IN14 - aux2 on FMUv2, unavaible on v3 & v4
+ * IN15 - pressure sensor on FMUv2, unavaible on v3 & v4
  *
  * IO:
  * IN4 - servo supply rail
- * IN5 - analog RSSI
+ * IN5 - analog RSSI on FMUv2 & v3
  *
  * The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
  */
@@ -249,6 +249,7 @@ private:
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
 	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
+	float _analog_rc_rssi_volt;			/**< analog RSSI indicator */
 
 	math::Matrix<3, 3>	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_mag_rotation[3];	/**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -552,6 +553,7 @@ Sensors::Sensors() :
 	_airspeed_validator(),
 
 	_param_rc_values{},
+	_analog_rc_rssi_volt(-1.0f),
 	_board_rotation{},
 	_mag_rotation{}
 {
@@ -1697,6 +1699,21 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 						}
 					}
 
+#endif
+#ifdef ADC_RC_RSSI_CHANNEL
+
+				} else if (ADC_RC_RSSI_CHANNEL == buf_adc[i].am_channel) {
+					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f;
+
+					if (voltage > 0.2f) {
+
+						if (_analog_rc_rssi_volt < 0.0f) {
+							_analog_rc_rssi_volt = voltage;
+						}
+
+						_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.999f + voltage * 0.001f;
+					}
+
 #endif
 				}
 			}
@@ -1907,6 +1924,12 @@ Sensors::rc_poll()
 
 		_rc.channel_count = rc_input.channel_count;
 		_rc.rssi = rc_input.rssi;
+
+		/* overwrite RSSI if analog RSSI input is present */
+		if (_analog_rc_rssi_volt > 0.2f) {
+			_rc.rssi = (_analog_rc_rssi_volt / 3.0f) * 100.0f;
+		}
+
 		_rc.signal_lost = signal_lost;
 		_rc.timestamp = rc_input.timestamp_last_signal;
 		_rc.frame_drop_count = rc_input.rc_lost_frame_count;