fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 17:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LandDetector: Fix for land detection without GPS

Browse Source
This commit is contained in:
Johan Jansen
2015-02-09 18:03:10 +01:00
parent c371eb9b90
commit fe21cb6a1a
2 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+6 -5
View File
@@ -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 +
src/modules/land_detector/MulticopterLandDetector.h
+3 -3
View File
@@ -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;