fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 03:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MultiCopterLandDetector: switch to local position

Browse Source 
Instead of using the global position topic for NED velocities, let's use
the local position topic. Also, use the xy and z valid flags instead of
the system state.
In case of no local position update, still rely on the throttle stick
position.
This commit is contained in:
Julian Oes
2016-03-14 09:45:34 +01:00
committed by tumbili
parent f5e0c72ea0
commit 70eb77453a
2 changed files with 35 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+28 -30
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -48,14 +48,12 @@
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle(), _paramHandle(),
_params(), _params(),
_vehicleGlobalPositionSub(-1), _vehicleLocalPositionSub(-1),
_vehicleStatusSub(-1),
_actuatorsSub(-1), _actuatorsSub(-1),
_armingSub(-1), _armingSub(-1),
_parameterSub(-1), _parameterSub(-1),
_attitudeSub(-1), _attitudeSub(-1),
_vehicleGlobalPosition{}, _vehicleLocalPosition{},
_vehicleStatus{},
_actuators{}, _actuators{},
_arming{}, _arming{},
_vehicleAttitude{}, _vehicleAttitude{},
@@ -70,9 +68,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
void MulticopterLandDetector::initialize() void MulticopterLandDetector::initialize()
{ {
// subscribe to position, attitude, arming and velocity changes // subscribe to position, attitude, arming and velocity changes
_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
_armingSub = orb_subscribe(ORB_ID(actuator_armed)); _armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update)); _parameterSub = orb_subscribe(ORB_ID(parameter_update));
@@ -83,9 +80,8 @@ void MulticopterLandDetector::initialize()
void MulticopterLandDetector::updateSubscriptions() void MulticopterLandDetector::updateSubscriptions()
{ {
orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
} }
@@ -111,12 +107,17 @@ bool MulticopterLandDetector::get_landed_state()
_arming_time = hrt_absolute_time(); _arming_time = hrt_absolute_time();
} }
// return status based on armed state if no position lock is available // Check if thrust output is less than max throttle param.
if (_vehicleGlobalPosition.timestamp == 0 || bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// no position lock - not landed if armed // Return status based on armed state and throttle if no position lock is available.
return !_arming.armed; if (_vehicleLocalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 ||
!_vehicleLocalPosition.xy_valid ||
!_vehicleLocalPosition.z_valid) {
// Minimal thrust means landed.
return minimalThrust;
} }
const uint64_t now = hrt_absolute_time(); const uint64_t now = hrt_absolute_time();
@@ -124,38 +125,35 @@ bool MulticopterLandDetector::get_landed_state()
float armThresholdFactor = 1.0f; float armThresholdFactor = 1.0f;
// Widen acceptance thresholds for landed state right after arming // Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives // so that motor spool-up and other effects do not trigger false negatives.
if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) { if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
armThresholdFactor = 2.5f; armThresholdFactor = 2.5f;
} }
// check if we are moving vertically - this might see a spike after arming due to // Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give // throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication // an accurate in-air indication.
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor; bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
// check if we are moving horizontally // Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
&& _vehicleStatus.condition_global_position_valid;
// next look if all rotation angles are not moving // Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation * armThresholdFactor; float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
// check if thrust output is minimal (about half of default)
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
// sensed movement, so reset the land detector // Sensed movement or thottle high, so reset the land detector.
_landTimer = now; _landTimer = now;
return false; return false;
} }
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
} }
void MulticopterLandDetector::updateParameterCache(const bool force) void MulticopterLandDetector::updateParameterCache(const bool force)
@@ -172,8 +170,8 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
if (updated || force) { if (updated || force) {
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
param_get(_paramHandle.maxRotation, &_params.maxRotation); param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s);
_params.maxRotation = math::radians(_params.maxRotation); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
param_get(_paramHandle.maxThrottle, &_params.maxThrottle); param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
} }
} }
src/modules/land_detector/MulticopterLandDetector.h
+7 -6
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -43,7 +43,7 @@
#define __MULTICOPTER_LAND_DETECTOR_H__ #define __MULTICOPTER_LAND_DETECTOR_H__
#include "LandDetector.h" #include "LandDetector.h"
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
@@ -97,25 +97,26 @@ private:
struct { struct {
float maxClimbRate; float maxClimbRate;
float maxVelocity; float maxVelocity;
float maxRotation; float maxRotation_rad_s;
float maxThrottle; float maxThrottle;
} _params; } _params;
private: private:
int _vehicleGlobalPositionSub; /**< notification of global position */ int _vehicleLocalPositionSub;
int _vehicleStatusSub; int _vehicleStatusSub;
int _actuatorsSub; int _actuatorsSub;
int _armingSub; int _armingSub;
int _parameterSub; int _parameterSub;
int _attitudeSub; int _attitudeSub;
struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct vehicle_local_position_s _vehicleLocalPosition;
struct vehicle_status_s _vehicleStatus; struct vehicle_status_s _vehicleStatus;
struct actuator_controls_s _actuators; struct actuator_controls_s _actuators;
struct actuator_armed_s _arming; struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude; struct vehicle_attitude_s _vehicleAttitude;
uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ /* timestamp in microseconds since a possible land was detected */
uint64_t _landTimer;
}; };
#endif //__MULTICOPTER_LAND_DETECTOR_H__ #endif //__MULTICOPTER_LAND_DETECTOR_H__