Add switch for landing gear, pass it to actuators

msg/manual_control_setpoint.msg

@@ -52,4 +52,5 @@ uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
 uint8 offboard_switch	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
 uint8 kill_switch		 # throttle kill: _NORMAL_, KILL
 uint8 transition_switch	 # VTOL transition switch: _HOVER, FORWARD_FLIGHT
+uint8 gear_switch	 # landing gear switch: _DOWN_, UP
 int8 mode_slot			 # the slot a specific model selector is in

msg/rc_channels.msg

@@ -21,11 +21,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
 uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
 uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
+uint8 RC_CHANNELS_FUNCTION_GEAR=22
 
 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[22] function						# Functions mapping
+int8[23] function						# Functions mapping
 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

msg/vehicle_attitude_setpoint.msg

@@ -30,5 +30,7 @@ bool disable_mc_yaw_control			# control yaw for mc (used for vtol weather-vane m
 
 bool apply_flaps
 
+float32 landing_gear
+
 # WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
 # TOPICS vehicle_attitude_setpoint

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1156,6 +1156,7 @@ FixedwingAttitudeControl::task_main()
 			_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
 			_actuators.control[5] = _manual.aux1;
 			_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
+			// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
 			_actuators.control[7] = _manual.aux3;
 
 			/* lazily publish the setpoint only once available */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -976,6 +976,7 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
 				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
 				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
+				_actuators.control[7] = _v_att_sp.landing_gear;
 				_actuators.timestamp = hrt_absolute_time();
 				_actuators.timestamp_sample = _ctrl_state.timestamp;
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -66,7 +66,6 @@
 #include <arch/board/board.h>
 
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/mc_virtual_attitude_setpoint.h>
@@ -1195,6 +1194,22 @@ void MulticopterPositionControl::control_auto(float dt)
 			_reset_alt_sp = true;
 		}
 
+		// During a mission or in loiter it's safe to retract the landing gear.
+		if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
+		     _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
+		    !_vehicle_land_detected.landed) {
+			_att_sp.landing_gear = 1.0f;
+
+			// During takeoff and landing, we better put it down again.
+
+		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
+			   _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+			_att_sp.landing_gear = -1.0f;
+
+		} else {
+			// For the rest of the setpoint types, just leave it as is.
+		}
+
 	} else {
 		/* no waypoint, do nothing, setpoint was already reset */
 	}
@@ -1240,6 +1255,9 @@ MulticopterPositionControl::task_main()
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 
+	// Let's be safe and have the landing gear down by default
+	_att_sp.landing_gear = -1.0f;
+
 
 	matrix::Dcmf R;
 	R.identity();
@@ -2064,6 +2082,15 @@ MulticopterPositionControl::task_main()
 				_att_sp.q_d_valid = true;
 			}
 
+			if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
+			    !_vehicle_land_detected.landed) {
+				_att_sp.landing_gear = 1.0f;
+
+			} else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+				_att_sp.landing_gear = -1.0f;
+			}
+
+
 			_att_sp.timestamp = hrt_absolute_time();
 
 		} else {

src/modules/sensors/sensor_params.c

@@ -2544,6 +2544,33 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
 
+/**
+ * Landing gear switch channel
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Switches
+ * @value 0 Unassigned
+ * @value 1 Channel 1
+ * @value 2 Channel 2
+ * @value 3 Channel 3
+ * @value 4 Channel 4
+ * @value 5 Channel 5
+ * @value 6 Channel 6
+ * @value 7 Channel 7
+ * @value 8 Channel 8
+ * @value 9 Channel 9
+ * @value 10 Channel 10
+ * @value 11 Channel 11
+ * @value 12 Channel 12
+ * @value 13 Channel 13
+ * @value 14 Channel 14
+ * @value 15 Channel 15
+ * @value 16 Channel 16
+ * @value 17 Channel 17
+ * @value 18 Channel 18
+ */
+PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
 /**
  * AUX1 Passthrough RC Channel
  *
@@ -2976,6 +3003,24 @@ PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
  */
 PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f);
 
+/**
+ * Threshold for the landing gear switch
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.25f);
+
 /**
  * PWM input channel that provides RSSI.
  *

src/modules/sensors/sensors.cpp

@@ -306,6 +306,7 @@ private:
 		int rc_map_offboard_sw;
 		int rc_map_kill_sw;
 		int rc_map_trans_sw;
+		int rc_map_gear_sw;
 
 		int rc_map_flaps;
 
@@ -330,6 +331,7 @@ private:
 		float rc_offboard_th;
 		float rc_killswitch_th;
 		float rc_trans_th;
+		float rc_gear_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_rattitude_inv;
@@ -340,6 +342,7 @@ private:
 		bool rc_offboard_inv;
 		bool rc_killswitch_inv;
 		bool rc_trans_inv;
+		bool rc_gear_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -379,6 +382,7 @@ private:
 		param_t rc_map_offboard_sw;
 		param_t rc_map_kill_sw;
 		param_t rc_map_trans_sw;
+		param_t rc_map_gear_sw;
 
 		param_t rc_map_flaps;
 
@@ -407,6 +411,7 @@ private:
 		param_t rc_offboard_th;
 		param_t rc_killswitch_th;
 		param_t rc_trans_th;
+		param_t rc_gear_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -656,6 +661,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
 	_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
 	_parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
+	_parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -685,6 +691,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
 	_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
 	_parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
+	_parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
 
 
 	/* Differential pressure offset */
@@ -862,6 +869,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_gear_sw, &(_parameters.rc_map_gear_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		PX4_WARN("%s", paramerr);
 	}
@@ -909,6 +920,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_trans_th, &(_parameters.rc_trans_th));
 	_parameters.rc_trans_inv = (_parameters.rc_trans_th < 0);
 	_parameters.rc_trans_th = fabs(_parameters.rc_trans_th);
+	param_get(_parameter_handles.rc_gear_th, &(_parameters.rc_gear_th));
+	_parameters.rc_gear_inv = (_parameters.rc_gear_th < 0);
+	_parameters.rc_gear_th = fabs(_parameters.rc_gear_th);
 
 	/* update RC function mappings */
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -925,6 +939,7 @@ Sensors::parameters_update()
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1;
 
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -2131,6 +2146,8 @@ Sensors::rc_poll()
 					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
 			manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
 						   _parameters.rc_trans_th, _parameters.rc_trans_inv);
+			manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
+					     _parameters.rc_gear_th, _parameters.rc_gear_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub != nullptr) {