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

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -43,26 +43,27 @@
 
 #include <cmath>
 #include <drivers/drv_hrt.h>
+#include <mathlib/mathlib.h>
 
 MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
 	_vehicleGlobalPositionSub(-1),
-	_sensorsCombinedSub(-1),
 	_waypointSub(-1),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
+    _attitudeSub(-1),
 	_vehicleGlobalPosition({}),
-	_sensors({}),
 	_waypoint({}),
 	_actuators({}),
 	_arming({}),
+    _vehicleAttitude({}),
 	_landTimer(0)
 {
-	_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX");
+	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
 	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
-	_paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX");
+	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
 	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
 }
 
@@ -70,7 +71,7 @@ void MulticopterLandDetector::initialize()
 {
 	// subscribe to position, attitude, arming and velocity changes
 	_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined));
+	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
@@ -83,7 +84,7 @@ void MulticopterLandDetector::initialize()
 void MulticopterLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition);
-	orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors);
+	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
 	orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint);
 	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
@@ -109,9 +110,9 @@ bool MulticopterLandDetector::update()
 					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
 
 	// next look if all rotation angles are not moving
-	bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
-			      _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
-			      _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
+	bool rotating = sqrtf(_vehicleAttitude.rollspeed*_vehicleAttitude.rollspeed + 
+				          _vehicleAttitude.pitchspeed*_vehicleAttitude.pitchspeed + 
+        				  _vehicleAttitude.yawspeed*_vehicleAttitude.yawspeed) > _params.maxRotation;
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
@@ -140,6 +141,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
 		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
 		param_get(_paramHandle.maxRotation, &_params.maxRotation);
+		_params.maxRotation = math::radians(_params.maxRotation);
 		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
 	}
 }

src/modules/land_detector/MulticopterLandDetector.h

@@ -44,7 +44,7 @@
 
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
@@ -98,17 +98,17 @@ private:
 
 private:
 	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
-	int _sensorsCombinedSub;
 	int _waypointSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
+    int _attitudeSub;
 
 	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
-	struct sensor_combined_s                  _sensors;                 /**< subscribe to sensor readings */
 	struct position_setpoint_triplet_s        _waypoint;                /**< subscribe to autopilot navigation */
 	struct actuator_controls_s                _actuators;
 	struct actuator_armed_s                   _arming;
+    struct vehicle_attitude_s                 _vehicleAttitude;
 
 	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
 };

src/modules/land_detector/land_detector_params.c

@@ -61,11 +61,11 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
 /**
  * Multicopter max rotation
  *
- * Maximum allowed around each axis to trigger a land (radians per second)
+ * Maximum allowed around each axis to trigger a land (degrees per second)
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f);
+PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 15.0f);
 
 /**
  * Multicopter max throttle