From 70e4554f76d302fc8a8932ef2352039004172871 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 9 Feb 2015 12:42:02 +0100 Subject: [PATCH 1/4] LandDetector: Fix parameter swapped bug --- src/modules/land_detector/MulticopterLandDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 277cb9363f..ce391c8170 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -60,9 +60,9 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _arming({}), _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"); } From 01dc7747032bc3d14408e23a13ef45c9f37e46c5 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 9 Feb 2015 12:42:20 +0100 Subject: [PATCH 2/4] LandDetector: Use vehicle_attitude instead of sensors_combined --- .../land_detector/MulticopterLandDetector.cpp | 14 +++++++------- .../land_detector/MulticopterLandDetector.h | 6 +++--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index ce391c8170..61b5867bf0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -48,16 +48,16 @@ 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_ROT_MAX"); @@ -70,7 +70,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 +83,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 +109,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; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 7bb7f1a907..eeb5175ccc 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -44,7 +44,7 @@ #include "LandDetector.h" #include -#include +#include #include #include #include @@ -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*/ }; From 004118f929a03e0d42e363dd2b74330ccf6b82fe Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 9 Feb 2015 12:47:31 +0100 Subject: [PATCH 3/4] LandDetector: Change rotation rate in rads/s to deg/s (more similar to mpc params) --- src/modules/land_detector/MulticopterLandDetector.cpp | 2 ++ src/modules/land_detector/land_detector_params.c | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 61b5867bf0..cfb3344f9b 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -43,6 +43,7 @@ #include #include +#include MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle(), @@ -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); } } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 0302bc7c14..0deba18352 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/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, 0.15f); /** * Multicopter max throttle From e38b25c1f78895ad4a1a8a6f4ab5429dbd38f456 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 9 Feb 2015 16:52:04 +0100 Subject: [PATCH 4/4] LandDetector: Fix default param scale error --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 0deba18352..d9ab474560 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/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, 0.15f); +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 15.0f); /** * Multicopter max throttle