Merge branch 'land_detector_no_gps_fix' of github.com:Zefz/Firmware

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -49,13 +49,13 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
 	_vehicleGlobalPositionSub(-1),
-	_waypointSub(-1),
+	_vehicleStatusSub(-1),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
     _attitudeSub(-1),
 	_vehicleGlobalPosition({}),
-	_waypoint({}),
+	_vehicleStatus({}),
 	_actuators({}),
 	_arming({}),
     _vehicleAttitude({}),
@@ -72,7 +72,7 @@ void MulticopterLandDetector::initialize()
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
 	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
@@ -85,7 +85,7 @@ void MulticopterLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition);
 	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
-	orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint);
+	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);
 }
@@ -107,12 +107,13 @@ bool MulticopterLandDetector::update()
 
 	// check if we are moving horizontally
 	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
-					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
+					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity
+					&& _vehicleStatus.condition_global_position_valid;
 
 	// next look if all rotation angles are not moving
-	bool rotating = sqrtf(_vehicleAttitude.rollspeed*_vehicleAttitude.rollspeed + 
-				          _vehicleAttitude.pitchspeed*_vehicleAttitude.pitchspeed + 
-        				  _vehicleAttitude.yawspeed*_vehicleAttitude.yawspeed) > _params.maxRotation;
+	bool rotating = fabsf(_vehicleAttitude.rollspeed)  > _params.maxRotation ||
+				    fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
+        			fabsf(_vehicleAttitude.yawspeed)   > _params.maxRotation;
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;

src/modules/land_detector/MulticopterLandDetector.h

@@ -45,7 +45,7 @@
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
@@ -98,14 +98,14 @@ private:
 
 private:
 	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
-	int _waypointSub;
+	int _vehicleStatusSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
     int _attitudeSub;
 
 	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
-	struct position_setpoint_triplet_s        _waypoint;                /**< subscribe to autopilot navigation */
+	struct vehicle_status_s 			      _vehicleStatus;
 	struct actuator_controls_s                _actuators;
 	struct actuator_armed_s                   _arming;
     struct vehicle_attitude_s                 _vehicleAttitude;

src/modules/land_detector/land_detector_params.c

@@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 15.0f);
+PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
 
 /**
  * Multicopter max throttle