land_detector: battery level status to adjust maximum altitude possible

msg/vehicle_land_detected.msg

@@ -1,3 +1,4 @@
 bool landed		# true if vehicle is currently landed on the ground
 bool freefall	# true if vehicle is currently in free-fall
 bool ground_contact # true if vehicle has ground contact but is not landed
+float32 alt_max 	# maximum altitude in [m] that can be reached

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -92,6 +92,12 @@ void FixedwingLandDetector::_update_params()
 	param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 }
 
+float FixedwingLandDetector::_get_max_altitude()
+{
+	//ToDo
+	return 0.0f;
+}
+
 bool FixedwingLandDetector::_get_freefall_state()
 {
 	// TODO

src/modules/land_detector/FixedwingLandDetector.h

@@ -68,6 +68,8 @@ protected:
 	virtual bool _get_ground_contact_state() override;
 
 	virtual bool _get_freefall_state() override;
+
+	virtual float _get_max_altitude() override;
 private:
 	struct {
 		param_t maxVelocity;

src/modules/land_detector/LandDetector.cpp

@@ -41,6 +41,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
+#include <float.h>
 
 #include "LandDetector.h"
 
@@ -123,6 +124,9 @@ void LandDetector::_cycle()
 
 	_update_state();
 
+	float alt_max_prev = _altitude_max;
+	_update_max_altitude();
+
 	bool freefallDetected = (_state == LandDetectionState::FREEFALL);
 	bool landDetected = (_state == LandDetectionState::LANDED);
 	bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
@@ -132,7 +136,8 @@ void LandDetector::_cycle()
 	if ((_landDetectedPub == nullptr) ||
 	    (_landDetected.freefall != freefallDetected) ||
 	    (_landDetected.landed != landDetected) ||
-	    (_landDetected.ground_contact != ground_contactDetected)) {
+	    (_landDetected.ground_contact != ground_contactDetected) ||
+	    (fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) {
 
 		if (!landDetected && _landDetected.landed) {
 			// We did take off
@@ -152,6 +157,7 @@ void LandDetector::_cycle()
 		_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
 		_landDetected.landed = (_state == LandDetectionState::LANDED);
 		_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
+		_landDetected.alt_max = _altitude_max;
 
 		int instance;
 		orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
@@ -168,7 +174,6 @@ void LandDetector::_cycle()
 		_taskIsRunning = false;
 	}
 }
-
 void LandDetector::_check_params(const bool force)
 {
 	bool updated;
@@ -216,6 +221,12 @@ void LandDetector::_update_state()
 	}
 }
 
+void LandDetector::_update_max_altitude()
+{
+	_altitude_max = _get_max_altitude();
+}
+
+
 bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer)
 {
 	bool newData = false;

src/modules/land_detector/LandDetector.h

@@ -61,6 +61,10 @@ public:
 		GROUND_CONTACT = 3
 	};
 
+	enum class BatteryLevel {
+
+	};
+
 	LandDetector();
 	virtual ~LandDetector();
 
@@ -123,6 +127,11 @@ protected:
 	 */
 	virtual bool _get_freefall_state() = 0;
 
+	/**
+	 *  @return
+	 */
+	virtual float _get_max_altitude() = 0;
+
 	/**
 	 * Convenience function for polling uORB subscriptions.
 	 *
@@ -153,6 +162,9 @@ protected:
 	systemlib::Hysteresis _landed_hysteresis;
 	systemlib::Hysteresis _ground_contact_hysteresis;
 
+	float _altitude_max;
+
+
 
 private:
 	static void _cycle_trampoline(void *arg);
@@ -163,6 +175,9 @@ private:
 
 	void _update_state();
 
+	void _update_max_altitude();
+
+
 	bool _taskShouldExit;
 	bool _taskIsRunning;
 
@@ -171,6 +186,7 @@ private:
 	uint64_t _total_flight_time; ///< in microseconds
 	hrt_abstime _takeoff_time;
 
+
 	struct work_s	_work;
 };
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -59,6 +59,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_manualSub(-1),
 	_ctrl_state_sub(-1),
 	_vehicle_control_mode_sub(-1),
+	_battery_sub(-1),
 	_vehicleLocalPosition{},
 	_actuators{},
 	_arming{},
@@ -66,6 +67,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_manual{},
 	_ctrl_state{},
 	_control_mode{},
+	_battery{},
 	_min_trust_start(0),
 	_arming_time(0)
 {
@@ -79,6 +81,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
 	_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
 	_paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR");
+	_paramHandle.altitude_max = param_find("LNDMC_ALT_MAX");
 }
 
 void MulticopterLandDetector::_initialize_topics()
@@ -92,6 +95,7 @@ void MulticopterLandDetector::_initialize_topics()
 	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -103,6 +107,7 @@ void MulticopterLandDetector::_update_topics()
 	_orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
 	_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
+	_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
 }
 
 void MulticopterLandDetector::_update_params()
@@ -119,6 +124,8 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
 	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
 	param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold);
+	param_get(_paramHandle.altitude_max, &_params.altitude_max);
+
 }
 
 
@@ -279,6 +286,26 @@ float MulticopterLandDetector::_get_takeoff_throttle()
 	return 0.0f;
 }
 
+float MulticopterLandDetector::_get_max_altitude()
+{
+	/* ToDo: add meaningful heights */
+	float valid_altitude_max = _params.altitude_max;
+
+	if (_battery.warning == battery_status_s::BATTERY_WARNING_LOW) {
+		valid_altitude_max = _params.altitude_max * 0.75f;
+	}
+
+	if (_battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+		valid_altitude_max = _params.altitude_max * 0.5f;
+	}
+
+	if (_battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
+		valid_altitude_max = _params.altitude_max * 0.25f;
+	}
+
+	return valid_altitude_max;
+}
+
 bool MulticopterLandDetector::_has_position_lock()
 {
 	return !(_vehicleLocalPosition.timestamp == 0 ||

src/modules/land_detector/MulticopterLandDetector.h

@@ -51,6 +51,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/battery_status.h>
 
 #include "LandDetector.h"
 
@@ -74,6 +75,8 @@ protected:
 	virtual bool _get_ground_contact_state() override;
 
 	virtual bool _get_freefall_state() override;
+
+	virtual float _get_max_altitude() override;
 private:
 
 	/**
@@ -90,6 +93,7 @@ private:
 		param_t freefall_acc_threshold;
 		param_t freefall_trigger_time;
 		param_t manual_stick_down_threshold;
+		param_t altitude_max;
 	} _paramHandle;
 
 	struct {
@@ -103,6 +107,7 @@ private:
 		float freefall_acc_threshold;
 		float freefall_trigger_time;
 		float manual_stick_down_threshold;
+		float altitude_max;
 	} _params;
 
 	int _vehicleLocalPositionSub;
@@ -112,6 +117,7 @@ private:
 	int _manualSub;
 	int _ctrl_state_sub;
 	int _vehicle_control_mode_sub;
+	int _battery_sub;
 
 	struct vehicle_local_position_s		_vehicleLocalPosition;
 	struct actuator_controls_s		_actuators;
@@ -120,6 +126,7 @@ private:
 	struct manual_control_setpoint_s	_manual;
 	struct control_state_s			_ctrl_state;
 	struct vehicle_control_mode_s		_control_mode;
+	struct battery_status_s _battery;
 
 	uint64_t _min_trust_start;		///< timestamp when minimum trust was applied first
 	uint64_t _arming_time;

src/modules/land_detector/land_detector_params.c

@@ -213,3 +213,15 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_HI, 0);
  *
  */
 PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0);
+
+/**
+ * Maximum altitude that can be reached prior to subconditions
+ *
+ * @unit m/s
+ * @min 10
+ * @max 150
+ * @decimal 2
+ * @group Land Detector
+ *
+ */
+PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, 15.0f);