Quickly separated low-level raw RC from mapped / scaled RC, supports FMU PPM and IO PPM / Spektrum now

1 files changed, 115 insertions, 83 deletions

diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 453502b05..3e9f35eaf 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -58,11 +58,13 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_rc_input.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+
 #include <systemlib/ppm_decode.h>
 
 #include <uORB/uORB.h>
@@ -143,6 +145,7 @@ private:
 	int		_gyro_sub;			/**< raw gyro data subscription */
 	int		_accel_sub;			/**< raw accel data subscription */
 	int		_mag_sub;			/**< raw mag data subscription */
+	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -162,6 +165,7 @@ private:
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
 		float ex[_rc_max_chan_count];
+		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
 		float mag_offset[3];
@@ -331,6 +335,7 @@ Sensors::Sensors() :
 	_gyro_sub(-1),
 	_accel_sub(-1),
 	_mag_sub(-1),
+	_rc_sub(-1),
 	_baro_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
@@ -464,14 +469,13 @@ Sensors::parameters_update()
 			warnx("Failed getting exponential gain for chan %d", i);
 		}
 
-		_rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
 		/* handle blowup in the scaling factor calculation */
-		if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) {
-			_rc.chan[i].scaling_factor = 0;
+		if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
+			_parameters.scaling_factor[i] = 0;
 		}
 
-		_rc.chan[i].mid = _parameters.trim[i];
 	}
 
 	/* update RC function mappings */
@@ -856,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 void
 Sensors::ppm_poll()
 {
-	struct manual_control_setpoint_s manual_control;
-
-	/* check to see whether a new frame has been decoded */
-	if (_ppm_last_valid == ppm_last_valid_decode)
-		return;
-	/* require at least two chanels to consider the signal valid */
-	if (ppm_decoded_channels < 4)
-		return;
-
-	unsigned channel_limit = ppm_decoded_channels;
-	if (channel_limit > _rc_max_chan_count)
-		channel_limit = _rc_max_chan_count;
-
-	/* we are accepting this decode */
-	_ppm_last_valid = ppm_last_valid_decode;
-
-	/* Read out values from HRT */
-	for (unsigned int i = 0; i < channel_limit; i++) {
-		_rc.chan[i].raw = ppm_buffer[i];
-
-		/* scale around the mid point differently for lower and upper range */
-		if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) {
-			_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
-		} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-			/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
-			_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
-			
-		} else {
-			/* in the configured dead zone, output zero */
-			_rc.chan[i].scaled = 0.0f;
+	/* fake low-level driver, directly pulling from driver variables */
+	static orb_advert_t rc_input_pub = -1;
+	struct rc_input_values raw;
+
+	raw.timestamp = ppm_last_valid_decode;
+
+	if (ppm_decoded_channels > 1) {
+
+		for (int i = 0; i < ppm_decoded_channels; i++) {
+			raw.values[i] = ppm_buffer[i];
 		}
 
-		/* reverse channel if required */
-		if (i == _rc.function[THROTTLE]) {
-			if ((int)_parameters.rev[i] == -1) {
-				_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
-			}
+		raw.channel_count = ppm_decoded_channels;
+
+		/* publish to object request broker */
+		if (rc_input_pub <= 0) {
+			rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
 		} else {
-			_rc.chan[i].scaled *= _parameters.rev[i];
+			orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
 		}
+	}
 
-		/* handle any parameter-induced blowups */
-		if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
-			_rc.chan[i].scaled = 0.0f;
 
-		//_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor;
-	}
+	/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
+	bool rc_updated;
+	orb_check(_rc_sub, &rc_updated);
 
-	_rc.chan_count = ppm_decoded_channels;
-	_rc.timestamp = ppm_last_valid_decode;
+	if (rc_updated) {
+		struct rc_input_values	rc_input;
 
-	manual_control.timestamp = ppm_last_valid_decode;
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
-	/* roll input - rolling right is stick-wise and rotation-wise positive */
-	manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
-	if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
-	if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
-	if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
-		manual_control.roll *= _parameters.rc_scale_roll;
-	}
+		struct manual_control_setpoint_s manual_control;
 
-	/*
-	 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-	 * so reverse sign.
-	 */
-	manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
-	if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
-	if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
-	if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
-		manual_control.pitch *= _parameters.rc_scale_pitch;
-	}
+		/* require at least two chanels to consider the signal valid */
+		if (rc_input.channel_count < 2)
+			return;
 
-	/* yaw input - stick right is positive and positive rotation */
-	manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
-	if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
-	if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
-	if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
-		manual_control.yaw *= _parameters.rc_scale_yaw;
-	}
-	
-	/* throttle input */
-	manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-	if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-	if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
+		unsigned channel_limit = rc_input.channel_count;
+		if (channel_limit > _rc_max_chan_count)
+			channel_limit = _rc_max_chan_count;
 
-	/* mode switch input */
-	manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
-	if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
-	if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
+		/* we are accepting this message */
+		_ppm_last_valid = rc_input.timestamp;
 
-	orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
-	orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+		/* Read out values from raw message */
+		for (unsigned int i = 0; i < channel_limit; i++) {
+
+			/* scale around the mid point differently for lower and upper range */
+			if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
+				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
+			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
+				/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
+				_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
+				
+			} else {
+				/* in the configured dead zone, output zero */
+				_rc.chan[i].scaled = 0.0f;
+			}
+
+			/* reverse channel if required */
+			if (i == _rc.function[THROTTLE]) {
+				if ((int)_parameters.rev[i] == -1) {
+					_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
+				}
+			} else {
+				_rc.chan[i].scaled *= _parameters.rev[i];
+			}
+
+			/* handle any parameter-induced blowups */
+			if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
+				_rc.chan[i].scaled = 0.0f;
+		}
+
+		_rc.chan_count = rc_input.channel_count;
+		_rc.timestamp = rc_input.timestamp;
+
+		manual_control.timestamp = rc_input.timestamp;
+
+		/* roll input - rolling right is stick-wise and rotation-wise positive */
+		manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+		if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
+		if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
+		if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
+			manual_control.roll *= _parameters.rc_scale_roll;
+		}
+
+		/*
+		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
+		 * so reverse sign.
+		 */
+		manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
+		if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
+		if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
+		if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
+			manual_control.pitch *= _parameters.rc_scale_pitch;
+		}
+
+		/* yaw input - stick right is positive and positive rotation */
+		manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
+		if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
+		if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
+		if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
+			manual_control.yaw *= _parameters.rc_scale_yaw;
+		}
+		
+		/* throttle input */
+		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
+		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
+
+		/* mode switch input */
+		manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
+		if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
+		if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
+
+		orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
+		orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+	}
 
 }
 #endif
@@ -979,6 +1010,7 @@ Sensors::task_main()
 	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
 	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
 	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));