'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately

1 files changed, 118 insertions, 106 deletions

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f890c4c7f..9f816f5e1 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -474,6 +474,7 @@ Sensors::Sensors() :
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
 {
+	memset(&_rc, 0, sizeof(_rc));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -1283,12 +1284,6 @@ Sensors::rc_poll()
 	if (rc_updated) {
 		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
 		struct rc_input_values	rc_input;
-
-		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
-
-		if (rc_input.rc_lost)
-			return;
-
 		struct manual_control_setpoint_s manual_control;
 		struct actuator_controls_s actuator_group_3;
 
@@ -1311,19 +1306,28 @@ Sensors::rc_poll()
 		manual_control.aux4 = NAN;
 		manual_control.aux5 = NAN;
 
-		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
-			return;
+		manual_control.signal_lost = true;
 
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
+		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+
+		/* detect RC signal loss */
+		/* check flags and require at least four channels to consider the signal valid */
+		if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) {
+			/* signal looks good */
+			manual_control.signal_lost = false;
+
+			/* check throttle failsafe */
+			if (_parameters.rc_fs_ch != 0) {
+				if (_parameters.rc_fs_mode == 0) {
+					if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) {
+						manual_control.signal_lost = true;
+					}
 
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
+				} else if (_parameters.rc_fs_mode == 1) {
+					if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) {
+						manual_control.signal_lost = true;
+					}
+				}
 			}
 		}
 
@@ -1332,10 +1336,7 @@ Sensors::rc_poll()
 		if (channel_limit > _rc_max_chan_count)
 			channel_limit = _rc_max_chan_count;
 
-		/* we are accepting this message */
-		_rc_last_valid = rc_input.timestamp_last_signal;
-
-		/* Read out values from raw message */
+		/* read out and scale values from raw message even if signal is invalid */
 		for (unsigned int i = 0; i < channel_limit; i++) {
 
 			/*
@@ -1382,105 +1383,124 @@ Sensors::rc_poll()
 		}
 
 		_rc.chan_count = rc_input.channel_count;
-		_rc.timestamp = rc_input.timestamp_last_signal;
+		_rc.rssi = rc_input.rssi;
+		_rc.signal_lost = manual_control.signal_lost;
 
-		manual_control.timestamp = rc_input.timestamp_last_signal;
+		if (!manual_control.signal_lost) {
+			_rc_last_valid = rc_input.timestamp_last_signal;
+		}
 
-		/* roll input - rolling right is stick-wise and rotation-wise positive */
-		manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
-		/*
-		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-		 * so reverse sign.
-		 */
-		manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
-		/* yaw input - stick right is positive and positive rotation */
-		manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
-		/* throttle input */
-		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+		_rc.timestamp = _rc_last_valid;
 
-		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
+		manual_control.timestamp = _rc_last_valid;
 
-		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
+		if (!manual_control.signal_lost) {
+			/* fill values in manual_control_setpoint topic only if signal is valid */
 
-		/* scale output */
-		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
-			manual_control.roll *= _parameters.rc_scale_roll;
-		}
+			/* roll input - rolling right is stick-wise and rotation-wise positive */
+			manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
+			/*
+			 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
+			 * so reverse sign.
+			 */
+			manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
+			/* yaw input - stick right is positive and positive rotation */
+			manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
+			/* throttle input */
+			manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
 
-		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
-			manual_control.pitch *= _parameters.rc_scale_pitch;
-		}
+			if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
 
-		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
-			manual_control.yaw *= _parameters.rc_scale_yaw;
-		}
+			if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
 
-		/* flaps */
-		if (_rc.function[FLAPS] >= 0) {
+			/* scale output */
+			if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
+				manual_control.roll *= _parameters.rc_scale_roll;
+			}
 
-			manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
+			if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
+				manual_control.pitch *= _parameters.rc_scale_pitch;
+			}
 
-			if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
-				manual_control.flaps *= _parameters.rc_scale_flaps;
+			if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
+				manual_control.yaw *= _parameters.rc_scale_yaw;
 			}
