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

land_detector: Add and max_altitude and timestamp validity checks to MulticopterLandDetector class. (#12681)

Browse Source 
* Rename local camelCase vars to snake_case and control_mode -> vehicle_control_mode to match typdef with established class convention.
This commit is contained in:
Mark Sauder
2019-09-02 00:30:46 -04:00
committed by Daniel Agar
parent c180d63441
commit 60e5e0821a
3 changed files with 31 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.h
+3 -3
View File
@@ -42,16 +42,16 @@
#pragma once
#include <px4_module.h>
#include <px4_module_params.h>
#include <lib/hysteresis/hysteresis.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
using namespace time_literals;
src/modules/land_detector/MulticopterLandDetector.cpp
+20 -16
View File
@@ -91,7 +91,7 @@ void MulticopterLandDetector::_update_topics()
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_control_mode_sub.update(&_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
}
@@ -137,20 +137,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// 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;
bool vertical_movement = false;
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
verticalMovement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
} else {
// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
float maxClimbRate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate;
// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate;
}
// Check if we are moving horizontally.
@@ -161,11 +161,11 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled()
&& (_vehicle_local_position_setpoint.vz >= land_speed_threshold);
bool hit_ground = _in_descend && !verticalMovement;
bool hit_ground = _in_descend && !vertical_movement;
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
&& (!verticalMovement || !_has_altitude_lock())) {
&& (!vertical_movement || !_has_altitude_lock())) {
return true;
}
@@ -178,7 +178,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
const hrt_abstime now = hrt_absolute_time();
// When not armed, consider to be maybe-landed
if (!_actuator_armed.armed) {
if (!_actuator_armed.armed || (_vehicle_attitude.timestamp == 0)) {
return true;
}
@@ -199,11 +199,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}
// Next look if all rotation angles are not moving.
float maxRotationScaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled);
// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
@@ -251,6 +251,10 @@ float MulticopterLandDetector::_get_max_altitude()
/* TODO: add a meaningful altitude */
float valid_altitude_max = _param_lndmc_alt_max.get();
if (valid_altitude_max < 0.0f) {
return INFINITY;
}
if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f;
}
@@ -283,7 +287,7 @@ bool MulticopterLandDetector::_is_climb_rate_enabled()
bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);
return (_control_mode.flag_control_climb_rate_enabled && has_updated
return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
}
@@ -303,7 +307,7 @@ bool MulticopterLandDetector::_has_minimal_thrust()
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
// Determine the system min throttle based on flight mode
if (!_control_mode.flag_control_climb_rate_enabled) {
if (!_vehicle_control_mode.flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}
src/modules/land_detector/MulticopterLandDetector.h
+8 -12
View File
@@ -42,22 +42,19 @@
#pragma once
#include "LandDetector.h"
#include <math.h>
#include <parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include "LandDetector.h"
using namespace time_literals;
@@ -122,7 +119,6 @@ private:
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -132,17 +128,17 @@ private:
actuator_controls_s _actuator_controls {};
battery_status_s _battery_status {};
vehicle_control_mode_s _control_mode {};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_attitude_s _vehicle_attitude {};
vehicle_angular_velocity_s _vehicle_angular_velocity{};
vehicle_attitude_s _vehicle_attitude {};
vehicle_control_mode_s _vehicle_control_mode {};
vehicle_local_position_s _vehicle_local_position {};
vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};
hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _landed_time{0};
bool _in_descend{false}; ///< vehicle is desending
bool _in_descend{false}; ///< vehicle is desending
bool _horizontal_movement{false}; ///< vehicle is moving horizontally
DEFINE_PARAMETERS_CUSTOM_PARENT(