Merge pull request #3184 from PX4/takeoff_detect

Takeoff detect

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -859,7 +859,9 @@ void AttitudePositionEstimatorEKF::publishControlState()
 	/* Accelerations in Body Frame */
 	_ctrl_state.x_acc = _ekf->accel.x;
 	_ctrl_state.y_acc = _ekf->accel.y;
-	_ctrl_state.z_acc = _ekf->accel.z;
+	_ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13];
+
+	_ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal();
 
 	/* Velocity in Body Frame */
 	Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);

src/modules/ekf_att_pos_estimator/estimator_22states.h

@@ -382,6 +382,8 @@ public:
 
     void get_covariance(float c[28]);
 
+    float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
+
 protected:
 
     /**

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -36,6 +36,7 @@
  * Land detection algorithm for fixedwings
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include "FixedwingLandDetector.h"
@@ -48,42 +49,44 @@
 FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
-	_vehicleLocalPositionSub(-1),
-	_vehicleLocalPosition( {}),
-		       _airspeedSub(-1),
-		       _vehicleStatusSub(-1),
-		       _armingSub(-1),
-		       _airspeed{},
-		       _vehicleStatus{},
-		       _arming{},
-		       _parameterSub(-1),
-		       _velocity_xy_filtered(0.0f),
-		       _velocity_z_filtered(0.0f),
-		       _airspeed_filtered(0.0f),
-		       _landDetectTrigger(0)
+	_controlStateSub(-1),
+	_vehicleStatusSub(-1),
+	_armingSub(-1),
+	_airspeedSub(-1),
+	_controlState{},
+	_vehicleStatus{},
+	_arming{},
+	_airspeed{},
+	_parameterSub(-1),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f),
+	_accel_horz_lp(0.0f),
+	_landDetectTrigger(0)
 {
 	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
 	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
+	_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
 }
 
 void FixedwingLandDetector::initialize()
 {
 	// Subscribe to local position and airspeed data
-	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
-	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+	_controlStateSub = orb_subscribe(ORB_ID(control_state));
 	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
+	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
 
 	updateParameterCache(true);
 }
 
 void FixedwingLandDetector::updateSubscriptions()
 {
-	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
 	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
 bool FixedwingLandDetector::update()
@@ -99,29 +102,33 @@ bool FixedwingLandDetector::update()
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 
-	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
-		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
-				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
+		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
+				_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;
 		}
 
-		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
+		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_z_filtered = val;
 		}
-	}
 
-	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
 		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+
+		// a leaking lowpass prevents biases from building up, but
+		// gives a mostly correct response for short impulses
+		_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
+
 	}
 
 	// crude land detector for fixedwing
 	if (_velocity_xy_filtered < _params.maxVelocity
 	    && _velocity_z_filtered < _params.maxClimbRate
-	    && _airspeed_filtered < _params.maxAirSpeed) {
+	    && _airspeed_filtered < _params.maxAirSpeed
+	    && _accel_horz_lp < _params.maxIntVelocity) {
 
 		// these conditions need to be stable for a period of time before we trust them
 		if (now > _landDetectTrigger) {
@@ -151,5 +158,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
 		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
 		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+		param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 	}
 }

src/modules/land_detector/FixedwingLandDetector.h

@@ -42,11 +42,11 @@
 #define __FIXED_WING_LAND_DETECTOR_H__
 
 #include "LandDetector.h"
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/airspeed.h>
 #include <systemlib/param/param.h>
 
 class FixedwingLandDetector : public LandDetector
@@ -83,28 +83,31 @@ private:
 		param_t maxVelocity;
 		param_t maxClimbRate;
 		param_t maxAirSpeed;
+		param_t maxIntVelocity;
 	}		_paramHandle;
 
 	struct {
 		float maxVelocity;
 		float maxClimbRate;
 		float maxAirSpeed;
+		float maxIntVelocity;
 	} _params;
 
 private:
-	int					_vehicleLocalPositionSub;	/**< notification of local position */
-	struct vehicle_local_position_s		_vehicleLocalPosition;		/**< the result from local position subscription */
-	int					_airspeedSub;
+	int					_controlStateSub;	/**< notification of local position */
 	int					_vehicleStatusSub;
 	int					_armingSub;
-	struct airspeed_s			_airspeed;
+	int					_airspeedSub;
+	struct control_state_s			_controlState;		/**< the result from local position subscription */
 	struct vehicle_status_s 		_vehicleStatus;
 	struct actuator_armed_s			_arming;
+	struct airspeed_s			_airspeed;
 	int 					_parameterSub;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;
 	float _airspeed_filtered;
+	float _accel_horz_lp;
 	uint64_t _landDetectTrigger;
 };
 

src/modules/land_detector/LandDetector.cpp

@@ -57,14 +57,14 @@ _work{} {
 
 LandDetector::~LandDetector()
 {
-	work_cancel(LPWORK, &_work);
+	work_cancel(HPWORK, &_work);
 	_taskShouldExit = true;
 }
 
 int LandDetector::start()
 {
 	/* schedule a cycle to start things */
-	work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
+	work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
 
 	return 0;
 }
@@ -107,10 +107,11 @@ void LandDetector::cycle()
 
 		// publish the land detected broadcast
 		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
+		//warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF");
 	}
 
 	if (!_taskShouldExit) {
-		work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
+		work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
 			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
 	}
 }

src/modules/land_detector/land_detector_params.c

@@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
  */
 PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
 
+/**
+ * Fixedwing max short-term velocity
+ *
+ * Maximum velocity integral in flight direction allowed in the landed state (m/s)
+ *
+ * @min 2
+ * @max 10
+ * @unit m/s
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f);
+
 /**
  * Airspeed max
  *