WIP: Landing switch

msg/adc.msg

@@ -0,0 +1,5 @@
+float32 virtual_pin_13		# Virtual pin 13 on ADC 3.3V connector pin 4.
+float32 virtual_pin_14		# Virtual pin 14 on ADC 3.3V connector pin 2.
+float32 virtual_pin_15		# Virtual pin 15 on ADC 6.6V connector pin 2.
+uint64 timestamp
+uint64 error_count

src/modules/land_detector/LandDetector.h

@@ -94,8 +94,9 @@ class LandDetector
 
 	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50;        /**< Run algorithm at 50Hz */
 
-	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
-                                                                          before triggering a land */
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;		/**< usec that landing conditions have to hold before triggering a land */
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_FAST = 1000000;  	/**< usec that landing conditions have to hold when confident before triggering a land */
+
 	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
 protected:

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -54,17 +54,20 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_armingSub(-1),
 	_parameterSub(-1),
 	_attitudeSub(-1),
+	_adcSub(-1),
 	_vehicleGlobalPosition{},
 	_vehicleStatus{},
 	_actuators{},
 	_arming{},
 	_vehicleAttitude{},
+	_adc{},
 	_landTimer(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
 	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
+	_paramHandle.landingSwitchEnable = param_find("LNDMC_SWITCH_ENABLE");
 }
 
 void MulticopterLandDetector::initialize()
@@ -76,6 +79,7 @@ void MulticopterLandDetector::initialize()
 	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
+	_adcSub = orb_subscribe(ORB_ID(adc));
 
 	// download parameters
 	updateParameterCache(true);
@@ -88,6 +92,7 @@ void MulticopterLandDetector::updateSubscriptions()
 	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
 	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	orb_update(ORB_ID(adc), _adcSub, &_adc);
 }
 
 bool MulticopterLandDetector::update()
@@ -141,7 +146,22 @@ bool MulticopterLandDetector::update()
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
 
-	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
+	// Check if landing gear switch support is enabled, if the switch(es) seem to be working and if they are active/on.
+	bool landedSwitchOff = (_params.landingSwitchEnable != SWITCH_OFF && _adc.virtual_pin_15 < 4.5f);
+
+	// If set via parameter, signal landing detection state using only the switch after a shorter wait time. We
+	// can't trigger immediately as there might be some bouncing on landing.
+	if (_params.landingSwitchEnable == SWITCH_TRUST) {
+		// Remain in a non-landed state if the switch is off or we have any lateral movement.
+		if (landedSwitchOff || rotating || horizontalMovement) {
+			_landTimer = now;
+			return false;
+		}
+
+		return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME_FAST;
+	}
+
+	if (verticalMovement || rotating || !minimalThrust || horizontalMovement || landedSwitchOff) {
 		// sensed movement, so reset the land detector
 		_landTimer = now;
 		return false;
@@ -167,5 +187,6 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxRotation, &_params.maxRotation);
 		_params.maxRotation = math::radians(_params.maxRotation);
 		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
+		param_get(_paramHandle.landingSwitchEnable, &_params.landingSwitchEnable);
 	}
 }

src/modules/land_detector/MulticopterLandDetector.h

@@ -49,6 +49,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/adc.h>
 #include <systemlib/param/param.h>
 
 class MulticopterLandDetector : public LandDetector
@@ -87,28 +88,38 @@ class MulticopterLandDetector : public LandDetector
 		param_t maxVelocity;
 		param_t maxRotation;
 		param_t maxThrottle;
+		param_t landingSwitchEnable;
 	}		_paramHandle;
 
 	struct {
 		float maxClimbRate;
 		float maxVelocity;
 		float maxRotation;
 		float maxThrottle;
+		int32_t landingSwitchEnable;
 	} _params;
 
+	enum {
+		SWITCH_OFF = 0,		// Disable the switch, it will be ignored.
+		SWITCH_ON = 1,		// Use switch in combination with other signals.
+		SWITCH_TRUST = 2	// Fully trust the switch, disregard other signals.
+	};
+
 private:
 	int _vehicleGlobalPositionSub;						/**< notification of global position */
 	int _vehicleStatusSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
 	int _attitudeSub;
+	int _adcSub;
 
 	struct vehicle_global_position_s	_vehicleGlobalPosition;		/**< the result from global position subscription */
 	struct vehicle_status_s 		_vehicleStatus;
 	struct actuator_controls_s		_actuators;
 	struct actuator_armed_s			_arming;
 	struct vehicle_attitude_s		_vehicleAttitude;
+	struct adc_s 				_adc;
 
 	uint64_t _landTimer;							/**< timestamp in microseconds since a possible land was detected*/
 };

src/modules/land_detector/land_detector_params.c

@@ -85,6 +85,19 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
  */
 PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f);
 
+/**
+ * Multicopter landing switch
+ *
+ * Enable landing switches if present as an additional signal by setting to 1.
+ * To trigger landing state based on the switch state immediately set to 2.
+ *
+ * @min 0
+ * @max 2
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_INT32(LNDMC_SWITCH_ENABLE, 0);
+
 /**
  * Fixedwing max horizontal velocity
  *

src/modules/sensors/sensors.cpp

@@ -94,6 +94,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/adc.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_parameter_map.h>
 
@@ -215,6 +216,7 @@ class Sensors
 
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
+	int		_adc_sub;			/**< ADC subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int		_rc_parameter_map_sub;		/**< rc parameter map subscription */
@@ -227,13 +229,15 @@ class Sensors
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
 	orb_advert_t	_diff_pres_pub;			/**< differential_pressure */
+	orb_advert_t	_adc_pub;			/**< ADC */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
 	struct battery_status_s _battery_status;	/**< battery status */
 	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _diff_pres;
+	struct adc_s _adc;				/**< ADC raw values */
 	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 */
@@ -511,6 +515,7 @@ Sensors::Sensors() :
 	_battery_pub(nullptr),
 	_airspeed_pub(nullptr),
 	_diff_pres_pub(nullptr),
+	_adc_pub(nullptr),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
@@ -1632,10 +1637,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					_battery_current_timestamp = t;
 
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
-
 					/* calculate airspeed, raw is the difference from */
 					float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f;  // V_ref/4096 * (voltage divider factor)
 
+					/* Announce the ADC state if needed, otherwise just publish */
+					_adc.timestamp = t;
+					_adc.virtual_pin_15 = voltage;
+
+					if (_adc_pub != nullptr) {
+						orb_publish(ORB_ID(adc), _adc_pub, &_adc);
+					} else {
+						_adc_pub = orb_advertise(ORB_ID(adc), &_adc);
+					}
+
 					/**
 					 * The voltage divider pulls the signal down, only act on
 					 * a valid voltage from a connected sensor. Also assume a non-
@@ -2021,6 +2035,7 @@ Sensors::task_main()
 
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	_adc_sub = orb_subscribe(ORB_ID(adc));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));

src/modules/uORB/objects_common.cpp

@@ -263,3 +263,6 @@ ORB_DEFINE(distance_sensor, struct distance_sensor_s);
 
 #include "topics/camera_trigger.h"
 ORB_DEFINE(camera_trigger, struct camera_trigger_s);
+
+#include "topics/adc.h"
+ORB_DEFINE(adc, struct adc_s);