fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-02-22 12:42:50 +01:00 committed by tumbili
parent f5e0c72ea0
commit 70eb77453a
2 changed files with 35 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
src/modules/land_detector/MulticopterLandDetector.cpp
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
* modification, are permitted provided that the following conditions
@ -48,14 +48,12 @@
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
_paramHandle(),
_params(),
_vehicleGlobalPositionSub(-1),
_vehicleStatusSub(-1),
_vehicleLocalPositionSub(-1),
_actuatorsSub(-1),
_armingSub(-1),
_parameterSub(-1),
_attitudeSub(-1),
_vehicleGlobalPosition{},
_vehicleStatus{},
_vehicleLocalPosition{},
_actuators{},
_arming{},
_vehicleAttitude{},
@ -70,9 +68,8 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
void MulticopterLandDetector::initialize()
{
// 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));
_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));
@ -83,9 +80,8 @@ void MulticopterLandDetector::initialize()
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_status), _vehicleStatusSub, &_vehicleStatus);
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
}
@ -111,12 +107,17 @@ bool MulticopterLandDetector::get_landed_state()
_arming_time = hrt_absolute_time();
}
// return status based on armed state if no position lock is available
if (_vehicleGlobalPosition.timestamp == 0 ||
hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) {
// Check if thrust output is less than max throttle param.
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
// no position lock - not landed if armed
return !_arming.armed;
// Return status based on armed state and throttle if no position lock is available.
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();
@ -124,38 +125,35 @@ bool MulticopterLandDetector::get_landed_state()
float armThresholdFactor = 1.0f;
// 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) {
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
// an accurate in-air indication
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
// an accurate in-air indication.
bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor;
// check if we are moving horizontally
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity
&& _vehicleStatus.condition_global_position_valid;
// Check if we are moving horizontally.
bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
// next look if all rotation angles are not moving
float maxRotationScaled = _params.maxRotation * armThresholdFactor;
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * armThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > 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) {
// sensed movement, so reset the land detector
// Sensed movement or thottle high, so reset the land detector.
_landTimer = now;
return false;
}
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME);
}
void MulticopterLandDetector::updateParameterCache(const bool force)
@ -172,8 +170,8 @@ void MulticopterLandDetector::updateParameterCache(const bool force)
if (updated || 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.maxRotation, &_params.maxRotation_rad_s);
_params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s);
param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
}
}

13
src/modules/land_detector/MulticopterLandDetector.h
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
* modification, are permitted provided that the following conditions
@ -43,7 +43,7 @@
#define __MULTICOPTER_LAND_DETECTOR_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_status.h>
#include <uORB/topics/actuator_controls.h>
@ -97,25 +97,26 @@ private:
struct {
float maxClimbRate;
float maxVelocity;
float maxRotation;
float maxRotation_rad_s;
float maxThrottle;
} _params;
private:
int _vehicleGlobalPositionSub; /**< notification of global position */
int _vehicleLocalPositionSub;
int _vehicleStatusSub;
int _actuatorsSub;
int _armingSub;
int _parameterSub;
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 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*/
/* timestamp in microseconds since a possible land was detected */
uint64_t _landTimer;
};
#endif //__MULTICOPTER_LAND_DETECTOR_H__