diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-09 18:03:10 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-09 18:06:29 +0100 |
commit | fe21cb6a1aa0c5ccd9610cb2a93139a91a9e5a72 (patch) | |
tree | b86dcfafbddd8debbbb3265ac2423111a7ceb539 | |
parent | c371eb9b901f0644bc4732934156a27cf5c4be5c (diff) | |
download | px4-firmware-fe21cb6a1aa0c5ccd9610cb2a93139a91a9e5a72.tar.gz px4-firmware-fe21cb6a1aa0c5ccd9610cb2a93139a91a9e5a72.tar.bz2 px4-firmware-fe21cb6a1aa0c5ccd9610cb2a93139a91a9e5a72.zip |
LandDetector: Fix for land detection without GPS
-rw-r--r-- | src/modules/land_detector/MulticopterLandDetector.cpp | 11 | ||||
-rw-r--r-- | src/modules/land_detector/MulticopterLandDetector.h | 6 |
2 files changed, 9 insertions, 8 deletions
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cfb3344f9..6d7803536 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/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,7 +107,8 @@ 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 + diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index eeb5175cc..70df91f43 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/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; |