fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 18:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LandDetector: Use vehicle_attitude instead of sensors_combined

Browse Source
This commit is contained in:
Johan Jansen
2015-02-09 12:42:20 +01:00
parent 70e4554f76
commit 01dc774703
2 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+7 -7
View File
@@ -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;
src/modules/land_detector/MulticopterLandDetector.h
+3 -3
View File
@@ -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*/
};