-		}
 
-		/* mode switch input */
-		if (_rc.function[MODE] >= 0) {
-			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-		}
+			/* flaps */
+			if (_rc.function[FLAPS] >= 0) {
 
-		/* assisted switch input */
-		if (_rc.function[ASSISTED] >= 0) {
-			manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-		}
+				manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
 
-		/* mission switch input */
-		if (_rc.function[MISSION] >= 0) {
-			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-		}
+				if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
+					manual_control.flaps *= _parameters.rc_scale_flaps;
+				}
+			}
 
-		/* return switch input */
-		if (_rc.function[RETURN] >= 0) {
-			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-		}
+			/* mode switch input */
+			if (_rc.function[MODE] >= 0) {
+				manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+			}
 
-//		if (_rc.function[OFFBOARD_MODE] >= 0) {
-//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-//		}
+			/* assisted switch input */
+			if (_rc.function[ASSISTED] >= 0) {
+				manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+			}
 
-		/* aux functions, only assign if valid mapping is present */
-		if (_rc.function[AUX_1] >= 0) {
-			manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
-		}
+			/* mission switch input */
+			if (_rc.function[MISSION] >= 0) {
+				manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
+			}
 
-		if (_rc.function[AUX_2] >= 0) {
-			manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
-		}
+			/* return switch input */
+			if (_rc.function[RETURN] >= 0) {
+				manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+			}
 
-		if (_rc.function[AUX_3] >= 0) {
-			manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
-		}
+	//		if (_rc.function[OFFBOARD_MODE] >= 0) {
+	//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+	//		}
 
-		if (_rc.function[AUX_4] >= 0) {
-			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
-		}
+			/* aux functions, only assign if valid mapping is present */
+			if (_rc.function[AUX_1] >= 0) {
+				manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
+			}
+
+			if (_rc.function[AUX_2] >= 0) {
+				manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
+			}
+
+			if (_rc.function[AUX_3] >= 0) {
+				manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
+			}
 
-		if (_rc.function[AUX_5] >= 0) {
-			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
+			if (_rc.function[AUX_4] >= 0) {
+				manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
+			}
+
+			if (_rc.function[AUX_5] >= 0) {
+				manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
+			}
+
+			/* copy from mapped manual control to control group 3 */
+			actuator_group_3.control[0] = manual_control.roll;
+			actuator_group_3.control[1] = manual_control.pitch;
+			actuator_group_3.control[2] = manual_control.yaw;
+			actuator_group_3.control[3] = manual_control.throttle;
+			actuator_group_3.control[4] = manual_control.flaps;
+			actuator_group_3.control[5] = manual_control.aux1;
+			actuator_group_3.control[6] = manual_control.aux2;
+			actuator_group_3.control[7] = manual_control.aux3;
+
+			/* publish actuator_controls_3 topic only if control signal is valid */
+			if (_actuator_group_3_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+
+			} else {
+				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+			}
 		}
 
-		/* copy from mapped manual control to control group 3 */
-		actuator_group_3.control[0] = manual_control.roll;
-		actuator_group_3.control[1] = manual_control.pitch;
-		actuator_group_3.control[2] = manual_control.yaw;
-		actuator_group_3.control[3] = manual_control.throttle;
-		actuator_group_3.control[4] = manual_control.flaps;
-		actuator_group_3.control[5] = manual_control.aux1;
-		actuator_group_3.control[6] = manual_control.aux2;
-		actuator_group_3.control[7] = manual_control.aux3;
-
-		/* check if ready for publishing */
+		/* publish rc_channels topic even if signal is invalid, for debug */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
 
@@ -1489,21 +1509,13 @@ Sensors::rc_poll()
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
 		}
 
-		/* check if ready for publishing */
+		/* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */
 		if (_manual_control_pub > 0) {
 			orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
 
 		} else {
 			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 		}
-
-		/* check if ready for publishing */
-		if (_actuator_group_3_pub > 0) {
-			orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
-
-		} else {
-			_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
-		}
 	}
 
 }