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

1 files changed, 11 insertions, 9 deletions

diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index 277cb9363..cfb3344f9 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/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);
 	}
 }